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

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

(上海交通大學(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ī)劃問題.