基于GPS/MATLAB的AUV路徑優(yōu)化分析

吳志峰， 吳子岳

(上海海洋大學工程學院，上海201306)

1 引 言

隨著我國海洋技術(shù)的發(fā)展，無線遠程控制技術(shù)已經(jīng)成為海洋環(huán)境監(jiān)測的一種趨勢，而移動測量也開始進入了研究范圍，大部分的淺海定測系統(tǒng)分為單點檢測和多點檢測，但價格較昂貴。為了降低成本實現(xiàn)大面積的測量海洋水質(zhì)環(huán)境，提出一種基于全球定位系統(tǒng)(GPS)/MATLAB規(guī)劃路徑的方法。

從模型建立方面考慮，綜合評估重力、浮力、推力和水動力對水下航行器(autonomous underwater vehicle,AUV)的影響。本文基于合適的刪選，建立了合理的6自由度水下航行器的動力學模型，設(shè)計一個精度合適的全球定位系統(tǒng)導航的路徑規(guī)劃方案。經(jīng)過多次水下試驗和MATLAB仿真，由GPS反饋給控制芯片STM32的實時定位數(shù)據(jù)，通過STM32處理MATLAB規(guī)劃數(shù)據(jù)和GPS反饋實時定位數(shù)據(jù)，最終實現(xiàn)移動AUV的定位以及運行軌跡的選擇，為糾正路徑規(guī)劃的路徑偏差提供一種參考方法。

2 AUV模型建立

2.1 AUV結(jié)構(gòu)

AUV的無人控制系統(tǒng)較為復雜,主要部分是一個能夠攜帶各種視覺傳感設(shè)備、探測裝置、推進器，且能在水下空間運動的載體。水下機器人通常由控制系統(tǒng)、監(jiān)察系統(tǒng)及載體3個部分組成。主要控制功能取決于水下機器人的具體功能，最簡單的是由視頻控制系統(tǒng)和用來反饋AUV運動。通過不斷論證、反復試驗，確定其結(jié)構(gòu)組成為：電機式水下推進器4組(控制速度差以實現(xiàn)偏航、行進速度可調(diào)共有9檔)、維持穩(wěn)定性的翼片3組(后緣鋒利、前緣漸變弧形以減小航行阻力)、57步進電機+絲杠滑臺組合(滑臺上的鉛塊前后滑動以控制航行器的重心并實現(xiàn)航行器的俯仰運動控制)、一體化玻璃鋼球形端蓋(便于密封及減阻)、φ160 mm耐壓艙體、電控系統(tǒng)(中央控制器、STM32單片機、3組電機驅(qū)動模塊、直流電池穩(wěn)壓模塊)、同波段透明協(xié)議無線數(shù)據(jù)傳輸收發(fā)組件(C語言開發(fā)的的專用配套軟件)、7.2 V飛思卡爾鋰電池(8組)、AG332 GPS信標差分機、密封式航空插頭及吊點緊箍等部件組成。AUV三維模型如圖1所示。

圖1 AUV三維模型Fig.1 3D model of AUV

2.2 AUV重心計算

將Cero中的總裝模型以“parasolid”格式導入ADAMS多體動力學仿真軟件中，對各部分已知參數(shù)和初始條件進行設(shè)置，包括質(zhì)量、材料屬性、轉(zhuǎn)動慣量、初始速度以及初始位置等，并添加約束條件以及外載荷，對其進行運動仿真。平均密度為2.753 g/cm3,質(zhì)量為18.85 kg，曲面面積為2.453 m2，重心在總體坐標位置為(x,y,z)=(-4.67，-4.63,-42.57)mm,AUV總體直徑160 mm，總長度1 000 mm。

相對總體坐標系的慣性張量參數(shù)值見表1。

表1 慣性張量參數(shù)值

2.3 AUV的控制設(shè)計

AUV的無線傳輸系統(tǒng)由無線傳輸系統(tǒng)和GPS差分定位系統(tǒng)確定AUV在水下的運動測量和觀察。由于水下作業(yè)要求精度較高，故AUV精度需要達到亞米級。本文AUV使用Trimble公司推出的AG332 GPS信標差分機進行GPS的定位，它自帶GPS羅盤定位性能和姿勢測量功能。采用無線傳輸模塊(透明協(xié)議)通過串口RS232與其他通訊，通過TXD/RXD直接連接方式發(fā)送信號；分別與工控機、岸基的PC和GPS差分定位系統(tǒng)連接。調(diào)試接口通過TCP協(xié)議與工控機連接。當AUV的無線通訊模塊參數(shù)設(shè)置后，通過透明協(xié)議傳輸信號，AUV上載體工控機和PC通過雙向通訊實現(xiàn)數(shù)據(jù)無線傳輸。AUV上位機控制界面如圖2所示，通過MSComm控件讀取和發(fā)送RS232串口數(shù)據(jù)后，使用VB的字符段編輯函數(shù)，如Instr函數(shù)，截取數(shù)據(jù)中的指令，然后主控系統(tǒng)根據(jù)指令作出響應。每當系統(tǒng)檢測到Start時，則系統(tǒng)開始截取指令；當系統(tǒng)截取到Over時，表明指令結(jié)束。在無線傳輸程序基礎(chǔ)上，主控程序主要由字符段編輯函數(shù)組成，負責識別并響應數(shù)據(jù)中的指令，系統(tǒng)的響應又是主要通過STM32驅(qū)動程序。

圖2 AUV上位機控制界面Fig.2 Host computer control interface of AUV

3 GPS定位系統(tǒng)

3.1 GPS定位系統(tǒng)設(shè)計

AUV控制系統(tǒng)體系結(jié)構(gòu)圖如圖3所示，GPS定位系統(tǒng)主要由GPS差分定位模塊、STM32控制模塊、無線傳輸模塊和PC機組成。在AUV航行后，AUV的上位機界面操作開始向定位模塊下達接收定位數(shù)據(jù)命；GPS定位模塊收到1/0數(shù)字信號后，模塊執(zhí)行命令，接收來自GPS衛(wèi)星的數(shù)據(jù)信號，并通過無線傳輸系統(tǒng)過濾出數(shù)據(jù)文件；無線傳輸系統(tǒng)通過RS-232串口經(jīng)過TXD/RXD數(shù)據(jù)傳輸線交叉連接方式將其傳送給STM32；由STM32通過程序算法分離出經(jīng)緯度、時間和海拔等信息，再通過RS232串口總線經(jīng)過TXD/RXD直接連接方式傳送給PC機處理及存儲。信標差分機通過RS232接口與工控機相連，設(shè)置差分格式等參數(shù)后，工控機可以采集相關(guān)數(shù)據(jù)并且可以對數(shù)據(jù)進行處理。該系統(tǒng)采用的是Trimble公司推出的AG332 GPS信標差分機 GPS定位方案，如圖4所示。

圖3 AUV控制系統(tǒng)Fig.3 Control System of AUV

圖4 定位方案設(shè)計Fig.4 Positioning scheme design of AUV

信標差分機主要通過DGPS模式進行全球定位，由GPS接收定位信號和信標機接收差分信息，信標機用來修正定位誤差。全球定位系統(tǒng)要實現(xiàn)具體物體的定位，最少需要4顆GPSIII型衛(wèi)星，其中3顆衛(wèi)星需要確定物體具體在地球上的三維坐標，第4顆衛(wèi)星需要修正衛(wèi)星在大氣和太空中所產(chǎn)生的誤差，以此信息來修正定位誤差。GPS使用的是美國的第三代GPSIII型衛(wèi)星進行DGPS定位模式。本文實驗采用民用C/A碼。

3.2 定位系統(tǒng)模型

當水下機器人在水下前進時，其運動可以近似看成在二維平面上的運動，水下機器人從A點到達B點的運動模型為：

(1)

式中：v0為水下機器人的初始前進速度；θ0為水下機器人的初始方位角；點A為水下機器人的初始位置(x0,y0)，可由GPS/MATLAB定位均值測出。

當水下機器人由A點前進到B點時，GPS/MATLAB均值定位可以測出此時點B的相對位置(x1，y1)；同時，也可以通過運動模型推斷出B點的相對位置，兩個數(shù)據(jù)經(jīng)過相互修正得出水下機器人的位置，提高系統(tǒng)整體的定位精度。

基于水下機器人的運動模型，構(gòu)建水下機器人的定位系統(tǒng)模型，以下公式通過運動模型考慮在時刻差之間的具體坐標位置表達出水下運動坐標，如式(2)所示：

(2)

式中：狀態(tài)變量x=[xd,yd,θd]T可以由GPS/MATLAB均值定位測量得到；xd是此時在x軸方向上的具體位置；yd是此時在y軸方向上的具體位置；θd是此時在傾角上的具體位置；Δd(k)是k時刻到k+1時刻水下機器人前進的距離；θ(k)為k時刻的運動方向；Δθ(k)為k時刻到k+1時刻水下機器人運行前進的轉(zhuǎn)彎角度；ω為定位系統(tǒng)噪聲，可認為在零均值狀態(tài)下的高斯白噪聲。

4 AUV運動模型建立

4.1 AUV受力分析

設(shè)計AUV的控制系統(tǒng)，首先需要構(gòu)建AUV的動力學模型。本文主要參考重力、浮力、推力、和水動力對水下航行運動軌跡的影響。重力、浮力、推力以及水動力的表達式為：

(3)

式中:F表示受到的力；τ表示受到的力矩；θ表示水平方向的轉(zhuǎn)角；φ表示垂直方向的傾角；G表示重力，這里取重心與坐標原點重合，故不產(chǎn)生重力矩。

(4)

式中:B表示水浮力;(xbybzb)T表示浮心在局部坐標系中的坐標。

第i個推進器產(chǎn)生的推力FTi：

(5)

式中:ρ表示水的密度；ni表示第i個推進器的螺旋槳轉(zhuǎn)速；Di表示螺旋槳直徑；KTi表示第i個推力器的推力系數(shù)。

推進器產(chǎn)生的合力和合力矩為

(6)

式中：Tx′、Ty′、Tz′表示沿x′、y′、z′軸方向上的合力；KT、MT、NT表示沿x′、y′、z′軸方向上的合力矩；RTi表示第i個推進器與之平行的坐標軸之間的垂直距離。

水動力和力矩為:

(7)

式中：Cd表示無因次阻力系數(shù)，對于大多數(shù)AUV，Cd=0.8～1.0；Sx′、Sy′、Sz′表示AUV分別垂直于x′、y′、z′軸的橫斷面面積;ur、vr、wr分別為x、y、z3個方向上的線速度。

水流產(chǎn)生的阻力矩與AUV角速度平方成正比，由試驗測得阻力矩系數(shù)Kx、Ky、Kz后，可以求得流體對AUV產(chǎn)生的阻力矩在x′、y′、z′軸上的投影分別為Kx′、Ky′、Kz′。

4.2 AUV動力模型的建立

AUV動力學方程的矩陣表達式[6]為

(8)

5 水下實驗與結(jié)果分析

根據(jù)建立的動力學模型和控制系統(tǒng)，采用MATLAB和SIMULINK對水下機器人以恒定速度跟蹤期望軌跡和GPS觀察規(guī)劃路線進行比較。AUV外形簡化為直徑0.16 m、長1 m的均質(zhì)圓柱體。水流的速度均為0.1 m/s，同時假定浮心和重心重合。假設(shè)水下航行器從坐標(0,0)到坐標(30,30)處沿直線運動，全球定位系統(tǒng)在實際監(jiān)控點測得的3處監(jiān)控位置經(jīng)緯度為(30.879 9 N,121.910 0 E)，(30.879 6 N,121.922 3 E)，(30.879 6 N,121.924 3 E),此時全球定位系統(tǒng)規(guī)劃的路線經(jīng)緯(30.879 9 N,121.910 0 E)，(30.879 9 N,121.928 3 E)，(30.881 9 N,121.929 3 E)，其中，N表示為北緯，E表示為東經(jīng)。

AUV實驗結(jié)果如圖5所示，能夠根據(jù)AUV的當前狀態(tài)，利用MATLAB指導AUV的路徑規(guī)劃，從而響應速度快，能夠相對比較穩(wěn)定和準確的規(guī)劃路線，驗證MATLAB控制器的穩(wěn)定性以及靈活性。

圖5 路徑曲線圖Fig.5 Path graph

圖6為經(jīng)緯測試數(shù)據(jù)圖。圖6中，可以根據(jù)AUV從坐標原點以0.2 m/s的速度追蹤目標，AUV目標軌跡是采用MATLAB和SIMULINK跟蹤期望，MATLAB同時接受GPS反饋的坐標信息，通過MATLAB和GPS雙坐標取雙坐標中值方法，通過閉環(huán)控制反饋給STM32控制芯片處理及時調(diào)整AUV路徑進行航行。比較MATLAB測試數(shù)據(jù)、GPS測試數(shù)據(jù)、真實測試數(shù)據(jù)和均值測試數(shù)據(jù)，測試結(jié)果顯示:基于MATLAB/GPS均值處理后路徑更加平穩(wěn),精確度更高。

圖6 經(jīng)緯測試數(shù)據(jù)圖Fig.6 Test chart of warp and weft

在AUV淺水下水試驗過程中，由如圖6得到四方差分析數(shù)值，實際測試數(shù)據(jù)為K，GPS測試數(shù)據(jù)為K1，MATLAB測試數(shù)據(jù)為K2以及均值測試數(shù)據(jù)為K3。以實際測試數(shù)據(jù)為參考依據(jù)，經(jīng)過方差分析得出K1為(1.020×10-8,2.060×10-8),K2為(1.070×10-6,2.070×10-10)，K3為(1.070×10-6,1.070×10-10)。均值處理在保持穩(wěn)定性上，在y軸精確性改善上有明顯提高。

6 結(jié)束語

(1) 通過將GPS與AUV相結(jié)合，確保AUV在淺水中能夠精確地定位與導航。基于全球定位系統(tǒng)生成的路徑軌跡，將MATLAB仿真實驗和GPS導航規(guī)劃的路線進行比較，驗證了MATLAB控制設(shè)計的實用性，可以通過GPS監(jiān)控MATLAB控制器的實用性。

(2) AUV路徑定位分析優(yōu)勢在于結(jié)合GPS實時同步反饋數(shù)據(jù)，通過STM32均值處理MATLAB規(guī)劃路徑和GPS實時定位數(shù)據(jù)，使得AUV能及時優(yōu)化路徑，得到更加優(yōu)化的路徑規(guī)劃的效果。