基于反步控制和人工勢場的多auv三維編隊(duì)與避障控制

基于反步控制和人工勢場的多auv三維編隊(duì)與避障控制

0多auv編碼控制近年來，多自治水下機(jī)器人（auv）的合作與質(zhì)量控制已成為獨(dú)立水機(jī)器人領(lǐng)域的研究熱點(diǎn)。編隊(duì)控制(formationcontrol)是AUV協(xié)作控制的基本問題,多AUV編隊(duì)控制的方法大致有領(lǐng)航者-跟隨者方法、基于行為的方法、虛擬結(jié)構(gòu)方法等三種方法?；谛袨榈姆椒ǖ幕舅枷胧菍㈥?duì)形控制分解為一系列基本行為,通過行為的綜合實(shí)現(xiàn)運(yùn)動(dòng)控制。該方法有明確的隊(duì)形反饋,并實(shí)現(xiàn)分布式控制,但是對(duì)群體的行為沒有明確定義,難以進(jìn)行數(shù)學(xué)分析,不能保證隊(duì)形的穩(wěn)定性。虛擬結(jié)構(gòu)方法將AUV編隊(duì)從整體上看作是一個(gè)剛體的虛擬結(jié)構(gòu),每個(gè)AUV是剛體上相對(duì)位置固定的一點(diǎn)。該方法可通過定義剛體的行為來控制AUV整體的運(yùn)動(dòng),但是不能根據(jù)環(huán)境的變化改變隊(duì)形,因而限制了其應(yīng)用范圍,而且目前僅適用于二維的環(huán)境中。領(lǐng)航者-跟隨者方法又叫主從式方法,該方法將AUV整體拆分為兩兩一組,一個(gè)領(lǐng)航AUV,一個(gè)跟隨AUV,通過跟隨者對(duì)領(lǐng)航者保持一定的角度和距離來實(shí)現(xiàn)隊(duì)形控制,根據(jù)領(lǐng)航者與跟隨者之間的相對(duì)位置關(guān)系,可以形成不同的網(wǎng)絡(luò)拓?fù)浣Y(jié)構(gòu),即不同的隊(duì)形,當(dāng)環(huán)境變化或者遇到障礙物時(shí),可以及時(shí)地改變隊(duì)形以通過障礙物區(qū)域。主從式的方法對(duì)整個(gè)隊(duì)形也有明確的定義,可以對(duì)其進(jìn)行數(shù)學(xué)分析,從而保證隊(duì)形的穩(wěn)定性,并且適用于三維環(huán)境中。但從現(xiàn)有的多AUV主從式編隊(duì)控制文獻(xiàn)看,多AUV編隊(duì)多為二維的情況,不能很好地體現(xiàn)真實(shí)的三維海洋環(huán)境,如崔榮鑫和Ge等提出的欠驅(qū)動(dòng)的主從式編隊(duì)控制,崔海英等提出的用模糊推理系統(tǒng)獲得避障角度實(shí)現(xiàn)避障的控制等皆為二維編隊(duì)。本文針對(duì)三維水下環(huán)境進(jìn)行了研究,提出了一種基于反步控制(backsteppingcontrol)和人工勢場(artificialpotentialfield)的多AUV三維主從式編隊(duì)與避障方法。該方法利用領(lǐng)航者的位置信息,引入反步控制實(shí)現(xiàn)跟隨AUV對(duì)虛擬AUV的軌跡跟蹤,實(shí)現(xiàn)三維水下編隊(duì)控制,而且結(jié)合人工勢場的方法變換隊(duì)形,有效地避開障礙物。1虛擬auv的跟蹤本文采用主從式編隊(duì)控制方法,在反步控制基礎(chǔ)上,設(shè)計(jì)運(yùn)動(dòng)學(xué)編隊(duì)控制律。利用領(lǐng)航AUV的位置信息,得到虛擬AUV的位置及速度信息,作為跟隨AUV的期望狀態(tài)量;隨后為跟隨AUV設(shè)計(jì)運(yùn)動(dòng)學(xué)編隊(duì)控制律,使其跟蹤虛擬AUV的軌跡;最后進(jìn)行多AUV編隊(duì)控制的仿真,驗(yàn)證算法的有效性。圖1給出了運(yùn)動(dòng)學(xué)編隊(duì)控制系統(tǒng)框圖。1.1模擬編碼控制利用領(lǐng)航AUV位置信息進(jìn)行虛擬AUV的設(shè)計(jì),得到其位置量及速度值作為跟隨AUV編隊(duì)航行的期望值,從而設(shè)計(jì)跟隨AUV編隊(duì)控制律,控制跟隨AUV跟蹤虛擬AUV軌跡,實(shí)現(xiàn)期望編隊(duì)效果。具體編隊(duì)控制過程參考圖2,編隊(duì)控制算法設(shè)計(jì)過程以一組AUV(包括兩個(gè)AUV:領(lǐng)航AUV、跟隨AUV)為例進(jìn)行說明,在仿真過程中將編隊(duì)控制擴(kuò)展到包含多AUV的系統(tǒng)。虛擬AUV的設(shè)計(jì)就是由領(lǐng)航AUV位置量ηm得到虛擬AUV的位置量ηv和速度量Vv的過程。1.1.1到載體坐標(biāo)系的轉(zhuǎn)換為得到虛擬AUV的位置量和速度量,設(shè)領(lǐng)航AUV的位置信息為ηm=[xmymzmψm]T,其中ψm為領(lǐng)航AUV的艏向角。首先根據(jù)領(lǐng)航AUV的位置量ηm得到其參考軌跡量:式中R(ψm)為載體坐標(biāo)系到慣性坐標(biāo)系的轉(zhuǎn)換矩陣,用式表示;l=[dsinαcosβ,dsinαsinβ,dcosα,0]T為載體坐標(biāo)系下虛擬AUV與領(lǐng)航AUV的相對(duì)位置;d為領(lǐng)航AUV到虛擬AUV的距離,α為領(lǐng)航與虛擬AUV連線和領(lǐng)航與領(lǐng)航AUV投影連線的夾角,β為領(lǐng)航AUV進(jìn)退方向的線速度和領(lǐng)航AUV投影與虛擬AUV連線的夾角;0度表示期望AUV與領(lǐng)航AUV的艏向角保持一致,具體數(shù)值在仿真過程中根據(jù)編隊(duì)任務(wù)設(shè)定,具體意義參見圖2。1.1.2虛擬auv誤差量為得到虛擬AUV的位置量ηv和速度量Vv,利用其參考軌跡量ηr,并且引入輔助控制變量及相關(guān)設(shè)計(jì)參數(shù),位置量ηv具體設(shè)計(jì)過程如下:設(shè)虛擬AUV位置量為ηv=[xvyvzvψv]T,定義虛擬AUV誤差量:虛擬AUV控制算法設(shè)計(jì)的目的是通過輔助變量的引入,使得誤差量ev為0。引入輔助誤差變量其中Φ=[Φ1,Φ2,Φ3,Φ4]T為輔助控制變量,Φi(i=1,2,3,4)為設(shè)計(jì)參數(shù),定義輔助控制變量Φ的導(dǎo)為其中K=diag[k1,k2,k3,k4],λi,為設(shè)計(jì)參數(shù)。將(3)式帶入(4)式,得定義虛擬AUV的位置量微分方程為式中Φi,ki(i=1,2,3,4)為設(shè)計(jì)參數(shù),與輔助控制變量Φ中Φi及K中ki設(shè)置值相同。1.1.3虛擬auv位置量和速度量計(jì)算已知在載體坐標(biāo)系到慣性坐標(biāo)系的轉(zhuǎn)換關(guān)系為,則。虛擬AUV速度量為將具體的參數(shù)帶入式(5)和式(7),得到微分方程組:根據(jù)式(8)和式(9),經(jīng)計(jì)算可得到虛擬AUV位置量ηv和輔助控制變量φ,結(jié)合式(7)可得到虛擬AUV速度量Vv。編隊(duì)控制過程中,進(jìn)行虛擬AUV位置量和速度量計(jì)算時(shí),需要設(shè)定參數(shù)λi,ki(i=1,2,3,4),具體設(shè)定值見仿真部分,給定領(lǐng)航AUV位置信息ηm及隊(duì)形參數(shù)d、α、β,據(jù)式(1)得到參考位置信息ηr,并且據(jù)此設(shè)定虛擬AUV初始位置,設(shè)定初始時(shí)刻Φi(0)=0。根據(jù)式(8)和式(9),利用MATLAB/SIMULINK平臺(tái)進(jìn)行微分方程的計(jì)算,得到虛擬AUV的位置量ηv及輔助控制變量Φ,據(jù)式(7)得到虛擬AUV速度量Vv,則可實(shí)現(xiàn)僅利用領(lǐng)航AUV位置量ηm得到虛擬AUV的位置量ηv和速度量Vv。將虛擬AUV位置量ηv及速度量Vv作為跟隨AUV編隊(duì)航行期望參考信息,據(jù)此為跟隨AUV設(shè)計(jì)編隊(duì)控制律,控制其跟蹤虛擬AUV的軌跡,實(shí)現(xiàn)期望編隊(duì)控制效果,虛擬AUV實(shí)現(xiàn)效果在仿真部分中進(jìn)行分析說明。1.2速度值vv的消費(fèi)運(yùn)動(dòng)學(xué)編隊(duì)控制系統(tǒng)中,利用領(lǐng)航AUV位置信息ηm得到虛擬AUV速度信息Vv及位置信息ηv。對(duì)反步跟蹤控制來說,則將跟隨AUV實(shí)際軌跡ηf與虛擬AUV軌跡ηv的誤差量ee和虛擬AUV的速度值Vv共同作為反步控制的輸入。反步控制方法因其控制性能簡單實(shí)用,廣泛應(yīng)用于移動(dòng)機(jī)器人中,現(xiàn)將其應(yīng)用在三維水下環(huán)境AUV編隊(duì)控制之中,對(duì)應(yīng)的控制律設(shè)計(jì)也是基于三維環(huán)境的。由反步控制方法得到運(yùn)動(dòng)學(xué)編隊(duì)控制律為式中,k,kz,kψ為正常系數(shù)。2動(dòng)態(tài)環(huán)境下的相對(duì)速度u由于海底環(huán)境復(fù)雜多變,AUV在編隊(duì)航行過程中,有可能遇到障礙物,本文考慮在這種情況下的編隊(duì)控制。對(duì)于領(lǐng)航AUV,當(dāng)遇到障礙物時(shí),根據(jù)當(dāng)前與障礙物的距離及方向采取避障措施;而AUV編隊(duì)則需要變換隊(duì)形以通過障礙物區(qū)域,變換隊(duì)形的過程中,跟隨AUV根據(jù)領(lǐng)航AUV的位置和軌跡變化來調(diào)整與領(lǐng)航AUV之間的相對(duì)位置,最終以直線隊(duì)形通過障礙物區(qū)域。此處引入人工勢場方法實(shí)現(xiàn)多AUV隊(duì)形變化與避障。借用陸地移動(dòng)機(jī)器人的引力勢場設(shè)計(jì)方法,設(shè)三維空間中有一靜止目標(biāo)點(diǎn)g(x1,x2,x3)和機(jī)器人r(y1,y2,y3)。定義空間內(nèi)機(jī)器人與目標(biāo)物距離函數(shù)為其產(chǎn)生的引力場為其中m是正常數(shù),其取值決定了勢場函數(shù)曲線的形狀。如果m=1,會(huì)使求導(dǎo)后出現(xiàn)引力無界情況,造成機(jī)器人在目標(biāo)點(diǎn)抖動(dòng)。此處取m=2,ξ是目標(biāo)勢場系數(shù)。目標(biāo)吸引力為其中nRG是機(jī)器人到目標(biāo)的單位矢量。當(dāng)目標(biāo)運(yùn)動(dòng)時(shí),將相對(duì)速度引入勢場模型,目標(biāo)點(diǎn)g(x1,x2,x3)的速度為VG(t),機(jī)器人r(y1,y2,y3)的速度為V(t),目標(biāo)與機(jī)器人的相對(duì)速度為:速度分量產(chǎn)生的引力勢場為速度分量產(chǎn)生相應(yīng)的勢場力為其中nVRG是垂直于Vatt(t)的單位矢量,可以取n=2,δ是常系數(shù)。則動(dòng)態(tài)環(huán)境下引力勢場的合成勢場力為障礙物位置是o(z1,z2,z3),Frep1是由機(jī)器人和障礙物的距離產(chǎn)生的距離斥力分量,Frep2是由相對(duì)速度V(t)-Vobs(t)產(chǎn)生的速度斥力分量,其大小與相對(duì)速度大小成正比,方向垂直于機(jī)器人與障礙物的相對(duì)距離方向,亦即相對(duì)速度V(t)-Vobs(t)在垂直于機(jī)器人與障礙物的相對(duì)距離方向上的分量的反方向。這里給出斥力勢場的距離分量表達(dá)式:其中N是障礙物的個(gè)數(shù),是機(jī)器人與第i個(gè)障礙物的距離函數(shù),則有三維空間中第i個(gè)障礙物的斥力勢場的距離分量對(duì)應(yīng)排斥力為則斥力勢場的距離分量對(duì)應(yīng)的總的排斥力為速度分量產(chǎn)生的勢場為式(22)表示第i個(gè)障礙物的速度分量產(chǎn)生的勢場,相應(yīng)的勢場力為其中θi是機(jī)器人與障礙物的和速度與距離斥力分量的夾角,λ是常數(shù)系數(shù)。本文利用指數(shù)函數(shù)作為勢場函數(shù)的方法。該勢場函數(shù)的結(jié)構(gòu)為在靜態(tài)環(huán)境下,對(duì)于引力場,X的表達(dá)式為X=Xr,g=β‖ρ(r,g)‖m;對(duì)于斥力場,x的表達(dá)式為X=Xr,o=γ‖ρ(r,o)‖n,合力的表達(dá)式為其中,nRG是機(jī)器人指向目標(biāo)的單位矢量,是第i個(gè)障礙物指向機(jī)器人的單位矢量,采用指數(shù)函數(shù)方法,人工勢場仍然是距離的函數(shù),與常規(guī)勢場函數(shù)不同的是,功能函數(shù)是指數(shù)曲線而非二次曲線。根據(jù)指數(shù)函數(shù)的特點(diǎn),m和n的取值大于1,β和γ均是大于0的常數(shù)系數(shù),所以X>0。3海事又上臺(tái)階本文的算法是基于上海海事大學(xué)水下機(jī)器人與智能系統(tǒng)實(shí)驗(yàn)室的海事一號(hào)AUV,該機(jī)器人一共包含5個(gè)推進(jìn)器,水平面4個(gè),垂直面1個(gè),能夠進(jìn)行4個(gè)自由度運(yùn)動(dòng):進(jìn)退,橫移,潛浮,回轉(zhuǎn)。3.1軌跡跟蹤控制器設(shè)計(jì)設(shè)定領(lǐng)航AUV的航行軌跡為螺旋線:ηm=[xmymzmψm]T,xm(t)=1.3*sin(0.2*t);ym(t)=-1.3*cos(0.2*t);zm(t)=0.2*t+0.4;ψm(t)=0.5*t。仿真環(huán)境設(shè)置為:3m×3m×4.5m的三維環(huán)境。隊(duì)形參數(shù)設(shè)置如下:d=0.5m,,λ1=2,λ2=1,λ3=4,λ4=0.5,k1=2,k2=1,k3=2,k4=0.1。設(shè)定仿真時(shí)間為20s,t從0時(shí)刻開始運(yùn)行,領(lǐng)航AUV的起始位置為ηm(0)=[0-1.30.40]T。通過主從式編隊(duì)控制得到虛擬AUV的速度及位置信息之后,將其輸入到軌跡跟蹤控制器模塊。仿真環(huán)境設(shè)置為:3m×3m×4.5m的三維環(huán)境,跟隨AUV的起始位置為ηf(0)=[0.5000]運(yùn)動(dòng)學(xué)控制律參數(shù)設(shè)置為:k=1,kz=2,kψ=2,設(shè)定仿真時(shí)間為20s,仿真結(jié)果如圖3所示。圖4所示為軌跡跟蹤和跟隨過程中距離d的變化。軌跡跟蹤過程中,t=0時(shí)刻,虛擬AUV的位置為ηv(0)=[0.01-1.0300],跟隨AUV的初始位置為ηf(0)=[0.5000],距離d為1.14m,t=8s時(shí)刻,距離趨近于0m,并穩(wěn)定在0m,表明跟隨AUV對(duì)虛擬AUV實(shí)現(xiàn)跟蹤;跟隨過程中,從t=8s時(shí)刻到仿真結(jié)束,距離一直穩(wěn)定在0.5m,表明跟隨AUV對(duì)領(lǐng)航AUV實(shí)現(xiàn)跟隨。驗(yàn)證了該跟隨算法與跟蹤算法的有效性。3.27真環(huán)境設(shè)置與運(yùn)動(dòng)學(xué)控制律實(shí)現(xiàn)了兩個(gè)AUV的編隊(duì)控制,接下來將其擴(kuò)展到7個(gè)AUV的編隊(duì)控制。首先給出領(lǐng)航AUV的位置信息,設(shè)定領(lǐng)航AUVA0的航行軌跡為螺旋線:ηm=[xmymZmψm]T,xm=5*sin(0.2*t);ym=-5*cos(0.2*t);zm=0.15*t;ψm=0.2*t。跟隨AUV6個(gè),分別為A1,A2,A3,A4,A5,A6。仿真環(huán)境設(shè)置為:16m×16m×11m的三維環(huán)境,A1、A2跟隨A0,A4跟隨A2,A6跟隨A4,A3跟隨A1,A5跟隨A3,對(duì)于A0,A2,A4,A6,隊(duì)形參數(shù)設(shè)置為:d=1.5m,,λ1=2,λ2=1,λ3=4,λ4=0.5k1=2,k2=1,k3=2,k4=0.1;對(duì)于A0,A1,A3,A5,隊(duì)形參數(shù)設(shè)置為d=1.5m,,,λ1=2,λ2=1,λ3=4,λ4=0.5,k1=2,k2=1,k3=2,k4=0.1;運(yùn)動(dòng)學(xué)控制律參數(shù)設(shè)置為:k=1,kz=2,kψ=2。A0首位置為(0-500),A1首位置(-1-4-10),A2首位置(-1-6-1.060),A3首位置(0-3-2.120),A4首位置(-1-7-2.120),A5首位置(-1-2-3.180)。A6首位置(-2-8-3.180),設(shè)定仿真時(shí)間為50s,結(jié)合以上兩個(gè)過程的仿真,結(jié)果如圖5所示。圖5中,1、2號(hào)AUV跟隨領(lǐng)航AUV(編號(hào)為0),4跟隨2,6跟隨4,3跟隨1,5跟隨3,領(lǐng)航AUV的航行信息決定了整個(gè)編隊(duì)的隊(duì)形及航行情況,結(jié)合上文中對(duì)跟隨過程和跟蹤過程,從圖中可以看出很好地實(shí)現(xiàn)了螺旋編隊(duì)控制。3.37auv以v型編碼行車避障仿真中,將AUV的主從式編隊(duì)與人工勢場結(jié)合起來,把領(lǐng)航AUV視為人工勢場中唯一的機(jī)器人,其他AUV跟隨領(lǐng)航AUV,按照既有的算法進(jìn)行編隊(duì)。障礙物圓心坐標(biāo)點(diǎn)為(22,25,23),(15,15,14),(8,8,12),半徑為1,目標(biāo)點(diǎn)圓心坐標(biāo)為(35,25,14),半徑為1,AUV以V型編隊(duì)航行,當(dāng)接近障礙物區(qū)域時(shí),根據(jù)勢場函數(shù)(27),取m=2,在引力場中取α為60,β為0.01,斥力場中取α為5.5,γ為1.5。以領(lǐng)航AUV當(dāng)前位置為圓心,半徑為1畫圓,將該圓水平方向360度30等分,垂直方向180度15等分,然后遍歷,找到一個(gè)合力最大的點(diǎn)即為領(lǐng)航AUV下一時(shí)刻前進(jìn)的方向。在障礙物區(qū)域,剩余的AUV分別沿著領(lǐng)航AUV的軌跡呈直線隊(duì)形航行,當(dāng)離開障礙物區(qū)域時(shí),再呈V型編隊(duì)航行。仿真過程如圖6所示。4auv線形編碼仿真本文從領(lǐng)航AUV的位置信息出發(fā),結(jié)合主從式編隊(duì)的控制方法,進(jìn)行了系統(tǒng)的數(shù)學(xué)推導(dǎo),建立了7AUV主從式編隊(duì)控制模型,并模擬復(fù)雜的海底環(huán)境,加入