一種欠驅(qū)動(dòng)無(wú)人船避障及目標(biāo)跟蹤魯棒控制方法

張亞博，姜 明，肖 強(qiáng)，王 軍，陳 霖，張海燕

（1. 北京航天發(fā)射技術(shù)研究所，北京100076；2. 北京中星時(shí)代科技有限公司，北京100070）

1 引 言

無(wú)人船是一種能夠在海洋環(huán)境中自主航行，并且能夠完成各種海上任務(wù)的運(yùn)載平臺(tái)。無(wú)人船因其加工成本低、制造周期短、環(huán)境適應(yīng)能力強(qiáng)等特點(diǎn)，可以用于各種海上任務(wù)。這也為進(jìn)一步開(kāi)發(fā)海洋資源提供了一種更為安全的方式。因此，無(wú)人船的研究已成為船舶行業(yè)的熱點(diǎn)問(wèn)題[1-2]。

為使無(wú)人船在復(fù)雜的海洋環(huán)境中具有自主航行的能力，首先需要解決其對(duì)目標(biāo)的跟蹤控制問(wèn)題。羅偉林等[3]針對(duì)船舶航向非線性系統(tǒng)，提出了一種基于神經(jīng)網(wǎng)絡(luò)方法的魯棒跟蹤控制器。夏云鵬等[4]針對(duì)無(wú)人船航跡控制問(wèn)題，提出了基于模擬退火優(yōu)化NMPC 算法的非線性模型預(yù)測(cè)控制算法，設(shè)計(jì)了直接型無(wú)人船航跡控制器，該算法能夠提高航跡控制的準(zhǔn)確性和實(shí)時(shí)性。Breivik等[5]采用一種新的導(dǎo)航方法，實(shí)現(xiàn)了對(duì)任意路徑的跟蹤控制。Miao J 等[6]基于時(shí)滯控制方法和降階線性擴(kuò)展?fàn)顟B(tài)觀測(cè)器（LESOS）技術(shù)，提出了一種改進(jìn)的復(fù)合視距制導(dǎo)律，其能估計(jì)未知的側(cè)滑角，并能補(bǔ)償時(shí)變海流的影響。廖煜雷等[7]考慮了外界干擾以及船舶模型參數(shù)不確定的問(wèn)題，采用滑模變結(jié)構(gòu)控制方法，設(shè)計(jì)了軌跡跟蹤控制器，通過(guò)仿真，驗(yàn)證了該控制器的有效性。

其次，在無(wú)人船航行的過(guò)程中，其周圍的環(huán)境可能會(huì)出現(xiàn)緊急情況，比如航路上出現(xiàn)障礙物。這就需要無(wú)人船有碰撞規(guī)避的能力。對(duì)此問(wèn)題劉琨等[8]提出了一種改進(jìn)的人工勢(shì)場(chǎng)法，設(shè)計(jì)了無(wú)人船碰撞規(guī)避控制算法。陳文玉[9]對(duì)無(wú)人船的避碰算法問(wèn)題，基于改進(jìn)的人工勢(shì)場(chǎng)法進(jìn)行了規(guī)則約束下具體避碰算法的設(shè)計(jì)與實(shí)現(xiàn)。

本文針對(duì)欠驅(qū)動(dòng)無(wú)人船的碰撞規(guī)避和目標(biāo)跟蹤問(wèn)題，提出了一種基于滑模變結(jié)構(gòu)控制和改進(jìn)人工勢(shì)函數(shù)理論的控制方法。首先建立了考慮不確定干擾和參數(shù)攝動(dòng)的無(wú)人船水平面二階非線性動(dòng)力學(xué)模型，通過(guò)引入滑?？刂品椒槠湓O(shè)計(jì)了目標(biāo)跟蹤魯棒控制器。同時(shí)基于改進(jìn)人工勢(shì)函數(shù)設(shè)計(jì)了無(wú)人船碰撞規(guī)避控制算法。最后對(duì)設(shè)計(jì)的控制算法進(jìn)行了穩(wěn)定性證明和數(shù)值仿真。仿真結(jié)果表明，提出的控制方法在無(wú)人船做平面運(yùn)動(dòng)時(shí)可以有效進(jìn)行目標(biāo)跟蹤和碰撞規(guī)避。

2 無(wú)人船動(dòng)力學(xué)模型

為描述無(wú)人船的運(yùn)動(dòng)，首先引入兩個(gè)參考坐標(biāo)系，即地面坐標(biāo)系Se和船體坐標(biāo)系Sb，如圖1 所示。地面坐標(biāo)系用來(lái)描述船在慣性系下的位置和姿態(tài)，選取地球表面任意位置為其原點(diǎn)，X軸指向正北方向，Y軸指向正東方向，Z指向地心方向，Y軸與X軸、Z軸構(gòu)成右手坐標(biāo)系。船體坐標(biāo)系用來(lái)描述船舶的受力和速度，其固連于船體，坐標(biāo)原點(diǎn)位于船體質(zhì)心，x軸沿徑向指向船頭，y軸指向右舷，z軸垂直于xy平面并指向船底方向。z軸與x軸、y軸構(gòu)成右手坐標(biāo)系。

圖1 無(wú)人船水面運(yùn)動(dòng)模型Fig.1 Motion model of unmanned ship

考慮到實(shí)際情況，在研究無(wú)人船運(yùn)動(dòng)控制過(guò)程中，一般忽略船舶垂蕩、橫搖和縱搖運(yùn)動(dòng)，只討論無(wú)人船在水平面的縱蕩、橫蕩和艏搖運(yùn)動(dòng)。同時(shí)，考慮無(wú)人船在運(yùn)動(dòng)過(guò)程中受到的風(fēng)、浪等環(huán)境引起的擾動(dòng)未知但有界，則無(wú)人船的運(yùn)動(dòng)模型可以由式（1）、（2）描述。

η?=J(η)v（1）Μv?+C(v)v+D(v)v=τT+τE（2）其中η=(x y ψ)T為無(wú)人船的位置狀態(tài)，x、y分別表示無(wú)人船在地面坐標(biāo)系中的位置，ψ表示無(wú)人船的艏向角。v=(u υ r)T表示無(wú)人船的速度狀態(tài)，各分量分別表示在船體坐標(biāo)系中的縱向速度、橫向速度和偏航角速度。τT=(Fu0Nr)T表示無(wú)人船的控制，其中Fu表示無(wú)人船受到的縱向推力，Nr表示無(wú)人船受到的偏航力矩。τE=(τuE τυE τrE)表示無(wú)人船受到的環(huán)境干擾以及動(dòng)力學(xué)方程中的參數(shù)攝動(dòng)。J(η)為地面坐標(biāo)系到船體坐標(biāo)系的轉(zhuǎn)換矩陣，Μ是船舶的慣量矩陣，一般無(wú)人船是左右對(duì)稱前后不對(duì)稱的，但通常來(lái)說(shuō)m23=m32≈0，因此為簡(jiǎn)單分析，本文假定Μ是一個(gè)對(duì)角正定矩陣，C(v)表示科里奧利/向心力矩陣，D(v)是阻尼矩陣，此處將阻尼中的非線性部分考慮在了系統(tǒng)的擾動(dòng)參數(shù)中，因此此處只考慮線性阻尼。各矩陣具體的值為：

其中不確定項(xiàng)τE可以由式（3）描述

無(wú)人船數(shù)學(xué)模型中的慣性矩陣、阻尼矩陣等都是與無(wú)人船本身屬性有關(guān)的常量，一般稱之為模型參數(shù)，在對(duì)算法做仿真驗(yàn)證時(shí)，必須利用這些參數(shù)構(gòu)建數(shù)學(xué)模型。對(duì)于一艘特定的無(wú)人船，這些參數(shù)的值是確定的。然而，這些參數(shù)的獲取是比較困難的，需要通過(guò)大量的航行實(shí)驗(yàn)數(shù)據(jù)，使用參數(shù)辨識(shí)理論獲得。本文采用最小二乘算法對(duì)研究無(wú)人船運(yùn)動(dòng)控制問(wèn)題需要的參數(shù)進(jìn)行了辨識(shí)。在系統(tǒng)辨識(shí)前，首先對(duì)無(wú)人船動(dòng)力學(xué)方程進(jìn)行了小角度線化，然后對(duì)線性化之后的動(dòng)力學(xué)方程采用最小二乘算法進(jìn)行參數(shù)辨識(shí)。表1給出最終的辨識(shí)結(jié)果。

表1 無(wú)人船模型參數(shù)辨識(shí)結(jié)果Table1 Parameter identification results of unmanned ship model

其中m11、m22表示無(wú)人船的廣義質(zhì)量，m33表示無(wú)人船廣義轉(zhuǎn)動(dòng)慣量。Xu、Yv、Zr分別表示無(wú)人船沿本地坐標(biāo)系三軸的阻尼。

3 無(wú)人船目標(biāo)跟蹤及碰撞規(guī)避魯棒控制方法

3.1 控制方法設(shè)計(jì)

滑?？刂颇軌蛴行Э朔到y(tǒng)的不確定性，對(duì)干擾和未建模動(dòng)態(tài)具有很強(qiáng)的魯棒性，且具有響應(yīng)迅速、物理實(shí)現(xiàn)簡(jiǎn)單等優(yōu)點(diǎn)。本文將以滑?？刂品椒榛A(chǔ)實(shí)現(xiàn)無(wú)人船的控制。

首先對(duì)無(wú)人船的動(dòng)力學(xué)模型進(jìn)行變換，為對(duì)各個(gè)狀態(tài)進(jìn)行統(tǒng)一的分析，將式（1）（2）合并為一個(gè)二階方程。對(duì)式（1）求導(dǎo)得到式（4）：

令

得到

又

將式（6）、（7）代入式（5）可以得到

進(jìn)一步可以寫為

設(shè)

上式可以改寫為

令無(wú)人船的目標(biāo)狀態(tài)為，可以求得當(dāng)前狀態(tài)和目標(biāo)狀態(tài)的偏差為

選取滑動(dòng)面為

其中Λ=diag｛λr｝，λr＞0，r=1，2，3，顯然滑動(dòng)面上的狀態(tài)運(yùn)動(dòng)是穩(wěn)定的。接下來(lái)設(shè)計(jì)到達(dá)運(yùn)動(dòng)，對(duì)式（11）的滑動(dòng)面函數(shù)求導(dǎo)。本文研究無(wú)人船對(duì)點(diǎn)目標(biāo)的目標(biāo)跟蹤問(wèn)題，因此有= 0= 0?？梢詫?duì)滑動(dòng)面求導(dǎo)公式做進(jìn)一步化簡(jiǎn)：

選擇到達(dá)控制律為：

式中H，ε∈R3×3。為了消除滑動(dòng)運(yùn)動(dòng)在滑模面上的抖顫，在此處選擇飽和函數(shù)sat（x）代替符號(hào)函數(shù)。

其中φ為飽和函數(shù)邊界層厚度。對(duì)比式（12）、（13）可以得到等效控制為：

式（15）控制中存在不確定參數(shù)，無(wú)法直接使用，為消去不確定參數(shù)，首先采用式（16）控制進(jìn)行替換：

為確定zE，將式（16）代入式（12）中得到式（17）：

又滑模控制到達(dá)條件為：

將式（17）帶入式（18）之后寫成分量形式得到式（19）：

因此為使得滑?？刂频竭_(dá)條件滿足，比較式（19）各項(xiàng)大小可以得到：

式（16）、（20）構(gòu)成了無(wú)人船魯棒控制律，該控制率能夠?qū)崿F(xiàn)無(wú)人船對(duì)點(diǎn)目標(biāo)的跟蹤，并且對(duì)干擾具有較強(qiáng)的魯棒性。但是在無(wú)人船實(shí)際航行的過(guò)程中，很有可能在規(guī)劃的運(yùn)動(dòng)路徑上發(fā)現(xiàn)障礙物，因此無(wú)人船設(shè)計(jì)碰撞規(guī)避的控制是很有必要的，本文接下來(lái)的部分將詳細(xì)敘述關(guān)于無(wú)人船碰撞規(guī)避的控制。

3.2 無(wú)人船碰撞規(guī)避控制設(shè)計(jì)

為提高無(wú)人船在真實(shí)環(huán)境中的自主運(yùn)行能力，且使得無(wú)人船在運(yùn)行過(guò)程中受到各種不確定干擾的情況下仍然能夠順利完成任務(wù)，在無(wú)人船進(jìn)行了目標(biāo)跟蹤的前提下，需要進(jìn)行碰撞規(guī)避的設(shè)計(jì)。本小節(jié)將利用人工勢(shì)場(chǎng)函數(shù)理論為無(wú)人船設(shè)計(jì)碰撞規(guī)避控制。

人工勢(shì)場(chǎng)法是通過(guò)構(gòu)建虛擬的勢(shì)能場(chǎng)，使無(wú)人船沿著勢(shì)能場(chǎng)的趨勢(shì)進(jìn)行運(yùn)動(dòng)，最終到達(dá)目的地的一種方法。這是一種非最優(yōu)的機(jī)動(dòng)策略，但相對(duì)于其他算法具有反應(yīng)快速、計(jì)算簡(jiǎn)單以及實(shí)時(shí)性強(qiáng)等優(yōu)點(diǎn)，對(duì)算法加以改進(jìn)能在無(wú)人船路徑規(guī)劃任務(wù)中得到很好的應(yīng)用。

本文前面部分已經(jīng)完成了無(wú)人船從起始點(diǎn)到目的地的控制設(shè)計(jì)問(wèn)題，因此本文的對(duì)障礙物的碰撞規(guī)避問(wèn)題只需要考慮障礙物和無(wú)人船之間的斥力。這能夠避開(kāi)同時(shí)存在引力和斥力時(shí)進(jìn)入局部極小值的問(wèn)題。

大多數(shù)人工勢(shì)場(chǎng)的斥力勢(shì)場(chǎng)函數(shù)都是以無(wú)人船與障礙物的相對(duì)位置的倒數(shù)為自變量的二次函數(shù)，無(wú)人船小幅度地移動(dòng)就能引起勢(shì)場(chǎng)強(qiáng)度大幅度的變化，即斥力場(chǎng)強(qiáng)度變化較快。而在實(shí)際應(yīng)用中，斥力場(chǎng)強(qiáng)度的數(shù)值變化過(guò)快會(huì)影響對(duì)運(yùn)動(dòng)路徑的判斷，對(duì)斥力常量的選擇要求較為苛刻，指數(shù)型勢(shì)函數(shù)能夠有效避免此問(wèn)題，因此本文構(gòu)建式（21）所示的指數(shù)型勢(shì)函數(shù)：

式（21）對(duì)η求導(dǎo)得到：

其中

式（21）中，V(η，ηg)表示勢(shì)函數(shù)，ηg表示障礙物的位置，Q為正定對(duì)稱陣，λ1、λ2為相對(duì)近距離限制參數(shù)，通過(guò)調(diào)整Q、λ1、λ2可以實(shí)現(xiàn)無(wú)人船和障礙物的距離大于設(shè)計(jì)的最小距離，從而防止碰撞的發(fā)生。下面將對(duì)上述勢(shì)函數(shù)進(jìn)行仿真，直觀地觀察其變化規(guī)律。

從圖2 可以看出，隨著無(wú)人船和障礙物的距離減小，人工勢(shì)函數(shù)將增大，無(wú)人船受到的斥力也會(huì)增加，該斥力能夠控制無(wú)人船遠(yuǎn)離障礙物，從而實(shí)現(xiàn)碰撞規(guī)避。

圖2 勢(shì)函數(shù)模型仿真Fig.2 Potential function model simulation

4 控制器的穩(wěn)定性證明

前兩節(jié)設(shè)計(jì)了無(wú)人船目標(biāo)跟蹤和碰撞規(guī)避算法，本小節(jié)將證明上面設(shè)計(jì)的算法的穩(wěn)定性。首先選擇李雅普諾夫函數(shù)為：

對(duì)上面函數(shù)求導(dǎo)得到：

根據(jù)zE的求解過(guò)程可以看出，式（25）中后三項(xiàng)的和是小于0 的。對(duì)于第一項(xiàng)，顯然當(dāng)無(wú)人船在障礙物附近運(yùn)動(dòng)時(shí)，η?Tw(η，ηg)(η-ηg)＜0，進(jìn)一步可以得到上面定義的李雅普諾夫函數(shù)對(duì)時(shí)間的導(dǎo)數(shù)小于0，因此設(shè)計(jì)的控制能夠使得該系統(tǒng)穩(wěn)定。

5 數(shù)值仿真

為驗(yàn)證設(shè)計(jì)的目標(biāo)跟蹤和碰撞規(guī)避控制器的性能，本文對(duì)此控制器進(jìn)行數(shù)值仿真。仿真中假設(shè)無(wú)人船和目標(biāo)距離1000m 以內(nèi)時(shí)，文中設(shè)計(jì)的碰撞規(guī)避算法開(kāi)始運(yùn)行。仿真用到的具體的參數(shù)設(shè)置如下?？刂破鞯膮?shù)選擇為H=10-2×I3×3、ε=10-7×I3×3、Λ=10-2×I3×3。

表2 無(wú)人船狀態(tài)信息Table2 State of unmanned ship

從圖3、4可以看出，隨著時(shí)間的推移，無(wú)人船的各個(gè)狀態(tài)均達(dá)到了其期望值，由此可以證明本文設(shè)計(jì)的目標(biāo)跟蹤控制的有效性。在大概600s左右，無(wú)人船在障礙物附近重新調(diào)整了路線，繞開(kāi)了障礙物。

從圖5 可以明顯看出，當(dāng)無(wú)人船在運(yùn)動(dòng)過(guò)程中遇到障礙物時(shí)，能夠及時(shí)調(diào)整自己的位置，最終避開(kāi)障礙物且到達(dá)期望的位置，這也說(shuō)明本文設(shè)計(jì)的碰撞規(guī)避算法能夠?qū)崿F(xiàn)無(wú)人船的避障。

圖3 無(wú)人船x坐標(biāo)Fig.3 X coordinate of unmanned ship

圖4 無(wú)人船y坐標(biāo)Fig.4 Y coordinate of unmanned ship

圖5 無(wú)人船運(yùn)動(dòng)軌跡Fig.5 Trajectory of unmanned ship

6 結(jié) 論

本文針對(duì)欠驅(qū)動(dòng)無(wú)人船的碰撞規(guī)避和目標(biāo)跟蹤問(wèn)題，提出了一種基于滑模變結(jié)構(gòu)控制和改進(jìn)人工勢(shì)函數(shù)理論的控制方法。建立了考慮不確定干擾和參數(shù)攝動(dòng)的無(wú)人船水平面二階非線性動(dòng)力學(xué)模型，并為其設(shè)計(jì)了基于滑?？刂频哪繕?biāo)跟蹤魯棒控制器，同時(shí)基于改進(jìn)人工勢(shì)函數(shù)設(shè)計(jì)了無(wú)人船碰撞規(guī)避控制算法。最后對(duì)設(shè)計(jì)的控制算法進(jìn)行了穩(wěn)定性證明和數(shù)值仿真。仿真結(jié)果表明，提出的控制方法在無(wú)人船做平面運(yùn)動(dòng)時(shí)可以有效進(jìn)行目標(biāo)跟蹤和碰撞規(guī)避。