国产日韩欧美一区二区三区三州_亚洲少妇熟女av_久久久久亚洲av国产精品_波多野结衣网站一区二区_亚洲欧美色片在线91_国产亚洲精品精品国产优播av_日本一区二区三区波多野结衣 _久久国产av不卡

?

基于零空間行為法的自主水下機(jī)器人避障策略

2020-04-08 13:37:08龐師坤梁曉鋒李英輝
上海交通大學(xué)學(xué)報 2020年3期
關(guān)鍵詞:控制目標(biāo)障礙物動態(tài)

龐師坤, 梁曉鋒, 李英輝, 易 宏

(上海交通大學(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)行分析.

1 NSB法

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ù)時綜合輸出速度的幾何模型

2 避障策略

本文主要針對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°.

2.1 障礙物探測

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避障過程

2.2 任務(wù)分解

為了使基于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ù)管理器工作示意圖

3 模擬分析

為更加有效地闡述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正面障礙物避障策略圖

4 結(jié)語

本文基于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ī)劃問題.

猜你喜歡
控制目標(biāo)障礙物動態(tài)
國內(nèi)動態(tài)
國內(nèi)動態(tài)
國內(nèi)動態(tài)
高低翻越
SelTrac?CBTC系統(tǒng)中非通信障礙物的設(shè)計和處理
動態(tài)
實現(xiàn)工程造價控制目標(biāo)的全方位管理方法
山東冶金(2019年2期)2019-05-11 09:12:18
血糖控制目標(biāo)應(yīng)“因人而異”
土釘墻在近障礙物的地下車行通道工程中的應(yīng)用
電驅(qū)動車輛動力學(xué)綜合控制目標(biāo)優(yōu)化研究
青川县| 会宁县| 磐石市| 察雅县| 白水县| 蒲城县| 平邑县| 台州市| 康乐县| 郧西县| 孟连| 山西省| 中方县| 泸定县| 天津市| 石台县| 安宁市| 西城区| 张家川| 张北县| 弋阳县| 临安市| 勃利县| 莱阳市| 监利县| 鄂伦春自治旗| 武功县| 元阳县| 崇信县| 永宁县| 宝丰县| 苏州市| 吴旗县| 长乐市| 固安县| 孟津县| 清河县| 双桥区| 措美县| 石河子市| 虹口区|