龐師坤, 梁曉鋒, 李英輝, 易 宏
(上海交通大學(xué) 海洋智能裝備與系統(tǒng)教育部重點實驗室;海洋工程國家重點實驗室,上海 200240)
自主水下機(jī)器人(AUV)是具有智能行為的高級水下機(jī)器人,具有活動范圍廣、機(jī)動靈活、隱蔽性好等特點,能夠適用于復(fù)雜的海底環(huán)境.為了在不可預(yù)知的海底環(huán)境中安全完成既定任務(wù),AUV要能夠利用自身攜帶的傳感探測設(shè)備(聲吶、攝像頭等)并結(jié)合相應(yīng)的算法進(jìn)行實時避障.人工勢場法[1-4]、動態(tài)窗口法[5-7]、A*及其改進(jìn)算法[8-13]、基于行為法[14-18]等,是目前應(yīng)用較多的避障方法.
基于行為法廣泛應(yīng)用于諸如移動機(jī)器人、無人艇、無人機(jī)等的無障礙路徑規(guī)劃領(lǐng)域.無障礙路徑規(guī)劃采用自底向上的系統(tǒng)構(gòu)建方法,基于行為法將任務(wù)目標(biāo)分解為一系列相互作用的基本子行為,如感知、探測、避障、規(guī)劃,構(gòu)成并行控制回路,通過協(xié)調(diào)配合后作用于執(zhí)行機(jī)構(gòu),產(chǎn)生相應(yīng)的目標(biāo)任務(wù)動作.單個子行為僅需要完成某項簡單任務(wù),能夠?qū)Νh(huán)境產(chǎn)生快速響應(yīng),并且可以靈活地擴(kuò)展整個系統(tǒng)的能力[19].行為選擇機(jī)制主要有行為抑制法、加權(quán)平均法和模糊邏輯法[20].其中模糊邏輯法可以實時地調(diào)整任務(wù)權(quán)值,從而優(yōu)化任務(wù)完成的質(zhì)量,因此在實際應(yīng)用領(lǐng)域很受歡迎.文獻(xiàn)[21]提出的零空間行為(NSB)法即為模糊邏輯法的一種.與其他基于行為的方法相比,NSB法在保持首要任務(wù)完全完成的前提下,可以充分利用零空間完成其他次要任務(wù),具有實時性強(qiáng)、各子任務(wù)間零沖突的優(yōu)點.
本文采用NSB法,針對AUV在復(fù)雜海底環(huán)境中對靜態(tài)和動態(tài)障礙物的規(guī)避進(jìn)行研究.在按需對整體任務(wù)進(jìn)行分解的基礎(chǔ)上,建立不同優(yōu)先級的子任務(wù)函數(shù),并對不同障礙物的避障策略進(jìn)行分析.
NSB法首先將整體任務(wù),即駛向目標(biāo)點,分解成多個各自獨立控制目標(biāo)的子任務(wù),并分別列出各子任務(wù)函數(shù),每個子任務(wù)函數(shù)都能完成對應(yīng)的控制目標(biāo);然后將子任務(wù)分成不同的優(yōu)先級,將由低優(yōu)先級的任務(wù)向量向高優(yōu)先級任務(wù)向量的零空間投影得到任務(wù)的整體綜合輸出函數(shù)傳遞給底層的執(zhí)行機(jī)構(gòu)以控制AUV的動作.設(shè):η=[xyψ]T為AUV慣性坐標(biāo)系下的位置和艏向角;ν=[uvr]T為AUV隨體坐標(biāo)系下的速度和轉(zhuǎn)艏角速度(u為縱向速度,v為橫向速度,r為轉(zhuǎn)艏角速度);σ為控制目標(biāo)的任務(wù)變量,其函數(shù)模型為
σ=f(η)
(1)
將σ對η求導(dǎo)可得
(2)
式中:J(η)為η的輸出雅可比矩陣.
速度矩陣的最小二乘解為
(3)
用i表示任務(wù)的優(yōu)先級(i=1時,任務(wù)優(yōu)先級最高),第i個任務(wù)的速度輸出可表示為
(4)
第i個任務(wù)的向較高一級任務(wù)零空間的投影為可表示為
(5)
式中:I為單位矩陣.
由低等級的任務(wù)向較高一級的任務(wù)空間投影,可得出整體控制目標(biāo)的速度輸出為
(6)
當(dāng)有3個不同等級任務(wù)時,綜合任務(wù)的速度輸出可表示為
vd=v1+N1(v2+N2v3)
(7)
以兩個任務(wù)為例,其綜合輸出速度的幾何模型如圖1所示.v2首先向v1的零空間投影,進(jìn)而得到vd=v1+N1v2,vd為任務(wù)速度v1和任務(wù)速度v2的NSB輸出.
圖1 兩任務(wù)時綜合輸出速度的幾何模型
本文主要針對AUV對平面內(nèi)不同類型障礙物的避障策略進(jìn)行分析討論.為便于分析模擬,根據(jù)實際情況和AUV繞過障礙物的方向,假設(shè):
(1)障礙物無法與AUV進(jìn)行通信且不會對AUV進(jìn)行主動規(guī)避.
(2)AUV可以探測到相對于動靜態(tài)障礙物的運動速度和方位角,選擇逆時針或者順時針方向繞過障礙.
(3)AUV可以探測到當(dāng)前時刻動態(tài)障礙物的速度,但是無法預(yù)知其下一時刻速度,因此假設(shè)動態(tài)障礙物的運動速度是固定的.
根據(jù)假設(shè)(1),當(dāng)AUV利用自身攜帶的傳感器(聲吶、攝像頭等)探測到障礙物時,需根據(jù)障礙物類型進(jìn)行主動規(guī)避.單個靜態(tài)障礙物類型相對簡單,文中不再贅述.針對動態(tài)障礙物,可分為正面障礙物(見圖2)和側(cè)面障礙物(見圖3)兩大類.對于正面障礙物,AUV可以選擇順時針或逆時針方向進(jìn)行規(guī)避(本文設(shè)為逆時針規(guī)避);對于側(cè)面障礙物,AUV需要判斷自身與障礙物的相對運動方向和方位,選擇合理的轉(zhuǎn)艏方向,從而順利規(guī)避障礙物.
圖2 正面障礙物
圖3 側(cè)面障礙物
根據(jù)假設(shè)(2),首先將障礙物的速度轉(zhuǎn)換到AUV隨體坐標(biāo)系上:
(8)
式中:上標(biāo)b和n分別表示隨體坐標(biāo)系和慣性坐標(biāo)系;R(θ)為轉(zhuǎn)換矩陣;θ為轉(zhuǎn)換角.
AUV在隨體坐標(biāo)系下相對障礙物運動方向為
(9)
由于β(見圖4,圖中vo和vv分別為障礙物和AUV的速度)的取值范圍為-180°<β<180°,而常規(guī)的四象限定義0°<β<360°,所以當(dāng)β<0°時,β需在原來的基礎(chǔ)上加360°,從而滿足要求.AUV根據(jù)β所在的象限,選擇繞過障礙物的方向,當(dāng)β在第 I 和第 II 象限時,選擇順時針方向,當(dāng)β在第 III 和第 IV 象限時,選擇逆時針方向.在此情形下,AUV從障礙物后面繞過且兩者方向沒有交叉,因此路徑是安全的.
圖4 β角象限示意圖
文中設(shè)定當(dāng)AUV運動方向相對于障礙物的運動方向角度的絕對值小于20° 時為正面障礙物,此時|β-180°|≤20°.
AUV和障礙物的路徑V(t)和O(t)分別表示為
(10)
(11)
式中:θvt為AUV到目標(biāo)點的方向角;θo為移動障礙物的方向角;t為時間變量;pv,x、pv,y和po,x、po,y分別為當(dāng)前時刻AUV和障礙物的位置,兩者之間的距離為
(12)
首先當(dāng)AUV探測到障礙物時,啟動避障算法.AUV根據(jù)當(dāng)前時刻自身和障礙物的速率以及相對方向角等信息,計算下一時刻兩者的相對位置.根據(jù)假設(shè)(3),當(dāng)f(t)進(jìn)入到障礙物的安全范圍內(nèi)時,AUV開啟避障子任務(wù).根據(jù)迭代信息和NSB算法,AUV判斷是否已經(jīng)安全繞過障礙物.如圖5所示,當(dāng)AUV在位置a時,探測到障礙物,同時估算路徑并啟動避障算法,此后則一直進(jìn)行探測和位置的迭代更新;在位置b時,AUV根據(jù)實時更新的障礙物相對位置和方位角,再次進(jìn)行路徑估算,判斷路徑是否安全;在位置c時,確認(rèn)路徑安全,避障算法停止,AUV駛向設(shè)定目標(biāo)點.
圖5 AUV避障過程
為了使基于NSB法的避障策略能夠處理靜態(tài)和動態(tài)障礙物,并根據(jù)前文所述的避障策略選擇繞過障礙物的方向,按照不同優(yōu)先級將避障任務(wù)分解為任務(wù)1(避障)、任務(wù)2(繞過障礙物)、任務(wù)3(駛向目標(biāo)點)3個子任務(wù),同時根據(jù)AUV與障礙物之間的相對距離將障礙物周圍分為安全區(qū)、常規(guī)避障區(qū)和緊急避障區(qū)3個區(qū)域(見圖6).在安全區(qū)內(nèi),認(rèn)為障礙物對AUV無威脅,AUV正常駛向目標(biāo)點,即為任務(wù)3;在常規(guī)避障區(qū)內(nèi),AUV探測到障礙物,啟動任務(wù)2,關(guān)閉任務(wù)1、3,以便AUV選擇繞過障礙物的方向和虛擬切點位置;在緊急避障區(qū)內(nèi)未繞過障礙物前,AUV啟動任務(wù)1、2,關(guān)閉任務(wù)3,確保AUV以最佳路徑順利繞過障礙物;確認(rèn)繞過障礙物后,在避障區(qū)內(nèi)任務(wù)1、3同時啟動,關(guān)閉任務(wù)2;駛出避障區(qū)后,關(guān)閉任務(wù)1、2,開啟任務(wù)3,AUV重新規(guī)劃路線,選擇最短路徑駛向目標(biāo)點.
圖6 不同避障區(qū)
任務(wù)1避障.在多控制目標(biāo)任務(wù)中,避障任務(wù)作為最高優(yōu)先級,任務(wù)變量σ1表示AUV的位置Pv到障礙物中心Po的距離:
(13)
σ1,d為障礙物的安全閾值(安全距離):
σ1,d=d
(14)
對應(yīng)的雅可比矩陣為
(15)
任務(wù)1的輸出速度為
(16)
式中:λ1為任務(wù)1的增益矩陣.其零空間向量為
(17)
任務(wù)2繞過障礙物.作為第2優(yōu)先級的控制目標(biāo)任務(wù)2,引導(dǎo)AUV按照順時針或者逆時針方向繞過障礙物,其任務(wù)變量為
(18)
(19)
式中:to為障礙物上的虛擬切點.
圖7 虛擬切點示意圖
首先定義起始點Ps分別到障礙物中心點Po和切點t1(見圖7)的向量Pso和Pst1:
Pso=Po-Ps
(20)
Pst1=t1-Ps
(21)
其長度分別為
(22)
(23)
式中:ro為障礙物半徑.
然后確定Pst1與xOy平面的夾角θt1:
式中:θt1,1為Pso和Pst1之間的夾角;θt1,2為Pso與xOy平面的夾角.
設(shè)二維轉(zhuǎn)換矩陣R(θ)為
(27)
則切點t1為
(28)
式中:dt為當(dāng)前時刻AUV距離障礙物的距離.
同理,當(dāng)AUV逆時針繞過障礙物時,
θt2=θt2,2-θt2,1
(29)
切點t2為
(30)
任務(wù)2的變量為一條直線,其雅可比矩陣為
提高高速公路工程建設(shè)質(zhì)量具有重要意義,對此公路建設(shè)管理工作必須在創(chuàng)新驅(qū)動下積極開展創(chuàng)新活動,從管理思路、管理方法、技術(shù)標(biāo)準(zhǔn)等多個環(huán)節(jié)開展創(chuàng)新活動,積極引入新技術(shù)、新方法,從管理制度開始調(diào)整,重視科技創(chuàng)新工作,多種方法共同使用,以提高高速公路建設(shè)管理水平。
J2=I
(31)
輸出速度為
v2=λ2(to-Pv)
(32)
式中:λ2為任務(wù)2的增益矩陣.
那么零空間N2=0,任何低級任務(wù)的輸出速度投影到該任務(wù)空間,投影都為0.考慮到任務(wù)2和任務(wù)3不可能同時激活,因此該零空間亦不會使用.
任務(wù)3駛向目標(biāo)點.在本文中最低優(yōu)先級為駛向目標(biāo)點的控制目標(biāo)任務(wù)3,在AUV避障任務(wù)完成前后,確保AUV沿最短路徑到達(dá)目標(biāo)點.考慮到AUV沿直線到達(dá)目標(biāo),其任務(wù)變量為
同任務(wù)2類比,其雅可比矩陣、輸出速度和零空間分別為
式中:Pt為目標(biāo)點位置;λ3為任務(wù)3的增益矩陣.
根據(jù)式(6),AUV的綜合任務(wù)速度輸出為
vd=v1+N1v2+N1N2v3
(38)
為了保證避障任務(wù)的順利完成,本文設(shè)計了適用于NSB法的任務(wù)管理器(見圖8和9).任務(wù)管理器根據(jù)AUV探測到的障礙物和避障算法,決定何時激活和抑制對應(yīng)的任務(wù).特別需要注意的是,當(dāng)任務(wù)1未激活時,任務(wù)2、3無需向該任務(wù)零空間進(jìn)行投影.
圖8 NSB任務(wù)管理器控制邏輯圖
圖9 NSB任務(wù)管理器工作示意圖
為更加有效地闡述NSB法的避障效果,將基于行為法中傳統(tǒng)的行為抑制法和加權(quán)平均法的避障效果與NSB法相比較,避障效果如圖10所示.設(shè)AUV的初始狀態(tài)、目標(biāo)點坐標(biāo)和障礙物位置信息分別為
ηs=[0 m 0 m 0°]T
ηt=[70 m 70 m -]T
圖10 不同方法避障效果對比圖
其中前兩個元素分別為障礙物的橫縱坐標(biāo)位置信息,最后一個元素為障礙物安全半徑.從圖中可以看出,對于靜態(tài)障礙物,相對于行為抑制法和加權(quán)平均法的避障,基于NSB法的避障路徑?jīng)]有突入障礙物的安全范圍,并且避障效果良好.說明NSB法按照設(shè)計方案,將低優(yōu)先級的駛向目標(biāo)點任務(wù)向較高優(yōu)先級的避障任務(wù)的零空間投影,能夠較好地處理不同優(yōu)先級任務(wù)之間的沖突,并且最終圓滿完成任務(wù).而行為抑制法和加權(quán)平均法的避障路徑中,有部分路徑突入了障礙物的安全范圍.如果障礙物的安全半徑設(shè)置較小,AUV在駛向目標(biāo)點的過程中很有可能與障礙物發(fā)生碰撞,這種情況一旦發(fā)生,不僅無法按預(yù)期完成任務(wù),而且還會對AUV的自身安全造成難以預(yù)料的影響.合理推測,當(dāng)面對動態(tài)障礙物時,行為抑制法和加權(quán)平均法的避障效果相對會更差.綜合對比說明,NSB法相對于行為抑制法和加權(quán)平均法,有一定的優(yōu)越性和實用性.在以上分析的基礎(chǔ)上,以3種情況為例繼續(xù)展開基于NSB法的避障效果仿真計算.
情況1首先設(shè)置4個靜態(tài)障礙物,其中兩個在AUV的路徑范圍內(nèi).因此,AUV在駛向目標(biāo)點的過程中,必須繞過這兩個障礙物,同時需要避免另外兩個障礙物的影響.
設(shè)AUV的初始狀態(tài)和目標(biāo)點位置分別為
ηs=[5 m 7 m -57.32°]T
ηt=[160 m 130 m -]T
4個障礙物的位置坐標(biāo)和安全半徑分別為
模擬結(jié)果如圖11所示.
圖11 靜態(tài)障礙物AUV避障策略圖
如圖11(a)所示,AUV面對靜態(tài)障礙物時能夠及時避開障礙物并駛向目標(biāo)點;如圖11(b)所示,在30.5 s時AUV探測到障礙物1,啟動避障算法,逆時針繞過障礙物;如圖11(c)所示,在98.7 s時AUV已經(jīng)繞過障礙物1繼續(xù)駛向目標(biāo)點,并探測到障礙物2的存在.相對于從障礙物2和4之間穿過,AUV從障礙物2和3之間穿過路徑較短,因此AUV選擇按順針方向繞過障礙物2;如圖11(d)所示,在140.9 s時AUV按照避障算法安全繞過障礙物2,駛向既定目標(biāo)點,基于NSB的避障算法對靜態(tài)障礙物避障效果良好.
情況2對于動態(tài)障礙物,本文設(shè)置了2個沿不同方向逼近AUV的動態(tài)障礙物,以考察NSB法對動態(tài)障礙物的應(yīng)對能力.AUV在駛向目標(biāo)點的過程中,需要實時探測相對于障礙物的距離和行駛方向,選擇避開障礙物的最佳路線.
設(shè)AUV的初始狀態(tài)和目標(biāo)點位置分別為
ηs=[0 m -20 m 0°]T
ηt=[120 m 140 m -]T
兩個障礙物的位置和安全半徑分別為
障礙物的運動速度分別為
vo1=[-1.0 m/s 1.5 m/s 0°]T
vo2=[-0.8 m/s 0.8 m/s 0°]T
模擬結(jié)果如圖12所示.
圖12表明AUV基于NSB法的避障策略對于動態(tài)障礙物同樣有效.如圖12(a)所示為AUV在駛向目標(biāo)點的過程中發(fā)現(xiàn)動態(tài)障礙物、繞過動態(tài)障礙物并最終駛向目標(biāo)點的完整路線圖.如圖12(b)所示,在10.7 s時AUV探測到動態(tài)障礙物2,根據(jù)障礙物運動方向,選擇以順時針方向繞過障礙物,并在51.7 s時繞過障礙物2(見圖12(c));如圖12(d)所示,在63.8 s時AUV在繼續(xù)行駛途中發(fā)現(xiàn)動態(tài)障礙物1,同時啟動避障算法;由圖12(e)可以看出,在110.8 s時AUV以逆時針方向繞過障礙物1之后繼續(xù)駛向目標(biāo)點;圖12(f)所示為AUV駛向目標(biāo)點.
情況3當(dāng)障礙物沿AUV正面運動方向相向運動時,被視為正面障礙物.當(dāng)AUV在駛向目標(biāo)點的過程中遇到正面障礙物時,需要判斷相對于障礙物的行駛角度,并及時選擇避開障礙物的方向(逆時針或順時針).
設(shè)AUV的初始狀態(tài)和目標(biāo)點位置分別為
ηs=[5 m 7 m 57.32°]T
ηt=[150 m 200 m -]T
障礙物的初始位置和安全半徑為
速度參數(shù)為
vo=[-1.0 m/s -1.2 m/s 0°]T
模擬結(jié)果圖13所示.
圖13(a)所示為遇到正面障礙物時AUV的避障軌跡圖;如圖13(b)所示,在43.7 s時AUV探測到障礙物,并根據(jù)相對行駛方向判斷為正面障礙物,同時啟動避障算法;如圖13(c)所示,在68.4 s時AUV繞過正面障礙物;如圖13(d)所示,在82.9 s時AUV逆時針繞過障礙物后繼續(xù)駛向目標(biāo)點,驗證了NSB算法對正面障礙物避障的可靠性.
圖12 動態(tài)障礙物AUV避障策略圖
圖13 AUV正面障礙物避障策略圖
本文基于NSB法,研究分析了AUV對于動靜態(tài)障礙物的避障問題.對于多任務(wù)目標(biāo)控制,NSB法能夠有效避免各任務(wù)間沖突,出色完成避障任務(wù).在介紹NSB法的基礎(chǔ)上,針對AUV在駛向目標(biāo)點過程中可能遇到動態(tài)和靜態(tài)障礙物的情形,設(shè)計了不同的避障策略,把總體任務(wù)分解為不同等級的任務(wù)控制目標(biāo),建立了相應(yīng)的輸出函數(shù),得到了AUV整體任務(wù)的函數(shù)輸出模型.對于何時激活子任務(wù),設(shè)置了任務(wù)管理器,以保證各控制目標(biāo)順利實現(xiàn).最后利用AUV模型進(jìn)行了仿真驗證,結(jié)果表明:對于靜態(tài)和動態(tài)障礙物,AUV均能夠順利繞過障礙物最終駛向目標(biāo)點,達(dá)到預(yù)期目標(biāo),驗證了該方法的實用性.
文中所設(shè)計的基于NSB法的AUV避障策略,結(jié)合實驗室現(xiàn)有AUV編隊控制(主從式(L-F)法,圖論法等)的相關(guān)積累,可開展更加深入和多方向的研究,如將該方法運用到編隊避障中,也可將NSB法拓展至三維空間,研究AUV或AUV編隊在三維空間中的避障和路徑規(guī)劃問題.