水下機器人運動控制系統(tǒng)設計與分析①

張歡歡,田軍委,熊靖武,趙彥飛,史珂路

(西安工業(yè)大學 電子信息工程學院,西安 710021)

隨著海洋科學研究的進一步開展,人類探索海洋的難度與深度也越來越大.水下帶纜遙控機器人(Remotely Operated Vchicle,ROV)因其安全、高效、續(xù)航力強而日益成為開發(fā)海洋資源的重要工具,是最早得到開發(fā)和目前應用最成熟的無人潛水器[1–3].

國內對ROV的研究起于20世紀70年代末,相比于歐美國家和日本處于落后水平,目前我國比較先進的ROV是“海龍?zhí)枴鄙詈C器人,它在前人的基礎上解決了臍帶纜斷裂的問題.2002年,中科院沈陽自動化研究所成功研制了中國第一臺能夠進行水下作業(yè)的ROV——“CISTAR”,隨后提出了一種基于水平面模糊避障規(guī)劃和垂直面模糊避障規(guī)劃相結合的三維實時避障規(guī)劃方法.2014 年,“海馬號”ROV 搭乘“海洋六號”綜合科學考察,分三個航段在南海進行海上實驗,標志著我國全面掌控了大深度無人遙控潛水器的各項關鍵技術[4,5].

本文以不斷發(fā)展的ROV為研究對象,設計了基于CAN總線ROV的運動控制系統(tǒng),以實現(xiàn)ROV參數采集、上浮下潛、轉向、懸停等姿態(tài)的控制.要達到以上運動要求,指令信息傳遞的載體則顯得極為重要,它起著控制箱與水下機器人信息傳遞的作用[6–9].

1 水下機器人運動控制系統(tǒng)設計

1.1 水下機器人結構組成

水下機器人控制系統(tǒng)中,操作人員只要下達指令,水下機器人就會做出相應的動作.水下機器人主要有水上控制平臺和水下執(zhí)行機構兩部分,水上控制平臺主要是一個控制箱,能夠對機器人下達指令和接收水下機器人不同時刻的狀態(tài)數據.水上控制平臺下達控制指令,其中CAN總線作為控制箱與水下機器人信息傳遞的橋梁,水下主控器則會控制各執(zhí)行機構作出相應動作.

姿態(tài)檢測系統(tǒng)輸出當前水下機器人的實時運動姿態(tài)和位置信息,與指定的位置信息進行比較,經過相關的程序和算法計算后,可以得到相應的位置誤差,對水下機器人進行操作,主控器下達PWM指令控制驅動系統(tǒng),調整推進器的速度,進而調整水下機器人的工作狀態(tài),使水下機器人到達預定的目標位置.

水下機器人的動力部分是安裝在四個不同位置的推進器,它是由相應的驅動器進行控制,驅動器接收到控制信息就會驅動推進器,按照我們的要求做出合理的調整,推動機器人運動,硬件框圖如圖1所示.

水下機器人的穩(wěn)定性可以被定義為在不存在修正行為的條件下,在受到擾動后能夠回到平衡狀態(tài)的能力,在運動控制系統(tǒng)硬件方案的設計中,整體方案的響應速度對水下機器人的穩(wěn)定性控制是有一定聯(lián)系的,基于此,對水下機器人運動控制系統(tǒng)進行設計.

圖1 機器人本體硬件框圖

1.2 水下機器人運動控制系統(tǒng)

如圖2,涵蓋了整個運動控制系統(tǒng)中的主控單元、通訊單元、檢測單元、驅動單元及輔助單元,整個系統(tǒng)由主控單元中的運動控制器STM32F407來控制;CAN總線實現(xiàn)水下機器人與水上操控箱之間的雙向通訊,是信息傳遞的橋梁;整個控制系統(tǒng)中的檢測模塊有9軸姿態(tài)傳感器、漏水檢測傳感器、水深傳感器以及溫度傳感器;擴展的接口包括無線通訊接口、GPS擴展預留接口、聲吶預留接口及陀螺儀預留接口,使得之后機器人功能更加完善;動力單元是安裝在四個不同位置的推進器及調重心步進電機,分別由電調及步進電機驅動板進行驅動;輔助單元包括攝像頭、LED燈及激光器,LED燈由控制器經過控制板進行控制,繼電器控制整個水下運動控制系統(tǒng)的運行.

圖2 水下機器人運動控制系統(tǒng)

如圖3為水下運動控制系統(tǒng)實體圖,目前已完成了對于主控板、控制繼電器、激光器以及攝像頭等器件的安裝及連接,出于節(jié)省空間的考慮,將各個傳感器如姿態(tài)傳感器、溫濕度傳感器以及電調都置于底板上,將單片機用螺柱支撐起來,整個控制系統(tǒng)呈現(xiàn)為二層結構.

圖3 水下運動控制實體圖

1.3 控制系統(tǒng)各模塊軟件設計1.3.1 軟件設計

水下機器人的運動控制系統(tǒng)包括網絡通信模塊、串口通信模塊、傳感器數據采集模塊、相應數據處理模塊、控制器模塊.主控器對傳感器的信息處理和水下機器人推力分配算法等統(tǒng)一在實時操作系統(tǒng)的架構下進行管理,從而有效提高動力定位控制系統(tǒng)的實時性和可靠性.實時操作系統(tǒng)接收采集到的數據和來自串口通信的數據,保證數據運行的高效性和實時性.

根據水下機器人運動控制系統(tǒng)功能結構,將系統(tǒng)軟件進行模塊化設計,使主程序分成若干個子程序模塊分別進行編寫、設計和調試,這樣可以提高編程的效率和準確性,易于查找錯誤,使程序更具可讀性.本課題的運動控制系統(tǒng)軟件部分主要由5個模塊組成,其結構框圖如圖4所示.

圖4 運動控制系統(tǒng)軟件結構框圖

1.3.2 各模塊流程框圖

主程序開始時,首先是對STM32F407單片機進行初始化,然后通過傳感器分別采集水下機器人外部周圍水介質溫度數據信息、內部溫濕度數據信息和水深數據信息,而通過采用中斷的方式,一方面是對水下機器人姿態(tài)信息的采集,另一方面是接收外部中斷控制指令,控制推進器,從而實現(xiàn)對水下機器人的姿態(tài)控制,主程序流程圖如圖5所示.

圖5 主程序流程圖

溫濕度采集模塊作為檢測水下機器人內部是否發(fā)生泄漏的傳感器,在保障水下機器人安全運行方面有著極其重要的作用.溫濕度采集模塊選用的SHT30-DIS溫濕度傳感器,其測量分辨率分別為16 bit(溫度),16 bit(濕度),其程序流程圖如圖6所示.

圖6 DHT30 數據采集程序流程圖

壓力采集模塊主要是對水下機器人當前位置水壓力及水溫數據的采集,它也是一個模擬量的采集過程,需要將其模擬量轉化為數字量,得到壓力值,再通過計算將壓力值轉化為距離(深度)信息,從而定位水下機器人的位置,選擇供電電壓為3.3 V的MS5837型號的壓力傳感器,水深測量分辨率高達2 mm.模擬量采集程序流程圖如圖7所示.

圖7 模擬量采集程序流程圖

姿態(tài)傳感器主要是采集水下機器人當前的姿態(tài)信息,該模塊與單片機之間通過串口協(xié)議進行數據的傳遞,每當給模塊發(fā)出一個中斷信號,數據采集就會更新一次,從而不斷記錄姿態(tài)信息,如圖8,圖9,本文選用的是JY901電子陀螺儀(三維角度傳感器,原理圖如圖10),它集成了高精度的陀螺儀、加速度計、地磁場傳感器,采用高性能的微處理器和先進的數字濾波技術,能有效降低測量噪聲,提高測量精度,模塊內部集成了姿態(tài)解算器,配合動態(tài)卡爾曼濾波算法,能夠在動態(tài)環(huán)境下準確輸出模塊的當前姿態(tài),在本文中即為水下機器人的姿態(tài)信息,圖11為其上位機顯示界面,界面內容包含有數據讀取窗口、圖像數據顯示窗口、三維方向指示窗口和磁場指示窗口,從數據讀取窗口可以分別讀取到當前模塊的時間、三維角速度、三維角度、三維加速度、三維磁場數據,同時可以在圖像數據顯示窗口各個參量變化趨勢.

圖8 接收中斷

圖9 數據讀取

圖10 JY901 電路原理圖

圖11 姿態(tài)傳感器上位機界面顯示結果

2 推進器推力及測試

2.1 推進器推力分布

圖12為推進器推力分示意圖,其中(a)為豎直方向的推進器分布圖,(b)為水平方向的推進器分布圖,如圖(c),F1、F2為水平方向的推進器轉動時ROV所受的力,F3、F4為豎直方向的推進器轉動時ROV所受的力.

圖12 推進器推力分布示意圖

水平和豎直面的推進器分布采用的是雙推進器平行分布,推力和力矩計算如下:

2.2 推進器測試

圖13為推進器測試時實物接線圖,其中電調為50 HZ PWM 信號控制,1-2 ms PWM 范圍,1.5 ms為中位停轉,驅動電機占空比為5%-10%,其中7.5%為中位停轉信號.

圖13 推進器測試

如表1、表2,表明此電調可作為推動推進器的裝置,推進器空轉的電壓、啟動電流與轉動電流隨轉速的增大而增加.

表1 推進器正轉數據

表2 推進器反轉數據

3 傾角與下潛深度控制方案及仿真分析

3.1 傾角與下潛深度控制方案

調整和保持下潛深度是水下機器人的基本航行能力之一,本文中水下機器人用垂直方向的兩個推進器調節(jié)深度,通過調整推進器轉速調節(jié)ROV的深度,從而實現(xiàn)其下潛及上浮運動,所以要考慮機器人浮力變化的過程,也就是要注重推力計算.推進器轉速v=0時,浮力F=0,ROV 在水面保持平衡,處于正浮力狀態(tài).當v增大到一定值不變時,浮力減少到0,進入水下平衡狀態(tài),繼續(xù)增大v,機器人下潛.同時調節(jié)四個不同位置推進器的v,產生各個角度相異的浮力可以使機器人具有不同的水下姿態(tài),俯仰傾斜,左右傾斜,以至于擴大照明系統(tǒng)的可視范圍,增加探測方位及面積,正常情況下機器人的傾斜角度無需控制,只需保持水平與豎直方向兩個推進器的v一致即可.

傾角控制的目的是在機器人初始下潛以及探測階段,控制機器人的航行姿態(tài),在水下大部分時間,保持水平即可,其中水平傾角θ以及豎直傾角σ的控制是通過調節(jié)推進器的v產生浮力差,即產生上下左右旋轉的偏頗力矩.

如圖14,ROV的動力系統(tǒng)是一個典型的閉環(huán)反饋控制系統(tǒng),其核心是帶有算法的控制器,首先給出姿態(tài)指令,然后將測量系統(tǒng)(傳感器)實時測得的ROV位置經融合處理后與給定值進行比較獲得誤差信號,并將其進行簡單的坐標變換處理后作為控制器的輸入,控制器根據相應算法計算并且輸出推力或推力矩指令,推進器根據指令使得ROV保持在給定深度.

3.2 ROV運動模型及仿真分析

如圖15采用兩種右手直角坐標系即固定坐標系E-ξηζ和運動坐標系O-xyz.

把水下機器人的運動坐標系的零點相對于固定坐標系的速度U在O-xyz坐標系上的三個投影的分量非別是u(橫向速度)、v(縱向速度)和w(垂向速度);水下機器人繞固定坐標系的原點的角速度Ω在O-xyz坐標系上的三個投影分量分別是p(橫搖角速度)、q(縱搖角速度)和r(搖舷角速度);我們把作用在水下機器人本體結構上的外力F在O-xyz坐標系上的三個分量稱為X(縱向力)、Y(橫向力)和Z(垂向力);對應的把水下機器人的三個力矩的分量稱為K(橫搖力矩)、M(縱傾力矩)和N(搖艏力矩).我們規(guī)定把坐標軸的正半軸作為水下機器人的運動速度和力的正向,按照右手螺旋規(guī)則確定角速度和力矩服的正向.用運動坐標系來表示水下機器人的六個自由度的運動和力(力矩).

圖14 ROV 定位控制系統(tǒng)框圖

圖15 水下機器人的空間運動坐標系

參考文獻[10],根據剛體動力學理論相關知識,利用牛頓—歐拉方程建立水下機器人空間六自由度運動的一般方程,由一般方程可得ROV縱垂面運動簡化方程為:

其中,m為水下機器人質量;u、w、q為 (角)速度;、、為(角)加速度;xG、zG是水下機器人的重心坐標參數;X、Z、M是力(矩);Iy為水下機器人圍繞運動坐標系的三個坐標軸的轉動慣量.

參考文獻[11],在將ROV結構參數確定好之后,考慮到推進器推力參數,ROV所受浮力與重力,水下慣性力系數及粘性阻力系數等因素影響,代入式(3)簡化后可得縱傾角速度的傳遞函數為:

由(4)式,可以得到縱傾角的傳遞函數:

進而可以得到深度傳遞函數為:

利用MATLAB中的simulink模塊進行PID控制器的設計及仿真.縱傾角閉環(huán)控制系統(tǒng)方框圖如圖16.在輸入單位階躍信號時,利用試湊法對水下機器人的數學模型進行調節(jié),最后選擇了一組較為理想的調節(jié)參數,其中Kp=7、Ki=0.01,Kd=3,得單位階躍輸入下的響應曲線結果如圖17所示.

圖17 縱傾角單位階躍響應曲線

深度值的閉環(huán)控制系統(tǒng)如圖18所示.

圖18 深度的閉環(huán)控制系統(tǒng)

Kp=15、Ki=0.01,Kd=8,得單位階躍輸入下的響應曲線結果如圖19所示.

圖19 深度的單位階躍響應曲線

由系統(tǒng)仿真圖可得,運動控制系統(tǒng)具有較快的響應速度,響應時間短等優(yōu)點,證實了整個系統(tǒng)的可靠性.

4 結論

本文建立了水下機器人的運動控制系統(tǒng),設計了基于STM32F407的水下機器人嵌入式控制器并介紹了各個功能模塊的具體實現(xiàn)方法,進行了縱傾角及深度仿真分析.在后期的研究中會進一步對水下機器人進行算法控制,提高其精確度,并會將機器人密封性完善后進行水下實時監(jiān)測控制.