負浮力四傾轉(zhuǎn)推進器水下機器人姿態(tài)跟蹤控制

王 濤，吳 超，葛 彤

(上海交通大學(xué) 船舶海洋與建筑工程學(xué)院，上海 200240)

0 引 言

在最近的幾十年中，大量的海洋探索工具被開發(fā)出來，擴展了人類的研究范圍，獲得了關(guān)于海洋的更多知識，使得深海的礦藏能夠得到有效利用[1–2]。其中，AUV（Autonomous underwater vehicle）是一種廣泛使用的潛水器，它有高速和活動范圍大的優(yōu)勢[3–4]。大部分AUV 在水中是中性浮力的，為了維持中性浮力，AUV 需要安裝很多浮力材料，增加了AUV 的體積及AUV 在水中航行時的阻力。

本文針對一種負浮力四傾轉(zhuǎn)推進器AUV（negative-buoyancy quad tilt-rotor autonomous underwater vehicle,NQTAUV），進行了抗干擾姿態(tài)跟蹤控制設(shè)計。NQTAUV 沒有安裝浮力材料，因此在水中有剩余重量。NQTAUV 質(zhì)量為 m ，浸沒于水中受到浮力為 B，且滿足 B ＜mg， g為重力加速度。有懸停、傾轉(zhuǎn)、水平巡航3 種工作模式。在懸停模式下，依靠垂直向下的推進器產(chǎn)生升力克服剩余重量；在傾轉(zhuǎn)模式下，推進器由垂直向下旋轉(zhuǎn)至朝向后方，推動AUV不斷水平加速。NQTAUV 安裝有水翼，隨著水平分量推力的增加，AUV 速度加快，機翼產(chǎn)生的流體升力克服水中剩余重量[5–6]，實現(xiàn)水平巡航。

NQTAUV 有獨特的機身傾轉(zhuǎn)懸停（body-tilt-hover,BTH）功能，即在機身實現(xiàn)大幅度俯仰角機動的時候，能夠懸停在水中一個位置點，而不產(chǎn)生水平的分力，不會引起水平的位移。對于普通AUV 來說，一般是在艇體尾部有一個推進器，部分在艇體上還有垂向和橫向的推進器，實現(xiàn)垂向和水平機動。不過，這些推進器一般固定在艇體上，只能提供固定方向的力或力矩，不能提供矢量推力，所以不能做機身傾轉(zhuǎn)懸停。然而，NQTAUV 可以借助自身的矢量推力進行機身傾轉(zhuǎn)懸停[7–8]。BTH 是一項有用的功能。比如，AUV 前部一般安裝有攝像機，用以提供視頻信息。通常來講，為了更大的視角，攝像機安裝在一個二自由度云臺上。然而，當(dāng)云臺損壞后，相機的視角被限制，此時，可以采用BTH 功能，操作手可以重新獲得原來的視角。不過，當(dāng)工作在BTH 模式時，因為舵機會隨著俯仰角而轉(zhuǎn)動，引起轉(zhuǎn)動慣量的變化，同時產(chǎn)生一定的干擾力矩，且不能計算或測量得到干擾量幅值。因此，需要更魯棒的姿態(tài)控制器，實現(xiàn)姿態(tài)的跟蹤。此外，在BTH 模式下，水動力矩也會隨著機身的旋轉(zhuǎn)運動而改變。因此，研究干擾的估計和補償策略，以及在干擾下的姿態(tài)跟蹤，是符合實際應(yīng)用需要的。

為了進行干擾下的控制，學(xué)者們進行了大量研究。Li[9]設(shè)計了四自由度非線性滑動狀態(tài)觀測器，建立了ROV 的洋流模型和簡化的臍帶干擾力模型，基于簡化的索力模型表征電纜的干擾力，并實時估計纜的干擾力，提高了觀測精度，減少了抖振。Yuan[10]設(shè)計了一種非線性自抗擾控制方案控制氣動肌肉執(zhí)行器，采用連續(xù)離散擴展?fàn)顟B(tài)觀測器估計總擾動，然后使用非線性復(fù)合控制器，在氣動平臺上進行了實驗，驗證了整個系統(tǒng)的可控性。Cui[11]對路徑跟蹤問題，設(shè)計了自適應(yīng)神經(jīng)網(wǎng)絡(luò)控制器，考慮了外部干擾、控制輸入非線性和模型不確定性等因素。Liu[12]用非線性干擾觀測器估算了作用在飛行器上的風(fēng)效應(yīng)，然后將風(fēng)信息與控制器結(jié)合，設(shè)計了一種抗干擾導(dǎo)航算法，實現(xiàn)風(fēng)干擾下的實時路徑跟蹤，并理論分析了復(fù)合控制器的全局漸近穩(wěn)定性，進行了軟件在環(huán)的仿真。對小型飛機進行的飛行仿真試驗，驗證了其性能?？梢钥闯?，在以往的研究中，對于干擾觀測的研究取得了大量進展，部分控制器的穩(wěn)定性得到了證明。不過，這些算法在實際應(yīng)用中存在調(diào)參數(shù)量多的問題，不利于控制器實際的部署和調(diào)整。

姿態(tài)跟蹤控制是NQTAUV 的重要功能，一般作為路徑跟蹤等更高一級控制的內(nèi)環(huán)，它的性能決定了上層控制的精度[13]。干擾力矩對姿態(tài)控制精度影響很大，為了補償干擾對姿態(tài)跟蹤控制的影響，設(shè)計調(diào)試簡單，易于部署的抗干擾控制器，本文提出了基于干擾觀測器的姿態(tài)跟蹤控制器設(shè)計框架，并進行了姿態(tài)跟蹤實驗。

首先，基于修正的羅德里格斯參數(shù)（modified Rodrigues parameters, MRPs）姿態(tài)表示法建立NQTAUV 的運動學(xué)和動力學(xué)模型，并進一步推導(dǎo)出姿態(tài)跟蹤誤差模型。其次，設(shè)計干擾觀測器，估計作用在姿態(tài)上的干擾；設(shè)計姿態(tài)跟蹤控制系統(tǒng)，并證明控制系統(tǒng)的穩(wěn)定性。最后，進行實時姿態(tài)跟蹤實驗，驗證了所提方法的控制性能。

1 NQTAUV 數(shù)學(xué)模型

1.1 運動學(xué)和動力學(xué)

在姿態(tài)跟蹤控制中用到了3 個坐標(biāo)系：地球坐標(biāo)系 Fe、 本體坐標(biāo)系 Fb和目標(biāo)坐標(biāo)系 Fd。坐標(biāo)系示意圖如圖1 所示。

圖 1 坐標(biāo)系Fig. 1 Coordinates

BTH 模式示意圖如圖2 所示。

表示剛體姿態(tài)的方法有多種，其中歐拉角是三參數(shù)姿態(tài)表示法，因為簡單、直觀，被廣泛應(yīng)用。然而，歐拉角在 ±90°有奇異；MPRs 也是三參數(shù)姿態(tài)表示法，不過在 ± 180°才有奇異點，能夠表示更大范圍的機動[14]。因此，本文選取MRPs 表示姿態(tài)。

圖 2 機身傾轉(zhuǎn)懸停模式Fig. 2 Body-tilt-hover mode

NQTAUV 的運動學(xué)和動力學(xué)方程為：

G(σ)有性質(zhì)：

1.2 姿態(tài)跟蹤誤差模型

在姿態(tài)跟蹤問題中，定義目標(biāo)姿態(tài) [σd,ωd,ω˙d]，因此，相對姿態(tài)就是：

相對角速度為：

因此，姿態(tài)跟蹤誤差模型為：

用反饋線性化消去系統(tǒng)動態(tài)中的非線性項：

其中， ν是要設(shè)計的控制量，那么公式（12）變成：

其中： d 為干擾，且 d 來源于很多方面。首先，推進器的旋轉(zhuǎn)會對俯仰產(chǎn)生干擾力矩；其次，當(dāng)NQTAUV在水中水平巡航的時候，水翼也會產(chǎn)生額外的力和力矩。在沒有速度傳感器的情況下，水動力矩不能被準(zhǔn)確計算。在此，將所有干擾當(dāng)做一個復(fù)合的干擾，估計這個復(fù)合干擾 d 。記d′=J-1d，方程（14）變成：

2 控制器和干擾觀測器設(shè)計

基于干擾觀測器的控制框架如圖3 所示。

圖 3 控制框架Fig. 3 Control scheme

2.1 干擾觀測器設(shè)計

作用在NQTAUV 上的干擾不能被直接測量，因此需要引入干擾觀測器估計這個干擾。假設(shè)干擾是常值，記的估計，估計誤差記為?；诜匠蹋?5），設(shè)計干擾觀測器為：

同時，因為 ，故得到：

故得到：

2.2 控制器設(shè)計

選取李雅普諾夫函數(shù)：

其中 k1＞0 ， V1的導(dǎo)數(shù)為：

2.3 穩(wěn)定性分析

用李雅普諾夫穩(wěn)定性理論分析整個系統(tǒng)的穩(wěn)定性。選取李雅普諾夫函數(shù)：

V2的導(dǎo)數(shù)為：

又選取李雅普諾夫函數(shù) 求導(dǎo)得到：

選取 k2,k3， 滿足，則。故由李雅普諾夫穩(wěn)定性理論，系統(tǒng)是全局漸進穩(wěn)定的。

3 實驗結(jié)果和分析

3.1 測試平臺

實驗采用浸入水中的三自由度測試平臺進行。NQTAUV 本體通過一個球鉸連接在固定的基座上，使得本體能夠?qū)崿F(xiàn)俯仰和橫搖各 ±30°的活動范圍，首向角可實現(xiàn) 360°的活動范圍。傾轉(zhuǎn)推進器可以進行360°的旋轉(zhuǎn)，最大旋轉(zhuǎn)速度6.9 rad/s，最大提供1 5 N的推力。姿態(tài)傳感器采用MPU9250，提供三軸的角速度、加速度和磁場強度，經(jīng)過卡爾曼濾波融合后得到姿態(tài)角和角速度?？刂齐娐钒宀捎肧TM32 F401RE，具有512 kB 內(nèi)存，84 MHz頻率?？刂破骺刂祁l率為100 Hz。NQTAUV 的機械參數(shù)見表1。

表 1 NQTAUV 機械參數(shù)Tab. 1 Mechanical parameters of the NQTAUV

3.2 姿態(tài)跟蹤實驗

控制器和觀測器參數(shù)選取為k1=3.58,k2=0.67,k3=10。對于BTH 模式，典型姿態(tài)跟蹤測試信號是俯仰軸的正弦信號，因此，定義目標(biāo)姿態(tài)正弦信號為：

在 x 和 y 軸施加約0.008 N.m 的常值干擾力矩。為了驗證所提干擾觀測器和姿態(tài)跟蹤控制器的有效性，在實驗的 0 ～50 s 內(nèi)，將干擾觀測器關(guān)閉；在 5 0 ～100 s內(nèi)，將干擾觀測器激活。實驗結(jié)果如圖4～圖6 所示。其中，圖4 顯示姿態(tài)跟蹤效果，圖5 顯示控制力矩，圖6顯示干擾估計值。

從圖4 可以看出，當(dāng)干擾觀測器關(guān)閉時，能跟蹤正弦跟蹤信號，但是會產(chǎn)生大約0.02 的姿態(tài)偏移量，且有大約1 s 的相位滯后。同時可以看出，姿態(tài)波動幅值很大，說明受干擾影響很大。當(dāng)干擾觀測器激活后，姿態(tài)偏移量為0，相位滯后也被消除，姿態(tài)波動非常小，說明干擾被補償?shù)簦瑢嶋H姿態(tài)能夠精確跟蹤目標(biāo)姿態(tài)。

從圖5 可以看出，在激活干擾觀測器后，控制輸出有一個整體的增加，說明對常值干擾力矩進行了補償。

圖 4 正弦信號姿態(tài)跟蹤結(jié)果Fig. 4 Sinusoidal attitude tracking performance

圖 5 控制輸入Fig. 5 Control input

圖 6 干擾觀測值Fig. 6 Disturbance estimation

從圖6 可以看出，當(dāng)干擾觀測器激活后，干擾估計值呈現(xiàn)波動，與正弦信號相位相吻合。在正弦姿態(tài)跟蹤時，NQTAUV 受到周期性的水動力，其周期和相位與本體運動相同，由于目標(biāo)姿態(tài)信號周期為20 s，因此這是緩慢變化的干擾。這也說明干擾觀測器不但可以估計常值干擾，還可以估計緩慢變化的干擾。

綜上，干擾觀測器能夠準(zhǔn)確估計常值干擾，對緩慢變化的干擾也有準(zhǔn)確的估計。所提的姿態(tài)跟蹤控制器能夠準(zhǔn)確跟蹤目標(biāo)姿態(tài)。

4 結(jié) 語

本文針對負浮力四傾轉(zhuǎn)推進器水下機器人，設(shè)計了抗干擾控制框架。基于修正的羅德里格斯參數(shù)，導(dǎo)出了負浮力四傾轉(zhuǎn)推進器水下機器人姿態(tài)跟蹤的數(shù)學(xué)模型。然后設(shè)計了干擾觀測器來估計干擾，并基于此干擾觀測器設(shè)計了姿態(tài)跟蹤控制器?？刂瓶蚣艿姆€(wěn)定性得到了證明。最后，通過實時姿態(tài)跟蹤實驗，比較干擾觀測器激活和關(guān)閉2 種不同情況下的姿態(tài)跟蹤表現(xiàn)，驗證了控制框架的性能。所提出的基于干擾觀測器的控制框架，抗干擾能力強，參數(shù)數(shù)量少，調(diào)節(jié)簡單，易于實際部署。