基于四元數(shù)的欠驅(qū)動(dòng)AUV的鎮(zhèn)定控制設(shè)計(jì)

吳琪, 李曄

(哈爾濱工程大學(xué) 船舶工程學(xué)院，黑龍江 哈爾濱 150001)

1 四元數(shù)

四元數(shù)向量寫法為

Q=t+xi+yj+zk

以原點(diǎn)為旋轉(zhuǎn)中心，旋轉(zhuǎn)的軸分別是α、β、γ(α2+β2+γ2=1)，用右手系的坐標(biāo)定義，望向向量α、β、γ的前進(jìn)方向反時(shí)針旋轉(zhuǎn)θ，用四元數(shù)表示為[4]

在大地坐標(biāo)系和聯(lián)體坐標(biāo)系中，當(dāng)給定物體的姿態(tài)用φ、θ、φ表示，并且分別以x、y、z旋轉(zhuǎn)，則實(shí)現(xiàn)從四元數(shù)到歐拉角的轉(zhuǎn)換公式為[5]

θ=arcsin[-2(q1q3-q0q2)]

式中：φ、θ、φ分別為橫滾角、縱傾角和偏航角，q0、q1、q2、q3分別為四元數(shù)的各個(gè)分量。實(shí)現(xiàn)上述坐標(biāo)轉(zhuǎn)換的方向余弦矩陣為

Abg=Ax(φ)Ay(θ)Az(φ)=

實(shí)現(xiàn)相應(yīng)坐標(biāo)轉(zhuǎn)換的四元數(shù)矩陣為

2 水下機(jī)器人空間運(yùn)動(dòng)方程

水下機(jī)器人6自由度空間運(yùn)動(dòng)方程可以寫為[1]

(1)

欠驅(qū)動(dòng)水下機(jī)器人一般采用槳—舵—翼形式，因此外部作用力軸向推進(jìn)力τ1、橫滾力矩τ4、縱傾力矩τ5和偏航力矩τ6，忽略二階非線性水動(dòng)力細(xì)數(shù)的影響，將方程(1)展開為式(2):

w(cosφsinθcosφ+sinφsinφ)

w(sinφsinθcosφ-cosφsinφ)

(Xu/m11)u]

(2)

為了獲得合適的同類型的運(yùn)動(dòng)模型，選擇單位四元數(shù)來參數(shù)化模型(2)，單位四元數(shù)定義為：在單位向量k附近旋轉(zhuǎn)角度φ，則4個(gè)參數(shù)有如下形式：

ε4=cos(φ/2)

假設(shè)|φ|<π，且ε4>0，單位四元數(shù)滿足：

(3)

方程(2)可以寫為

(Xu/m11)u]

(Yv/m22)v]

(Zw/m33)w]

(4)

定義向量η=[η1η2]T，其中η1=[xyz]T是慣性坐標(biāo)系下的位置向量，η2=[ε1ε2ε3]T。定義τ=[τ1τ2τ3τ4τ5τ6]T分別代表縱向、橫向、垂向、橫滾、縱傾、首搖方向的控制輸入。

3 AUV的鎮(zhèn)定設(shè)計(jì)

這一部分只考慮AUV的鎮(zhèn)定控制輸入只有4個(gè)方向：軸向推進(jìn)力τ1、橫滾力矩τ4、縱傾力矩τ5和偏航力矩τ6。根據(jù)Brockett定理的第3個(gè)條件[6]，可知六自由度空間運(yùn)動(dòng)方程不存在任何光滑時(shí)不變反饋控制使系統(tǒng)在平衡點(diǎn)附近漸進(jìn)穩(wěn)定。因此本文提出一個(gè)連續(xù)時(shí)變反饋控制律。

考慮虛擬設(shè)計(jì)控制輸入：

ud(η,ε,v,w,t)=-kxx+

ρa(bǔ)(η,ε,v,w)sin(t/ε)

(5)

pd(η,ε,v,w,t)=-kε1ε1

(6)

qd(η,ε,v,w,t)=-kε2ε2-2(kz+

(7)

rd(η,ε,v,w,t)=-kε3ε3+2(ky+

(8)

式中：kx、kε1、kε2、kε3、k、d、ε均大于零。ρa(bǔ)(η,ε,v,w)是一個(gè)任意可微的同類型的標(biāo)準(zhǔn)函數(shù)[3]，考慮到其形式為

(λx,λ2y,λ2z,λε1,λε2,λε3,λ2v,λ2w,t)

進(jìn)一步定義控制輸入：

τ1(η,v,t)=-k1m11(u-ud(η,v,w,t))

(9)

τ4(η,v,t)=-k4m44(p-pd(η,v,w,t))

(10)

τ5(η,v,t)=-k5m55(q-qd(η,v,w,t))

(11)

τ6(η,v,t)=-k6m66(r-rd(η,v,w,t))

(12)

則存在ε0>0，對(duì)于任意的ε∈(0,ε0)，當(dāng)k1、k2、k3、k4足夠大且全為正數(shù)，則控制律(9)～(12)可使系統(tǒng)(4)局部指數(shù)鎮(zhèn)?？紤]如下的形式：

(λx,λ2y,λ2z,λε1,λε2,λε3,λ2v,λ2w,t)

證明：AUV的空間運(yùn)動(dòng)方程模型(1)在控制輸入τ2=0,τ3=0的情況下，可寫為

(13)

式中：

(14)

由于ε4沒有包含在f(η,v,t)中，且ε4可由式(11)惟一定義，因此可以將系統(tǒng)(14)中的第7個(gè)方程去除。如果簡(jiǎn)化后的方程局部漸進(jìn)穩(wěn)定，則原系統(tǒng)方程(13)也漸近穩(wěn)定。

(15)

方程組(5)～(8)定義了(15)的控制輸入。鑒于其是周期時(shí)變輸入，因此產(chǎn)生的系統(tǒng)結(jié)果也是周期時(shí)變的，可寫為

(16)

取用自治的平均系統(tǒng)近似的代替系統(tǒng)(16)，方程(16)的平均系統(tǒng)為

式中：g0(ξ)是其平均值。將控制輸入(13)～(16)帶入到(15)，系統(tǒng)(15)的平均系統(tǒng)可寫為

(17)

方程(17)可以寫成如下的級(jí)聯(lián)系統(tǒng)：

(18)

(19)

根據(jù)線性理論知識(shí)容易得到式(19)是全局穩(wěn)定的。當(dāng)系統(tǒng)(19)全局穩(wěn)定時(shí)，其狀態(tài)向量全部趨近于零。且由(18)可以看出，關(guān)聯(lián)項(xiàng)是高階小的形式，因此只要設(shè)計(jì)證明系統(tǒng)(18)的名義系統(tǒng)穩(wěn)定即可。根據(jù)Hurwitz穩(wěn)定性定理，當(dāng)名義系統(tǒng)矩陣的所有特征值具有負(fù)實(shí)部，則系統(tǒng)可以實(shí)現(xiàn)穩(wěn)定。因此根據(jù)文獻(xiàn)[4]命題，存在k1、k2、k3、k4足夠大且全為正數(shù)，合理地選擇參數(shù)k、d，能夠使得系(18)漸近穩(wěn)定。

4 鎮(zhèn)定仿真

AUV的基本參數(shù)為[7]：m11=215，m22=265，m33=265，m44=40，m55=80，m66=80，d11=70，d22=100，d33=100，d44=30，d55=50，d66=50，且AUV是中性浮力，不計(jì)入重力和浮力(假設(shè)重力和浮力平衡，且重心浮心重合)。

采用式(9)～(12)的控制律進(jìn)行控制輸入，從上一節(jié)可知τ4=0，滿足漸近收斂。相關(guān)控制參數(shù)如下：k1=3，k5=3，k6=3，kx=2，kε2=2，kε3=2，k=0.3，d=0.3，ε=2.5，名義準(zhǔn)則取為

(30)

初始位置為

仿真結(jié)果如圖1～6所示，圖1表示平移速度u、v、w變化情況，圖2表示轉(zhuǎn)動(dòng)角速度p、q、r的變化情況，圖3表示位移x、y、z的變化情況，圖4表示轉(zhuǎn)換變量ε1、ε2、ε3曲線，圖5反映機(jī)器人的實(shí)際鎮(zhèn)定運(yùn)動(dòng)軌跡，圖6代表名義標(biāo)準(zhǔn)ρα變化曲線。

圖1 平移速度u、 v、 wFig.1 Velocity of vehicle u, v, w

圖2 旋轉(zhuǎn)速度p、 q、 rFig.2 Rotational speed of vehicle p, q, r

圖3 位移x、y 、zFig.3 Displacement of vehicle x, y, z

圖4 轉(zhuǎn)換變量ε1,ε2、ε3 Fig.4 Transformation variables ε1,ε2,ε3

圖5 實(shí)際軌跡圖Fig.5 Actual path of vehicle

圖6 名義標(biāo)準(zhǔn)ρα響應(yīng)Fig.6 Response of the dilation ρα

圖1中速度在初始時(shí)刻具有較大的變化率和超調(diào)量，說明初始時(shí)刻有較大的控制輸入以及輸入變化，即輸入的力和力矩是突然加入的，引起欠驅(qū)動(dòng)AUV有較大的速度變化，在仿真中這是允許的。位移圖3在穩(wěn)定點(diǎn)附近存在震蕩，因此實(shí)際的運(yùn)動(dòng)軌跡在穩(wěn)定點(diǎn)附近會(huì)比較密集，仿真結(jié)果與此相符合。因此在實(shí)際的AUV操控中，就需要考慮物理量的變化快慢，不能急劇地改變AUV的運(yùn)動(dòng)參數(shù)，使得AUV發(fā)生抖動(dòng)，影響實(shí)際的軌跡跟蹤。

由圖1可以看出，縱向速度相比于橫向和垂向速度收斂較慢，圖3中縱向位移相比于橫向和垂向位移收斂也較慢，因?yàn)榭v向存在控制輸入，而控制輸入是各個(gè)變量的綜合函數(shù)，其收斂要滯后于單個(gè)變量，因此縱向方向的變量收斂會(huì)比較慢。

轉(zhuǎn)換變量ε1、ε2、ε3代表了橫滾、縱傾、首搖的角速度，由于初始控制輸入的較大加入，其變化震蕩也比較明顯，與平移速度相一致需要考慮實(shí)際情況的變化快慢。名義標(biāo)準(zhǔn)ρα是各個(gè)變量的偶次方之和，因此必大于零，隨著各個(gè)變量的收斂到零，ρα必趨于零。初始時(shí)刻其變化較快，是因?yàn)楦魑锢砹渴諗康幂^快，與之相對(duì)應(yīng)ρα也較快減小。

從仿真結(jié)果曲線可以看出，采用四元數(shù)進(jìn)行變量轉(zhuǎn)換以后，設(shè)計(jì)的控制輸入能夠?qū)崿F(xiàn)水下機(jī)器人的點(diǎn)鎮(zhèn)定控制，欠驅(qū)動(dòng)AUV的狀態(tài)變量速度和位移均可以收斂到零。仿真結(jié)果證明在3個(gè)控制輸入的情況下，設(shè)計(jì)的控制器能夠?qū)崿F(xiàn)三維鎮(zhèn)定。

5 結(jié)束語

本文討論了欠驅(qū)動(dòng)AUV的三維鎮(zhèn)定控制問題。通過對(duì)欠驅(qū)動(dòng)AUV的六自由度空間運(yùn)動(dòng)方程和軌跡方程的簡(jiǎn)化，采用四元數(shù)方法進(jìn)行了模型變換，提出一種具有4個(gè)控制輸入的連續(xù)時(shí)變鎮(zhèn)定控制律，證明提出的控制律的收斂性，并且進(jìn)行鎮(zhèn)定仿真實(shí)驗(yàn)演示。如果在橫滾方向的水動(dòng)力恢復(fù)力足夠大的情況下，在3個(gè)控制輸入的情況下AUV可以實(shí)現(xiàn)指數(shù)鎮(zhèn)定。仿真結(jié)果表明控制律的有效性，能夠在任意初始條件下實(shí)現(xiàn)鎮(zhèn)定控制。

參考文獻(xiàn):

[1]畢鳳陽. 欠驅(qū)動(dòng)自主水下航行器的非線性魯棒控制策略研究[D]. 哈爾濱：哈爾濱工業(yè)大學(xué)，2010: 15-18.

BI Fengyang. Research on nonlinear robust control strategies of underactuated autonomous undewater vehicle[D]. Harbin： Harbin Industrial of Technology University, 2010: 15-18.

[2]張銘鈞，宋煒胥，褚振忠.自主式水下機(jī)器人模糊定性建模方法研究[J]. 哈爾濱工程大學(xué)學(xué)報(bào)，2013， 34(1)： 116-122.

ZHANG Mingjun, SONG Weixu, CHU Zhenzhong. Research on the method of fuzzy qualitative modeling for AUV[J]. Journal of Harbin Engineering University, 2013， 34(1)： 116-122.

[3]PETTERSEN K Y, EGELAND O. Robust control of an underactuated surface vessel with thruster dynamics[C]//Proc 1997 Amer Control Conf. Albuquerque， NM, USA， 1997: 3411-3416.

[4]倪振松,廖啟征,魏世民,等. 空間6R機(jī)器人位置反解的對(duì)偶四元數(shù)法[J]. 機(jī)械工程學(xué)報(bào), 2009， 45(11) : 25-29.

NI Zhensong, LIAO Qizheng, WEI Shimin, et al. Dual four element method for inverse kinematics analysis of spatial 6R manipulator[J]. Journal of Mechanical Engineering, 2009, 45(11) : 25-29.

[5]劉忠,梁曉庚,曹秉剛等. 基于四元數(shù)的空間全方位算法研究[J]. 西安交通大學(xué)學(xué)報(bào), 2006， 40(5) : 618-620.

LIU Zhong, LIANG Xiaogeng, CAO Binggang, et al. Space omnidircetional algorithm study based on quaternion[J]. Journal of Xi′an Jiaotong University, 2006, 40(5) : 618-620.

[6]賈鶴鳴, 程相勤, 張利軍,等. 基于自適應(yīng)Backstepping的AUV的三維航跡跟蹤控制[J]. 控制與決策, 2012， 5(27): 652-657.

JIA Heming, CHENG Xiangqin, ZHANG Lijun, et al. Three-dimensional path tracking control for underactuated AUV based on adaptive backstepping[J]. Control and Decision, 2012， 5(27): 652-657.