于 林，孟慶浩，劉科顯，徐雪寒

(天津大學電氣自動化與信息工程學院，天津 300072)

1 引言

深海蘊藏著豐富的資源，深海探測是實現(xiàn)深海資源開發(fā)與利用的必要手段[1]。近些年，隨著技術(shù)的快速發(fā)展，機器人在深海探測領(lǐng)域展現(xiàn)出了誘人的潛力。深海機器人也稱深海潛水器，可以分為載人潛水器、無人潛水器和其它深?？碧皆O(shè)備(如深海滑翔機[2-3]等)，其中，無人潛水器按與母船之間是否有電纜連接分為有纜遙控潛水器(Remotely Operated Vehicle，ROV)[4]和無纜自主潛水器(Autonomous Underwater Vehicle，AUV)[5-6]。海底復(fù)雜的地形結(jié)構(gòu)、未知的突發(fā)狀況對AUV的機動性提出了更高的要求。現(xiàn)有的深海潛水器多為單剛體結(jié)構(gòu)，采用尾舵和姿態(tài)調(diào)整系統(tǒng)控制運動方向，轉(zhuǎn)彎半徑大，機動性較差。蛇形機器人[7-8]、仿鰻機器人[9]和仿生魚機器人[10]模仿生物行為方式，具有較高的靈活性，但一般用于水平方向運動，很少用于豎直方向探測。為實現(xiàn)深海縱向剖面探測，要求AUV具有靈活的縱向機動性，為此本文提出了一種新型多關(guān)節(jié)深海潛水器(Multi-Joint Autonomous Underwater Vehicle，MJ-AUV)。MJ-AUV由導(dǎo)流艙、導(dǎo)航控制艙、推進艙三部分組成，每兩個艙段之間用正交關(guān)節(jié)連接，可以通過轉(zhuǎn)動關(guān)節(jié)調(diào)整各艙段的相對姿態(tài)，改變MJ-AUV的水動力外形，進而調(diào)整機體姿態(tài)。MJ-AUV可以實現(xiàn)多種運動模式，比如水平蜿蜒運動，俯仰運動，螺旋上升、下潛運動等。

MJ-AUV屬于多剛體無根系統(tǒng)，運動學和動力學建模是研究其運動和控制問題的前提。Kelasidi等人[11]使用力和力矩平衡理論建立了水下蛇形機器人在二維水平面上的運動學、動力學模型；夏丹等人[12]提出了基于凱恩方法的仿生魚模型；唐敬閣等人[13]提出了一種水下滑翔蛇形機器人結(jié)構(gòu)，使用動量定理建立機器人滑翔運動模型，使用遞推牛頓-歐拉法建立機器人蛇形游動模型；除此之外，歐拉-拉格朗日法[14]和Schiehlen[15]等方法也常用于處理多剛體建模問題。

近年來，水下機器人的控制研究越來越多，文獻[16]針對多輸入多輸出AUV系統(tǒng)設(shè)計一種模糊自適應(yīng)比例積分微分控制器，用于航向和深度控制。Sakiyama等人[17]提出了一種考慮建模誤差的魯棒控制器，實現(xiàn)對AUV的運動和姿態(tài)控制。Cui等人[18]采用干擾觀測器對AUV擾動進行動態(tài)補償。為解決水下滑翔蛇形機器人[13]縱傾控制過程中輸入受限和擾動未知的問題，陳恩志等[19]將Nussbaum函數(shù)和雙曲正切函數(shù)相結(jié)合，解決系統(tǒng)輸入飽和問題，并通過非線性干擾觀測器實現(xiàn)對外界擾動的觀測和補償，仿真結(jié)果表明控制器具有較強的魯棒性。文獻[20-23]分別在自抗擾控制器的基礎(chǔ)上，做了不同的改進，例如結(jié)合滑模控制器、自搜索最優(yōu)算法等方法，提高AUV運動控制的快速性、精確性、抗干擾性等性能。

MJ-AUV是一個高度非線性系統(tǒng)，具有強耦合、滯后、擾動未知等特性，不易建立準確的數(shù)學模型。此外，在俯仰控制過程中，MJ-AUV屬于雙輸入單輸出(即兩個關(guān)節(jié)的角度變化作為系統(tǒng)輸入，機體俯仰角度作為輸出)的過驅(qū)動系統(tǒng)，這也給控制器的設(shè)計帶來難題。本文參考AUV在機體坐標系中的建模方法，考慮重力和浮力不平衡的情況，使用牛頓第二定律和力矩平衡原理建立MJ-AUV在縱垂面上的運動學和動力學模型。以關(guān)節(jié)角度作為被控模型輸入，符合MJ-AUV的實際操縱要求。為提高俯仰控制的精準性和抗干擾能力，本文提出一種將線性二次型最優(yōu)控制器(linear quadratic regulator，LQR)與線性自抗擾控制器(linear active disturbance rejection control，LADRC)[24]相結(jié)合的線性二次型最優(yōu)自抗擾控制器(linear quadratic regulator active disturbance rejection control，LQR-ADRC)，設(shè)計線性擴張狀態(tài)觀測器(linear extended state observer，LESO)估計系統(tǒng)總擾動，使用奇異值分解(singular value decomposition，SVD)法求解控制系數(shù)陣的偽逆矩陣，用于擾動補償和輸入分配，采用LQR優(yōu)化線性反饋增益，使其按照期望輸入輸出效果達到最優(yōu)。仿真結(jié)果表明LQR-ADRC控制器具有超調(diào)量小、抗干擾、精準度高等優(yōu)點。

2 多關(guān)節(jié)深海潛水器縱垂面動力學模型

2.1 多關(guān)節(jié)深海潛水器坐標系建立

圖1定義了MJ-AUV的慣性坐標系OEXEYEZE、機體坐標系(導(dǎo)航控制艙坐標系)OBXBYBZB、導(dǎo)流艙坐標系OHXHYHZH和推進艙坐標系OTXTYTZT。慣性系原點建立在海平面一點處，OEXE在海平面內(nèi)指向東方，OEZE垂直于海平面并指向天空，OEYE在海平面內(nèi)并垂直于OEXE，指定方向滿足右手螺旋定則；機體系原點固連在導(dǎo)航控制艙的形心位置，OBXB沿艙體的軸線方向向前，OBZB垂直于OBXB向上，OBYB的建立滿足右手螺旋定則；導(dǎo)流艙和推進艙的坐標系建立方式與機體坐標系建立方式類似。

2.2 多關(guān)節(jié)深海潛水器縱垂面運動學分析

圖1 多關(guān)節(jié)深海潛水器坐標系定義

各艙體坐標系原點相對于機體坐標系的位置BPi如式(1)所示

(1)

其中，li為對應(yīng)艙體長度的二分之一，i=H，B，T，下文同理；BRH和BRT分別為導(dǎo)流艙坐標系和推進艙坐標系到機體坐標系的轉(zhuǎn)換矩陣，分別表示為

(2)

式中，θ1為關(guān)節(jié)1(導(dǎo)流艙與導(dǎo)航控制艙之間的關(guān)節(jié))的俯仰角度，θ2為關(guān)節(jié)2(導(dǎo)航控制艙與推進艙之間的關(guān)節(jié))的俯仰角度，關(guān)節(jié)繞機體坐標系Y軸逆時針轉(zhuǎn)動方向為正。

各艙體坐標系在機體坐標系下表示的角速度Bωi與線速度Bvi分別為

(3)

(4)

機體系速度在慣性系中表示為

(5)

(6)

各艙段坐標系在機體系下表示的角加速度Bαi與線加速度Bai分別為

(7)

(8)

2.3 多關(guān)節(jié)深海潛水器動力學分析

2.3.1 動力學分析

圖2 多關(guān)節(jié)深海潛水器各艙段受力分析

根據(jù)牛頓第二定律，對MJ-AUV各艙段進行受力分析，為便于處理，所有的力將放在機體坐標系下進行計算，受力分析如式(9)所示

(9)

其中，mH、mB、mT分別表示各艙段的質(zhì)量，I為3×3的單位矩陣。將(9)式中三個式子相加，可以消除關(guān)節(jié)間的相互作用力，得

BFbH+BFdH+BFaH-BGH+FbB+FdB+

FaB-GB+BFbT+BFdT+BFaT-BGT+

BFth-mHIBaH-mBIaB-mTIBaT=0

(10)

根據(jù)力矩平衡原理，力矩分析如下式所示

(11)

其中

Mgi是各艙段由于重心與形心不重合而產(chǎn)生的對各自艙段的重力矩；HMB→H為控制艙對導(dǎo)流艙產(chǎn)生的力矩在導(dǎo)流艙坐標系中表示的結(jié)果，BMB→H、BMT→H、TMT→B的定義與HMB→H類似；Ji為各艙段的轉(zhuǎn)動慣量矩陣；由于MJ-AUV只在垂直面運動，因此αH=BαH，αT=BαT，HM1=BM1，TM2=BM2。

MJ-AUV通過改變關(guān)節(jié)角度來控制系統(tǒng)姿態(tài)，將(11)中三式相加，消除關(guān)節(jié)扭矩，得

MaH-MgH+MaB-MgB+MaT-MgT+

HMB→H-BMB→H+BMT→B-TMB→T-

JHαH-JBαB-JTαT=0

(12)

2.3.2 水動力

MJ-AUV工作在深海環(huán)境中，運動速度較為緩慢，關(guān)節(jié)不會頻繁擺動，可做如下假設(shè)：

假設(shè)1：水體流速相對于慣性坐標系為0；

假設(shè)2：各艙段水動力系數(shù)僅和自身形狀尺寸有關(guān)。

2.3.2.1 阻力

MJ-AUV外部掛載多個傳感器，并非規(guī)則的圓柱體，考慮壓差阻力和尾渦脫落效應(yīng)對其影響，阻力表達式如下

(13)

其中

2.3.2.2 附加質(zhì)量效應(yīng)

MJ-AUV在做變速運動時，會與周圍流體形成相對加速運動，引起附加質(zhì)量效應(yīng)，產(chǎn)生與加速度方向相反的作用效果，可用下式表示

BFai=-BRi(λmiai)

Mai=-λJiαi

(14)

式中

2.3.2.3 浮力

浮力等于MJ-AUV所排開水的重力，假設(shè)海水不同深度密度變化不大，近似為常值，當MJ-AUV完全浸入海水中時，滿足下式

(15)

式中，ρliquid為海水密度，Vi表示各艙段的體積，g表示重力加速度。

浮力作用在各艙段形心位置處，因此各艙段所受浮力對本艙段產(chǎn)生的力矩為0。

2.3.3 重力和重力矩

空氣中，MJ-AUV各艙段所受重力在機體坐標系中表示為

式中mi為各艙段質(zhì)量。

重心在形心正下方位置，因此各艙段會受到重力所產(chǎn)生的重力矩作用，如下式所示

(17)

式中，lc為各艙段重心與形心的距離。

2.3.4 推進器推力

推進器安裝在MJ-AUV的推進艙尾部，且為單向力，方向沿著推進艙軸向方向向前，因此推力對推進艙不產(chǎn)生力矩作用。推進器推力在機體坐標系中表示為

BFth=BRTFth

(18)

2.3.5 各艙段間相互作用力和力矩

雖然在受力分析中消掉了各艙段間的相互作用力，但在式(12)中，各艙段間相互作用力矩是無法消掉的，根據(jù)(9)式，推導(dǎo)各艙段間相互作用力如下

(19)

式(12)中的相互作用力矩為

(20)

2.4 狀態(tài)空間方程建立

將式(10)和(12)整理成狀態(tài)空間方程形式：

(21)

上述動力學模型為解析模型，可用于基于模型的現(xiàn)代控制器設(shè)計。該模型將關(guān)節(jié)角度作為系統(tǒng)的輸入量，符合實驗樣機設(shè)計要求。

3 多關(guān)節(jié)深海潛水器俯仰控制器設(shè)計

通過調(diào)節(jié)關(guān)節(jié)1、2的俯仰角度調(diào)整各艙段間的相對姿態(tài)，改變MJ-AUV水動力外形，調(diào)整機體的俯仰角度，此時，系統(tǒng)屬于過驅(qū)動系統(tǒng)。將LQR和LADRC相結(jié)合，并合理分配系統(tǒng)輸入，提出一種LQR-ADRC俯仰控制方案，控制系統(tǒng)結(jié)構(gòu)如圖3所示。

圖3 LQR-ADRC控制器結(jié)構(gòu)圖

3.1 LESO觀測器設(shè)計

根據(jù)MJ-AUV動力學模型，其俯仰姿態(tài)模型可整理為

(22)

式中，fac1、fac2、bac1、bac2、bac3為關(guān)于X的非線性函數(shù)，wd為系統(tǒng)的外部干擾。

式(22)表明MJ-AUV是一個強耦合的高度非線性系統(tǒng)，很難將模型解耦，因此本文參考文獻[21]的思想，將MJ-AUV所受的內(nèi)力、外力定義為系統(tǒng)總擾動，取b1≈bac1，b2≈bac2，其中b1、b2為常值。式(22)可改寫為

+(bac2-b2)θ2+bac3θ1θ2+b1θ1+b2θ2

=f+b1θ1+b2θ2

(23)

式中f=fac1θ+fac2q+wd+(bac1-b1)θ1+(bac2-b2)θ2+bac3θ1θ2，f為系統(tǒng)總擾動，b1、b2為關(guān)節(jié)1和關(guān)節(jié)2 的俯仰角控制系數(shù)。

(24)

(25)

(26)

(27)

3.2 LQR-ADRC觀測器設(shè)計

引入誤差e=x1d-x1，則式(27)可改寫為

(28)

其中x1d為目標俯仰角值。

定義z1=e，z2=x2，則整理式(28)為

(29)

控制器設(shè)計主要包括兩部分。第一部分為擾動補償輸入uf，第二部分采用LQR控制器設(shè)計u0，即u=u0+uf。

擾動分配補償控制器為

(30)

將(30)帶入(29)得

(31)

u0=-Kz，

(32)

K值有四個參數(shù)，本文通過LQR來調(diào)整。LQR可以降低調(diào)整參數(shù)難度，以最小化性能指標來獲取適合控制目標的最優(yōu)控制律。選取LQR性能指標函數(shù)為

(33)

式中Q為半正定矩陣，R為正定矩陣，前一項為系統(tǒng)狀態(tài)誤差的懲罰函數(shù)，后一項為系統(tǒng)輸入狀態(tài)的懲罰函數(shù)。該性能指標函數(shù)對應(yīng)的Riccati方程組為

(34)

式中，P為(34)的解，為對稱正定矩陣。此時線性反饋增益矩陣為

K=R-1BTP

(35)

3.3 穩(wěn)定性分析

MJ-AUV的系統(tǒng)模型為

(36)

(37)

將(30)、(32)代入(37)得

(38)

將(35)代入(38)得

(39)

4 仿真分析

4.1 模型搭建

4.2 控制參數(shù)

根據(jù)MJ-AUV的數(shù)學模型，選取系統(tǒng)控制系數(shù)b1=0.1，b2=0.1?？紤]系統(tǒng)俯仰控制性能要求，選取權(quán)重矩陣為Q=diag(600，300)，R=diag(0.1，0.1)，diag表示對角矩陣。利用MATLAB的lqr()函數(shù)得到反饋增益矩陣為

參考文獻[26]，ω0的選取影響觀測器的精度，ω0越大，觀測精度越高，但過大容易引起觀測器不穩(wěn)定，通過觀察仿真結(jié)果對其進行調(diào)整，最終選取ω0=80。

4.3 仿真結(jié)果

圖4-圖7為無干擾情況下MJ-AUV的俯仰角控制效果。圖4表明系統(tǒng)遇到階躍信號能夠快速做出調(diào)整，且低超調(diào)，控制效果良好，圖4 (b)為0-15s過程的放大圖，出現(xiàn)這種現(xiàn)象的原因是MJ-AUV的凈浮力為正，各艙段間的合力矩不平衡，初始關(guān)節(jié)角度為0時，機體不為平衡狀態(tài)，因此系統(tǒng)通過調(diào)整關(guān)節(jié)角度控制機體俯仰角度為0。圖5為系統(tǒng)在無干擾輸入情況下觀測器對總擾動的估計結(jié)果，證明LESO對擾動觀測的有效性。圖7為參考信號與實際俯仰角度之間的誤差值，系統(tǒng)穩(wěn)定后差值為10-8rad，說明控制系統(tǒng)具有較高的準確性。

圖5 無干擾情況下總擾動估計

圖6 無干擾情況下MJ-AUV輸入

圖7 無干擾情況下MJ-AUV俯仰控制誤差

圖8 有輸入干擾情況下MJ-AUV俯仰控制

圖9 有輸入干擾情況下總擾動估計

圖10 有輸入干擾情況下MJ-AUV輸入

圖8-圖11為有輸入干擾情況下的俯仰控制仿真結(jié)果，干擾加在關(guān)節(jié)1的輸入位置，在70s時，輸入0.1rad的階躍干擾信號，90s時輸入-0.2rad的階躍干擾信號。仿真結(jié)果表明系統(tǒng)輸入在受到干擾情況下能夠迅速調(diào)整，使系統(tǒng)穩(wěn)定下來，表現(xiàn)良好的抗干擾特性。

圖11 有輸入干擾情況下MJ-AUV俯仰控制誤差

5 結(jié)論

本文提出一種具有正交關(guān)節(jié)結(jié)構(gòu)的新型多關(guān)節(jié)深海潛水器(MJ-AUV)，考慮重力和浮力不平衡的情況，根據(jù)牛頓第二定律和力矩平衡原理建立潛水器縱垂面運動學、動力學模型，提出一種將LQR與LADRC相結(jié)合的LQR-ADRC控制器，在SIMULINK中進行MJ-AUV俯仰姿態(tài)仿真與分析，得出以下結(jié)論：

1)所建立的模型為解析模型，可用于對MJ-AUV運動姿態(tài)的研究和基于模型的控制器的設(shè)計。

2)采用線性狀態(tài)觀測器觀測系統(tǒng)總擾動，采用SVD法求解控制系數(shù)矩陣的偽逆矩陣，實現(xiàn)對總擾動的合理補償與分配，有效解決了系統(tǒng)過驅(qū)動的問題。

3)采用LQR實現(xiàn)線性反饋控制增益按照期望輸入輸出效果達到最優(yōu)，使參數(shù)整定工作更加高效。

4)仿真結(jié)果表明LQR-ADRC控制器具有低超調(diào)、抗干擾、控制精確等優(yōu)勢，同時該控制器具有計算量小、不依賴系統(tǒng)精確模型的特點，具有很好的工程應(yīng)用價值。

在未來工作中，計劃將該控制算法應(yīng)用于MJ-AUV實際控制器系統(tǒng)中，并完成實驗驗證。