自治水下機器人三維軌跡跟蹤控制研究

自治水下機器人三維軌跡跟蹤控制研究

自主水下機器人（auv）作為海洋開發(fā)的重要工具，近年來取得了很大進展。由于自治水下機器人欠驅(qū)動、強耦合、非線性等一系列特性,再加上水下作業(yè)環(huán)境比陸地上更為復(fù)雜,其軌跡跟蹤控制研究仍然十分具有挑戰(zhàn)性。反步控制(backsteppingcontrol)是一種設(shè)計簡單而又滿足系統(tǒng)穩(wěn)定的控制方法,它首先應(yīng)用于地面移動機器人軌跡跟蹤控制,近年來,反步方法被進一步應(yīng)用到AUV的軌跡跟蹤控制中,通常情況下是將反步控制與其他方法集成。如L.Lionel等將反步控制與Lyapunov函數(shù)結(jié)合用于AUV非線性路徑跟蹤控制;高劍等利用級聯(lián)系統(tǒng)理論與反步控制結(jié)合來研究欠驅(qū)動AUV水平面軌跡跟蹤策略;廖煜雷等將反步滑模控制應(yīng)用到欠驅(qū)動水面無人艇航跡控制中,并給出了仿真計算結(jié)果。反步控制方法原理雖然簡單,但它的跟蹤誤差大,當(dāng)期望軌跡存在拐點時,會出現(xiàn)速度跳變,而實現(xiàn)該跳需要超大數(shù)值的驅(qū)動力(力矩)支撐,在現(xiàn)實的AUV中顯然無法或者很難實現(xiàn)。針對這一問題,Yang等將反步控制方法與生物啟發(fā)神經(jīng)動力學(xué)模型結(jié)合,解決了地面移動機器人跟蹤控制中的速度跳變問題,取得了較滿意的控制效果。而在AUV反步跟蹤控制方面,相關(guān)研究并不多。此外,AUV在作業(yè)時受到能源約束、海流影響、欠驅(qū)動特性等方面的限制,推進系統(tǒng)輸出動力有限,滿足跟蹤時速度跳變的可能性極小,所以解決該問題的實際意義明顯。本文針對AUV反步控制過程中的速度跳變問題,同時考慮到海流因素影響,將生物啟發(fā)神經(jīng)動力學(xué)模型應(yīng)用于AUV的水下軌跡跟蹤控制中,通過由生物啟發(fā)神經(jīng)動力學(xué)模型構(gòu)造的中間虛擬變量,并結(jié)合Lyapunov函數(shù)設(shè)計了AUV的軌跡跟蹤控制算法。1auv潛浮運動方程實驗AUV模型為上海海事大學(xué)水下機器人與智能系統(tǒng)實驗室的“海箏二號”水下機器人,如圖1所示。該AUV有4個推進器,其中水平面2個推進器對稱安裝于機器人尾部,控制AUV的進退(surge)和回轉(zhuǎn)(yaw)運動;垂直面2個推進器對稱安裝于機器人重心前后,控制AUV的潛浮運動。取海面上某一固定點為慣性坐標系的原點O,OX軸和OY軸在水平面內(nèi),且互相垂直,OZ軸垂直于XOY面指向地心;取AUV的重心E為載體坐標系坐標原點,EX0軸為AUV前進方向,EY0為橫移方向,EZ0為潛浮方向,如圖2所示。圖2中,φ、θ、ψ分別對應(yīng)AUV中慣性坐標系下的橫傾角、縱傾角、艏向角(以逆時針方向為正);u、v、w分別為AUV在載體坐標系中的3個速度分量;p、q、r分別為AUV在載體坐標系中的3個角速度分量。在慣性坐標系下,AUV的狀態(tài)用六自由度表示的向量η=[xyzφψθ]T表示,x、y、z為慣性坐標系下位置,運動狀態(tài)用V=[uvwprq]T表示。1.1原位姿狀態(tài)變換從圖1可以看出,AUV無側(cè)向推進器,在三維空間中只能前后、轉(zhuǎn)艏、潛浮和縱傾,不能側(cè)移和橫搖。設(shè)φ=0,p=v=0,慣性坐標系下位姿狀態(tài)變?yōu)棣?[xyzψθ]T;設(shè)AUV期望位姿為ηd=[xdydzdydqd]T,運動狀態(tài)V=[uvwrq]T,參考速度為Vd=[udvdwdrdqd]T。AUV的三維運動學(xué)模型為式中:定義慣性坐標系下AUV的位姿誤差為eη=[exeyezeψeθ]T,則AUV的誤差方程為對式(2)求時間導(dǎo)數(shù)并化簡,得:本文在反步方法的基礎(chǔ)上,結(jié)合生物啟發(fā)神經(jīng)動力學(xué)模型和Lyapunov函數(shù)設(shè)計AUV的三維軌跡跟蹤控制律。1.2海流環(huán)境下auv的運動學(xué)建模設(shè)慣性坐標系下海流速度為[UxUyUz]T,則海流速度由慣性坐標系到載體坐標系的轉(zhuǎn)換矩陣為設(shè)海流在載體坐標系上的速度為[ucvcwc]T,則AUV相對于海流的速度為[ucivciwci]T=[u-ucv-vcw-wc]T,即結(jié)合式(1)和(4)得到海流環(huán)境下AUV的運動學(xué)方程為2軌道跟蹤法的設(shè)計2.1auv軌跡跟蹤控制系統(tǒng)令[xdydzdψdθd]T為AUV的期望位姿矢量,即為AUV需要跟蹤的參考路徑。由于不考慮水動力項對AUV的影響,并且AUV沒有橫向推進器配置,所以當(dāng)設(shè)定AUV的初始橫移速度v=0時,AUV的橫移速度始終保持為零。如式(2),期望軌跡與實際AUV之間的位姿誤差向量為eη=[exeyezeψeθ]T,軌跡跟蹤控制即設(shè)計適當(dāng)?shù)目刂屏?使得在一定的時間內(nèi),AUV的期望軌跡與實際AUV之間的誤差能逐漸趨向于零,即[exeyezeψeθ]T→0。AUV軌跡跟蹤控制系統(tǒng)如圖3所示。AUV實際位置與期望軌跡的誤差[exeyezeψeθ]T,經(jīng)生物啟發(fā)神經(jīng)動力學(xué)模型,得到虛擬的中間速度變量Vs=[VsxVsyVszVsψVsθ]T,經(jīng)反步控制得到軌跡跟蹤的運動學(xué)控制律[uwrq]T,乘以轉(zhuǎn)換矩陣J并積分,得到AUV下一時刻的位姿矢量。2.2生物啟發(fā)方程本文中的生物啟發(fā)神經(jīng)動力學(xué)模型的作用有:1)通過位姿與誤差信號,有控制器產(chǎn)生虛擬離散軌跡;2)構(gòu)造虛擬中間誤差,即使速度有跳變,系統(tǒng)輸出結(jié)果也能較平滑。生物啟發(fā)神經(jīng)動力學(xué)模型是生物膜電壓模型,整個膜電壓的狀態(tài)可表示為式中:參數(shù)Ep、ENa、Ek分別是負漏極電流、鈉離子、鉀離子和在細胞膜中相應(yīng)的能量,gp、gNa、gk分別是負極、鈉、鉀的導(dǎo)納,Cm是膜電勢。若令Cm=1,ζ=Ep+Vm,A=gp,B=ENa+Ep,D=Ek-Ep,S+=gNa,S-=gk?？梢缘蒙飭l(fā)方程為式中:ζ是膜電勢。A、B、D分別代表神經(jīng)元活動的負衰減率、上限和下限。S+、S-對應(yīng)激勵與抑制輸入,表示外界刺激。在AUV軌跡跟蹤控制中,則代表誤差大小與方向。式(5)中,神經(jīng)元活動ζ被限制,僅在[-D,B]內(nèi)變化,系統(tǒng)穩(wěn)定。存在激勵輸入S+(S+≥0)時,ζ增大并自動獲取控制項B-ζ。如果(B-ζ)S+使ζ正向變大,當(dāng)ζ超過B,(B-ζ)<0,這時(B-ζ)S+為負,并使ζ趨于B。顯然,ζ始終小于B,而抑制性輸入迫使神經(jīng)元活動大于-D。該模型用于軌跡跟蹤則可解決速度跳變。本文利用生物啟發(fā)神經(jīng)動力學(xué)模型構(gòu)造的虛擬速度為式中:2.3auv控制律設(shè)計顯然,當(dāng)且僅當(dāng)ex=ey=ez=eψ=eθ=0時,Γ=0;否則,Γ>0恒成立。對式(7)求導(dǎo),有將式(3)代入式(8),得設(shè)計運動學(xué)控制律:式中:f1、f2為待求的未知函數(shù)。將式(10)代入式(9),得設(shè)由式(11)解得將(12)式代入(11)式可得該方法下的AUV控制律為將式(13)代入Lyapunov函數(shù)的導(dǎo)函數(shù),得到由Lyapunov定理可知,系統(tǒng)全局穩(wěn)定。將AUV相對于海流的速度[ucivciwci]T代入式(13),得海流環(huán)境下AUV的運動學(xué)控制律:應(yīng)用生物啟發(fā)神經(jīng)動力學(xué)模型來產(chǎn)生與位移誤差有關(guān)的輔助信號Vsx、Vsy、Vsz、Vsψ、Vsθ,如式(6)所示,并用它們代替控制律中的誤差ex、ey、ez、eψ、eθ,所以得到新的控制律:3海流速度控制參數(shù)在MATLAB編程環(huán)境下,分別采用本文所提的控制算法和傳統(tǒng)反步控制算法,進行AUV三維空間折線的跟蹤控制,仿真結(jié)果的對比可證明本文所提控制算法的優(yōu)越性。給定三維折線期望路徑的參考輸入為ud=0.5,wd=0.2,rd=0,qd=0。在全局坐標下的初始位姿為xd=3,yd=0,zd=0,ψd=0,θd=0;同時AUV實際軌跡的初始位置為x=2,y=0,z=0,ψ=0,θ=0;控制參數(shù)A=1,B=1,D=2;采樣時間間隔是0.01s;海流速度[UxUyUz]T=[0.20.20.1]T。該三維期望路徑的起點是(3,0,0),在(6,3,3)和(9,3,3)各有一處拐點,終點是(12,6,6)。圖4表明,在考慮了海流因素時,與傳統(tǒng)反步控制方法相比,基于生物啟發(fā)的軌跡跟蹤控制器在跟蹤過程中誤差小,誤差變化范圍較小,跟蹤誤差能夠快速趨于零,精度較高。通過對圖5中凸點處的速度大小比較,可以看出加入生物啟發(fā)的控制律的速度輸出更加連續(xù)、平滑;生物啟發(fā)軌跡跟蹤控制在拐點處,速度變化明顯小于傳統(tǒng)反步跟蹤控制,以圖5(a)的前向速度為例,在跟蹤開始的698s拐點處,傳統(tǒng)反步跟蹤達到1.267m/s,而生物啟發(fā)跟蹤的控制速度僅為1.016m/s,其他拐點處同樣可以看出生物啟發(fā)跟蹤的控制速度跳變遠小于傳統(tǒng)反步跟蹤控制,針對欠驅(qū)動水下機器人系統(tǒng)來說,短時間內(nèi)較大的速度變化,意味著需要產(chǎn)生較大加速度,這時機器人需要提供足夠的推力,但實際的水下機器人其推力有限,常常無法滿足這一要求。生物啟發(fā)模型的加入較好克服