參考航速時變的無人船軌跡跟蹤控制方法

雷超凡 ，初秀民 ，柳晨光 ，吳文祥 ，李松龍

(1. 武漢理工大學 國家水運安全工程技術研究中心，湖北 武漢 430063；2. 武漢理工大學 智能交通系統研究中心，湖北 武漢 430063；3. 武漢理工大學 交通與物流工程學院，湖北 武漢 430063)

0 引 言

隨著現代智能控制理論被廣泛應用于各行各業，船舶控制研究也進入了智能控制時代。船舶運動控制是其中的核心問題，軌跡跟蹤問題是船舶運動控制的基礎，其含義是在所有控制時刻內使船舶達到對應時刻的參考位置。欠驅動水面船舶是遠洋運輸的重要船舶類型，其特點是船舶缺少側推裝置，僅依靠船尾推進裝置提供的縱向推力和舵提供的旋轉扭矩控制船舶運動[1]。

在研究確定期望軌跡的軌跡跟蹤問題時，Serret-Frenet 坐標系（以下簡稱SF 坐標系）是常用的一種坐標構建方法。其原理是在運動的每一時刻都在期望軌跡上建立一個隨時間變化的坐標構架，構架的坐標原點及坐標軸方向均隨曲線上期望點的移動而變化。齊雪等[2]針對水下機器人的運動問題引入了SF 坐標系，采用反步法，通過靈活設置虛擬路徑上的參考點放寬了控制器的初始條件，提高了系統的魯棒性。王宏健等[3]結合SF 坐標系對船舶軌跡跟蹤進行研究，運用反步法設計了動力學控制器，使得航行器能夠精準地跟蹤曲線路徑。王威等[4]在SF 坐標系下對智能車輛的路徑跟蹤問題進行了研究，運用模型預測控制，將速度設為定值，解決了控制延時的問題。綜合上述文獻可以看出，由于SF 坐標系的建立具有時變特性，所以現有研究多結合反步法，而在模型預測控制當中難以解決預測問題，現有研究則選擇了設置固定速度來簡化速度控制精度的問題。

對USV 的軌跡跟蹤控制，經典控制方法有滑模控制、反步法（Backstepping）、神經網絡法和模型預測控制法（model predictive control，MPC）等此外眾多學者也在不斷提出改進算法。張成舉等[5]基于反步法和自適應技術，提出一種非線性魯棒軌跡跟蹤控制策略，有效減小了反步法中虛擬變量直接求導的復雜性問題。葛科奇[6]提出了一種動態神經模糊網絡的水面欠驅動船舶運動控制算法，算法描述了船舶動力系統的受力特性，以黑箱形式模擬了船舶非線性模型，并對算法進行了仿真驗證。相較而言，模型預測控制方法采用滾動優化機制，且能夠顯式處理約束，所以解決船舶運動非線性系統及多約束問題具有獨特優勢[1]。軌跡跟蹤控制算法在不斷進步，但現有研究方向都聚焦于船舶位置控制的精度，而缺少對其航速控制效果的關注。本文提出一種Serret-Frenet 坐標系下USV的時變參考狀態軌跡跟蹤模型預測控制方法。

1 坐標系與模型

USV 水面運動模型的建立需要依托相應的坐標系，通常運動學特征依托于慣性坐標系，記作{n}=動力學特征依托于附體坐標系，記作描述參考狀態依托于SF坐標系，記作慣性坐標系定義為：原點On位 于期望軌跡起點，Onxn指向正北，Onyn指 向正東；附體坐標系定義為：原點Ob位于船舶重心，Obxb沿 船舶對稱剖面指向船首，Obyb指向右舷；SF 坐標系定義為：原點Of位于期望路徑上當前時刻的目標參考點，Of xf指向期望軌跡的切線方向，Of yf指向垂線方向。水面坐標系關系如圖1 所示。

圖1 水面坐標關系Fig. 1 Water surface coordinate relationship

基于Fossen 提出的船舶水動力建模方法構建欠驅動船舶的動力學模型，其狀態空間方程表示為：

式中：x(t)=[x,y,φ,u,v,r]T為系統狀態變量；u(t)=[τu,0,τr]T為模型輸入控制量；e(t)=[eu,ev,er]T設定為干擾；x,y,φ 為 船舶位置及首向； τu,τr分別為船尾推力及轉向力矩；eu,ev,er分別為3 個自由度上的干擾，f表示映射關系，表示為：

式中：M表示慣性矩陣；C表示科式向心矩陣；D表示阻尼矩陣，上述矩陣均由水動力導數構成，水動力導數可由經驗公式法求得；T為坐標變換矩陣，用于附體坐標系下變量向慣性坐標系轉化：

現已知船舶期望軌跡表示為函數yr=g(xr)，將船舶實際位置用其重心位置點Q表示，則Q點在慣性坐標系下坐標為（x,y）,同時定義其在SF坐標系下坐標為（a,d）；引入合速度其與Onyn軸的夾角θ表示為那么基于坐標可將軌跡跟蹤問題的控制目標描述為以下兩點：

1）控制船舶重心靠近期望點，即

式中：i表示當前仿真時刻；Np表示控制器預測步長。

2）控制船舶合速度方向趨近于期望軌跡在期望點的切線方向，切線方向通過對期望軌跡求一階導數得到，即

為了對曲線有更好的描述，建立從期望軌跡起點到當前期望點的弧長s對坐標x的變上限積分對應關系:

利用式(5)可將式(3)和式(4)中的自變量x轉 換為s。

2 控制器設計

2.1 預測模型

為了減輕控制器計算負擔，保證控制實時性，采用線性模型預測控制器。運用船舶運動模型作為控制器預測迭代模型，將模型代入控制器還需要對其進行線性化和離散化處理。

首先利用近似線性化方法對模型進行線性化處理，將任意時刻的船舶運動狀態空間方程以上一控制時刻平衡點 (xr,ur)作為參考點進行Taylor 展開，并與平衡點做差，得到線性差值表達式如下：

對其進行近似離散化，引入步長Ts，則A，B變化可分步表示為：

經過離散化后得到的預測模型為：

式中：k=1,2,···,Np為預測步數。

2.2 參考量設計

設計控制系統參考量。首先需要在期望軌跡上找到每一控制時刻對應的期望點 (xe,g(xe))，選取船舶當前位置在期望軌跡上的投影點作為期望點，則其可通過求解方程得到：

引入對船舶的速度控制，將其設定為關于s的函數：

則根據期望速度，期望點在期望軌跡上的弧長變化量可表示為下式：

在預測步長Np內 得到期望點慣性坐標序列為下式：

對應期望點坐標序列的合速度參考量序列由期望軌跡的一階導數得到，表示為下式：

2.3 目標函數及約束設計

為完成可變航速軌跡跟蹤控制的目的，基于以下4 個方面設置目標函數：

1）使預測到船舶位置點的投影點靠近期望點序列位置；

2）使船舶當前位置靠近期望點位置；

3）使船舶合速度方向靠近期望點的切線方向；

4）使控制系統輸出控制量盡可能小。

由此設計的目標函數表示為：

式中：Qn(n=1,2,...,5) 和R為 權值和權重矩陣；Nc為控制步長；u為系統控制量。

通過目標函數的設置，可變航速的軌跡跟蹤控制的優化問題轉化為求解所設約束下目標函數最小值對應的控制量問題。依據禁止船舶倒航、動力機構性能設置約束條件為：

3 仿真實驗

3.1 仿真模型

選擇Cybership Ⅱ模型船作為研究對象[7]，該船是一艘以1∶70 的比例建造的補給船模型，其長度為LC=1.255 m ，寬度為BC=0.29 m，最大推進力為2 N，最大推力下的平衡船速為0.58 m/s，最大轉向力矩為1.5 N·m 。其對應矩陣M和D大小分別為：

3.2 控制器參數設置

期望軌跡函數及期望速度設置為：

USV 初始狀態向量設置為：

權重及權重矩陣設置為：

根據所選船型的動力機構性能，將約束設置為：

預測步長和控制步長設置為：

基于風、浪、流等設置隨機干擾：

式中， randi(·)表示均勻分布的隨機整數。

3.3 結果分析

對USV 運動進行210 步仿真，以文獻[1]所用軌跡跟蹤方法作為實驗對比組，將每一仿真時刻的期望點慣性坐標序列P1(i) 以 及期望速度序列P2(i)的第一項組合成矩陣P210×2，并將P作為文獻[1]所用方法的參考量矩陣，其余參數均與本文方法設置相同。得到的運行對比結果如圖2～圖5 所示。

圖2 跟蹤結果對比Fig. 2 Tracking results comparison

由圖2 和圖3 可知，本文設計的控制方法良好地完成了軌跡跟蹤任務，初始位置與期望軌跡的偏差使得USV 在航行初與投影點存在較大距離，20 s 后能夠與期望軌跡貼合，此后仿真過程最大距離為0.023 m，與對比組進行對比可以看出，本文方法在調整初始位置誤差時，超出量更少且收斂速度更快，但總體而言，2 種方法位置點與目標點的距離保持能力相差不大。由圖4 可以看出，USV 航速從0 m/s 開始，經過15 s 后能夠跟隨期望航速曲線，此后在干擾的作用下有小幅的波動，對比組表現出的航速波動大，本文方法具有更好的航速控制效果。由圖5 可以看出，軌跡跟蹤初期，控制器輸出較大推力以追逐期望曲線，在完成追逐后迅速保持較小的控制輸出，輸出推力和轉矩均由隨干擾的小幅度震蕩，相較而言，干擾對扭矩輸出的影響更為明顯，對比而言，本文提出的控制方法控制量的波動較小，控制更為穩定。

圖3 位置點與目標點的距離Fig. 3 Distance between position point and projection point

圖4 航速變換Fig. 4 Speed

圖5 控制器輸出Fig. 5 Controller output

4 結 語

本文以Serret-Frenet 坐標系為基礎，建立USV 三自由度運動模型，針對軌跡跟蹤航速時變控制問題，提出了一種基于模型預測控制算法的USV 軌跡跟蹤控制方法，通過仿真得到的結論如下：

1）在Serret-Frenet 坐標系下利用投影法尋找當前船舶位置在期望軌跡上對應的期望點；

2）引入隨航行路程而變的期望速度，用對應控制時刻的期望速度構建參考位置序列作為系統參考量；

3）控制方法對USV 軌跡跟蹤和速度控制均有良好的效果。本文設計的控制方法可為USV 航行行為控制提供參考。