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

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

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

0 引 言

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

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

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

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

1.1 潛艇水平面運(yùn)動(dòng)模型

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

圖1 潛艇的運(yùn)動(dòng)坐標(biāo)系以及固定坐標(biāo)系Fig.1 Body-fixed frame and earth-fixed frame for submarine

由于潛艇垂直面的運(yùn)動(dòng)與水平面運(yùn)動(dòng)之間的耦合作用較小，只考慮潛艇的水平面運(yùn)動(dòng)情況時(shí)可忽略潛艇的垂直面運(yùn)動(dòng)項(xiàng)，故結(jié)合施生達(dá)［6］給出的潛艇標(biāo)準(zhǔn)運(yùn)動(dòng)方程，對(duì)其進(jìn)行轉(zhuǎn)換，得到潛艇的水平面動(dòng)力學(xué)以及運(yùn)動(dòng)學(xué)方程如下：

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

1.2 LOS

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

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

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

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

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

令 Δx=xk-xk-1，Δy=yk-yk-1，則當(dāng) Δx≠0且Δy≠0時(shí)，可解得

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

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

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

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

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

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

2.1 線性化模型

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

即

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

式中，I為單位矩陣。

令

即

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

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

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

2.3 約束優(yōu)化

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

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

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

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

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

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

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

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

3 仿真實(shí)驗(yàn)結(jié)果及分析

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

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

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

設(shè)定方向舵的舵角以及舵速需滿足的約束條件為 -15°≤δr≤15°,-1.5°≤Δδr≤1.5°，預(yù)測(cè)時(shí)域Ny=60，控制時(shí)域Nc=5，設(shè)定的離散航跡點(diǎn)序列為（（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，潛艇能有效實(shí)現(xiàn)水平面的航跡控制。在此基礎(chǔ)上，分別采用MPC控制方法與傳統(tǒng)比例積分微分（Proportion-Integral-Derivative，PID）控制方法進(jìn)行仿真實(shí)驗(yàn)，2種控制方法下的航跡偏差、舵角以及艏向角的仿真結(jié)果分別如圖5～圖7所示。由圖5可知，除在航跡點(diǎn)切換階段航跡控制的偏差較大之外，2種控制方法下的潛艇航跡偏差均保持在一個(gè)較小的范圍內(nèi)，符合航跡控制的要求。但是綜合分析圖6和圖7可看出，在航跡偏差大致一致的情況下，MPC控制方法相較傳統(tǒng)的PID控制方法能有效降低潛艇的打舵次數(shù)，降低了舵機(jī)的損耗，同時(shí)，控制系統(tǒng)的輸出，也即艏向角相比PID控制方法下的輸出也更為光滑；并且通過采用MPC，可將舵角以及舵速有效地限制在預(yù)定的幅值范圍內(nèi)，驗(yàn)證了預(yù)測(cè)控制中約束優(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é) 語

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