基于模型預測控制的仿生海豚路徑跟蹤研究*

付樂樂，陳 宏，葉鎮豪，鞏偉杰，何 莉

（深圳大學機電與控制工程學院，廣東 深圳 518061）

0 引 言

水下機器人的路徑跟蹤技術在海洋資源勘探、軍事巡航、水下精確制導等方面有著巨大發展前景和應用價值，各國對于海洋資源爭奪競爭日趨激烈，對于水下機器人路徑規劃、路徑跟蹤等方向的研究受到了十分廣泛關注［1，2］。水下機器人的路徑跟蹤方法有PID控制，模糊控制，反步控制，及模型預測控制（model predictive control，MPC）等［3］。水下機器人屬于多路輸入和多路輸出，是非線性系統，而PID基本情況下適用于單輸入單輸出線性系統，所以不太適合［4］。模糊控制需人為的控制方式指導，雖不必精確的系統建模，但過度依賴主觀性，考慮情況不全面［5］。反步控制常用于水下機器人路徑跟蹤，其思想是設計一個速度控制器使系統穩定，可解決初始狀態誤差較大的問題，但是速度和加速度將會發生跳變，不符合實際控制［6］。

綜上分析，本文對水下機器人路徑跟蹤控制方法采用MPC，可預測未來模型，進行反復優化求解并持續滾動控制［7］，針對海流影響進行了干擾分析，以及對于三維的水下路徑跟蹤研究，均驗證了MPC 算法的路徑跟蹤效果極佳。

1 仿生海豚運動學

描述仿生海豚在空間中運動，通常需要兩種坐標系，分別是慣性坐標系和載體坐標系［8］，慣性坐標系E-ξηζ，以地球上的某點作為坐標原點，載體坐標系O-xyz，固定在仿生海豚重心，隨著海豚的運動而運動，如圖1所示。

圖1 慣性坐標系和載體坐標系

在慣性坐標系下，水下仿生海豚的位姿可用位置（x，y，z）和姿態角（φ，θ，ψ）表示，φ，θ，ψ分別為橫擺角、縱傾角和艏向角；在載體坐標系下，仿生海豚的線速度和角速度分別為（u，v，w）和（p，q，r），u，v，w分別表示方向為進退、側移、潛浮，在6自由度下仿生海豚的運動學轉換方程為［9］

式中η＝［xyzφθψ］T為慣性坐標系下的位置及姿態量；v ＝［uvwpqr］T為載體坐標系下的線速度和角速度量；J（η）為兩種坐標系之間的位姿轉換矩陣［10］。

2 仿生海豚MPC算法

2.1 仿生海豚跟蹤誤差模型

通常情況下不必考慮全部6 個自由度的運動，由于仿生海豚的縱傾與橫搖運動微乎其微，可忽略不考慮［11，12］，故仿生海豚的狀態向量分別為η＝［x，y，z，ψ］T，v ＝［u，v，w，r］T，由于在本文中只研究了二維環境下路徑跟蹤效果，故取z，w為零，兩者之間的關系由式（1）進一步得出

定義擬跟蹤的目標曲線在慣性坐標系下軌跡ηd＝［xd ydzdψd］T，則仿生海豚跟蹤控制的誤差模型為

2.2 仿生海豚輸出預測

誤差模型是連續量，不能直接作用于模型預測控制器中，需要對式（3）進行離散化處理，處理后結果為

式中 ?η（k）為狀態量，?ν（k）為控制量，k為某時刻，其中

式中T為采樣時間，考慮到實際建模不準確性帶來的誤差，為了能夠跟蹤更加精確，考慮以ΔV作為控制量，對式（4）進行變換，得到

從而，可進一步得到新的狀態空間表達式為

式中 ΔV為控制增量矩陣，為系數矩陣，＝

為簡化計算，可令

根據新的離散化后的狀態空間表達式，可進行輸出預測為

其中

可通過當前狀態量和控制增量計算得到預測的狀態量和輸出量，之后在約束條件下進行二次規劃求解，得到控制增量的第一個序列，并以此求出控制量，作用于系統。

2.3 約束條件下的目標函數求解

通過MPC進行預測后，最終要轉為目標函數優化問題求解，目標函數要確保仿生海豚能平穩地跟蹤到參考軌跡，要反映出跟隨能力和平穩能力，故對目標函數的設計如下

式中R和Q為權重矩陣。前項能反映對參考路徑的跟隨能力，后項能反映系統的穩定性。通過一定方式可將目標函數轉化為二次規劃問題，將式（7）代入式（11）并化簡得到標準形式的目標函數

因此，在每一步的目標函數最優解問題都可以轉為約束下二次規劃問題求解

其中，約束條件為

在每個控制周期內，對式（13）進行求解，得到一系列的控制增量，由此會得到一系列的控制量，并將序列中第一個控制量作為控制輸入量作用在系統中，直到下一時刻來臨產生新的控制量序列，再次將第一個控制量作用于系統，循環往復，直到跟蹤路徑完成。

2.4 海流模型建立

海洋中海流對水下仿生海豚的影響處處可見，海洋中真實海流運動不規律，時變性較強，文中給出了海流模型，并分析某處海流對仿生海豚產生的路線偏移影響，其分析方法將適用于整片海流擾動之中，本文給出了一種基于流函數的周期變動的雙曲流的計算實例，驗證存在海流情況下，路徑跟蹤算法的有效性，其雙曲流為

其中，f（x，t）＝a（t）x2＋b（t）x，a（t）＝?sin（ωt），b（t）＝1 -2?sin（ωt）。

海流速度場如下

式中A取0.1，代表速度矢量大??；ω取0.2π，ω／2π為振蕩頻率；?取0.25，代表振蕩幅度。

3 仿生海豚跟蹤實驗仿真

3.1 二維環境下圓形跟蹤

首先對二維環境下的未封閉圓形軌跡進行跟蹤，將圓形軌跡作為期望跟蹤點，以R＝10 為半徑，以（0，10）為圓心畫出圓形，便于觀察起點與終點，采用不封閉圓形作為參考跟蹤路徑。取初始狀態量，即各軸姿態及艏向角［x，y，z，ψ］T＝［0，0，0，0］T，將采樣點分為3 000 個，采樣時間為0.01 s，預測步長為5，并對線速度、角速度控制約束設為［-2 m／s，2 m／s］，模型預測的權重參數Q設為［1 0 0 0，0 1 0 0，0 0 0 0，0 0 0 5］；R設為［10 0 0 0，0 10 0 0，0 0 100 0，0 0 0 1］。MPC與反步控制對圓形跟蹤效果如圖2所示。

圖2 圓形軌跡跟蹤效果對比

通過圖2（a）中可看出2 種控制算法都可達到跟蹤的效果，但是MPC 要比反步控制精度高，更加穩定。在圖2（b）中，雖然反步控制跟蹤誤差較小，在允許范圍內，但存在波動情況，并沒有MPC 穩定，且跟蹤誤差極小。在圖2（c）中，反步控制進退速度，轉艏速度輸出變化較大，并在進退速度中超出了預設速度界限，而MPC 進退速度、橫移速度和轉艏速度的變化均小于反步控制，較穩定且在預設的±2 m／s范圍內。

3.2 路徑規劃曲線跟蹤

根據對圓形軌跡跟蹤效果對比，發現MPC算法在允許輸出速度限制之內，不僅穩定，而且誤差值也比反步控制算法小很多，于是采用MPC算法繼續進行對改進蟻群算法并使用3次B樣條平滑后的路徑規劃曲線跟蹤，觀察其效果是否可觀。其跟蹤結果如圖3所示。

在無外界干擾環境下，使用MPC算法對其平滑處理的蟻群算法路徑曲線實現了精確跟蹤，其起始速度及其速度跳變量均被抑制在仿生海豚允許的速度范圍之內，且跟蹤誤差極小，達到了預期要求。

3.3 海流干擾路徑規劃曲線跟蹤

在無干擾環境下，其跟蹤達到預設效果，為驗證算法可靠性，在某處海流干擾下進行路徑跟蹤，驗證在擾動環境致其偏離路線下，是否能夠繼續精確跟蹤到路線。其跟蹤效果如圖4所示。

在改進蟻群算法并平滑處理的路徑規劃下，跟蹤過程中，某處海流干擾致其偏離規劃路線，此時會觸發重規劃，先用直線連接當前點和未來一定距離的規劃點，然后用Teb優化算法優化出一條平滑的軌跡到達未來一定距離的規劃點上，最終，對重規劃的軌跡進行軌跡跟蹤，直到跟蹤到達終點。在多處海流影響下，繼續按照此方法重規劃。根據圖4（a）跟蹤效果可見，偏離路線后，進行重新規劃，仍可穩定、精確地跟蹤到規劃路徑。在圖4（b）中，海流干擾造成一定誤差，但Teb算法路徑重規劃之后，立即跟蹤上規劃路徑，跟蹤誤差減小。在圖4（c）中，干擾幾乎不影響輸出速度，均在可控范圍之內，相對較穩定。

4 仿生海豚實驗環境

本文進行實驗驗證采用的實驗環境及仿生海豚如圖5所示，該仿生海豚采用STM32F4 開發，左、右胸鰭分別采用2個舵機控制，可進行拍翼與搖翼運動，頭部由3 個舵機組成，可進行左右轉、俯仰及旋轉運動，尾部由定制的直流無刷電機控制，而海豚沉浮則采用步進電機控制，并在頭部裝配了聲吶，攝像頭，在海豚重心位置裝配姿態傳感器，壓力傳感器等輔助設備。由于實驗設備條件及環境的限制，在水池中進行了簡單路徑跟蹤實驗。

圖5 實驗環境與仿生海豚

5 結束語

在二維環境下，通過MPC算法和反步控制算法對圓形軌跡的對比及在有無海流干擾情況下，本文均研究了仿生海豚路徑跟蹤問題，其提出算法得到了有效驗證，避免了反步控制的缺陷問題，由于受到篇幅及實驗環境限制，目前只進行了簡單二維曲線路徑跟蹤實驗，后續工作中，將會在海邊完成對二維及三維有無海流情況的仿真驗證實驗，并考慮將三維環境解耦成深度控制和水平面控制進行驗證。