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

張愛國 孫宏放

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

關鍵詞：水面無人艇;改進視線制導（SFLOS）;路徑跟蹤

引言

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

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

針對曲線路徑跟蹤，本文引入Serret-Frenet坐標體系，對原有的LOS進行改進，設計了將SFLOS導引算法與航速控制器、航向控制器相結合設計曲線路徑跟蹤控制算法。用SIMULINK對LOS導引算法以及SFLOS導引算法的路徑跟蹤控制系統進行路徑點以及正弦函數跟蹤的仿真試驗，提高了無人艇在路徑跟蹤下的穩定性。

1 改進視線制導法（SFLOS）

1.1 Serret-Frenet坐標框架

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

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

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

將該向量順時針旋轉90°，為該點的單位法向量，如下式：

以該點為原點，其切向量以及法向量方向為橫軸和縱軸，此為曲線l在l=（ω）下的Serret-Frenet坐標框架。

1.2 建立位置誤差系統

假定無人艇所跟蹤的路徑為水平面一條以θ為參數的自由曲線Pp（θ），取該曲線上任意一點為[xp（θ），yp（θ）]T，設該點為目標點，引領無人艇沿著跟蹤路徑行駛，充當“虛擬向導”，該點的速度為。

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

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

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

其中為旋轉矩陣，如下式（1-5）：

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

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

對式（1-4）進行求導，得到跟蹤誤差的微分形式：

現得到在Serret-Frenet坐標系下位姿跟蹤誤差的導數形式，如式（1-12）：

1.3 SFLOS導引算法設計

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

根據上文的位置跟蹤誤差，設計路徑跟蹤控制器，構造李雅普諾夫函數：

為使縱向位置誤差收斂，設計路徑參數跟蹤率為：

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

根據圖1.2，由幾何關系可知，無人艇期望艏向角為：

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

由此，基于李雅普諾夫完成了SFLOS導引算法，該算法主要分為兩部分：一是設計了期望路徑的參數更新率，建立期望路徑目標點與無人艇航行時的動態關系，二是根據SFLOS原理圖的幾何關系設計了期望艏向角，SFLOS導引算法的具體表達式如下：

2 路徑跟蹤仿真實驗

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

SFLOS中的設計參數為：kx=1，前視距離為10m，仿真步長0.02。仿真結果如下：

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

3 結語

本文通過引入SF坐標系，設計了期望路徑的參數更新率，建立期望路徑目標點與無人艇航行時的動態關系，根據SFLOS原理圖的幾何關系設計了期望艏向角，將SFLOS導引算法與航速控制器、航向控制器相結合設計曲線路徑跟蹤控制算法。最后，用SIMULINK對LOS導引算法以及SFLOS導引算法的路徑跟蹤控制系統進行路徑點以及正弦函數跟蹤的仿真試驗，驗證相關算法的有效性以及優越性。

參考文獻

[1]柳晨光 ，初秀民 ，毛慶洲 ，謝朔.無人船自適應路徑跟蹤控制系統[J].機械工程學報，2020，56（08）：216-227.

[2]陳霄，劉忠，羅亞松，等.海洋環境下欠驅動無人艇航跡跟蹤控制算法[J].哈爾濱工業大學學報，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]董早鵬，萬磊，廖煜雷，等.基于非對稱模型的欠驅動USV路徑跟蹤控制[J].中國造船，2016（1）：116-126.

[5]瞿洋，徐海祥，等.基于ILOS的欠驅船舶循跡控制[J].武漢理工大學學報（交通科學與工程版），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]周利.基于擾動補償的無人艇路徑跟蹤控制算法研究[D].哈爾濱：哈爾濱工程大學，2017.

作者簡介：

孫宏放（1978.8），男漢族 安徽桐城人 學歷：博士研究生 職稱：副研究員，從事科技創新、人才引進、成果轉化和產業化等工作。

張愛國（1978.4），男漢族 山東巨野人 學歷：博士研究生 職稱：助理研究員，從事平臺搭建、創業孵化、成果轉化和產業推廣等工作。