基于SFLOS的水面無人艇路徑跟蹤控制技術(shù)研究

張愛國 孫宏放

摘要：相比直線路徑跟蹤控制，曲線路徑跟蹤控制往往由于操縱性約束和外界干擾，導(dǎo)致水面無人艇難以精確、穩(wěn)定跟蹤期望路徑。本文通過引入Serret-Frenet坐標(biāo)系，采用帶有前視距離的視線導(dǎo)航法，然后設(shè)計(jì)出目標(biāo)點(diǎn)的路徑參數(shù)跟蹤率，將對曲線路徑跟蹤轉(zhuǎn)化為對一系列目標(biāo)點(diǎn)的跟蹤，最后再根據(jù)前視距離以及橫向誤差計(jì)算出期望艏向角，以此推導(dǎo)出SFLOS引導(dǎo)算法的具體表達(dá)式并進(jìn)行仿真實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明，基于Serret-Frenet下的視線制導(dǎo)法與傳統(tǒng)方法相比，尤其是在曲線路徑跟蹤上，在控制誤差、穩(wěn)定性和抗干擾性等方面性能均有提升，能快速收斂到目標(biāo)軌跡。

關(guān)鍵詞：水面無人艇;改進(jìn)視線制導(dǎo)（SFLOS）;路徑跟蹤

引言

水面無人艇（unmanned surface vehicle，USV）集合了人工智能、自動化控制等多項(xiàng)技術(shù)，是一個(gè)復(fù)雜的系統(tǒng)。從上世紀(jì)九十年代至今，USV進(jìn)入一個(gè)飛速發(fā)展的過程。在軍事和民用方面應(yīng)用廣泛。其中，路徑跟蹤控制是USV技術(shù)發(fā)展的核心。路徑跟蹤控制是指USV從任意位置出發(fā)，能夠按照規(guī)定的路徑行駛并到達(dá)預(yù)定位置。路徑跟蹤控制是USV可以實(shí)現(xiàn)其他功能的前提，具有重要的實(shí)用價(jià)值。

在視線法中，在目標(biāo)軌跡上定義一個(gè)虛擬的節(jié)點(diǎn)，并控制USV跟隨該節(jié)點(diǎn)，從而使得 USV 位于目標(biāo)軌跡上。視線法簡單易用，但抗干擾性差。近年來，學(xué)者們提出了多種改進(jìn)方法，以解決實(shí)際運(yùn)用中遇到的難題。其中，柳晨光等[1]提出了考慮水流干擾條件下的自適應(yīng)視距制導(dǎo)算法，通過在室外水池環(huán)境下的模型船路徑跟蹤控制試驗(yàn)平臺，驗(yàn)證了其具有更高的路徑跟蹤控制精度和可靠性。陳霄等[2]出了一種基于改進(jìn)視線導(dǎo)引算法，與傳統(tǒng)的視線導(dǎo)引算法相比，改進(jìn)的導(dǎo)引算法通過引入自適應(yīng)觀測器能夠?qū)崿F(xiàn)對漂角的實(shí)時(shí)估計(jì)和補(bǔ)償，同時(shí)時(shí)變前視距離的設(shè)計(jì)使得無人艇的操縱更加靈活。Lei W等[3]提出了一種改進(jìn)的無人機(jī)路徑跟蹤積分光瞄準(zhǔn)制導(dǎo)律，其最大的特點(diǎn)是相比較傳統(tǒng)制導(dǎo)律，該積分制導(dǎo)律可以適應(yīng)不同航速。董早鵬等[4]提出了一種非對稱模型下的改進(jìn)視線法導(dǎo)向路徑跟蹤控制方法，不僅能夠?qū)崿F(xiàn)直線路徑跟蹤導(dǎo)向，還能實(shí)現(xiàn)一般曲線路徑跟蹤的視線導(dǎo)向，同時(shí)該方法在收斂速度和系統(tǒng)穩(wěn)定性方面體現(xiàn)了一定的優(yōu)越性，具有全局漸進(jìn)穩(wěn)定性。瞿洋等[5]提出了一種 ILOS 引導(dǎo)律的改進(jìn)方式，消除了緩變環(huán)境載荷對船舶位置所產(chǎn)生的橫向偏離。Wang H等[6]提出了基于相對速度模型設(shè)計(jì)新型的ILOS制導(dǎo)律，并證明了控制器的穩(wěn)定性。

針對曲線路徑跟蹤，本文引入Serret-Frenet坐標(biāo)體系，對原有的LOS進(jìn)行改進(jìn)，設(shè)計(jì)了將SFLOS導(dǎo)引算法與航速控制器、航向控制器相結(jié)合設(shè)計(jì)曲線路徑跟蹤控制算法。用SIMULINK對LOS導(dǎo)引算法以及SFLOS導(dǎo)引算法的路徑跟蹤控制系統(tǒng)進(jìn)行路徑點(diǎn)以及正弦函數(shù)跟蹤的仿真試驗(yàn)，提高了無人艇在路徑跟蹤下的穩(wěn)定性。

1 改進(jìn)視線制導(dǎo)法（SFLOS）

1.1 Serret-Frenet坐標(biāo)框架

視線導(dǎo)航法的原理是根據(jù)船舶自身位置與目標(biāo)路徑之間的距離，引入前視距離，使船舶的航向角對準(zhǔn)LOS期望航向角，當(dāng)艏向角誤差趨向于零時(shí)表示船舶的實(shí)際航行路徑趨向于目標(biāo)路徑，視線導(dǎo)航法多用于直線路徑跟蹤，本節(jié)的SFLOS導(dǎo)引算法解決曲線路徑跟蹤。首先引入Serret-Frenet坐標(biāo)系，采用帶有前視距離的視線導(dǎo)航法，然后設(shè)計(jì)出目標(biāo)點(diǎn)的路徑參數(shù)跟蹤率，將對曲線路徑跟蹤轉(zhuǎn)化為對一系列目標(biāo)點(diǎn)的跟蹤，最后再根據(jù)前視距離以及橫向誤差計(jì)算出期望艏向角[7]。

本節(jié)引入Serret-Frenet坐標(biāo)框架，SF坐標(biāo)框架來自微分幾何學(xué)中，通過旋轉(zhuǎn)坐標(biāo)系以及應(yīng)用微分同胚變換，將大地坐標(biāo)系的位置誤差轉(zhuǎn)換為SF坐標(biāo)系下的位置誤差，再經(jīng)過對船舶艏向角以及航跡角、船舶縱向速度以及橫向速度夾角的幾何分析，最終獲得船舶的期望艏向角。

假設(shè)l為一條可微曲線，以弧長w為參數(shù)并且任意點(diǎn)處的曲率不為零，曲線l所在坐標(biāo)系為大地坐標(biāo)系，曲線l上任意點(diǎn)為l（ω）=（x（ω），y（ω）），在任意點(diǎn)處的單位切向量如下式：

將該向量順時(shí)針旋轉(zhuǎn)90°，為該點(diǎn)的單位法向量，如下式：

以該點(diǎn)為原點(diǎn)，其切向量以及法向量方向?yàn)闄M軸和縱軸，此為曲線l在l=（ω）下的Serret-Frenet坐標(biāo)框架。

1.2 建立位置誤差系統(tǒng)

假定無人艇所跟蹤的路徑為水平面一條以θ為參數(shù)的自由曲線Pp（θ），取該曲線上任意一點(diǎn)為[xp（θ），yp（θ）]T，設(shè)該點(diǎn)為目標(biāo)點(diǎn)，引領(lǐng)無人艇沿著跟蹤路徑行駛，充當(dāng)“虛擬向?qū)А?，該點(diǎn)的速度為。

先作如下假設(shè)：期望路徑Pp（θ）存在一階以及二階導(dǎo)數(shù)并且有界，且滿足，其中。

如圖1.2，P為無人艇跟蹤的期望軌跡，以Pp（θ）為原點(diǎn)構(gòu)造Serret-Frenet坐標(biāo)系，以無人艇重心為原點(diǎn)構(gòu)造附體坐標(biāo)系，以及船舶初始位置為原點(diǎn)構(gòu)造大地坐標(biāo)系，大地坐標(biāo)系與Serret-Frenet坐標(biāo)系之間的相對旋轉(zhuǎn)角度為Ψp（θ），表達(dá)式如下： （1-3）

定義在Serret-Frenet坐標(biāo)系下無人艇位姿誤差為，大地坐標(biāo)系下位姿誤差為ηen，經(jīng)過微分同胚變換可知： （1-4）

其中為旋轉(zhuǎn)矩陣，如下式（1-5）：

其中Ψw=Ψ+β為無人艇的航跡角。

根據(jù)微分同胚變換的原理，如果無人艇的位姿誤差在Serret-Frenet坐標(biāo)系下收斂，則無人艇的位姿誤差在大地坐標(biāo)系下仍收斂，此時(shí)只需考慮令。

對式（1-4）進(jìn)行求導(dǎo)，得到跟蹤誤差的微分形式：

現(xiàn)得到在Serret-Frenet坐標(biāo)系下位姿跟蹤誤差的導(dǎo)數(shù)形式，如式（1-12）：

1.3 SFLOS導(dǎo)引算法設(shè)計(jì)

定義在Serret-Frenet坐標(biāo)系下無人艇的位置誤差為，控制目標(biāo)為：

根據(jù)上文的位置跟蹤誤差，設(shè)計(jì)路徑跟蹤控制器，構(gòu)造李雅普諾夫函數(shù)：

為使縱向位置誤差收斂，設(shè)計(jì)路徑參數(shù)跟蹤率為：

由SFLOS導(dǎo)引原理圖可知表示在Serret-Frenet坐標(biāo)系下無人艇的期望速度與Serret-Frenet坐標(biāo)系橫軸夾角。

根據(jù)圖1.2，由幾何關(guān)系可知，無人艇期望艏向角為：

其中Δ為虛擬導(dǎo)航點(diǎn)沿著期望路徑切向方向上的前視距離，合適的前視距離可以避免控制器出現(xiàn)飽和現(xiàn)象，為艏向誤差。

由此，基于李雅普諾夫完成了SFLOS導(dǎo)引算法，該算法主要分為兩部分：一是設(shè)計(jì)了期望路徑的參數(shù)更新率，建立期望路徑目標(biāo)點(diǎn)與無人艇航行時(shí)的動態(tài)關(guān)系，二是根據(jù)SFLOS原理圖的幾何關(guān)系設(shè)計(jì)了期望艏向角，SFLOS導(dǎo)引算法的具體表達(dá)式如下：

2 路徑跟蹤仿真實(shí)驗(yàn)

在曲線路徑跟蹤控制中，在靜水水域中分別進(jìn)行路徑跟蹤仿真，設(shè)定無人艇的初始狀態(tài)為位于大地坐標(biāo)系原點(diǎn)且艏向角為零，期望速度同樣設(shè)定為0.5m/s，航向控制器與航速控制器的參數(shù)與直線路徑跟蹤仿真試驗(yàn)中的一致，曲線跟蹤路徑如下：

SFLOS中的設(shè)計(jì)參數(shù)為：kx=1，前視距離為10m，仿真步長0.02。仿真結(jié)果如下：

由圖3.1.6偏航距離曲線可以看出無人艇能較為準(zhǔn)確地完成正弦函數(shù)的路徑跟蹤，誤差在0.2m之內(nèi)，在跟蹤過程中沒有出現(xiàn)偏航距離過大的現(xiàn)象，但根據(jù)圖3.1.6和圖3.1.8可以看出當(dāng)無人艇進(jìn)行大轉(zhuǎn)角時(shí)，偏航距離達(dá)到最大值，最終無人艇實(shí)際航行路徑能收斂到期望路徑上，且無人艇的航速開始快速增加最終在航速控制器下穩(wěn)定在0.5m/s左右，曲線路徑跟蹤控制滿足路徑跟蹤的要求，對比上文基于LOS導(dǎo)引算法的直線路徑跟蹤控制的偏航距離誤差達(dá)到1m，表明SFLOS導(dǎo)引算法的跟蹤控制效果更好，性能更優(yōu)。

3 結(jié)語

本文通過引入SF坐標(biāo)系，設(shè)計(jì)了期望路徑的參數(shù)更新率，建立期望路徑目標(biāo)點(diǎn)與無人艇航行時(shí)的動態(tài)關(guān)系，根據(jù)SFLOS原理圖的幾何關(guān)系設(shè)計(jì)了期望艏向角，將SFLOS導(dǎo)引算法與航速控制器、航向控制器相結(jié)合設(shè)計(jì)曲線路徑跟蹤控制算法。最后，用SIMULINK對LOS導(dǎo)引算法以及SFLOS導(dǎo)引算法的路徑跟蹤控制系統(tǒng)進(jìn)行路徑點(diǎn)以及正弦函數(shù)跟蹤的仿真試驗(yàn)，驗(yàn)證相關(guān)算法的有效性以及優(yōu)越性。

參考文獻(xiàn)

[1]柳晨光 ，初秀民 ，毛慶洲 ，謝朔.無人船自適應(yīng)路徑跟蹤控制系統(tǒng)[J].機(jī)械工程學(xué)報(bào)，2020，56（08）：216-227.

[2]陳霄，劉忠，羅亞松，等.海洋環(huán)境下欠驅(qū)動無人艇航跡跟蹤控制算法[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2018，50（10）：116-123.

[3]Wan L ，Su Y ，Zhang H ，et al.An improved integral light-of-sight guidance law for path following of unmanned surface vehicles[J].Ocean Engineering，2020，205：107302.

[4]董早鵬，萬磊，廖煜雷，等.基于非對稱模型的欠驅(qū)動USV路徑跟蹤控制[J].中國造船，2016（1）：116-126.

[5]瞿洋，徐海祥，等.基于ILOS的欠驅(qū)船舶循跡控制[J].武漢理工大學(xué)學(xué)報(bào)（交通科學(xué)與工程版），2016，40（5）：834-838.

[6]WANG H，WANG D，PENG Z H.Adaptive neural control for cooperative path following of marine surface vehicles：State and output feedback[J].International Journal of Systems Science，2016，47（2）：343-359.

[7]周利.基于擾動補(bǔ)償?shù)臒o人艇路徑跟蹤控制算法研究[D].哈爾濱：哈爾濱工程大學(xué)，2017.

作者簡介：

孫宏放（1978.8），男漢族 安徽桐城人 學(xué)歷：博士研究生 職稱：副研究員，從事科技創(chuàng)新、人才引進(jìn)、成果轉(zhuǎn)化和產(chǎn)業(yè)化等工作。

張愛國（1978.4），男漢族 山東巨野人 學(xué)歷：博士研究生 職稱：助理研究員，從事平臺搭建、創(chuàng)業(yè)孵化、成果轉(zhuǎn)化和產(chǎn)業(yè)推廣等工作。