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

吳琪, 李曄

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

1 四元數(shù)

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

Q=t+xi+yj+zk

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

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

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

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

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

實現(xiàn)相應坐標轉(zhuǎn)換的四元數(shù)矩陣為

2 水下機器人空間運動方程

水下機器人6自由度空間運動方程可以寫為[1]

(1)

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

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

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

(Xu/m11)u]

(2)

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

ε4=cos(φ/2)

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

(3)

方程(2)可以寫為

(Xu/m11)u]

(Yv/m22)v]

(Zw/m33)w]

(4)

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

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

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

考慮虛擬設計控制輸入：

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

ρa(η,ε,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(η,ε,v,w)是一個任意可微的同類型的標準函數(shù)[3]，考慮到其形式為

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

進一步定義控制輸入：

τ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，對于任意的ε∈(0,ε0)，當k1、k2、k3、k4足夠大且全為正數(shù)，則控制律(9)～(12)可使系統(tǒng)(4)局部指數(shù)鎮(zhèn)。考慮如下的形式：

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

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

(13)

式中：

(14)

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

(15)

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

(16)

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

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

(17)

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

(18)

(19)

根據(jù)線性理論知識容易得到式(19)是全局穩(wěn)定的。當系統(tǒng)(19)全局穩(wěn)定時，其狀態(tài)向量全部趨近于零。且由(18)可以看出，關(guān)聯(lián)項是高階小的形式，因此只要設計證明系統(tǒng)(18)的名義系統(tǒng)穩(wěn)定即可。根據(jù)Hurwitz穩(wěn)定性定理，當名義系統(tǒng)矩陣的所有特征值具有負實部，則系統(tǒng)可以實現(xiàn)穩(wěn)定。因此根據(jù)文獻[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是中性浮力，不計入重力和浮力(假設重力和浮力平衡，且重心浮心重合)。

采用式(9)～(12)的控制律進行控制輸入，從上一節(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，名義準則取為

(30)

初始位置為

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

圖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 實際軌跡圖Fig.5 Actual path of vehicle

圖6 名義標準ρα響應Fig.6 Response of the dilation ρα

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

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

轉(zhuǎn)換變量ε1、ε2、ε3代表了橫滾、縱傾、首搖的角速度，由于初始控制輸入的較大加入，其變化震蕩也比較明顯，與平移速度相一致需要考慮實際情況的變化快慢。名義標準ρα是各個變量的偶次方之和，因此必大于零，隨著各個變量的收斂到零，ρα必趨于零。初始時刻其變化較快，是因為各物理量收斂得較快，與之相對應ρα也較快減小。

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

5 結(jié)束語

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

參考文獻:

[1]畢鳳陽. 欠驅(qū)動自主水下航行器的非線性魯棒控制策略研究[D]. 哈爾濱：哈爾濱工業(yè)大學，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]. 哈爾濱工程大學學報，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機器人位置反解的對偶四元數(shù)法[J]. 機械工程學報, 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]. 西安交通大學學報, 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]賈鶴鳴, 程相勤, 張利軍,等. 基于自適應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.