基于MPC 算法的AUV 空間航跡跟蹤控制

2021-01-19 08:11:04張子昌徐雪峰侯成剛

艦船科學技術 2020年12期

張子昌，徐雪峰，侯成剛

(天津航海儀器研究所九江分部，江西九江 332007)

0 引言

AUV 作為一種高度自動化水下自主航行裝備，對海洋開發和探測有著重要的作用和意義。對AUV 來說，自主航行控制作為其核心關鍵技術，是AUV 真正實現自治的必要條件，因此也一直是備受關注的研究熱點。對AUV 自主航行控制技術來說，除基礎的航向、深度和姿態控制外，跟蹤預設的期望路徑點航行是其非常重要和關鍵的功能。本文主要針對采用首、尾舵控制機構的AUV，研究其空間路徑點的跟蹤控制算法。

水下潛器的路徑跟蹤控制問題是近年來備受關注的熱門課題，Do K.D[1]考慮了僅有4 個推力器的AUV路徑跟蹤問題，結合Lyapunov 直接法、反演方法以及參數映射等方法提出了一種AUV 以定常軸向速度跟蹤指定路徑的方法。王銀濤等[2]研究了AUV 定常速度下的非時間參考路徑跟蹤問題，將跟蹤控制問題分為導引和控制2 個部分，導引部分基于視線導引原理輸出AUV 參考航向，控制部分構造了一種將航向控制器和路徑跟蹤控制器并行結合的混合型路徑跟蹤控制器。So-Ryeok Oh 等[3]針對欠驅動船舶的路徑點跟蹤控制在視線導航法的基礎上采用了基于狀態空間形式的模型預測控制算法，并通過仿真驗證了模型預測控制相較傳統的PID 控制算法具有更佳的控制效果。王宏健等[4]研究了欠驅動自主水下航行器的三維路徑跟蹤控制問題，針對基于虛擬向導建立的三維路徑跟蹤誤差模型，采用濾波反步法設計跟蹤控制器，增加了系統對噪聲的魯棒性。朱大奇等[5]針對水下機器人軌跡跟蹤控制的速度跳變問題，提出一種基于生物啟發神經動力學模型的自治水下機器人三維軌跡跟蹤控制算法，基于水下機器人對算法進行了仿真實驗。文獻[6 - 7]提出了自適應區域跟蹤控制方法，通過對AUV 建模不確定性項進行在線辨識，使AUV 能夠從任意初始位姿收斂到目標區域，但在目標區域邊界上出現容易出現震蕩而難以收斂于目標區域中。文獻[8]進一步地提出了一種PD 神經滑模自適應區域跟蹤控制方法，采用徑向基函數神經網絡對動力學模型中的未知項進行自適應估計，采用滑?？刂祈棇ι窠浘W絡逼近誤差進行補償。程相勤[9]針對欠驅動UUV 的三維路徑跟蹤問題提出了一種離散滑模預測的控制方法，提高了欠驅動UUV 在海流干擾情況下的空間曲線路徑跟蹤控制的精度并提升了控制系統的魯棒性。

本文首先應用視線導航法和垂直面制導算法將路徑點跟蹤控制問題轉化為航向、深度和縱傾的跟蹤控制問題，再設計基于狀態空間模型的模型預測控制算法使得AUV 能收斂到期望航向、深度和縱傾達到預期的路徑點跟蹤控制效果。

1 系統模型建立

由于AUV 在水下的航行是六自由度的空間運動，對于其六自由度的運動學以及動力學建模，可參考一般潛艇以及AUV（自主水下機器人）的建模方法。參考文獻[10]中給出的AUV6 空間運動坐標系，如圖1所示。

結合剛體動力學方程的矢量表達形式，可將AUV 的動力學以及運動學方程[11]表示為：

圖 1 AUV 的運動坐標系以及固定坐標系Fig. 1Body-fixed frame and earth-fixed frame for AUV

式中：ν=[u,v,w,p,q,r]T為AUV 在運動坐標系下分解的速度以及角速度，u，v，w分別為運動坐標系下的縱向、橫向和垂向線速度，p，q，r分別為運動坐標系下的橫傾角速度、縱傾角速度和轉首角速度；η=[x,y,z,φ,θ,ψ]為固定坐標系下的位置和歐拉角，x，y，z 分別為固定坐標系下的縱向、橫向以及垂向位置，φ，θ，ψ分別為固定坐標系下的橫傾角、縱傾角和航向角； M 為包含附加質量的系統慣性矩陣， C (ν)為包含附加質量的剛體科里奧利向心力矩陣；D(ν)為阻尼系數矩陣；g(η)為重力和浮力引起的均衡力矢量；g0為AUV 壓載水艙提供的均衡力矢量； ω為風、浪、流等外部環境干擾作用力和力矩；τ=[Fx,Fy,Fz,Mx,My,Mz]為AUV 推進系統的力和力矩矢量，考慮到航行的能耗，在不需要精確動力定位航行時，依靠全方位推進器產生縱向推力 Fx，轉首力矩 Mz和縱傾力矩 My作為航行的主要驅動力更為節省能源，所以可認為系統的被控輸入量只有此3 項，而其他3 項可由控制器生成的控制輸入再計算得出，且經計算驗證均為較小的值。J(η)為運動坐標系到固定坐標系的轉換矩陣，其表達式為：

2 航跡制導算法

針對水下潛器的路徑跟蹤問題的誤差建模主要有2 種思路，一種是在Serret-frenet 坐標系下建立誤差模型，另一種為運用導航視線法將跟蹤誤差模型簡化為簡單的航速控制、航向控制以及縱傾控制問題，目前水平面視線導航法已經應用得相當成熟，其原理如圖2 所示。要驅使AUV 收斂到由水平面離散路徑點(···,Pk-1,Pk,Pk+1,···)構成的路徑，則艇體質心至路徑段(Pk-1,Pk) 的橫向距離 ek需收斂至0，同時航向角 ψ收斂至αk-1。運用視線導航法，首先由艇體質心至(Pk-1,Pk)段路徑的垂直距離 ek以及Δ=n1L （ L為艇長， n1為一正實數）可計算出視線導引點PLOS(xlos,ylos)在固定坐標系下的坐標，再求取艇體質心至視線導引點PLOS的視線導航航向角ψlos，則航向角 ψ收斂至 ψlos可確保艇體位置收斂到直線路徑段(Pk-1,Pk)。當艇體質心位于以路徑點 Pk半徑為 R =n2L（ n2為正實數）的圓內，則將導航的目標點切換至下一個目標點Pk+1。

圖 2 直線航跡段視線導航法示意圖Fig. 2Body-fixed frame and earth-fixed frame for AUV

垂直面航跡制導算法如果采用視線導航算法將垂直面位置偏差解算為指令縱傾的會造成以下問題：1）AUV 垂直面內航行對縱傾角有一定限制，采用視線導航法進行解算將造成部分參數難以調整。2）AUV在進行垂直面機動控制的過程中，深度速率的控制必須同時考慮垂向速度和縱傾，采用視線導航法解算指令縱傾角而忽略垂向速度，將引起較大的航跡控制偏差。因此，本文通過分析AUV 垂直面運動特性以及仿真驗證等方法，采用指令深度速率、指令深度以及指令縱傾三者結合的方式作為垂直面的航跡跟蹤控制算法。

其中指令深度與指令縱傾的解算方法如圖3 所示。當前時刻的指令深度為AUV 當前位置點在航跡線上的投影點的垂直位置，其解算方法為：

式中：為距目標航跡點的縱向位移，指令縱傾為當前航跡線的期望潛浮角χk:=atan2(zk+1-zk,xk+1-xk)。由航跡線解算出的指令深度速率與AUV 的當前航速相關，其解算方法為：

圖 3 垂直面指令深度、指令縱傾、指令深度速率解算方法Fig. 3Calculation method of vertical plane command depth，command trim ，command depth rate

3 控制器設計

模型預測控制（Model Predictive Control, MPC）是20 世紀70 年代發展起來的一類控制算法，算法采用多步預測、滾動優化以及反饋校正等控制策略，使得模型預測控制具有控制效果好、魯棒性強且對模型精確性要求較低等優點，同時可以考慮到執行機構的約束性。預測控制算法另一個明顯的優勢便是可以將各類約束條件加入到控制過程當中使得控制系統更具有安全性[12]，因此基于以上幾點考慮，本文設計一種基于狀態空間模型的預測控制算法用于AUV 深度控制算法。

3.1 線性化模型

由于AUV 的非線性模型十分復雜，不宜直接作為控制器設計的模型，故需將AUV 的六自由度非線性模型簡化成線性模型以便于控制器的設計。本文考慮路徑點跟蹤控制問題，并假定AUV 的縱向速度 u保持不變，即u˙=0，同時忽略水平面和垂直面運動的耦合作用，將AUV 的非線性模型線性化成狀態方程形式的系統模型，下面以垂直面控制為例介紹控制器設計過程。

其中，x=[w,q,θ,ζ]T是系統的狀態量， u =Mz為系統的控制輸入量是被控系統的輸出量。 Ad，Bd， Cd分別為離散化后的狀態空間模型矩陣。當外部環境存在干擾時，由于典型的狀態反饋控制律u(k)=-Kx(k)中不包含積分環節，所以針對狀態反饋控制，狀態量一般都會存在靜差，考慮到實際建模的誤差以及控制過程中存在的噪聲，為消除穩態時的靜差，在基于狀態空間方程的預測控制中，采用基于增廣狀態的狀態空間模型，用Δu(k) （其中Δu(k)=u(k)-u(k-1)為控制輸入的變化率）代替u(k)作為控制輸入量使系統能夠實現無靜差控制[13]，則將狀態空間方程改寫成如下形式：

3.2 未來狀態預測

不考慮測量噪聲對狀態量的影響，即整個狀態向量是可測的，得z?(k|k)=z(k)（其中 ξ ?(k|k)代表 k時刻預估的 k 時刻的狀態量估計值），設定預測時域為 Ny以及控制時域 Nc，系統的預測模型為：

3.3 約束優化

將性能指標函數定義為：

其中 r為設定的參考軌跡，也即由垂直面制導算法得出的動態期望航向角ψlos，其中 Q =diag(q1,···,qNy)為Ny維的輸出誤差權值矩陣， R =diag(r1,···,rNc) 為 Nc維的輸入權值矩陣。

對于AUV 的路徑跟蹤控制問題而言，由于執行器的物理約束，轉首力矩的大小受到一定的限制，也即控制輸入量需滿足一定的約束條件，也即u(k)需滿足：

將式（13）代入式（15）并展開得到下式：

式中等號右邊第1 項為確定值，與優化無關，在計算時可只考慮后2 項的影響，并將后2 項寫成標準的二次型形式，同時將輸入量的約束寫成向量形式，則可將式（16）寫成新的標準二次型約束優化問題，得到：

其中 S =HTQH+R， f=HTQT(Pξ(k)-r)，W=[I -I]T，U=[，則式（18）也即標準的QP（二次規劃）問題，采用二次規劃算法便可求出式（18）的最優解，假設由式（19）計算出的最優解為 Δ Uopt(k)，依據模型預測控制的滾動優化策略，取最優解的第1 列Δuopt(k)即可作為當前時刻的控制器輸出增量：

則當前時刻的控制器最優輸出值為上一時刻的控制器輸出值加上當前時刻的控制器增量值，即

在此基礎上進行滾動優化便可得出整個控制過程的最優控制輸入。

4 仿真示例

本文采用Matlab/Simulink 仿真工具，首先設定空間離散路徑點，并由AUV 實時位置信息，采用水平面視線導航算法和垂直面制導算法推算出動態的期望航向角、期望深度、期望縱傾、或深度速率，再由MPC控制器解算出期望控制輸入量 Mz*，并通過執行器閉環模塊得出最終的系統控制輸入量，整個控制系統的原理框圖如圖3 所示。

設定控制機構需滿足的約束條件為umin＜u ＜umax,Δumin＜Δu ＜Δumax，預測時域為 Ny為35，控制時域Nc為4。仿真算例1 為垂直面航跡控制，離散路徑點序列為[0 0 30] [2000 0 260] [4 000 0 60] [6 000 0 160] [8 000 0 60] [10 000 0 160] [12 000 0 60] [14 000 0 60] [2 000 0 260] m，其路徑點跟蹤效果仿真示意圖如圖4 所示。

可知，AUV 能實現垂直面的路徑點跟蹤。圖5 為路徑跟蹤過程中的縱傾和深度曲線，AUV 與設定路徑的垂直距離誤差仿真結果如圖6 所示。

圖 4 路徑點跟蹤控制原理框圖Fig. 4Block diagram of waypoint tracking control principle

圖 5 垂直面路徑點跟蹤曲線Fig. 5The curve of way-point following with current

圖 6 縱傾和深度曲線Fig. 6The curve of trim and depth

仿真算例2 為空間路徑點跟蹤控制，空間路徑點序列為[0 0 60]，[2 000 2 000 140]，[2 000 4 000 60]， [0 6 000 140]，[-2 000 4 000 60]，[-2 000 2 000 140]，[0 0 60] m，路徑跟蹤效果如圖7 所示。圖8 和圖9 分別為水平面和垂直面的航跡偏差。

5 結語

本文針對AUV 的空間路徑點跟蹤控制問題，采用水平面視線導航法計算AUV 指令航向，采用垂直面制導算法計算AUV 的垂直面指令深度、指令縱傾和指令深度速率，采用模型預測控制算法用于AUV 的航向和深度控制。仿真結果表明，空間航跡控制算法能達到較高的控制精度，采用的指令深度+指令縱傾+指令深度速率的控制方法能使AUV 盡快的航行至指令軌跡并跟蹤垂直面航跡，分析航跡偏差曲線，可發現只有在航跡切換過程中才有明顯的航跡偏差出現，且垂直面切換時潛浮角的改變越小，也即垂直面航跡越平坦時，航跡控制的精度也越高，對應的航跡切換過程中的航跡偏差也越小。