多關(guān)節(jié)深海潛水器縱垂面建模與俯仰控制

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

(天津大學(xué)電氣自動(dòng)化與信息工程學(xué)院，天津 300072)

1 引言

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

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

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

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

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

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

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

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

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

各艙體坐標(biāo)系原點(diǎn)相對(duì)于機(jī)體坐標(biāo)系的位置BPi如式(1)所示

(1)

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

(2)

式中，θ1為關(guān)節(jié)1(導(dǎo)流艙與導(dǎo)航控制艙之間的關(guān)節(jié))的俯仰角度，θ2為關(guān)節(jié)2(導(dǎo)航控制艙與推進(jìn)艙之間的關(guān)節(jié))的俯仰角度，關(guān)節(jié)繞機(jī)體坐標(biāo)系Y軸逆時(shí)針轉(zhuǎn)動(dòng)方向?yàn)檎?/p>

各艙體坐標(biāo)系在機(jī)體坐標(biāo)系下表示的角速度Bωi與線(xiàn)速度Bvi分別為

(3)

(4)

機(jī)體系速度在慣性系中表示為

(5)

(6)

各艙段坐標(biāo)系在機(jī)體系下表示的角加速度Bαi與線(xiàn)加速度Bai分別為

(7)

(8)

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

2.3.1 動(dòng)力學(xué)分析

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

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

(9)

其中，mH、mB、mT分別表示各艙段的質(zhì)量，I為3×3的單位矩陣。將(9)式中三個(gè)式子相加，可以消除關(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)生的對(duì)各自艙段的重力矩；HMB→H為控制艙對(duì)導(dǎo)流艙產(chǎn)生的力矩在導(dǎo)流艙坐標(biāo)系中表示的結(jié)果，BMB→H、BMT→H、TMT→B的定義與HMB→H類(lèi)似；Ji為各艙段的轉(zhuǎn)動(dòng)慣量矩陣；由于MJ-AUV只在垂直面運(yùn)動(dòng)，因此αH=BαH，αT=BαT，HM1=BM1，TM2=BM2。

MJ-AUV通過(guò)改變關(guān)節(jié)角度來(lái)控制系統(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 水動(dòng)力

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

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

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

2.3.2.1 阻力

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

(13)

其中

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

MJ-AUV在做變速運(yùn)動(dòng)時(shí)，會(huì)與周?chē)黧w形成相對(duì)加速運(yùn)動(dòng)，引起附加質(zhì)量效應(yīng)，產(chǎn)生與加速度方向相反的作用效果，可用下式表示

BFai=-BRi(λmiai)

Mai=-λJiαi

(14)

式中

2.3.2.3 浮力

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

(15)

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

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

2.3.3 重力和重力矩

空氣中，MJ-AUV各艙段所受重力在機(jī)體坐標(biāo)系中表示為

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

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

(17)

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

2.3.4 推進(jìn)器推力

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

BFth=BRTFth

(18)

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

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

(19)

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

(20)

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

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

(21)

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

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

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

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

3.1 LESO觀測(cè)器設(shè)計(jì)

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

(22)

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

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

+(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)總擾動(dòng)，b1、b2為關(guān)節(jié)1和關(guān)節(jié)2 的俯仰角控制系數(shù)。

(24)

(25)

(26)

(27)

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

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

(28)

其中x1d為目標(biāo)俯仰角值。

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

(29)

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

擾動(dòng)分配補(bǔ)償控制器為

(30)

將(30)帶入(29)得

(31)

u0=-Kz，

(32)

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

(33)

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

(34)

式中，P為(34)的解，為對(duì)稱(chēng)正定矩陣。此時(shí)線(xiàn)性反饋增益矩陣為

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ù)學(xué)模型，選取系統(tǒng)控制系數(shù)b1=0.1，b2=0.1?？紤]系統(tǒng)俯仰控制性能要求，選取權(quán)重矩陣為Q=diag(600，300)，R=diag(0.1，0.1)，diag表示對(duì)角矩陣。利用MATLAB的lqr()函數(shù)得到反饋增益矩陣為

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

4.3 仿真結(jié)果

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

圖5 無(wú)干擾情況下總擾動(dòng)估計(jì)

圖6 無(wú)干擾情況下MJ-AUV輸入

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

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

圖9 有輸入干擾情況下總擾動(dòng)估計(jì)

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

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

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

5 結(jié)論

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

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

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

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

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

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