基于模型預(yù)測控制的潛艇水平面航跡控制算法

張子昌，林莉，徐雪峰，趙光

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

0 引 言

潛艇的水平面操縱控制可分為航向控制和航跡控制2種形式［1］，航跡控制指的是被操控對象在操控系統(tǒng)的作用下，從起始位置能沿著預(yù)定軌跡航行到目的地，但由于潛艇一般不裝備側(cè)向推進器，而只采用方向舵控制潛艇的艏向角，所以潛艇的航跡控制與一般欠驅(qū)動船舶以及欠驅(qū)動水下潛器的航跡控制相似，都存在欠驅(qū)動的特性。

近年來，欠驅(qū)動水下潛器以及欠驅(qū)動水面船舶的航跡控制問題一直是學(xué)術(shù)界備受關(guān)注的熱門課題。李鐵山等［2］針對水面船舶的直線航跡控制問題采用了重定義輸入輸出線性化方法，將單輸入多輸出誤差模型簡化為單輸入單輸出模型，從而簡化了控制器的設(shè)計，但設(shè)定的直線航跡較為簡單，具有一定的局限性。Oh等［3-4］運用模型預(yù)測控制研究了水面船舶的直線路徑跟蹤控制問題，并與傳統(tǒng)的PID控制效果進行對比，驗證了模型預(yù)測控制算法的優(yōu)勢。劉昌鑫等［5］針對水下機器人的曲線路徑跟蹤控制的約束問題，采用模型預(yù)測控制得到了能滿足約束要求的控制輸入，驗證了模型預(yù)測控制算法對約束問題的求解能力。

但上述研究主要是針對特定的直線或者曲線路徑進行分析，而潛艇執(zhí)行任務(wù)時，許多航跡由多個離散航跡點構(gòu)成。所以，本文將針對某型潛艇的水平面航跡控制問題，首先采用視線導(dǎo)航法（Line-of-Sight，LOS）將航跡控制問題簡化成艏向控制問題，艏向控制算法設(shè)計上，在增量形式的狀態(tài)空間模型的基礎(chǔ)上設(shè)計模型預(yù)測控制算法，并將通過由離散航跡點構(gòu)成的航跡控制仿真實驗來驗證控制算法的有效性。

1 系統(tǒng)模型建立

1.1 潛艇水平面運動模型

參照目前國際上普遍采用的國際拖曳船模水池會議（International Towing Tank Conference，ITTC）推薦的坐標系，其坐標軸設(shè)置如圖1所示，其中：E-ξηζ為固定坐標系；O(G)-xyz為運動坐標系。

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

由于潛艇垂直面的運動與水平面運動之間的耦合作用較小，只考慮潛艇的水平面運動情況時可忽略潛艇的垂直面運動項，故結(jié)合施生達［6］給出的潛艇標準運動方程，對其進行轉(zhuǎn)換，得到潛艇的水平面動力學(xué)以及運動學(xué)方程如下：

式中：m為潛艇質(zhì)量；ρ為潛艇所處深度的海水密度；L為艇長；u，v，r，p分別為潛艇的縱向速度、橫向速度、轉(zhuǎn)艏角速度和橫傾角速度；分別為縱向加速度、橫向加速度、轉(zhuǎn)艏角加速度以及橫傾角加速度；Fxp為螺旋槳推力減去阻力在縱向上的分量；Ixx和Izz為潛艇的轉(zhuǎn)動慣量；φ，ψ，ξ，η分別為潛艇的橫傾角、艏向角、縱向位移以及橫向位移；W，B分別為潛艇靜載和潛艇受到的浮力；δr為潛艇方向舵舵角；其它項為潛艇的無因次化水動力系數(shù)項。

1.2 LOS

目前學(xué)術(shù)界研究水面船舶及水下潛器的航跡控制問題主要有2種思路：一種是在Serret-frenet坐標系下直接建立誤差模型［4-5，7］；另一種為運用LOS將航跡控制問題簡化為更為簡單的航速控制、艏向控制以及縱傾控制問題［3，8-9］，針對由離散航跡點構(gòu)成的航跡，本文采用更為適用且易于實現(xiàn)的LOS。

航跡控制的目標如圖2所示。要控制潛艇收斂到由水平面離散航跡點(...,Pk-1,Pk,Pk+1,...)構(gòu)成的航跡，則艇體質(zhì)心至航跡段(Pk-1,Pk)的垂直距離（即航跡偏差ek）需收斂至0。

設(shè)定航跡點Pk-1的坐標為(xk-1,yk-1)，Pk的坐標為 (xk,yk)，潛艇當前的位置點為P(t)=[x(t),y(t)]，αk-1為航跡段 (Pk-1,Pk)與固定坐標系縱向坐標軸ξ之間的夾角。本文采用Breivik［9］的第 2 種方法，其具體過程可概括為：以當前位置點作一個半徑為R1的圓，R1=n1L（n1為正實數(shù)），當R1足夠大時，所作圓必與航跡點之間的連線相交，將交點中更接近于Pk的交點定為視線導(dǎo)引點Plos(xlos,ylos)，即可求出導(dǎo)引艏向角ψlos作為潛艇的期望動態(tài)艏向角：

圖2 直線航跡段視線導(dǎo)航法示意圖Fig.2 LOS guidance of a straight line

為求解出視線導(dǎo)引點Plos的位置坐標，也即所作圓與直線航跡段的交點，需求解下列方程組：

令 Δx=xk-xk-1，Δy=yk-yk-1，則當 Δx≠0且Δy≠0時，可解得

式中：a=1+d2，其中，其 中g(shù)=f-de，e=xk，f=yk；c=x(t)2+y(t)2+g2-2gy(t)-R2。當 Δx=0時，可得

同時，潛艇至航跡段(Pk-1,Pk)的航跡偏差e(t)可由下式計算得出：

當艇體質(zhì)心位于以航跡點Pk為圓心，半徑為R2=n2L（n2為正實數(shù)）的圓內(nèi)時，將導(dǎo)航的目標點切換至下一個目標點Pk+1。通過采用LOS，求取艇體質(zhì)心至視線導(dǎo)引點Plos的視線導(dǎo)航艏向角ψlos，控制潛艇的艏向角ψ收斂至ψlos，可確保艇體位置收斂至直線航跡段(Pk-1,Pk)，對此Breivik［9］給出了詳細的證明過程，在此不再贅述。

綜上所述，運用LOS能將水平面的航跡控制問題轉(zhuǎn)化為較為簡單的艏向控制問題，也即將問題轉(zhuǎn)換為問題。

2 艏向控制器設(shè)計

模型預(yù)測控制（Model Predictive Control，MPC）由于采用了多步預(yù)測、滾動優(yōu)化以及反饋校正等控制策略，使其一般具有控制效果好、魯棒性強等優(yōu)點，同時考慮到潛艇方向舵的舵角與舵速的約束要求，模型預(yù)測控制另一個明顯的優(yōu)勢便是可以將各類約束條件加入到控制過程當中，使得控制系統(tǒng)更具有安全性［10］。因此，本文設(shè)計了一種基于增量式狀態(tài)空間模型的預(yù)測控制算法用于潛艇的艏向控制。

2.1 線性化模型

由于潛艇水平面運動的非線性模型十分復(fù)雜，不宜直接作為控制器設(shè)計的模型，故需將潛艇的水平面運動模型簡化成線性模型，以便于控制器的設(shè)計。本文假定潛艇的縱向速度u保持不變，即=0，且潛艇航行時的深度保持不變，同時忽略垂直面運動與橫搖運動對潛艇水平面運動的耦合作用，將潛艇的水平面運動非線性模型線性化成狀態(tài)空間形式的系統(tǒng)模型，其結(jié)果形式如下：

即

再將其轉(zhuǎn)換為離散時間的線性狀態(tài)空間模型：

式中，I為單位矩陣。

令

即

2.2 未來狀態(tài)預(yù)測

設(shè)定預(yù)測時域Ny以及控制時域Nc，通過迭代式（12），可得到系統(tǒng)的預(yù)測模型

式中：z(k+n|k)為k時刻預(yù)測k+n時刻的系統(tǒng)狀態(tài)值；Δu(k+n|k)為k時刻預(yù)測k+n時刻的系統(tǒng)輸入增量值；y(k+n|k)為k時刻預(yù)測k+n時刻的系統(tǒng)輸出值。

2.3 約束優(yōu)化

將狀態(tài)空間模型中的輸出y重新記為ψ，為簡化公式，將輸入u簡單記為δ，即δr=δ，性能指標函數(shù)可定義為：

式中：WQ=diag(q1,…,qNy)，為Ny維輸出誤差權(quán)值矩陣；WR=diag(r1,…,rNc)為Nc維輸入權(quán)值矩陣。

對于潛艇的水平面航跡跟蹤控制問題而言，由于執(zhí)行機構(gòu)的物理約束，方向舵的幅值大小與方向舵的舵速大小均受到一定的限制，即控制輸入量需滿足一定的約束條件，δ(k)需滿足

式中：δmin，δmax分別為舵角的飽和下、上限值；Δδmin，Δδmax分別為舵速的飽和下、上限值。因此，預(yù)測控制問題便轉(zhuǎn)化為實時的在線約束優(yōu)化問題：

將式（13）代入式（16）并展開，得到

式中：S=HTWQH+WR；f=HTWQT(Pz(k)-Ψlos)。式（18）即標準的約束二次規(guī)劃問題，采用一般的二次規(guī)劃算法便可求出式（18）的最優(yōu)解。設(shè)定由式（18）計算出的最優(yōu)解為，依據(jù)模型預(yù)測控制的滾動優(yōu)化策略，取最優(yōu)解的第一列即可作為當前時刻的控制器輸出增量值，也即當前時刻的期望舵角增量值：

則當前時刻的控制器最優(yōu)輸出值為上一時刻控制器輸出值加上當前時刻控制器輸出的最優(yōu)增量值，便得到當前時刻的期望舵角值為

在此基礎(chǔ)上進行滾動優(yōu)化，便可得出整個控制過程的最優(yōu)控制輸入。

3 仿真實驗結(jié)果及分析

本文采用Matlab/Simulink仿真工具，首先設(shè)定水平面離散航跡點，并由潛艇實時位置信息，采用LOS推算出動態(tài)的期望艏向角ψlos，再由MPC艏向控制器解算出期望的控制輸入量，也即期望的方向舵舵角δr*，并通過簡單的執(zhí)行器閉環(huán)得出實際的系統(tǒng)控制輸入量δr。整個控制系統(tǒng)的原理框圖如圖3所示。

圖3 航跡控制原理框圖Fig.3 The system schematic of track control in horizontal plane

圖4 潛艇航跡點控制曲線Fig.4 The curve of submarine's track control

設(shè)定方向舵的舵角以及舵速需滿足的約束條件為 -15°≤δr≤15°,-1.5°≤Δδr≤1.5°，預(yù)測時域Ny=60，控制時域Nc=5，設(shè)定的離散航跡點序列為（（0，0），（-4 000，4 000），（0，8 000），（8 000，12 000），（20 000，6 000），（24 000，0），（16 000，-4 000），（8 000，0），（4 000，-4 000），（0，0）），潛艇的初始位置為（0，0），初始艏向角為 90°，航速u=9 kn，艇長L=80 m，n1,n2分別取3和6，航跡控制的仿真效果示意圖如圖4所示。

由圖4可看出，結(jié)合LOS與MPC，潛艇能有效實現(xiàn)水平面的航跡控制。在此基礎(chǔ)上，分別采用MPC控制方法與傳統(tǒng)比例積分微分（Proportion-Integral-Derivative，PID）控制方法進行仿真實驗，2種控制方法下的航跡偏差、舵角以及艏向角的仿真結(jié)果分別如圖5～圖7所示。由圖5可知，除在航跡點切換階段航跡控制的偏差較大之外，2種控制方法下的潛艇航跡偏差均保持在一個較小的范圍內(nèi)，符合航跡控制的要求。但是綜合分析圖6和圖7可看出，在航跡偏差大致一致的情況下，MPC控制方法相較傳統(tǒng)的PID控制方法能有效降低潛艇的打舵次數(shù)，降低了舵機的損耗，同時，控制系統(tǒng)的輸出，也即艏向角相比PID控制方法下的輸出也更為光滑；并且通過采用MPC，可將舵角以及舵速有效地限制在預(yù)定的幅值范圍內(nèi)，驗證了預(yù)測控制中約束優(yōu)化的有效性。

圖5 航跡偏差曲線Fig.5 The curve of tracking error

圖6 控制器輸出量曲線Fig.6 The curve of controller output

圖7 艏向角曲線Fig.7 The curve of heading angle

4 結(jié) 語

本文針對潛艇的水平面航跡控制問題，首先運用LOS簡化了潛艇的航跡控制問題，并采用MPC用于潛艇的艏向控制。通過仿真驗證，所設(shè)計的MPC能有效解決控制過程中的方向舵舵角與舵速的約束限制問題，并且相較傳統(tǒng)的PID控制方法能夠有效減少打舵次數(shù)，證明了預(yù)測控制算法所具有的優(yōu)點。同時，仿真實驗也驗證了采用結(jié)合LOS與MPC的航跡控制方法能夠?qū)⒑桔E偏差穩(wěn)定在一個較小的區(qū)間范圍內(nèi)，從而證明了所設(shè)計的航跡控制算法能夠有效解決潛艇的水平面航跡控制問題。