生物啟發(fā)AUV三維軌跡跟蹤控制算法

朱大奇，張光磊，李蓉

(上海海事大學(xué) 水下機(jī)器人與智能系統(tǒng)實(shí)驗(yàn)室，上海 201306)

自治水下機(jī)器人(AUV)作為海洋開發(fā)的一項(xiàng)重要工具，近年來取得了長(zhǎng)足的發(fā)展。由于自治水下機(jī)器人欠驅(qū)動(dòng)、強(qiáng)耦合、非線性等一系列特性，再加上水下作業(yè)環(huán)境比陸地上更為復(fù)雜，其軌跡跟蹤控制研究仍然十分具有挑戰(zhàn)性[1-2]。

反步控制(backstepping control)[3]是一種設(shè)計(jì)簡(jiǎn)單而又滿足系統(tǒng)穩(wěn)定的控制方法，它首先應(yīng)用于地面移動(dòng)機(jī)器人軌跡跟蹤控制[4-5]，近年來，反步方法被進(jìn)一步應(yīng)用到AUV的軌跡跟蹤控制[6]中，通常情況下是將反步控制與其他方法集成。如L.Lionel 等[7]將反步控制與Lyapunov函數(shù)結(jié)合用于AUV非線性路徑跟蹤控制；高劍等[8]利用級(jí)聯(lián)系統(tǒng)理論與反步控制結(jié)合來研究欠驅(qū)動(dòng)AUV水平面軌跡跟蹤策略；廖煜雷等[9]將反步滑?？刂茟?yīng)用到欠驅(qū)動(dòng)水面無人艇航跡控制中，并給出了仿真計(jì)算結(jié)果。

反步控制方法原理雖然簡(jiǎn)單，但它的跟蹤誤差大，當(dāng)期望軌跡存在拐點(diǎn)時(shí)，會(huì)出現(xiàn)速度跳變，而實(shí)現(xiàn)該跳需要超大數(shù)值的驅(qū)動(dòng)力(力矩)支撐，在現(xiàn)實(shí)的AUV中顯然無法或者很難實(shí)現(xiàn)。針對(duì)這一問題，Yang等[10-11]將反步控制方法與生物啟發(fā)神經(jīng)動(dòng)力學(xué)模型結(jié)合，解決了地面移動(dòng)機(jī)器人跟蹤控制中的速度跳變問題，取得了較滿意的控制效果。而在AUV反步跟蹤控制方面，相關(guān)研究并不多。此外，AUV在作業(yè)時(shí)受到能源約束、海流影響、欠驅(qū)動(dòng)特性等方面的限制，推進(jìn)系統(tǒng)輸出動(dòng)力有限，滿足跟蹤時(shí)速度跳變的可能性極小，所以解決該問題的實(shí)際意義明顯。

本文針對(duì)AUV反步控制過程中的速度跳變問題，同時(shí)考慮到海流因素影響，將生物啟發(fā)神經(jīng)動(dòng)力學(xué)模型應(yīng)用于AUV的水下軌跡跟蹤控制中[12]，通過由生物啟發(fā)神經(jīng)動(dòng)力學(xué)模型構(gòu)造的中間虛擬變量，并結(jié)合Lyapunov函數(shù)設(shè)計(jì)了AUV的軌跡跟蹤控制算法。

1 海流環(huán)境下AUV的三維運(yùn)動(dòng)學(xué)模型

實(shí)驗(yàn)AUV模型為上海海事大學(xué)水下機(jī)器人與智能系統(tǒng)實(shí)驗(yàn)室的“海箏二號(hào)”水下機(jī)器人，如圖1所示。

(a)“海箏二號(hào)”AUV (b)推進(jìn)器布置圖1 AUV推進(jìn)器布置Fig.1 Thruster arrangements of AUV

該AUV有4個(gè)推進(jìn)器，其中水平面2個(gè)推進(jìn)器對(duì)稱安裝于機(jī)器人尾部，控制AUV的進(jìn)退(surge)和回轉(zhuǎn)(yaw)運(yùn)動(dòng)；垂直面2個(gè)推進(jìn)器對(duì)稱安裝于機(jī)器人重心前后，控制AUV的潛浮運(yùn)動(dòng)。取海面上某一固定點(diǎn)為慣性坐標(biāo)系的原點(diǎn)O，OX軸和OY軸在水平面內(nèi)，且互相垂直，OZ軸垂直于XOY面指向地心；取AUV的重心E為載體坐標(biāo)系坐標(biāo)原點(diǎn)，EX0軸為AUV前進(jìn)方向，EY0為橫移方向，EZ0為潛浮方向，如圖2所示。

圖2 AUV載體坐標(biāo)系和慣性坐標(biāo)系Fig.2 Body-fixed frame and inertial frame of AUV

1.1 三維空間內(nèi)AUV的運(yùn)動(dòng)學(xué)方程

AUV的三維運(yùn)動(dòng)學(xué)模型為[14]

(1)

式中：

J(η)=

(2)

對(duì)式(2)求時(shí)間導(dǎo)數(shù)并化簡(jiǎn),得：

(3)

本文在反步方法的基礎(chǔ)上，結(jié)合生物啟發(fā)神經(jīng)動(dòng)力學(xué)模型和Lyapunov函數(shù)設(shè)計(jì)AUV的三維軌跡跟蹤控制律。

1.2 海流環(huán)境下的運(yùn)動(dòng)學(xué)方程

(4)

結(jié)合式(1)和(4)得到海流環(huán)境下AUV的運(yùn)動(dòng)學(xué)方程為

2 軌跡跟蹤控制律的設(shè)計(jì)

2.1 控制器設(shè)計(jì)問題描述

圖3 系統(tǒng)工作圖Fig.3 The system working drawing

2.2 生物啟發(fā)神經(jīng)動(dòng)力學(xué)模型

本文中的生物啟發(fā)神經(jīng)動(dòng)力學(xué)模型的作用有:1)通過位姿與誤差信號(hào)，有控制器產(chǎn)生虛擬離散軌跡；2)構(gòu)造虛擬中間誤差，即使速度有跳變，系統(tǒng)輸出結(jié)果也能較平滑。

生物啟發(fā)神經(jīng)動(dòng)力學(xué)模型是生物膜電壓模型，整個(gè)膜電壓的狀態(tài)[15]可表示為

(Ek+Vm)gk

式中：參數(shù)Ep、ENa、Ek分別是負(fù)漏極電流、鈉離子、鉀離子和在細(xì)胞膜中相應(yīng)的能量，gp、gNa、gk分別是負(fù)極、鈉、鉀的導(dǎo)納，Cm是膜電勢(shì)。若令Cm=1，ζ=Ep+Vm，A=gp，B=ENa+Ep，D=Ek-Ep，S+=gNa，S-=gk。可以得生物啟發(fā)方程為

(D+ζ)S-(t)

(5)

式中：ζ是膜電勢(shì)。A、B、D分別代表神經(jīng)元活動(dòng)的負(fù)衰減率、上限和下限。S+、S-對(duì)應(yīng)激勵(lì)與抑制輸入，表示外界刺激。在AUV軌跡跟蹤控制中，則代表誤差大小與方向。

式(5)中，神經(jīng)元活動(dòng)ζ被限制，僅在[-D,B]內(nèi)變化，系統(tǒng)穩(wěn)定。存在激勵(lì)輸入S+(S+≥0)時(shí)，ζ增大并自動(dòng)獲取控制項(xiàng)B-ζ。如果(B-ζ)S+使ζ正向變大，當(dāng)ζ超過B，(B-ζ)<0，這時(shí)(B-ζ)S+為負(fù)，并使ζ趨于B。顯然，ζ始終小于B，而抑制性輸入迫使神經(jīng)元活動(dòng)大于-D。該模型用于軌跡跟蹤則可解決速度跳變。

本文利用生物啟發(fā)神經(jīng)動(dòng)力學(xué)模型構(gòu)造的虛擬速度為

(6)

式中：ex=xd-x，ey=yd-y，ez=zd-z，eψ=ψd-ψ，eθ=θd-θ，f(ei)=max(ei,0)，g(ei)=max(-ei,0)，i=x，y，z，ψ，θ。

自由主義向每個(gè)人承諾分得更大的蛋糕，從而使無產(chǎn)階級(jí)與資產(chǎn)階級(jí)、有信仰者與無神論者、土著與移民、歐洲人與亞洲人和解。如果存在一個(gè)不斷增長(zhǎng)的蛋糕，上述的和解是可能達(dá)到的。而且這個(gè)蛋糕很可能還會(huì)繼續(xù)增長(zhǎng)。然而，經(jīng)濟(jì)增長(zhǎng)可能無法解決目前因顛覆性技術(shù)而產(chǎn)生的社會(huì)問題，因?yàn)檫@種增長(zhǎng)越來越依賴于更具顛覆性的技術(shù)的發(fā)明。

2.3 生物啟發(fā)控制律設(shè)計(jì)及穩(wěn)定性分析

設(shè)計(jì)Lyapunov函數(shù):

(7)

顯然，當(dāng)且僅當(dāng)ex=ey=ez=eψ=eθ=0時(shí)，Γ=0；否則，Γ>0恒成立。對(duì)式(7)求導(dǎo)，有

(8)

將式(3)代入式(8)，得

(9)

設(shè)計(jì)運(yùn)動(dòng)學(xué)控制律：

(10)

式中：f1、f2為待求的未知函數(shù)。將式(10)代入式(9)，得

f2cosψsinθ)+ey(f1sinψcosθ+

f2sinψsinθ)+ez(-f1sinθ+f2cosθ)

設(shè)

(11)

由式(11)解得

(12)

將(12)式代入(11)式可得該方法下的AUV控制律為

(13)

應(yīng)用生物啟發(fā)神經(jīng)動(dòng)力學(xué)模型來產(chǎn)生與位移誤差有關(guān)的輔助信號(hào)Vsx、Vsy、Vsz、Vsψ、Vsθ，如式(6)所示，并用它們代替控制律中的誤差ex、ey、ez、eψ、eθ，所以得到新的控制律：

3 仿真實(shí)驗(yàn)

在MATLAB編程環(huán)境下，分別采用本文所提的控制算法和傳統(tǒng)反步控制算法，進(jìn)行AUV三維空間折線的跟蹤控制，仿真結(jié)果的對(duì)比可證明本文所提控制算法的優(yōu)越性。

圖4表明，在考慮了海流因素時(shí)，與傳統(tǒng)反步控制方法相比，基于生物啟發(fā)的軌跡跟蹤控制器在跟蹤過程中誤差小，誤差變化范圍較小，跟蹤誤差能夠快速趨于零，精度較高。

圖4 2種控制律的跟蹤軌跡對(duì)比Fig.4 Comparisons between trajectories of two tracking control laws

(a)前向速度的對(duì)比

(b)潛浮速度的對(duì)比

(c) 轉(zhuǎn)艏角速度的對(duì)比

(d)縱傾角速度的對(duì)比 圖5 2種控制律的跟蹤速度對(duì)比Fig.5 Tracking velocity comparison between two control laws

通過對(duì)圖5中凸點(diǎn)處的速度大小比較，可以看出加入生物啟發(fā)的控制律的速度輸出更加連續(xù)、平滑；生物啟發(fā)軌跡跟蹤控制在拐點(diǎn)處，速度變化明顯小于傳統(tǒng)反步跟蹤控制，以圖5(a)的前向速度為例，在跟蹤開始的698 s拐點(diǎn)處，傳統(tǒng)反步跟蹤達(dá)到1.267 m/s，而生物啟發(fā)跟蹤的控制速度僅為1.016 m/s，其他拐點(diǎn)處同樣可以看出生物啟發(fā)跟蹤的控制速度跳變遠(yuǎn)小于傳統(tǒng)反步跟蹤控制，針對(duì)欠驅(qū)動(dòng)水下機(jī)器人系統(tǒng)來說，短時(shí)間內(nèi)較大的速度變化，意味著需要產(chǎn)生較大加速度，這時(shí)機(jī)器人需要提供足夠的推力，但實(shí)際的水下機(jī)器人其推力有限，常常無法滿足這一要求。生物啟發(fā)模型的加入較好克服了控制控制的速度跳變，從而較好地實(shí)現(xiàn)水下機(jī)器人軌跡跟蹤。

4 結(jié)束語(yǔ)

通過對(duì)于三維折線軌跡進(jìn)行仿真研究，分別比較了傳統(tǒng)反步控制與生物啟發(fā)方法在海流環(huán)境下的AUV跟蹤控制效果，可以看到生物啟發(fā)方法在跟蹤效果上要優(yōu)于傳統(tǒng)反步方法，同時(shí)很好解決了反步方法的速度跳變問題，顯示了很好的控制性能，同時(shí)在本文基礎(chǔ)上可以進(jìn)一步考慮將運(yùn)動(dòng)學(xué)控制擴(kuò)展到動(dòng)力學(xué)控制。

參考文獻(xiàn):

[1]賈鶴鳴，程相勤，張利軍，等.基于離散滑模預(yù)測(cè)的欠驅(qū)動(dòng)AUV三維航跡跟蹤控制[J].控制與決策， 2011, 26(10): 1452-1458.

JIA Heming, CHENG Xiangqin, ZHANG Lijun, et al. Three-dimensional path tracking control for an underactuated AUV based on discrete-time sliding mode prediction[J]. Control and Decision, 2011, 26(10): 1452-1458.

[2]王芳，萬(wàn)磊，李曄，等.欠驅(qū)動(dòng)AUV的運(yùn)動(dòng)控制技術(shù)綜述[J].中國(guó)造船， 2010, 51(2): 227- 241.

WANG Fang, WAN Lei, LI Ye, et al. A survey on development of motion control for underactuated AUV[J]. Shipbuilding of China, 2010, 51(2): 227-241.

[3]TSAI P S, WANG L S, CHANG F R. Systematic backstepping design for b-spline trajectory tracking control of the mobile robot in hierarchical model[C]//IEEE International Conference on Networking, Sensing and Control. Taipei, China， 2004： 713-718.

[4]WALLACE M B, MAX S D. EDWIN K. Depth control of remotely operated underwater vehicles using an adaptive fuzzy sliding mode controller[J]. Robotics and Autonomous Systems, 2008, 56(8): 670-677.

[5]BAGHERI A, MOGHADDAM J J. Simulation and tracking control based on neural-network strategy and sliding-mode control for underwater remotely operated vehicle[J]. Neurocomputing, 2009, 72(8): 1934-1950.

[6]JON E R, ASGEIR J S, KRISTIN Y P. Model-based output feedback control of slender-body underactuated AUVs: theory and experiments[J]. IEEE Transactions on Control Systems Technology, 2008, 16(5): 930-946.

[7]LIONEL L, BRUNO J. Robust nonlinear path-following control of AUV[J]. IEEE Journal of Oceanic Engineering, 2008, 33(2): 89-102.

[8]高劍，徐德民，嚴(yán)衛(wèi)生.基于級(jí)聯(lián)方法的欠驅(qū)動(dòng)AUV全局K指數(shù)3維直線跟蹤控制[J]. 控制與決策， 2012, 27(9): 1281-1287.

GAO Jian, XU Demin, YAN Weisheng. Global K-exponential straight-line tracking control of an underactuated AUV in 3 dimensions using a cascaded approach[J]. Control and Decision, 2012, 27(9): 1281-1287.

[9]廖煜雷,龐永杰，馬偉佳，等.噴水推進(jìn)無人水面艇直線航跡系統(tǒng)的反步自適應(yīng)滑?？刂芠J]. 高技術(shù)通訊， 2013, 23(1)： 79-84.

LIAO Yulei，PANG Yongjie，MA Weijia, et al. Backstepping adaptive sliding-mode control for the straight-line trajectory system of water-jet-propelled unmanned surface vessels[J]. Chinese High Technology Letters, 2013, 23(1)： 79- 84.

[10]唐旭東，龐永杰，李曄，等. 基于混沌過程神經(jīng)元的水下機(jī)器人運(yùn)動(dòng)控制方法[J]. 控制與決策，2010, 25(2): 213-217.

TANG Xudong, PANG Yongjie, LI Ye, et al. Chaotic process neuron control for AUVs[J]. Control and Decision, 2010, 25(2):213-217.

[11]YANG S X , ZHU A. A bioinspired neurodynamics based approach to tracking control of mobile robots[J]. IEEE Transactions on Industrial Electronics, 2012, 59(8): 3211-3220.

[12]馬嶺，崔維成. NTSM控制的AUV路徑跟蹤控制研究[J]. 中國(guó)造船， 2006, 47(4): 77-82.

MA Ling, CUI Weicheng. Path following control study of an autonomous underwater vehicle controlled by non-singular terminal sliding mode[J]. Shipbuilding of China, 2006, 47(4): 77-82.

[13]SANTHAKUMAR M, ASOKAN T. Investigations on the hybrid tracking control of an underactuated autonomous underwater robot[J]. Advanced Robotics, 2010, 24(11): 1529-1556.

[14]邊宇樞，高志慧，贠超. 6自由度水下機(jī)器人動(dòng)力學(xué)分析與運(yùn)動(dòng)控制[J].機(jī)械工程學(xué)報(bào)， 2007, 43(7): 87- 92.

BIAN Yushu, GAO Zhihui, YUN Chao. Dynamic analysis and motion control of 6-DOF underwater robot[J]. Chinese Journal of Mechanical Engineering, 2007, 43(7): 87- 92.

[15]SIMON X Y, LUO C. A neural network approach to complete coverage path planning[J]. IEEE Transactions on Systems, Man, and Cybernetics, Part B: Cybernetics, 2004, 34(1): 718-725.