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

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

(上海交通大學 海洋智能裝備與系統(tǒng)教育部重點實驗室；海洋工程國家重點實驗室，上海 200240)

自主水下機器人(AUV)是具有智能行為的高級水下機器人，具有活動范圍廣、機動靈活、隱蔽性好等特點，能夠適用于復雜的海底環(huán)境.為了在不可預知的海底環(huán)境中安全完成既定任務，AUV要能夠利用自身攜帶的傳感探測設備(聲吶、攝像頭等)并結合相應的算法進行實時避障.人工勢場法[1-4]、動態(tài)窗口法[5-7]、A*及其改進算法[8-13]、基于行為法[14-18]等，是目前應用較多的避障方法.

基于行為法廣泛應用于諸如移動機器人、無人艇、無人機等的無障礙路徑規(guī)劃領域.無障礙路徑規(guī)劃采用自底向上的系統(tǒng)構建方法，基于行為法將任務目標分解為一系列相互作用的基本子行為，如感知、探測、避障、規(guī)劃，構成并行控制回路，通過協(xié)調(diào)配合后作用于執(zhí)行機構，產(chǎn)生相應的目標任務動作.單個子行為僅需要完成某項簡單任務，能夠對環(huán)境產(chǎn)生快速響應，并且可以靈活地擴展整個系統(tǒng)的能力[19].行為選擇機制主要有行為抑制法、加權平均法和模糊邏輯法[20].其中模糊邏輯法可以實時地調(diào)整任務權值，從而優(yōu)化任務完成的質(zhì)量，因此在實際應用領域很受歡迎.文獻[21]提出的零空間行為(NSB)法即為模糊邏輯法的一種.與其他基于行為的方法相比，NSB法在保持首要任務完全完成的前提下，可以充分利用零空間完成其他次要任務，具有實時性強、各子任務間零沖突的優(yōu)點.

本文采用NSB法，針對AUV在復雜海底環(huán)境中對靜態(tài)和動態(tài)障礙物的規(guī)避進行研究.在按需對整體任務進行分解的基礎上，建立不同優(yōu)先級的子任務函數(shù)，并對不同障礙物的避障策略進行分析.

1 NSB法

NSB法首先將整體任務，即駛向目標點，分解成多個各自獨立控制目標的子任務，并分別列出各子任務函數(shù)，每個子任務函數(shù)都能完成對應的控制目標；然后將子任務分成不同的優(yōu)先級，將由低優(yōu)先級的任務向量向高優(yōu)先級任務向量的零空間投影得到任務的整體綜合輸出函數(shù)傳遞給底層的執(zhí)行機構以控制AUV的動作.設：η=[xyψ]T為AUV慣性坐標系下的位置和艏向角；ν=[uvr]T為AUV隨體坐標系下的速度和轉艏角速度(u為縱向速度，v為橫向速度，r為轉艏角速度)；σ為控制目標的任務變量，其函數(shù)模型為

σ=f(η)

(1)

將σ對η求導可得

(2)

式中：J(η)為η的輸出雅可比矩陣.

速度矩陣的最小二乘解為

(3)

用i表示任務的優(yōu)先級(i=1時，任務優(yōu)先級最高)，第i個任務的速度輸出可表示為

(4)

第i個任務的向較高一級任務零空間的投影為可表示為

(5)

式中：I為單位矩陣.

由低等級的任務向較高一級的任務空間投影，可得出整體控制目標的速度輸出為

(6)

當有3個不同等級任務時，綜合任務的速度輸出可表示為

vd=v1+N1(v2+N2v3)

(7)

以兩個任務為例，其綜合輸出速度的幾何模型如圖1所示.v2首先向v1的零空間投影，進而得到vd=v1+N1v2，vd為任務速度v1和任務速度v2的NSB輸出.

圖1 兩任務時綜合輸出速度的幾何模型

2 避障策略

本文主要針對AUV對平面內(nèi)不同類型障礙物的避障策略進行分析討論.為便于分析模擬，根據(jù)實際情況和AUV繞過障礙物的方向，假設：

(1)障礙物無法與AUV進行通信且不會對AUV進行主動規(guī)避.

(2)AUV可以探測到相對于動靜態(tài)障礙物的運動速度和方位角，選擇逆時針或者順時針方向繞過障礙.

(3)AUV可以探測到當前時刻動態(tài)障礙物的速度，但是無法預知其下一時刻速度，因此假設動態(tài)障礙物的運動速度是固定的.

根據(jù)假設(1)，當AUV利用自身攜帶的傳感器(聲吶、攝像頭等)探測到障礙物時，需根據(jù)障礙物類型進行主動規(guī)避.單個靜態(tài)障礙物類型相對簡單，文中不再贅述.針對動態(tài)障礙物，可分為正面障礙物(見圖2)和側面障礙物(見圖3)兩大類.對于正面障礙物，AUV可以選擇順時針或逆時針方向進行規(guī)避(本文設為逆時針規(guī)避)；對于側面障礙物，AUV需要判斷自身與障礙物的相對運動方向和方位，選擇合理的轉艏方向，從而順利規(guī)避障礙物.

圖2 正面障礙物

圖3 側面障礙物

根據(jù)假設(2)，首先將障礙物的速度轉換到AUV隨體坐標系上:

(8)

式中：上標b和n分別表示隨體坐標系和慣性坐標系；R(θ)為轉換矩陣；θ為轉換角.

AUV在隨體坐標系下相對障礙物運動方向為

(9)

由于β(見圖4，圖中vo和vv分別為障礙物和AUV的速度)的取值范圍為-180°<β<180°，而常規(guī)的四象限定義0°<β<360°，所以當β<0°時，β需在原來的基礎上加360°，從而滿足要求.AUV根據(jù)β所在的象限，選擇繞過障礙物的方向，當β在第 I 和第 II 象限時，選擇順時針方向，當β在第 III 和第 IV 象限時，選擇逆時針方向.在此情形下，AUV從障礙物后面繞過且兩者方向沒有交叉，因此路徑是安全的.

圖4 β角象限示意圖

文中設定當AUV運動方向相對于障礙物的運動方向角度的絕對值小于20° 時為正面障礙物，此時|β-180°|≤20°.

2.1 障礙物探測

AUV和障礙物的路徑V(t)和O(t)分別表示為

(10)

(11)

式中：θvt為AUV到目標點的方向角；θo為移動障礙物的方向角；t為時間變量；pv,x、pv,y和po,x、po,y分別為當前時刻AUV和障礙物的位置，兩者之間的距離為

(12)

首先當AUV探測到障礙物時，啟動避障算法.AUV根據(jù)當前時刻自身和障礙物的速率以及相對方向角等信息，計算下一時刻兩者的相對位置.根據(jù)假設(3)，當f(t)進入到障礙物的安全范圍內(nèi)時，AUV開啟避障子任務.根據(jù)迭代信息和NSB算法，AUV判斷是否已經(jīng)安全繞過障礙物.如圖5所示，當AUV在位置a時，探測到障礙物，同時估算路徑并啟動避障算法，此后則一直進行探測和位置的迭代更新；在位置b時，AUV根據(jù)實時更新的障礙物相對位置和方位角，再次進行路徑估算，判斷路徑是否安全；在位置c時，確認路徑安全，避障算法停止，AUV駛向設定目標點.

圖5 AUV避障過程

2.2 任務分解

為了使基于NSB法的避障策略能夠處理靜態(tài)和動態(tài)障礙物，并根據(jù)前文所述的避障策略選擇繞過障礙物的方向，按照不同優(yōu)先級將避障任務分解為任務1(避障)、任務2(繞過障礙物)、任務3(駛向目標點)3個子任務，同時根據(jù)AUV與障礙物之間的相對距離將障礙物周圍分為安全區(qū)、常規(guī)避障區(qū)和緊急避障區(qū)3個區(qū)域(見圖6).在安全區(qū)內(nèi)，認為障礙物對AUV無威脅，AUV正常駛向目標點，即為任務3；在常規(guī)避障區(qū)內(nèi)，AUV探測到障礙物，啟動任務2，關閉任務1、3，以便AUV選擇繞過障礙物的方向和虛擬切點位置；在緊急避障區(qū)內(nèi)未繞過障礙物前，AUV啟動任務1、2，關閉任務3，確保AUV以最佳路徑順利繞過障礙物；確認繞過障礙物后，在避障區(qū)內(nèi)任務1、3同時啟動，關閉任務2；駛出避障區(qū)后，關閉任務1、2，開啟任務3，AUV重新規(guī)劃路線，選擇最短路徑駛向目標點.

圖6 不同避障區(qū)

任務1避障.在多控制目標任務中，避障任務作為最高優(yōu)先級，任務變量σ1表示AUV的位置Pv到障礙物中心Po的距離：

(13)

σ1,d為障礙物的安全閾值(安全距離)：

σ1,d=d

(14)

對應的雅可比矩陣為

(15)

任務1的輸出速度為

(16)

式中：λ1為任務1的增益矩陣.其零空間向量為

(17)

任務2繞過障礙物.作為第2優(yōu)先級的控制目標任務2，引導AUV按照順時針或者逆時針方向繞過障礙物，其任務變量為

(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平面的夾角.

設二維轉換矩陣R(θ)為

(27)

則切點t1為

(28)

式中：dt為當前時刻AUV距離障礙物的距離.

同理，當AUV逆時針繞過障礙物時，

θt2=θt2,2-θt2,1

(29)

切點t2為

(30)

任務2的變量為一條直線，其雅可比矩陣為

提高高速公路工程建設質(zhì)量具有重要意義，對此公路建設管理工作必須在創(chuàng)新驅動下積極開展創(chuàng)新活動，從管理思路、管理方法、技術標準等多個環(huán)節(jié)開展創(chuàng)新活動，積極引入新技術、新方法，從管理制度開始調(diào)整，重視科技創(chuàng)新工作，多種方法共同使用，以提高高速公路建設管理水平。

J2=I

(31)

輸出速度為

v2=λ2(to-Pv)

(32)

式中：λ2為任務2的增益矩陣.

那么零空間N2=0，任何低級任務的輸出速度投影到該任務空間，投影都為0.考慮到任務2和任務3不可能同時激活，因此該零空間亦不會使用.

任務3駛向目標點.在本文中最低優(yōu)先級為駛向目標點的控制目標任務3，在AUV避障任務完成前后，確保AUV沿最短路徑到達目標點.考慮到AUV沿直線到達目標，其任務變量為

同任務2類比，其雅可比矩陣、輸出速度和零空間分別為

式中：Pt為目標點位置；λ3為任務3的增益矩陣.

根據(jù)式(6)，AUV的綜合任務速度輸出為

vd=v1+N1v2+N1N2v3

(38)

為了保證避障任務的順利完成，本文設計了適用于NSB法的任務管理器(見圖8和9).任務管理器根據(jù)AUV探測到的障礙物和避障算法，決定何時激活和抑制對應的任務.特別需要注意的是，當任務1未激活時，任務2、3無需向該任務零空間進行投影.

圖8 NSB任務管理器控制邏輯圖

圖9 NSB任務管理器工作示意圖

3 模擬分析

為更加有效地闡述NSB法的避障效果，將基于行為法中傳統(tǒng)的行為抑制法和加權平均法的避障效果與NSB法相比較，避障效果如圖10所示.設AUV的初始狀態(tài)、目標點坐標和障礙物位置信息分別為

ηs=[0 m 0 m 0°]T

ηt=[70 m 70 m -]T

圖10 不同方法避障效果對比圖

其中前兩個元素分別為障礙物的橫縱坐標位置信息，最后一個元素為障礙物安全半徑.從圖中可以看出，對于靜態(tài)障礙物，相對于行為抑制法和加權平均法的避障，基于NSB法的避障路徑?jīng)]有突入障礙物的安全范圍，并且避障效果良好.說明NSB法按照設計方案，將低優(yōu)先級的駛向目標點任務向較高優(yōu)先級的避障任務的零空間投影，能夠較好地處理不同優(yōu)先級任務之間的沖突，并且最終圓滿完成任務.而行為抑制法和加權平均法的避障路徑中，有部分路徑突入了障礙物的安全范圍.如果障礙物的安全半徑設置較小，AUV在駛向目標點的過程中很有可能與障礙物發(fā)生碰撞，這種情況一旦發(fā)生，不僅無法按預期完成任務，而且還會對AUV的自身安全造成難以預料的影響.合理推測，當面對動態(tài)障礙物時，行為抑制法和加權平均法的避障效果相對會更差.綜合對比說明，NSB法相對于行為抑制法和加權平均法，有一定的優(yōu)越性和實用性.在以上分析的基礎上，以3種情況為例繼續(xù)展開基于NSB法的避障效果仿真計算.

情況1首先設置4個靜態(tài)障礙物，其中兩個在AUV的路徑范圍內(nèi).因此，AUV在駛向目標點的過程中，必須繞過這兩個障礙物，同時需要避免另外兩個障礙物的影響.

設AUV的初始狀態(tài)和目標點位置分別為

ηs=[5 m 7 m -57.32°]T

ηt=[160 m 130 m -]T

4個障礙物的位置坐標和安全半徑分別為

模擬結果如圖11所示.

圖11 靜態(tài)障礙物AUV避障策略圖

如圖11(a)所示，AUV面對靜態(tài)障礙物時能夠及時避開障礙物并駛向目標點；如圖11(b)所示，在30.5 s時AUV探測到障礙物1，啟動避障算法，逆時針繞過障礙物；如圖11(c)所示，在98.7 s時AUV已經(jīng)繞過障礙物1繼續(xù)駛向目標點，并探測到障礙物2的存在.相對于從障礙物2和4之間穿過，AUV從障礙物2和3之間穿過路徑較短，因此AUV選擇按順針方向繞過障礙物2；如圖11(d)所示，在140.9 s時AUV按照避障算法安全繞過障礙物2，駛向既定目標點，基于NSB的避障算法對靜態(tài)障礙物避障效果良好.

情況2對于動態(tài)障礙物，本文設置了2個沿不同方向逼近AUV的動態(tài)障礙物，以考察NSB法對動態(tài)障礙物的應對能力.AUV在駛向目標點的過程中，需要實時探測相對于障礙物的距離和行駛方向，選擇避開障礙物的最佳路線.

設AUV的初始狀態(tài)和目標點位置分別為

η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

模擬結果如圖12所示.

圖12表明AUV基于NSB法的避障策略對于動態(tài)障礙物同樣有效.如圖12(a)所示為AUV在駛向目標點的過程中發(fā)現(xiàn)動態(tài)障礙物、繞過動態(tài)障礙物并最終駛向目標點的完整路線圖.如圖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ù)駛向目標點；圖12(f)所示為AUV駛向目標點.

情況3當障礙物沿AUV正面運動方向相向運動時，被視為正面障礙物.當AUV在駛向目標點的過程中遇到正面障礙物時，需要判斷相對于障礙物的行駛角度，并及時選擇避開障礙物的方向(逆時針或順時針).

設AUV的初始狀態(tài)和目標點位置分別為

η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

模擬結果圖13所示.

圖13(a)所示為遇到正面障礙物時AUV的避障軌跡圖；如圖13(b)所示，在43.7 s時AUV探測到障礙物，并根據(jù)相對行駛方向判斷為正面障礙物，同時啟動避障算法；如圖13(c)所示，在68.4 s時AUV繞過正面障礙物；如圖13(d)所示，在82.9 s時AUV逆時針繞過障礙物后繼續(xù)駛向目標點，驗證了NSB算法對正面障礙物避障的可靠性.

圖12 動態(tài)障礙物AUV避障策略圖

圖13 AUV正面障礙物避障策略圖

4 結語

本文基于NSB法，研究分析了AUV對于動靜態(tài)障礙物的避障問題.對于多任務目標控制，NSB法能夠有效避免各任務間沖突，出色完成避障任務.在介紹NSB法的基礎上，針對AUV在駛向目標點過程中可能遇到動態(tài)和靜態(tài)障礙物的情形，設計了不同的避障策略，把總體任務分解為不同等級的任務控制目標，建立了相應的輸出函數(shù)，得到了AUV整體任務的函數(shù)輸出模型.對于何時激活子任務，設置了任務管理器，以保證各控制目標順利實現(xiàn).最后利用AUV模型進行了仿真驗證，結果表明：對于靜態(tài)和動態(tài)障礙物，AUV均能夠順利繞過障礙物最終駛向目標點，達到預期目標，驗證了該方法的實用性.

文中所設計的基于NSB法的AUV避障策略，結合實驗室現(xiàn)有AUV編隊控制(主從式(L-F)法，圖論法等)的相關積累，可開展更加深入和多方向的研究，如將該方法運用到編隊避障中，也可將NSB法拓展至三維空間，研究AUV或AUV編隊在三維空間中的避障和路徑規(guī)劃問題.