無(wú)人水面艇航跡跟蹤控制仿真

張 臣, 劉 收, 李寶安,3

(1.北京航空航天大學(xué) 自動(dòng)化科學(xué)與電氣工程學(xué)院，北京 100191; 2.北京航天測(cè)控技術(shù)有限公司，北京 100041;3.北京航空航天大學(xué) 無(wú)人駕駛飛行器設(shè)計(jì)研究所，北京 100191)

?

無(wú)人水面艇航跡跟蹤控制仿真

張 臣1, 劉 收2, 李寶安1,3

(1.北京航空航天大學(xué) 自動(dòng)化科學(xué)與電氣工程學(xué)院，北京 100191; 2.北京航天測(cè)控技術(shù)有限公司，北京 100041;3.北京航空航天大學(xué) 無(wú)人駕駛飛行器設(shè)計(jì)研究所，北京 100191)

針對(duì)模型參數(shù)不確定、存在外界風(fēng)浪流干擾的欠驅(qū)動(dòng)無(wú)人水面艇的航跡跟蹤問(wèn)題，為提高無(wú)人水面艇自主航行能力，減小航跡跟蹤過(guò)程中的軌跡偏差與跟蹤遲滯，提出了一種分層控制結(jié)構(gòu)的方法；由外至內(nèi)采取航跡、航向、舵角三層控制，外環(huán)航跡控制層使用過(guò)渡目標(biāo)點(diǎn)策略，根據(jù)無(wú)人水面艇的偏航距確定實(shí)時(shí)目標(biāo)點(diǎn)，中環(huán)航向控制層使用最小轉(zhuǎn)角策略，根據(jù)無(wú)人水面艇艏向角與目標(biāo)方向角之差確定偏轉(zhuǎn)打角，內(nèi)環(huán)舵角控制層使用模糊自適應(yīng)整定PID控制器，對(duì)模型參數(shù)的動(dòng)態(tài)變化進(jìn)行補(bǔ)償；基于MATLAB GUI進(jìn)行無(wú)人水面艇航跡跟蹤控制仿真試驗(yàn)，結(jié)果表明，模糊自適應(yīng)整定PID控制器提高了舵角的控制響應(yīng)速度，無(wú)人水面艇在航行過(guò)程中的直線行駛、連續(xù)折線行駛均能得到較好的航跡跟蹤控制。

無(wú)人水面艇；航跡跟蹤控制；分層控制結(jié)構(gòu)；模糊比例積分微分控制

0 引言

無(wú)人水面艇是一類能夠?qū)崿F(xiàn)無(wú)人自主操縱，具有自動(dòng)感知、規(guī)劃與航行能力的水面艦艇，主要用于執(zhí)行不適于有人船只執(zhí)行的軍事或民用任務(wù)。

無(wú)人水面艇航跡跟蹤控制是指在無(wú)人水面艇整體系統(tǒng)的作用下，使無(wú)人水面艇從預(yù)定的起始位置沿著指定軌跡一直航行到達(dá)目的位置。良好的航跡跟蹤控制系統(tǒng)是無(wú)人水面艇工作的基礎(chǔ)，能夠?yàn)闊o(wú)人水面艇各項(xiàng)功能的實(shí)現(xiàn)提供基本保證，具有重要的應(yīng)用價(jià)值。

1 航跡跟蹤控制結(jié)構(gòu)

1.1 航跡跟蹤控制結(jié)構(gòu)框圖

航跡跟蹤控制結(jié)構(gòu)如圖1所示，由3個(gè)閉環(huán)控制共同組成：外環(huán)航跡控制環(huán)、中環(huán)航向控制環(huán)和內(nèi)環(huán)舵角控制環(huán)。

圖1 航跡跟蹤控制結(jié)構(gòu)框圖

外環(huán)航跡控制環(huán)將定位裝置接收到的無(wú)人艇位置信息與預(yù)定航線進(jìn)行比對(duì)。若無(wú)人艇位置不在預(yù)定航線上，則航跡控制算法根據(jù)航跡偏差進(jìn)行運(yùn)算，輸出一個(gè)能消除航跡偏差的航向控制命令，作為中環(huán)航向控制環(huán)的輸入。

中環(huán)航向控制環(huán)將姿態(tài)檢測(cè)裝置獲取的無(wú)人艇實(shí)際航向與命令航向進(jìn)行比對(duì)。若實(shí)際航向與命令航向存在偏差，則航向控制算法根據(jù)航向偏差進(jìn)行運(yùn)算，輸出能消除航向偏差的舵角控制命令，作為內(nèi)環(huán)舵角控制環(huán)的輸入。

內(nèi)環(huán)舵角控制環(huán)檢測(cè)當(dāng)前舵角，與接收到的命令舵角進(jìn)行比對(duì)，若有偏差則驅(qū)動(dòng)舵機(jī)伺服系統(tǒng)使實(shí)際舵角到達(dá)命令位置。

航跡跟蹤控制系統(tǒng)通過(guò)3個(gè)閉環(huán)控制的共同作用，使無(wú)人艇在航行過(guò)程中消除航跡偏差，實(shí)現(xiàn)航跡跟蹤控制。這種控制結(jié)構(gòu)將位置控制與方向控制分離，捉高了系統(tǒng)的運(yùn)行可靠性。

1.2 外環(huán)航跡控制層

假設(shè)無(wú)人水面船在航行過(guò)程中，海平面為二維平面，船的實(shí)時(shí)位置用經(jīng)緯度表示。

假定船在某一時(shí)刻行駛某一指定航線，航線由起點(diǎn)A指向終點(diǎn)B，船的實(shí)時(shí)位置為點(diǎn)P。將船抽象為一個(gè)點(diǎn)，以船為中心，一定半徑的圓內(nèi)區(qū)域稱為船的鄰域。船的鄰域位置由船的經(jīng)緯度決定，船的鄰域大小由鄰域半徑R決定，可表示為：

(1)

定義以航線起點(diǎn)A為中心、半徑為R的圓為區(qū)域K1，以航線終點(diǎn)B為中心、半徑為R的圓為區(qū)域K2，垂直于AB且與線段AB相交的所有直線組成的區(qū)域?yàn)镵3，以K1、K2與K3的并集作為區(qū)域K。

如圖2所示，區(qū)域K將整個(gè)平面分割為三塊區(qū)域。當(dāng)船在區(qū)域K中，認(rèn)為船在當(dāng)前航路行駛中。當(dāng)船在區(qū)域K外且|PA|距離小于|PB|距離，認(rèn)為船正在駛向當(dāng)前航路。當(dāng)船在區(qū)域K外且|PA|距離大于|PB|距離，認(rèn)為船正在駛離當(dāng)前航路。

圖2 航域劃分與航行狀態(tài)區(qū)分

船的位置在航行過(guò)程中保持動(dòng)態(tài)變化，根據(jù)船位置P的不同，實(shí)時(shí)的過(guò)渡目標(biāo)點(diǎn)不同。

過(guò)點(diǎn)P作直線AB的垂線與直線AB相交于垂點(diǎn)Q，計(jì)算|PQ|的距離。連接PA、PB，并計(jì)算|PA|、|PB|的距離。求取目標(biāo)點(diǎn)D作為中環(huán)航向控制環(huán)的輸入：

1)Q在線段AB外，且R<|PA|<|PB|：

以A作為目標(biāo)點(diǎn)D，駛向當(dāng)前航路。

2)Q在線段AB外，且|PA|

P在A的鄰域內(nèi)，P鄰域圓圓周交線段AB于M，以M作為目標(biāo)點(diǎn)D。

3)Q在線段AB上，且|PQ|>R，|QB|>R：

P距離航路AB大于臨界值R，Q作為目標(biāo)點(diǎn)D進(jìn)行航路修正。

4)Q在線段AB上，且|PQ|R：

P鄰域圓圓周與線段AB相交于兩點(diǎn)M1、M2，計(jì)算|BM1|、|BM2|并比較，取距離B點(diǎn)較遠(yuǎn)的一點(diǎn)M作為目標(biāo)點(diǎn)D。

5)Q在線段AB上，且|QB|

P在B的鄰域內(nèi)，認(rèn)為P已經(jīng)到達(dá)當(dāng)前航路終點(diǎn)，舍棄當(dāng)前航路，若存在則裝載下一航路，以B作為下一航路起始點(diǎn)，以C作為下一航路終點(diǎn)，重新進(jìn)行航路規(guī)劃。

6)Q在線段AB外，且|PA|>|PB|：

以B作為目標(biāo)點(diǎn)D。當(dāng)P進(jìn)入B鄰域，若存在則裝載下一航路，以B作為起始點(diǎn)，重新進(jìn)行航路規(guī)劃。

1.3 中環(huán)航向控制層

船航行過(guò)程中，忽略真北與磁北差別。以真北為基準(zhǔn)，順時(shí)針旋轉(zhuǎn)到船艏方向，定義轉(zhuǎn)角為船的方向角。船的方向角α由姿態(tài)檢測(cè)裝置測(cè)量，范圍為0°到360°。

船的位置和方向角在航行過(guò)程中保持動(dòng)態(tài)變化。船的位置P與由外環(huán)航跡控制環(huán)解算的實(shí)時(shí)目標(biāo)點(diǎn)D形成矢量PD，真北方向順時(shí)針旋轉(zhuǎn)到PD方向形成的角度可作為目標(biāo)方向角θ，范圍為0°到360°。

船的當(dāng)前方向角α與目標(biāo)方向角θ形成方向角偏差，Δα=θ-α，范圍為-360°到360°。根據(jù)不同的Δα值，求取實(shí)際偏轉(zhuǎn)方向角r：

(2)

1.4 內(nèi)環(huán)舵角控制層

中環(huán)航向控制層解算的需求偏轉(zhuǎn)角r作為內(nèi)環(huán)舵角控制層輸入，內(nèi)環(huán)舵角控制層采取模糊PID進(jìn)行舵角控制。

如圖3所示為舵機(jī)模糊PID控制框圖，r(k)作為系統(tǒng)輸入，采集當(dāng)前輸出y(k)，則系統(tǒng)誤差e(k)=r(k)-y(k)。根據(jù)前次e(k-1)和此次e(k)可得de(k)/dt。e(k)及de(k)/dt作為模糊控制器輸入，經(jīng)過(guò)模糊運(yùn)算得到參數(shù)修正值ΔKp、ΔKi、ΔKd。e(k)作為PID控制器輸入，Kp’、Ki’、Kd’作為控制器實(shí)際參數(shù)，輸出舵機(jī)調(diào)節(jié)值。

圖3 舵機(jī)模糊PID控制

PID參數(shù)整定結(jié)果為：

(3)

位置式PID離散表達(dá)式如下：

(4)

2 MATLAB GUI仿真

MATLAB GUI仿真可分為前臺(tái)和后臺(tái)兩個(gè)部分。前臺(tái)對(duì)人界面友好，主要用于實(shí)時(shí)人工輸入指令和觀察仿真輸出狀態(tài)。后臺(tái)將前臺(tái)設(shè)置的具體變量值代入，進(jìn)行具體的運(yùn)算，將運(yùn)算結(jié)果反饋到前臺(tái)狀態(tài)欄，繪制航跡曲線，并通過(guò)UDP網(wǎng)絡(luò)端口將數(shù)據(jù)傳輸?shù)狡渌窒到y(tǒng)如VegaPrime三維視景仿真系統(tǒng)、GoogleEarth航跡地圖繪制系統(tǒng)等進(jìn)行互聯(lián)仿真。

MATLAB GUI程序包含.m代碼文件和.GUI界面文件。后臺(tái)程序主要包含在.m文件中，算法流程圖如圖4所示。

圖4 MATLAB GUI仿真代碼流程圖

首先進(jìn)行外環(huán)航跡控制運(yùn)算，獲取系統(tǒng)輸入的當(dāng)前航路AB，通過(guò)定位裝置獲取船的位置P，求取垂點(diǎn)Q，根據(jù)Q點(diǎn)的位置不同進(jìn)行分類討論，求取目標(biāo)點(diǎn)D。

其次進(jìn)行中環(huán)航向控制運(yùn)算，根據(jù)當(dāng)前船位置P以及目標(biāo)點(diǎn)位置D可以得到目標(biāo)方向角θ，通過(guò)姿態(tài)檢測(cè)裝置獲取船實(shí)時(shí)方向角α，可得到方向角偏差Δα，根據(jù)Δα值不同進(jìn)行分類，求取實(shí)際偏轉(zhuǎn)角r。

再次進(jìn)行內(nèi)環(huán)舵角控制運(yùn)算，航向控制運(yùn)算結(jié)果得到的偏轉(zhuǎn)角r，與實(shí)際輸出偏轉(zhuǎn)角y進(jìn)行比對(duì)，可以得到偏轉(zhuǎn)角誤差e(k)=r(k)-y(k)，根據(jù)前次e(k-1)以及此次e(k)可以得到微分值ec(k)=e(k)-e(k-1)，將e(k)與ec(k)進(jìn)行模糊化，模糊整定得到ΔKp、ΔKi、ΔKd，計(jì)算Kp’、Ki’、Kd’計(jì)算PID控制器輸出。

最后由PID控制器的輸出作用在無(wú)人水面艇，使艇發(fā)生位置改變和方向角轉(zhuǎn)動(dòng)，跟蹤目標(biāo)航線。重復(fù)上述過(guò)程，重新進(jìn)行下一輪的運(yùn)算，則可以使無(wú)人水面艇持續(xù)進(jìn)行航跡跟蹤控制，直到抵達(dá)目的地。

3 仿真結(jié)果及分析

選取江蘇省太湖水域作為航路規(guī)劃的參考位置，選取水域的經(jīng)度范圍在123.324°E至120.344°E，緯度范圍在31.437°N至31.447°N。規(guī)劃航路中的各個(gè)航點(diǎn)的經(jīng)緯度坐標(biāo)如表1所示。

表1 航點(diǎn)經(jīng)緯度坐標(biāo)

3.1 舵角模糊PID控制仿真

舵角控制環(huán)系統(tǒng)輸入為偏轉(zhuǎn)角r=15°，PID控制器初始參數(shù)kp0=21，ki0=10，kd0=17，設(shè)定各參數(shù)模糊規(guī)則語(yǔ)句，控制器輸出經(jīng)過(guò)一階濾波處理，無(wú)人水面船的傳遞函數(shù)為帶有實(shí)參的二階模型，仿真結(jié)果如圖5所示。

圖5 普通PID與模糊PID仿真結(jié)果對(duì)比

由于機(jī)械延遲，舵機(jī)在系統(tǒng)輸入2 s后開(kāi)始響應(yīng)。普通PID仿真值在4.5 s達(dá)到最大值，而模糊PID仿真值在4 s達(dá)到最大值，兩者超調(diào)均接近10%，在6 s左右系統(tǒng)趨于平緩無(wú)振蕩。由此可見(jiàn)，模糊PID相比普通PID響應(yīng)更為迅速，控制效果得到改善。

3.2 直線航跡跟蹤控制仿真

選取表1規(guī)劃航路的第五點(diǎn)(120.334°E，31.438°N)、第六點(diǎn)(120.340°E，31.440°N)組成當(dāng)前直線航路，設(shè)定船的初始位置為(120.335°E，31.4375°N)，仿真結(jié)果如圖6。

圖6 直線航跡跟蹤控制仿真

圖示連續(xù)直線表示指定航路，點(diǎn)狀曲線表示船的跟蹤軌跡。船起始位置不在指定航路上，在航跡跟蹤初始階段船朝著指定航路靠近，一段時(shí)間后船行駛至指定航路并保持穩(wěn)定行駛。

3.3 折線航跡跟蹤控制仿真

選取表1規(guī)劃航路的第五點(diǎn)(120.334°E，31.438°N)、第六點(diǎn)(120.340°E，31.440°N)與第七點(diǎn)(120.338°E，31.444°N)組成當(dāng)前折線航路，設(shè)定船的初始位置為(120.335°E，31.4375°N)，仿真結(jié)果如圖7。

圖7 折線航跡跟蹤控制仿真

船起始位置在指定航路外，初始階段靠攏并行駛至第一條航路，當(dāng)行駛中判斷到拐點(diǎn)時(shí)，進(jìn)行拐彎并行駛至下一航路。仿真曲線拐彎處的平滑度以及響應(yīng)快慢與設(shè)定的觸發(fā)距離以及航路拐角大小有關(guān)。

3.4 預(yù)定航路航跡跟蹤控制仿真

依表1所示航路點(diǎn)繪制預(yù)定航路，船初始位置設(shè)定為(120.325°E，31.4375°N)，航跡跟蹤曲線圖如圖8所示。

圖8 預(yù)定航路航跡跟蹤控制仿真

連續(xù)直線表示預(yù)定航路，點(diǎn)狀曲線表示跟蹤航跡。船從初始位置出發(fā)，首先完成“M”形折線航路行駛，再完成若干條直線航路行駛，在不同的直線航路之間包含角度各異的銳角和鈍角，最后船完成環(huán)路航行，回到起點(diǎn)。由圖可見(jiàn)，全程航跡跟蹤沒(méi)有發(fā)生航路偏離，直線行駛平穩(wěn)，拐角轉(zhuǎn)彎過(guò)渡自然。

4 結(jié)論

通過(guò)研究無(wú)人水面艇航跡跟蹤控制方法，按照分層結(jié)構(gòu)進(jìn)行獨(dú)立控制，仿真結(jié)果表明：

1)船航行過(guò)程中，內(nèi)層舵角控制采取模糊PID控制器簡(jiǎn)潔有效，較短時(shí)間內(nèi)達(dá)到預(yù)期控制目標(biāo)，控制器參數(shù)具有自適應(yīng)性，可靠性強(qiáng)；

2)外層航跡控制采取過(guò)渡目標(biāo)點(diǎn)策略以及中層航向控制采取最小偏轉(zhuǎn)角策略能夠快捷的幫助無(wú)人船實(shí)現(xiàn)航行決策，直線行駛和折線行駛均能平滑穩(wěn)定的實(shí)現(xiàn)航跡跟蹤。

本次仿真基于Matlab GUI平臺(tái)，理論計(jì)算結(jié)果可通過(guò)通信鏈路傳輸?shù)狡渌抡婺K進(jìn)行二次驗(yàn)證，也可以作為無(wú)人水面艇實(shí)際工程的測(cè)試依據(jù)，有助于無(wú)人水面艇實(shí)際航跡跟蹤控制系統(tǒng)的實(shí)現(xiàn)。

[1] 董祖舜.快艇動(dòng)力學(xué)[M].武漢:華中理工大學(xué)出版社,1991.

[2] 高 雙.高速無(wú)人艇的建模與控制仿真[D].哈爾濱：哈爾濱工程大學(xué),2007.

[3] 姚云熙.水面無(wú)人艇在風(fēng)浪流干擾下的運(yùn)動(dòng)仿真[D].哈爾濱：哈爾濱工程大學(xué),2007.

[4] 吳恭興.水面智能高速無(wú)人艇的控制與仿真[D].哈爾濱：哈爾濱工程大學(xué),2008.

[5] Caccia M, Bibuli M, Bono R, et al. Basic navigation, guidance and control of an unmanned surface vehicle[J]. Autonomous Robots, 2008, 25(4): 349-365.

[6] Bibuli M, Bruzzone G, Caccia M, et al. Path-following algorithms and experiments for an unmanned surface vehicle[J]. Journal of Field Robotics, 2009, 8(26): 669-688.

[7] 吳恭興,鄒 勁,孫寒冰,等.高速無(wú)人艇的運(yùn)動(dòng)建模及其視景仿真[J].大連海事大學(xué)學(xué)報(bào),2010(1)：9-13.

[8] 王國(guó)英.欠驅(qū)動(dòng)水面艇運(yùn)動(dòng)控制建模及三維仿真實(shí)現(xiàn)研究[D].哈爾濱：哈爾濱工程大學(xué),2011.

[9] 隋曉麗.水面無(wú)人艇航跡控制算法研究[D].大連：大連海事大學(xué),2013.

[10] 耿以才,黃立新,陳凌珊,等.無(wú)人船安全目標(biāo)追蹤與自動(dòng)避障算法[J].計(jì)算機(jī)測(cè)量與控制,2015,23(7):2467-2469.

Simulation of Path Tracking Control of Unmanned Surface Vessel

Zhang Chen1, Liu Shou2, Li Baoan1,3

(1.School of Automation Science and Electrical Engineering, Beihang University, Beijing 100191, China;2.Beijing Aerospace Measurement and Control Technology Co., Ltd, Beijing 100041, China;3.Unmanned Aerial Vehicle Design Institute, Beihang University, Beijing 100191, China)

In order to improve the autonomous navigation ability of Unmanned Surface Vessel (USV) and reduce path error and track delay of path tracking, this paper proposes a hierarchical control structure for the trajectory tracking of unmanned surface vehicle under external wind, wave and current disturbances with model parameters uncertainly. The structure can be divided into three layers from outside to inside. Outermost layer of track control takes transition destination strategy to determine the real-time target position by the cross track distance. Intermediate layer of direction control takes minimum rotation angle strategy to determine the deflection angle by the difference between the heading angle and the given path angle. Innermost layer of rudder angle control designs a fuzzy PID controller to compensate the dynamic changes of model parameters. Under the MATLAB GUI environment a simulation was completed and the results indicate that the fuzzy PID controller increases the response speed of the rudder angle control and the USV have a good path tracking control in linear driving as well as nonlinear driving.

unmanned surface vessel; path tracking control; hierarchical control structure; fuzzy PID

2015-09-25；

2015-10-26。

張 臣(1990-)，男，湖北荊州人，碩士研究生，主要從事無(wú)人器建模與控制方向的研究。

1671-4598(2016)03-0088-04

10.16526/j.cnki.11-4762/tp.2016.03.024

TP242.6

A