郭亦平 王益民 林莉 任元洲

（中船重工集團(tuán)公司第七〇七研究所九江分部，九江市，333005）

水下自主無人航行器已廣泛應(yīng)用于海洋救助、打撈、海洋資源調(diào)查、石油開采等民用領(lǐng)域，并可承擔(dān)掃雷、偵察、情報(bào)搜集及海洋探測等軍事任務(wù)。作為AUV的核心技術(shù)之一，運(yùn)動控制技術(shù)得到國內(nèi)外學(xué)者和工程技術(shù)人員的廣泛關(guān)注，并開展了大量的理論研究和工程實(shí)踐工作。水下航跡跟蹤控制是AUV的基本航行模式，主要應(yīng)用于水下管網(wǎng)巡檢、海洋資源調(diào)查等場合。高劍[1]基于級聯(lián)系統(tǒng)理論研究了欠驅(qū)動自主無人潛航器的水平面直線航跡跟蹤控制和三維直線航跡跟蹤控制問題，證明了直線航跡跟蹤的全局K指數(shù)穩(wěn)定性。Morten Breivik、Fossen[2]基于視線導(dǎo)引法（Line-of-Sight，LOS）提出了三維路徑跟隨的制導(dǎo)律實(shí)現(xiàn)了三維航跡跟蹤控制，Khac Duc Do和Jie Pan[3]利用反步自適應(yīng)控制方法實(shí)現(xiàn)了欠驅(qū)動AUV的三維航跡跟蹤控制。本文利用空間三維視線導(dǎo)引法，將AUV三維航跡跟蹤控制轉(zhuǎn)換為航向跟蹤控制和縱傾角跟蹤控制，實(shí)現(xiàn)了欠驅(qū)動AUV的三維航跡跟蹤控制。

1 基于空間LOS的三維制導(dǎo)算法

圖1所示為AUV的空間三維視線導(dǎo)引法示意圖。圖中，P為AUV的實(shí)際位置，表示期望直線航跡，由圖1可知，三維直線航跡跟蹤控制的目的為：

圖1 AUV的3維直線航跡跟蹤LOS

由圖1可知，在包含期望航跡線的平面內(nèi)對AUV實(shí)際航行位置進(jìn)行投影，可消除橫向航跡偏差，AUV的指令航向：

式中

為直線航跡線方位角。

按照空間LOS方法，為了消除航跡橫向偏差，AUV需以航跡線上某一點(diǎn)作為當(dāng)前目標(biāo)點(diǎn)，由此可得

為了消除海流和漂角影響，可在式（3）中引入一積分項(xiàng)，如此可導(dǎo)出如下具有飽和控制作用的航向修正律：垂向航跡偏差控制與航跡橫向偏差控制相仿，簡述如下：

（2）當(dāng)前時刻AUV的指令縱傾角可設(shè)置為：

式中

（3）為消除海流和攻角影響，可在式（7）中引入一積分項(xiàng)，可得修正后的指令縱傾角為：

當(dāng)AUV沿著直線航跡航行到距下一航路點(diǎn)一定距離的時候，需要進(jìn)行航路點(diǎn)切換，切換機(jī)制有兩種實(shí)現(xiàn)方式：

（1）當(dāng)AUV的位置與直線航路點(diǎn)Pi的距離與兩個航路點(diǎn)距離的差小于某一設(shè)定值時，則當(dāng)選擇下一航路點(diǎn)，即

一般而言，可選Ri+1=(1.5~5)L，L為AUV的艇長。

（2）可參照文獻(xiàn)[4]的橢球切換機(jī)制選擇下一航路點(diǎn)，即

2 縱傾MPC控制算法

本文中AUV的縱傾控制和航向控制均采用一種基于拉蓋爾（Laguerre）函數(shù)逼近的MPC方法[5]，該方法可大幅減少計(jì)算負(fù)擔(dān)，因而可用于AUV自主航行控制系統(tǒng)中。航向MPC控制算法的設(shè)計(jì)可參照文獻(xiàn)[6]。

一般具有魚雷外形的欠驅(qū)動AUV僅配置十字型尾鰭對航向、縱傾實(shí)施控制。理論研究表明， 這種AUV的垂直面運(yùn)動與水平面運(yùn)動耦合性較小，因而AUV垂直面運(yùn)動線性方程可用狀態(tài)空間方程描述如下：

式中，w為垂向速度；q為縱傾變化率；θ為縱傾角；δs為艉水平鰭角。式（13）和式（14）簡記為

式中：

利用后向差分法離散化式（15）后，可得

將式（16）離散化兩端差分運(yùn)算后，可得

引入如下符號：

則可由式（18）得如下增量式狀態(tài)方程和狀態(tài)輸出方程：

上式簡記為

假設(shè)在k時刻的m個采樣間隔后解算的預(yù)測指令鰭角增量為

拉蓋爾函數(shù)向量具有下述正交特性

假設(shè)在k時刻，式（20）的狀態(tài)為則在此時刻的m個采樣間隔的預(yù)測狀態(tài)為

為設(shè)計(jì)縱傾預(yù)測控制律，設(shè)代價(jià)函數(shù)為

將式（23）所述預(yù)測狀態(tài)代入上述代價(jià)函數(shù)，可得

則利用拉蓋爾函數(shù)的正交特性，可得

考慮到控制縱傾的尾鰭在施加運(yùn)動控制過程中存在角度和角速度物理限制，因而，上述代價(jià)函數(shù)的求解需考慮下述物理約束：

鰭角約束不等式為：

上式可簡記為：

鰭速約束不等式為：

上式可簡記為：

將式（28）和上式聯(lián)立后簡記為：

則縱傾MPC控制器的求解問題轉(zhuǎn)換為二次規(guī)劃問題：

該二次規(guī)劃問題可采用活動集法求解，可利用Hildreth二次規(guī)劃方法[7]。故縱傾MPC控制器可設(shè)計(jì)為：

由于一般廉價(jià)的AUV出于性價(jià)比考慮，不安裝速率陀螺和加速度計(jì)等測量裝置，不具備垂向速度w和縱傾角速度q的測量能力，故需設(shè)計(jì)狀態(tài)觀測器進(jìn)行狀態(tài)估計(jì)，狀態(tài)觀測器的設(shè)計(jì)可利用離散時間卡爾曼估計(jì)理論設(shè)計(jì)[8]：

3 仿真分析

為了驗(yàn)證本文提出的三維航跡控制算法的有效性，利用文獻(xiàn)[8]中的AUV運(yùn)動數(shù)學(xué)模型進(jìn)行仿真驗(yàn)證。設(shè)計(jì)了多折線式三維空間搜索任務(wù)，選擇如下航路點(diǎn)：

上述12個三維航路點(diǎn)中，A點(diǎn)為預(yù)定航行任務(wù)的起點(diǎn)，B~L為待完成的航路點(diǎn)。控制任務(wù)為使AUV沿12個航路點(diǎn)依次連接的航跡進(jìn)行運(yùn)動。試驗(yàn)初始條件為：初始位置（-820,90,35）m，初始方位角20°，航速3 kn。三維航跡控制效果見圖2~5。

圖2 三維航跡變化圖

圖3 水平面投影

圖4 垂直面投影

圖5 軌跡側(cè)向與法向偏差

由圖2可以看出，AUV在整個航行過程中都能夠跟蹤航路點(diǎn)，無論航跡側(cè)向偏差還是垂向偏差都能夠逐漸收斂，航跡貼合度良好。由圖5軌跡側(cè)向與法向偏差變化圖中可以看出，在搜索過程中，最大航跡側(cè)向偏差為22 m，發(fā)生在航路段DE向航路段EF切換過程中。一般而言，在航路段切換過程中，不對航跡側(cè)向跟蹤偏差進(jìn)行考核，若僅考核直線航路段的航跡穩(wěn)定精度，則以AB航路段為考核段，其航跡穩(wěn)定精度僅為0.066 m（RMS值）。由圖5還可以看出，在搜索過程中，最大法向偏差為7 m，也以直線航路段AB航路段作為考核段，則法向航路段法向跟蹤穩(wěn)定精度為0.1 074 m（RMS值），對于僅裝備深度傳感器和縱橫傾儀的AUV而言，側(cè)向軌跡偏差和法向軌跡偏差這兩項(xiàng)性能指標(biāo)均可滿足工程使用要求。

由圖6~9可知，在三維航跡跟蹤控制過程中，航向角的跟蹤控制非常平穩(wěn)。圖6、7中，航向大幅變化，艉垂直鰭能夠平穩(wěn)控制AUV改變并保持航向。航向修正過程中，超調(diào)量僅為2.23%，完全滿足一般AUV航向超調(diào)量10%的性能指標(biāo)要求。從圖8、9可知，航跡跟蹤控制過程的16次較大指令縱傾變化，實(shí)際縱傾角都能夠快速跟蹤指令縱傾角，且水平鰭出鰭收鰭動作平穩(wěn)，表明本文所設(shè)計(jì)的縱傾MPC控制算法具有較好的控制性能和控制品質(zhì)。結(jié)合圖2~5可知，本文提出的空間LOS制導(dǎo)算法能夠適用于AUV的三維航跡跟蹤控制，具有良好的控制性能。

圖6 航向變化曲線

圖7 艉垂直鰭角變化曲線

圖8 縱傾變化曲線

圖9 艉水平鰭角變化曲線

4 結(jié)論

本文針對AUV三維航跡跟蹤控制問題，利用空間視線導(dǎo)引法，將航跡橫向和垂向偏差的跟蹤控制轉(zhuǎn)換為航向跟蹤控制和縱傾跟蹤控制，提出了基于LOS的空間航跡跟蹤制導(dǎo)算法；基于垂直面線性運(yùn)動數(shù)學(xué)模型，設(shè)計(jì)了縱傾MPC控制算法，實(shí)現(xiàn)了AUV的空間軌跡跟蹤控制。仿真結(jié)果表明：（1）文中設(shè)計(jì)的空間LOS制導(dǎo)算法計(jì)算簡潔，有較高的工程實(shí)用價(jià)值；（2）文中提出的三維航跡跟蹤控制具有較好的航跡控制性能，能夠滿足工程使用要求；（3）文中設(shè)計(jì)的縱傾MPC控制算法，利用拉蓋爾網(wǎng)絡(luò)逼近方法和Hildreth二次規(guī)劃方法，具有良好的控制性能和控制品質(zhì)，有較大的工程實(shí)用價(jià)值。