基于模型預(yù)測(cè)控制的仿生海豚路徑跟蹤研究*

付樂(lè)樂(lè)，陳 宏，葉鎮(zhèn)豪，鞏偉杰，何 莉

（深圳大學(xué)機(jī)電與控制工程學(xué)院，廣東 深圳 518061）

0 引 言

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

綜上分析，本文對(duì)水下機(jī)器人路徑跟蹤控制方法采用MPC，可預(yù)測(cè)未來(lái)模型，進(jìn)行反復(fù)優(yōu)化求解并持續(xù)滾動(dòng)控制［7］，針對(duì)海流影響進(jìn)行了干擾分析，以及對(duì)于三維的水下路徑跟蹤研究，均驗(yàn)證了MPC 算法的路徑跟蹤效果極佳。

1 仿生海豚運(yùn)動(dòng)學(xué)

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

圖1 慣性坐標(biāo)系和載體坐標(biāo)系

在慣性坐標(biāo)系下，水下仿生海豚的位姿可用位置（x，y，z）和姿態(tài)角（φ，θ，ψ）表示，φ，θ，ψ分別為橫擺角、縱傾角和艏向角；在載體坐標(biāo)系下，仿生海豚的線速度和角速度分別為（u，v，w）和（p，q，r），u，v，w分別表示方向?yàn)檫M(jìn)退、側(cè)移、潛浮，在6自由度下仿生海豚的運(yùn)動(dòng)學(xué)轉(zhuǎn)換方程為［9］

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

2 仿生海豚MPC算法

2.1 仿生海豚跟蹤誤差模型

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

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

2.2 仿生海豚輸出預(yù)測(cè)

誤差模型是連續(xù)量，不能直接作用于模型預(yù)測(cè)控制器中，需要對(duì)式（3）進(jìn)行離散化處理，處理后結(jié)果為

式中 ?η（k）為狀態(tài)量，?ν（k）為控制量，k為某時(shí)刻，其中

式中T為采樣時(shí)間，考慮到實(shí)際建模不準(zhǔn)確性帶來(lái)的誤差，為了能夠跟蹤更加精確，考慮以ΔV作為控制量，對(duì)式（4）進(jìn)行變換，得到

從而，可進(jìn)一步得到新的狀態(tài)空間表達(dá)式為

式中 ΔV為控制增量矩陣，為系數(shù)矩陣，＝

為簡(jiǎn)化計(jì)算，可令

根據(jù)新的離散化后的狀態(tài)空間表達(dá)式，可進(jìn)行輸出預(yù)測(cè)為

其中

可通過(guò)當(dāng)前狀態(tài)量和控制增量計(jì)算得到預(yù)測(cè)的狀態(tài)量和輸出量，之后在約束條件下進(jìn)行二次規(guī)劃求解，得到控制增量的第一個(gè)序列，并以此求出控制量，作用于系統(tǒng)。

2.3 約束條件下的目標(biāo)函數(shù)求解

通過(guò)MPC進(jìn)行預(yù)測(cè)后，最終要轉(zhuǎn)為目標(biāo)函數(shù)優(yōu)化問(wèn)題求解，目標(biāo)函數(shù)要確保仿生海豚能平穩(wěn)地跟蹤到參考軌跡，要反映出跟隨能力和平穩(wěn)能力，故對(duì)目標(biāo)函數(shù)的設(shè)計(jì)如下

式中R和Q為權(quán)重矩陣。前項(xiàng)能反映對(duì)參考路徑的跟隨能力，后項(xiàng)能反映系統(tǒng)的穩(wěn)定性。通過(guò)一定方式可將目標(biāo)函數(shù)轉(zhuǎn)化為二次規(guī)劃問(wèn)題，將式（7）代入式（11）并化簡(jiǎn)得到標(biāo)準(zhǔn)形式的目標(biāo)函數(shù)

因此，在每一步的目標(biāo)函數(shù)最優(yōu)解問(wèn)題都可以轉(zhuǎn)為約束下二次規(guī)劃問(wèn)題求解

其中，約束條件為

在每個(gè)控制周期內(nèi)，對(duì)式（13）進(jìn)行求解，得到一系列的控制增量，由此會(huì)得到一系列的控制量，并將序列中第一個(gè)控制量作為控制輸入量作用在系統(tǒng)中，直到下一時(shí)刻來(lái)臨產(chǎn)生新的控制量序列，再次將第一個(gè)控制量作用于系統(tǒng)，循環(huán)往復(fù)，直到跟蹤路徑完成。

2.4 海流模型建立

海洋中海流對(duì)水下仿生海豚的影響處處可見(jiàn)，海洋中真實(shí)海流運(yùn)動(dòng)不規(guī)律，時(shí)變性較強(qiáng)，文中給出了海流模型，并分析某處海流對(duì)仿生海豚產(chǎn)生的路線偏移影響，其分析方法將適用于整片海流擾動(dòng)之中，本文給出了一種基于流函數(shù)的周期變動(dòng)的雙曲流的計(jì)算實(shí)例，驗(yàn)證存在海流情況下，路徑跟蹤算法的有效性，其雙曲流為

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

海流速度場(chǎng)如下

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

3 仿生海豚跟蹤實(shí)驗(yàn)仿真

3.1 二維環(huán)境下圓形跟蹤

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

圖2 圓形軌跡跟蹤效果對(duì)比

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

3.2 路徑規(guī)劃曲線跟蹤

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

在無(wú)外界干擾環(huán)境下，使用MPC算法對(duì)其平滑處理的蟻群算法路徑曲線實(shí)現(xiàn)了精確跟蹤，其起始速度及其速度跳變量均被抑制在仿生海豚允許的速度范圍之內(nèi)，且跟蹤誤差極小，達(dá)到了預(yù)期要求。

3.3 海流干擾路徑規(guī)劃曲線跟蹤

在無(wú)干擾環(huán)境下，其跟蹤達(dá)到預(yù)設(shè)效果，為驗(yàn)證算法可靠性，在某處海流干擾下進(jìn)行路徑跟蹤，驗(yàn)證在擾動(dòng)環(huán)境致其偏離路線下，是否能夠繼續(xù)精確跟蹤到路線。其跟蹤效果如圖4所示。

在改進(jìn)蟻群算法并平滑處理的路徑規(guī)劃下，跟蹤過(guò)程中，某處海流干擾致其偏離規(guī)劃路線，此時(shí)會(huì)觸發(fā)重規(guī)劃，先用直線連接當(dāng)前點(diǎn)和未來(lái)一定距離的規(guī)劃點(diǎn)，然后用Teb優(yōu)化算法優(yōu)化出一條平滑的軌跡到達(dá)未來(lái)一定距離的規(guī)劃點(diǎn)上，最終，對(duì)重規(guī)劃的軌跡進(jìn)行軌跡跟蹤，直到跟蹤到達(dá)終點(diǎn)。在多處海流影響下，繼續(xù)按照此方法重規(guī)劃。根據(jù)圖4（a）跟蹤效果可見(jiàn)，偏離路線后，進(jìn)行重新規(guī)劃，仍可穩(wěn)定、精確地跟蹤到規(guī)劃路徑。在圖4（b）中，海流干擾造成一定誤差，但Teb算法路徑重規(guī)劃之后，立即跟蹤上規(guī)劃路徑，跟蹤誤差減小。在圖4（c）中，干擾幾乎不影響輸出速度，均在可控范圍之內(nèi)，相對(duì)較穩(wěn)定。

4 仿生海豚實(shí)驗(yàn)環(huán)境

本文進(jìn)行實(shí)驗(yàn)驗(yàn)證采用的實(shí)驗(yàn)環(huán)境及仿生海豚如圖5所示，該仿生海豚采用STM32F4 開(kāi)發(fā)，左、右胸鰭分別采用2個(gè)舵機(jī)控制，可進(jìn)行拍翼與搖翼運(yùn)動(dòng)，頭部由3 個(gè)舵機(jī)組成，可進(jìn)行左右轉(zhuǎn)、俯仰及旋轉(zhuǎn)運(yùn)動(dòng)，尾部由定制的直流無(wú)刷電機(jī)控制，而海豚沉浮則采用步進(jìn)電機(jī)控制，并在頭部裝配了聲吶，攝像頭，在海豚重心位置裝配姿態(tài)傳感器，壓力傳感器等輔助設(shè)備。由于實(shí)驗(yàn)設(shè)備條件及環(huán)境的限制，在水池中進(jìn)行了簡(jiǎn)單路徑跟蹤實(shí)驗(yàn)。

圖5 實(shí)驗(yàn)環(huán)境與仿生海豚

5 結(jié)束語(yǔ)

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