基于反步滑?？刂频淖灾嗡聶C(jī)器人三維控制

基于反步滑?？刂频淖灾嗡聶C(jī)器人三維控制

1編制控制相關(guān)理論自主水下機(jī)器人在海洋開發(fā)和開發(fā)中發(fā)揮著非常重要的作用。由于單個auv的工作能力有限，往往無法執(zhí)行一些復(fù)雜的任務(wù)。多auv的合作可以提高整個系統(tǒng)的安全性和魯棒性。多auv的合作控制已成為研究領(lǐng)域的熱點。編隊控制是多AUV協(xié)作的一個典型性問題,也是研究其他協(xié)作問題的基礎(chǔ),在科學(xué)考察、深海探測、沉船打撈、旅游探險等領(lǐng)域有廣泛的應(yīng)用,對于編隊控制問題的研究有重要的現(xiàn)實意義。編隊控制是指多個機(jī)器人在到達(dá)目的地的過程中,保持某種隊形,同時又要適應(yīng)環(huán)境約束的控制技術(shù)。目前,編隊控制方法大致有基于行為法、人工勢場法、虛擬結(jié)構(gòu)法、領(lǐng)航-跟隨法、分布式控制法等,其中領(lǐng)航-跟隨方法又稱主從式編隊方法,僅僅給定領(lǐng)航者的行為,為跟隨者設(shè)計跟蹤控制律就可以控制整個機(jī)器人群體的行為,簡化了隊形間的合作問題。考慮到水下環(huán)境通訊帶寬的限制及水下噪聲的干擾等實際情況,希望能夠減少AUV間的交互信息量,因此在領(lǐng)航-跟隨式編隊控制方法中,僅利用領(lǐng)航者的位置信息,而不利用其速度及角速度信息,得到跟隨者的期望軌跡及速度,據(jù)此設(shè)計跟蹤控制律。由于滑?？刂撇恍枰_的數(shù)學(xué)模型及抗干擾能力強(qiáng),而廣泛應(yīng)用于AUV的控制系統(tǒng)中,對此本文給出了三維環(huán)境中AUV主從式編隊控制自適應(yīng)滑?？刂扑惴?并將該控制算法應(yīng)用到具體AUV動力學(xué)模型中,得到推進(jìn)器推力值,驗證了算法的有效性及實用性。2問題描述2.1完整運動學(xué)描述通常將AUV的坐標(biāo)系統(tǒng)分成慣性坐標(biāo)系(E-XYZ)和載體坐標(biāo)系(O-X0Y0Z0)兩類,三維空間中完整運動學(xué)表示為其中:M,C,D,g,τ∈R6×6分別為慣性矩陣、離心項和科氏矩陣、阻尼矩陣、靜力項(重力與浮力合力)、AUV作用力與力矩;v=[uvwpqr]T,為廣義速度向量;η=[xyzφθψ]T,為廣義位置向量;J(η)為轉(zhuǎn)換矩陣;B是適當(dāng)維度的推力配置矩陣;T是各個推進(jìn)器的推力項。2.2auv的方向在不同高度領(lǐng)航-跟隨式編隊控制過程需要設(shè)定領(lǐng)航AUV的行為,并且設(shè)計虛擬AUV作為期望參考量,三維空間位置關(guān)系如圖1所示。圖1中,領(lǐng)航AUV、虛擬AUV、跟隨AUV和領(lǐng)航AUV在跟隨AUV所在平面投影,分別用Am,Av,Af,Am'表示,編隊任務(wù)描述為:Am與Af空間距離d,水平面觀望角度χ為AvAm'與Am線速度矢量um的夾角,垂直方向觀望角度ξ為AmAv與AmAm'的夾角。若某一時刻領(lǐng)航AUV的廣義位置為ηm,得出虛擬AUV的位置ηv,對于Af,只要設(shè)計出合理的控制輸入τf,得到AUV各推進(jìn)器推力值,使得lti→m!ηf-ηv=0,即可保證Af正確路徑。在n維AUV系統(tǒng)中,將n維AUV組分為n-1個子系統(tǒng),各子系統(tǒng)包括2個AUV(領(lǐng)航AUV,跟隨AUV),如圖2所示。3auv的動力學(xué)方程本文中仿真模型為上海海事大學(xué)水下機(jī)器人與智能系統(tǒng)實驗室海箏號AUV模型,如圖3所示。該機(jī)器人一共包含4個推進(jìn)器,水平面2個推進(jìn)器和垂直面2個推進(jìn)器,無側(cè)向推進(jìn)器,具體推進(jìn)器布置如圖4所示(TH表示推進(jìn)器thruster)。針對該AUV推進(jìn)器的布置,將AUV完整動力學(xué)方程進(jìn)行簡化解耦工作,考慮AUV空間中3自由度運動:進(jìn)退(surge),回轉(zhuǎn)(yaw)和潛浮(heave)。因此,慣性坐標(biāo)及載體坐標(biāo)系下AUV狀態(tài)為η=[xyzψ]T和v=[uwr]T。推算出海箏號AUV空間中各個自由度上產(chǎn)生的合力與合力矩和推進(jìn)器的關(guān)系:其中:[τXτZτN]T是作用于AUV重心的各個自由度上的合力與合力矩;K為常數(shù);R為兩個垂直推進(jìn)器之間的距離;[T1T2T3T4]T是水平面布置的4個推進(jìn)器的推力。歸一化推力如式(5)所示:4酶活力集的設(shè)置根據(jù)領(lǐng)航位置信息及編隊任務(wù)的要求,得到期望軌跡:其中:l=[dsinξcosχ,dsinξcosχ,dcosξ,0]T;ηm為領(lǐng)航機(jī)器人的廣義位置;d,ξ,χ為編隊航行過程中設(shè)定的常數(shù)。設(shè)虛擬機(jī)器人廣義位置量為ηv,定義虛擬機(jī)器人誤差量:ev=ηv-ηd∈R4,定義輔助誤差變量:其中:φ=[φ1,φ2,φ3,φ4]T,定義其導(dǎo)數(shù):定義虛擬機(jī)器人的速度值為:5跟蹤控制器組成編隊控制的目標(biāo)是為跟隨AUV各推進(jìn)器設(shè)計合適的推力值作為輸入量,控制跟隨AUV的速度和角速度量,使其能夠跟蹤虛擬AUV的軌跡,即誤差ee收斂于0。系統(tǒng)控制結(jié)構(gòu)如圖5所示:跟蹤控制系統(tǒng)由兩部分組成:外部環(huán)路為輔助速度控制器,由位置及方位狀態(tài)誤差得到;內(nèi)部環(huán)路為滑?？刂?由速度狀態(tài)矢量得到,輔助速度控制輸入由反步方法生成的速度控制器得到。5.1正常系數(shù)k由反步方法得到AUV輔助速度量:式中:k,kz,kψ為正常系數(shù)。設(shè)計Lyapunov函數(shù)式對函數(shù)式(12)求導(dǎo)數(shù),得到:因此系統(tǒng)是一致穩(wěn)定的。5.2基于滑模面時的等式控制策略將速度誤差值ec=vc-vf=[eueωer]T作為滑?？刂破鞯妮斎?選擇滑模面:對等式(14)求導(dǎo),得到系統(tǒng)運行于滑模面時,式(15)為零等價控制律為加入自適應(yīng)控制項取代切換項,得最終得到完整的控制律:6生成速度控制選擇參數(shù)和增益為λ1=2,λ2=1,λ3=1,λ4=0.5,k1=2,k2=1,k3=1,k4=0.5,設(shè)定t從0時刻開始運行,A1,A2的起始位置分別為η1(0)=[-10-1500],η2(0)=[-5-2000],仿真結(jié)果如圖6所示。當(dāng)A0沿著螺旋軌跡航行時,據(jù)編隊任務(wù)要求,各時刻A1,A2的誤差值,如圖7和圖8所示,經(jīng)計算與理論值相符。根據(jù)A0航行軌跡及編隊任務(wù)要求,得到A1和A2的期望位置信息η1d和η2d,以A1為例進(jìn)行說明,由A1期望位置信息ηd,得到虛擬AUV速度值vv=[uvwvrv]T,結(jié)合式(2)得到虛擬AUV的位置信息ηv,與A1實際航行軌跡比較得到誤差量,將vv和ηv作為反步控制器的輸入,得到A1輔助速度值vc=[ucwcrc]T,將其vc作為滑模控制器輸入生成速度控制的力和力矩,得到歸一化推力值,如圖9所示。將推力作用到A1,產(chǎn)生實際速度值如圖10所示,實現(xiàn)期望的編隊控制。7數(shù)值模擬auv模型本文應(yīng)用主從式編隊控制方法,研究了三維環(huán)境中多AUV的編隊控制問題,僅利用領(lǐng)航AUV的位置信息,結(jié)合編隊任務(wù)要求,設(shè)計虛擬AUV,得到跟隨AUV的