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

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

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

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

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

1 水下機(jī)器人運(yùn)動(dòng)控制系統(tǒng)設(shè)計(jì)

1.1 水下機(jī)器人結(jié)構(gòu)組成

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

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

水下機(jī)器人的動(dòng)力部分是安裝在四個(gè)不同位置的推進(jìn)器,它是由相應(yīng)的驅(qū)動(dòng)器進(jìn)行控制,驅(qū)動(dòng)器接收到控制信息就會(huì)驅(qū)動(dòng)推進(jìn)器,按照我們的要求做出合理的調(diào)整,推動(dòng)機(jī)器人運(yùn)動(dòng),硬件框圖如圖1所示.

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

圖1 機(jī)器人本體硬件框圖

1.2 水下機(jī)器人運(yùn)動(dòng)控制系統(tǒng)

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

圖2 水下機(jī)器人運(yùn)動(dòng)控制系統(tǒng)

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

圖3 水下運(yùn)動(dòng)控制實(shí)體圖

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

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

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

圖4 運(yùn)動(dòng)控制系統(tǒng)軟件結(jié)構(gòu)框圖

1.3.2 各模塊流程框圖

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

圖5 主程序流程圖

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

圖6 DHT30 數(shù)據(jù)采集程序流程圖

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

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

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

圖8 接收中斷

圖9 數(shù)據(jù)讀取

圖10 JY901 電路原理圖

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

2 推進(jìn)器推力及測試

2.1 推進(jìn)器推力分布

圖12為推進(jìn)器推力分示意圖,其中(a)為豎直方向的推進(jìn)器分布圖,(b)為水平方向的推進(jìn)器分布圖,如圖(c),F1、F2為水平方向的推進(jìn)器轉(zhuǎn)動(dòng)時(shí)ROV所受的力,F3、F4為豎直方向的推進(jìn)器轉(zhuǎn)動(dòng)時(shí)ROV所受的力.

圖12 推進(jìn)器推力分布示意圖

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

2.2 推進(jìn)器測試

圖13為推進(jìn)器測試時(shí)實(shí)物接線圖,其中電調(diào)為50 HZ PWM 信號控制,1-2 ms PWM 范圍,1.5 ms為中位停轉(zhuǎn),驅(qū)動(dòng)電機(jī)占空比為5%-10%,其中7.5%為中位停轉(zhuǎn)信號.

圖13 推進(jìn)器測試

如表1、表2,表明此電調(diào)可作為推動(dòng)推進(jìn)器的裝置,推進(jìn)器空轉(zhuǎn)的電壓、啟動(dòng)電流與轉(zhuǎn)動(dòng)電流隨轉(zhuǎn)速的增大而增加.

表1 推進(jìn)器正轉(zhuǎn)數(shù)據(jù)

表2 推進(jìn)器反轉(zhuǎn)數(shù)據(jù)

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

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

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

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

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

3.2 ROV運(yùn)動(dòng)模型及仿真分析

如圖15采用兩種右手直角坐標(biāo)系即固定坐標(biāo)系E-ξηζ和運(yùn)動(dòng)坐標(biāo)系O-xyz.

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

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

圖15 水下機(jī)器人的空間運(yùn)動(dòng)坐標(biāo)系

參考文獻(xiàn)[10],根據(jù)剛體動(dòng)力學(xué)理論相關(guān)知識(shí),利用牛頓—?dú)W拉方程建立水下機(jī)器人空間六自由度運(yùn)動(dòng)的一般方程,由一般方程可得ROV縱垂面運(yùn)動(dòng)簡化方程為:

其中,m為水下機(jī)器人質(zhì)量;u、w、q為 (角)速度;、、為(角)加速度;xG、zG是水下機(jī)器人的重心坐標(biāo)參數(shù);X、Z、M是力(矩);Iy為水下機(jī)器人圍繞運(yùn)動(dòng)坐標(biāo)系的三個(gè)坐標(biāo)軸的轉(zhuǎn)動(dòng)慣量.

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

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

進(jìn)而可以得到深度傳遞函數(shù)為:

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

圖17 縱傾角單位階躍響應(yīng)曲線

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

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

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

圖19 深度的單位階躍響應(yīng)曲線

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

4 結(jié)論

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