基于BDS的USV路徑規(guī)劃研究

王 儒,吳子岳,李二楊

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

0 引 言

為了保護長江流域生物資源,特別是中華鱘、江豚、長江刀魚等稀少魚類資源,國家出臺了相關政策法規(guī),在2020年底以前,完成水生生物保護區(qū)以及長江干流和重要支流水域的漁民退捕,暫定實行10年禁捕期[1],若發(fā)現(xiàn)非法捕魚行為,將進行嚴厲的打擊．但長江干流經(jīng)過11個省,全長6300多千米,實現(xiàn)全面禁捕的工作非常艱難,目前水面主要以水上執(zhí)法部門漁政監(jiān)管為主,船只、裝備不能滿足大水域下的監(jiān)管問題．

水面無人船(USV)是一種智能化的水面機器人,具有自主航行能力并可以完成信息采集、環(huán)境勘測、目標跟蹤等特殊任務[2]．在軍用和民用領域均發(fā)揮著重要的作用．在國際上,美國最早提出并發(fā)展USV[3],代表的有Sea Owl和Ghost Guard型號USV,服役海軍部隊,全面增強了軍隊的作戰(zhàn)能力．法國于2007年成功試水“Rodeur”號USV,實現(xiàn)海面?zhèn)刹?、反潛和監(jiān)視等特殊軍用級別的功能[4]．在國內(nèi), USV發(fā)展與發(fā)達國家相比,仍存在差距,目前仍處于起步發(fā)展階段,處于民用應用階段,代表的有:隸屬中國航天科工集團的沈陽新光公司研發(fā)設計的“天象1號”是應用于海上氣象監(jiān)測的USV[5];上海海洋大學研發(fā)的無人船裝置,可以在大水域環(huán)境下實現(xiàn)環(huán)境的在線監(jiān)測等功能[6]．

隨著科學技術的不斷向前發(fā)展,遠程控制技術成為水面監(jiān)測的一種重要手段．與傳統(tǒng)船只相比,微型USV更加方便、安全、經(jīng)濟．在監(jiān)測水面石油泄漏,發(fā)現(xiàn)非法捕撈漁船,水域環(huán)境檢測等發(fā)揮著不可替代的作用．本文主要研究設計了微型USV的整體結構系統(tǒng),針對路徑規(guī)劃問題提出了基于MATLAB/BDS相結合的方法,在多水域環(huán)境下實現(xiàn)路徑規(guī)劃的準確性．

1 USV模型建立

1.1 USV結構簡介

USV的結構系統(tǒng)較為復雜,通常是由機載本體、控制系統(tǒng)、信息采集系統(tǒng)等三部分組成,如圖1所示．USV的結構組成為:兩組70涵道無刷轉子電機涵道風扇,SKYWALKER ESC 80A電調(diào)、JH 6000 mAh 35C 14.8V可充電鋰電池、一體化的3D打印塑料機身;控制系統(tǒng)分為:STM32單片機作為主控制器、直流電池穩(wěn)壓模塊、正點原子北斗衛(wèi)星導航系統(tǒng)(BDS)定位模塊ATK1218-BD、 障礙物激光測距避障檢測選用型號為 RPLIDAR A1 ROS的激光雷達模塊、無線傳輸采用USR-LTE-7S4 V2無線模塊實現(xiàn)遠程3G/4G信息傳輸功能(C語言開發(fā)的專用上位機配套軟件),支持TCP/IP協(xié)議、GY-521 MPU6050電子陀螺儀模塊．信息采集系統(tǒng)有:圖像識別OV2640攝像頭模塊、水質傳感器、PH值檢測模塊等．

1.2 USV模型基本參數(shù)

在SOLIDWORKS中建立三維數(shù)字模型,添加約束條件,賦予USV船體材料屬性．使用3D打印制造出USV機身,如圖2所示．可以測定USV模型的基本參數(shù)值:重心相對于原點所在的位置為X=4.204 mm;Y=10.488 mm;Z=225.166 mm．表面積=189580.563 mm2．體積=361694.243 mm3．由重心決定的慣性主軸和慣性主力矩如表1所示.

表1 慣性主軸和慣性主力矩

(1:涵道風扇;2:USV機身;3:電調(diào);4:主控制器;5:其他功能模塊)圖2 USV三維模型

1.3 USV控制設計

本USV是由上位機軟件通過無線通訊模塊傳輸指令控制船載控制器 STM32 主控制器運行,還可以實現(xiàn)水面無人自主運行．驅動方式為兩個無刷電機涵道風扇提供推力,轉向是通過兩個電機的轉速差來實現(xiàn)的．本USV使用的 BDS 模塊,可以獲得USV機體的經(jīng)緯度、軌跡航線和對地航速等信息,為USV運動軌跡規(guī)劃提供位置信息支持．電子陀螺儀提供對地的船頭方向角等USV水面上的姿態(tài)．采用無線通訊模塊可以實現(xiàn)遠距離信息傳輸,通過串口RS232與其他模塊通訊,實現(xiàn)發(fā)送和接收信號,如圖3所示,使用VB編寫上位機軟件界面．本USV通過串口RS232與上位機、船載控制器 STM32 主控制連接．當USV的無線通訊模塊參數(shù)設置過后,通過預先設定的通信協(xié)議傳輸信息,USV上載體工控機和PC通過雙向通信實現(xiàn)數(shù)據(jù)無線傳輸．在讀取和發(fā)送RS232串口數(shù)據(jù)后,使用C語言編譯設定的程序指令,截取數(shù)據(jù)中的指令,然后船載控制器根據(jù)控制指令做出響應．信息采集系統(tǒng)中攝像頭可以監(jiān)測水面環(huán)境,其他環(huán)境檢測模塊可以進行大水域下的環(huán)境監(jiān)測等可拓展性功能．

圖3 微型USV上位機控制界面

2 BDS應用于USV原理

2.1 定位系統(tǒng)原理介紹

BDS是我國自主研發(fā)的定位導航系統(tǒng)，著眼于國家安全與社會發(fā)展進步，為全球用戶提供高精度的定位、導航、授時服務[7].BDS定位精度，在亞太地區(qū)可以達到2.5 m，全球范圍內(nèi)10 m；測試精度0.2 m/s；授時精度10 ns；還具有其特有的短報文通訊功能.

USV正常運轉時,接收到上位機的執(zhí)行命令后,定位模塊發(fā)出位置信號數(shù)據(jù)請求,接收來自BDS衛(wèi)星的數(shù)據(jù)信號;BDS定位模塊收到原始的數(shù)字信號后,通過信號預處理過濾出數(shù)據(jù)文件;通過RS-232串口經(jīng)過TXD/RXD數(shù)據(jù)傳輸線交叉連接方式將數(shù)據(jù)文件傳送于STM32;由STM32通過程序算法分離出經(jīng)緯度、時間和海拔等信息,并通過RS232串口總線經(jīng)過TXD/RXD無線傳輸方式傳送給管理中心存儲,如圖4所示.

圖4 導航系統(tǒng)工作原理

BDS衛(wèi)星的定位原理是:在任意時刻在地球上任一點能夠定位到 4 顆以上數(shù)量的衛(wèi)星．用戶發(fā)出定位信號請求后,每顆衛(wèi)星到發(fā)射信號源的距離通過電磁波來測定距離[8]．利用三維坐標中的距離公式可以求得信號源的具體位置．公式如下:

(1)

式中:(Xi,Yi,Zi)表示第i顆BDS衛(wèi)星的位置,i值取1,2,3,4;(X,Y,Z)表示發(fā)出信號的三維坐標;cT表示誤差時間與光速的乘積.

2.2 USV水面定位系統(tǒng)模型

當USV在水面運動時,為了簡化定位系統(tǒng)模型,其運動規(guī)律可以近似看作是在二維XY平面上的運動,因此,USV從M點到達N點的運動定位系統(tǒng)模型為

(2)

式中：(x0,y0)為M點的位置坐標,其坐標信息可以由BDS定位測量出；v0為USV的前進的速度;φ0為USV初始運動時的方位角度．當USV從M點前進到N點時,BDS/MATLAB均值定位系統(tǒng)可以測出此時點N的相對位置(x1,y1),同時也可以通過運動模型推斷出N點的相對計算位置,兩個數(shù)據(jù)坐標經(jīng)過相互修正最終得出USV的位置,可以通過這種方法多測量幾組位置坐標信息,提高USV運動系統(tǒng)整體的定位系統(tǒng)模型的精度．

3 USV運動公式模型建立

3.1 建立USV坐標系

為了能夠更加準確地描述USV的運動關系,采用慣性坐標系XoOoYo與運動坐標系XsOsYs,如圖 5 所示．慣性坐標系用以描述USV相對于空間的位置;運動坐標系用來描述作用在USV上的力,其坐標原點Os取在USV的重心,Xs軸的正向指向船首,Ys軸的正向指向船的右舷,Zs軸垂直于水平面,方向由右手定則確立．

圖5 慣性坐標系與運動坐標系

3.2 建立USV運動學關系

為了簡化運動學模型,只考慮USV在水平面內(nèi)的運動,其中沿著Xs軸直線運動速度用μ表示;沿Ys軸直線運動速度用υ表示;沿Zs軸轉動角速度用γ表示;平面內(nèi)USV運動坐標系的Xs軸與固定坐標系Xo軸的夾角為θ．

由于USV在重心所受的外力F外主要為涵道風扇推力Fpt慣性水動力Fi、粘性水動力Fe[9],它們的關系為

F外=Fpt+Fi+Fe,

(3)

式中,涵道風扇推力的合力Fpt、慣性水動力Fi、粘性水動力Fe在XsOsYs運動坐標軸上的投影分別可用Xpt和Ypt、Xi和Yi、Xe和Ye表示，Xe主要做阻力的作用,取負值.本文的USV是由兩個無刷電機涵道風扇驅動,依靠無刷電機轉動的差速實現(xiàn)轉向(如圖5所示),推進力主要作用在Xs軸上,因此Ypt可忽略．因此式(3)可表示為

(4)

3.3 涵道風扇推力方程

本文設計的USV采用兩個無刷電機涵道風扇作推動力．相對于其他采用船用螺旋槳作為推進形式,具有以下優(yōu)點:1)相比于涵道風扇,船用螺旋槳在傳動處密封性要求高,進水會使得整個控制電路以及電機嚴重損壞;2)在水下運行的螺旋槳,可能會纏繞到水草等不可確定的雜物；3)涵道風扇在水面上運行,推進器迎風面積小,且可以在空氣中產(chǎn)生較大推力,可以滿足USV的推進需求．風扇旋轉所產(chǎn)生的推力可表示為[10]

則推力可以表示為

(5)

式中：nt為第t臺電機的轉速;D為涵道風扇直徑;p為螺距;N為涵道風扇葉片．

3.4 慣性水動力

假設流體是在理想的狀態(tài)下,慣性水動力根據(jù)勢流理論可推導[11]為以下方程:

(6)

式中,mx、my是流體力學中在x和y方向上的附加質量．

3.5 粘性水動力方程

粘性水動力坐標系上可以分為橫向粘性水動力和縱向粘性水動力[12],下面依次建立兩種水動力的方程:

(7)

式中:Xe為橫向粘性水動力;S為船體濕面積；Cf為摩擦阻力系數(shù);Re為雷諾數(shù)；ρ為流體密度；L為特征長度；υ為水的動力粘性系數(shù)；ω為流場的特征速度;U為USV的前進速度．

Ye=Yνν+Yγγ+YNL,

(8)

式中：Ye為縱向粘性水動力;YNL為非線性水動力;Yνν和Yγγ為線性水動力方程．

Yν=-0.5ρLdu(0.5πk+1.4cbB/L)×

(1+τ/d×2/3)，

(9)

Yγ=0.5ρL2du[0.25πk(1+0.8τ/d)].

(10)

式中，k為船體進水部分展比弦,k=2d/L，L為USV長度，d為平均吃水深度．

3.6 推力方程

USV在水面上相對于固定坐標系的運動速度可表示為

(11)

而外力作用在USV上在固定坐標系可以表示為

(12)

外力在運動坐標系上分量可以表示為:Xs、Ys,它們與Xo、Yo的關系可表示為

(13)

聯(lián)合以上方程,經(jīng)過求導變換可以求得:

(14)

結合前面的外力方程式,可以得到推力方程為

(15)

3.7 沿Z軸旋轉角速度方程

USV不僅需要直線行駛,而且需要改變一定的角度來運動．旋轉一定的角度是通過兩個無刷電機涵道風扇轉速差來實現(xiàn)轉向,需要建立轉向方程,結合風扇推力方程(5),根據(jù)USV的轉向方程,可得到沿Zs軸旋轉的角速度與雙電機轉速的關系式為

(16)

式中：J為USV的轉動慣量;kω為轉動的阻尼系數(shù);α為推力差系數(shù);B為USV的物理寬度．

3.8 USV數(shù)學模型的建立

聯(lián)立以上求得的方程,可得到兩個涵道風扇無刷電機驅動的USV的三自由度運動控制系統(tǒng)的方程組

(17)

建立USV數(shù)學模型后,可以得到基于MATLAB/Simulink的USV模型,如圖 6 所示

圖6 基于MATLAB/Simulink的USV模型

4 路徑規(guī)劃原理

本文的USV路徑規(guī)劃是基于實時傳感信息的模糊控制算法[13],通過監(jiān)測障礙物與USV的距離和USV運動狀態(tài)信息,通過MATLAB基于知識庫模糊推理,解算出USV此時需要的速度變化Δν和轉角變化Δθ,模糊控制算法通過得到規(guī)劃信息,實現(xiàn)局部路徑規(guī)劃．該方法適用于未知環(huán)境下的路徑規(guī)劃研究,容易做到邊規(guī)劃邊跟蹤運動物體,實時性較好;同時克服了傳統(tǒng)的勢場法易產(chǎn)生的局部不穩(wěn)定問題．模糊控制算法程序流程如圖7所示．

圖7 路徑規(guī)劃模糊控制算法流程圖

5 實驗和結果與分析

建立控制系統(tǒng)運動模型后,需要進一步實驗在路徑規(guī)劃研究上的效果,水流的速度經(jīng)現(xiàn)場觀測后設定為0.2 m/s,USV可以一個穩(wěn)定的速度在水面上運動．

實驗1:目的是為了檢驗控制器能否可以正常地工作．通過控制USV至實驗指定的位置處,位置坐標設定為(0,0),初始的航向狀態(tài)角度為0°,船速設定為0.5 m/s．勻速行駛一段距離后,左涵道風扇以額定轉速繼續(xù)轉動,右涵道風扇停轉,此時USV實現(xiàn)了90°轉向,兩風扇以額定轉速繼續(xù)轉動使USV直線行駛一段距離．之后左涵道風扇停轉,右電機正常轉動,當USV轉向至0°時,繼續(xù)重復之前的控制步驟,實現(xiàn)了上階梯形運動．為了保證整體控制更加穩(wěn)定可靠,風扇旋轉的次序顛倒,實現(xiàn)下階梯運動．利用BDS觀測到階梯形實驗曲線效果如圖8(a)所示,實驗結果顯示USV整體控制系統(tǒng)穩(wěn)定,滿足進一步實驗要求．

實驗2:結合BDS與MATLAB均值數(shù)據(jù)驗證規(guī)劃路徑上的準確性．USV目標軌跡是采用SIMULINK運動中的模型,MATLAB同時接受BDS反饋的坐標信息,通過MATLAB和BDS雙坐標取雙坐標中值方法,通過無線通訊反饋給無人船的STM32主控制器處理,及時調(diào)整USV路徑規(guī)劃的軌跡．如圖8(b)所示,比較MATLAB測試數(shù)據(jù)、BDS測試數(shù)據(jù)、真實測試數(shù)據(jù)和MATLAB/BDS數(shù)據(jù)均值測試數(shù)據(jù),通過測試結果可以得出：基于MATLAB/BDS數(shù)據(jù)均值處理過后的路徑規(guī)劃精確度更高．BDS在USV實際運動中抽樣獲得了4處監(jiān)控位置的經(jīng)緯度信息,它們分別為開始位置坐標S:(30.8900N,121.9013E);中間經(jīng)過的兩個點的位置坐標M:(30.8894N,121.9007E)和N:(30.8885N,121.9011E);終點位置坐標E:(30.8887N,121.9021E),路徑信息與現(xiàn)場測試如圖9所示．

(b)路徑優(yōu)化圖8 USV實驗結果對比

圖9 試驗現(xiàn)場

6 結論與展望

本文以兩個無刷電機涵道風扇驅動的USV為研究對象,首先建立了USV整體結構模型,介紹了BDS路徑規(guī)劃原理及定位系統(tǒng)模型．針對研究對象,建立了USV運動學方程,最后研究了USV在水面二維空間內(nèi)基于MATLAB/BDS路徑規(guī)劃問題,USV路徑規(guī)劃的控制研究在水面無人船的領域內(nèi)具有廣泛的應用前景,通過將BDS與MATLAB結合起來應用于USV中,更好地為USV路徑規(guī)劃提供了更加精確的方案．通過對其進行現(xiàn)場的水上實驗得出:結合BDS實時同步的數(shù)據(jù)反饋、MATLAB規(guī)劃路徑、STM32控制板控制USV運行,使得USV能夠及時準確地優(yōu)化行駛路徑,得到更加精確的路徑規(guī)劃結果．

但用于控制和監(jiān)測USV的控制界面較為簡單,需要進一步編寫更加完善的上位機軟件控制系統(tǒng),滿足USV的綜合控制、定位巡航以及數(shù)據(jù)采集等更多的需求;在USV水面試驗過程中,整體表現(xiàn)良好,但供電系統(tǒng)不能提供長久的續(xù)航能力,后續(xù)需進一步設計研究,滿足航程更遠的需求．