張 臣, 劉 收, 李寶安,3

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

?

無人水面艇航跡跟蹤控制仿真

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

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

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

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

(1)

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

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

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

船的位置在航行過程中保持動態(tài)變化，根據(jù)船位置P的不同，實時的過渡目標點不同。

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

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

以A作為目標點D，駛向當前航路。

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

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

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

P距離航路AB大于臨界值R，Q作為目標點D進行航路修正。

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

P鄰域圓圓周與線段AB相交于兩點M1、M2，計算|BM1|、|BM2|并比較，取距離B點較遠的一點M作為目標點D。

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

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

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

以B作為目標點D。當P進入B鄰域，若存在則裝載下一航路，以B作為起始點，重新進行航路規(guī)劃。

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

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

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

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

(2)

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

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

如圖3所示為舵機模糊PID控制框圖，r(k)作為系統(tǒ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)過模糊運算得到參數(shù)修正值ΔKp、ΔKi、ΔKd。e(k)作為PID控制器輸入，Kp’、Ki’、Kd’作為控制器實際參數(shù)，輸出舵機調(diào)節(jié)值。

圖3 舵機模糊PID控制

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

(3)

位置式PID離散表達式如下：

(4)

2 MATLAB GUI仿真

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

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

圖4 MATLAB GUI仿真代碼流程圖

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

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

再次進行內(nèi)環(huán)舵角控制運算，航向控制運算結(jié)果得到的偏轉(zhuǎn)角r，與實際輸出偏轉(zhuǎn)角y進行比對，可以得到偏轉(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)進行模糊化，模糊整定得到ΔKp、ΔKi、ΔKd，計算Kp’、Ki’、Kd’計算PID控制器輸出。

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

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

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

表1 航點經(jīng)緯度坐標

3.1 舵角模糊PID控制仿真

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

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

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

3.2 直線航跡跟蹤控制仿真

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

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

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

3.3 折線航跡跟蹤控制仿真

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

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

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

3.4 預定航路航跡跟蹤控制仿真

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

圖8 預定航路航跡跟蹤控制仿真

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

4 結(jié)論

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

1)船航行過程中，內(nèi)層舵角控制采取模糊PID控制器簡潔有效，較短時間內(nèi)達到預期控制目標，控制器參數(shù)具有自適應性，可靠性強；

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

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

[1] 董祖舜.快艇動力學[M].武漢:華中理工大學出版社,1991.

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

[3] 姚云熙.水面無人艇在風浪流干擾下的運動仿真[D].哈爾濱：哈爾濱工程大學,2007.

[4] 吳恭興.水面智能高速無人艇的控制與仿真[D].哈爾濱：哈爾濱工程大學,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] 吳恭興,鄒 勁,孫寒冰,等.高速無人艇的運動建模及其視景仿真[J].大連海事大學學報,2010(1)：9-13.

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

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

[10] 耿以才,黃立新,陳凌珊,等.無人船安全目標追蹤與自動避障算法[J].計算機測量與控制,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-)，男，湖北荊州人，碩士研究生，主要從事無人器建模與控制方向的研究。

1671-4598(2016)03-0088-04

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

TP242.6

A