新聞資訊

聯(lián)系方式

青島恒昌機器人科技有限公司
地址：青島市黃島區臨港工業(yè)園陡樓山路367號手機：13806390681
服務(wù)熱線(xiàn)：0532-86131102
郵箱：qdhengchangkeji@163.com
m.jinluosha.com

新聞資訊

當前位置：網(wǎng)站首頁(yè) > 新聞資訊

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

摘要：

針對自主水下機器人(AUV)在復雜海底環(huán)境駛向目標點(diǎn)的過(guò)程中可能遇到動(dòng)靜態(tài)障礙物的問(wèn)題,設計了基于零空間行為(NSB)法的避障策略.首先將AUV駛向目標點(diǎn)的整體任務(wù)分解成不同的子任務(wù),并把避障子任務(wù)設為高優(yōu)先級任務(wù);對于多任務(wù)控制目標,將由低優(yōu)先級任務(wù)向量向高優(yōu)先級任務(wù)向量的零空間進(jìn)行投影得到的綜合任務(wù)向量作為終輸出函數;在完成高優(yōu)先級任務(wù)的同時(shí),部分或全部完成低優(yōu)先級任務(wù),以避免各任務(wù)目標的相互沖突.隨后根據不同子任務(wù)的優(yōu)先級設計相應的任務(wù)函數,研究了針對靜態(tài)和動(dòng)態(tài)障礙物的避障策略;推導并建立了AUV運動(dòng)的綜合輸出函數,以確保其在駛向目標點(diǎn)的過(guò)程中對不同類(lèi)型障礙物進(jìn)行有效規避.模擬計算結果證明該方法是有效的和可行的,在復雜障礙物環(huán)境中能夠達到預期的避障效果.

關(guān)鍵詞：

零空間行為法; 自主水下機器人; 避障;

作者簡(jiǎn)介： 龐師坤(1987-),男,安徽省宿州市人,博士生,主要從事水下機器人控制研究.; *梁曉鋒,男,副研究員,電話(huà)(Tel.):021-34207165;E-mail:liang_xiaofeng@sjtu.edu.cn.;

收稿日期：2018-10-18

Collision Avoidance Strategy for Autonomous Underwater Vehicle Based on Null-Space-Based Behavioral Approach

PANG Shikun LIANG Xiaofeng LI Yinghui YI Hong

Key Laboratory of Marine Intelligent Equipment and System of Ministry of Education;State Key Laboratory of Ocean Engineering

Abstract：

An autonomous underwater vehicle(AUV) obstacle avoidance strategy based on null-space-based behavioral(NSB) approach is designed, aiming at solving dynamic or static obstacle avoidance problem the AUV will encounter while moving to the target in complex underwater environment. Firstly, the AUV overall task moving to the target is decomposed into different subtasks, and the obstacle avoidance subtask is set as the highest priority. As for multi-task control targets, the low-level task vector is projected to the null space of the higher task vector, and the integrated task output is used as the final output function. The low-level task is partially or completely completed while completing the higher task, thereby the mutual conflict between different level targets can be avoided in this way. In order to study the obstacle avoidance strategy for static and dynamic obstacles, the corresponding task functions are designed in accordance with different subtask priorities. The comprehensive output function of AUV motion is deduced and established to ensure that it can avoid different types of obstacles effectively in the process of heading to the target point. The simulation results demonstrate the effectiveness and the feasibility of the proposed method, which could achieve an expected obstacle avoidance effect in complex underwater obstacle environments.

Keyword：

null-space-based behavioral(NSB) approach; autonomous underwater vehicle(AUV); obstacle avoidance;

Received： 2018-10-18

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

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

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

1 NSB法

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

σ=f(η) (1)

將σ對η求導可得

$σ^{'} = \frac{\partial f (η)}{\partial η} = J (η) v (2)$

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

速度矩陣的小二乘解為

$v_{d} = J^{†} (σ^{'}_{d} + λ \tilde{σ}) (3)$

式中:J?=JT(JJT)-1;σd為任務(wù)期望值; $\tilde{σ} = σ_{d} - σ$ ;λ>0為增益矩陣.式(3)表示單個(gè)任務(wù)的速度輸出.

用i表示任務(wù)的優(yōu)先級(i=1時(shí),任務(wù)優(yōu)先級高),第i個(gè)任務(wù)的速度輸出可表示為

$v_{i, d} = J_{i}^{†} (σ^{'}_{i, d} + λ \tilde{σ}_{i}) (4)$

第i個(gè)任務(wù)的向較高一級任務(wù)零空間的投影為可表示為

$Ν_{i} (J) = Ι - J_{i}^{†} J_{i} (5)$

式中:I為單位矩陣.

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

$v_{d} = v_{1} + \sum_{i = 2}^{n} Ν_{i} (J) v_{i} (6)$

當有3個(gè)不同等級任務(wù)時(shí),綜合任務(wù)的速度輸出可表示為

$v_{d} = v_{1} + Ν_{1} (v_{2} + Ν_{2} v_{3}) (7)$

以?xún)蓚€(gè)任務(wù)為例,其綜合輸出速度的幾何模型如圖1所示.v2首先向v1的零空間投影,進(jìn)而得到vd=v1+N1v2,vd為任務(wù)速度v1和任務(wù)速度v2的NSB輸出.

圖1 兩任務(wù)時(shí)綜合輸出速度的幾何模型下載原圖

Fig.1 Geometrical model of vd for a two-task example

2 避障策略

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

(1) 障礙物無(wú)法與AUV進(jìn)行通信且不會(huì )對AUV進(jìn)行主動(dòng)規避.

(2) AUV可以探測到相對于動(dòng)靜態(tài)障礙物的運動(dòng)速度和方位角,選擇逆時(shí)針或者順時(shí)針?lè )较蚶@過(guò)障礙.

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

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

圖2 正面障礙物下載原圖

Fig.2 Head-on obstacle

圖3 側面障礙物下載原圖

Fig.3 Lateral obstacle

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

$v_{o}^{b} = R (θ) v_{o}^{n} (8)$

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

AUV在隨體坐標系下相對障礙物運動(dòng)方向為

$β = \arctan 2 (v_{o, y}^{b}, v_{o, x}^{b}) (9)$

式中:v $_{o, x}^{b}$ 和v $_{o, y}^{b}$ 分別表示AUV在隨體坐標系下相對于障礙物在x和y方向上的分速度.

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

圖4 β角象限示意圖下載原圖

Fig.4 Quadrant diagram of angle β

文中設定當AUV運動(dòng)方向相對于障礙物的運動(dòng)方向角度的絕對值小于20° 時(shí)為正面障礙物,此時(shí) $| β - 180 ° | \leq 20 ° .$

2.1 障礙物探測

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

$\begin{array}{l} V (t) = [\begin{matrix} x_{v} (t) \\ y_{v} (t) \end{matrix}] = \\ [\begin{matrix} | v_{v} | t \cos θ_{v t} + p_{v, x} \\ | v_{v} | t \sin θ_{v t} + p_{v, y} \end{matrix}] (10) \\ Ο (t) = [\begin{matrix} x_{o} (t) \\ y_{o} (t) \end{matrix}] = \\ [\begin{matrix} | v_{o} | t \cos θ_{o} + p_{o, x} \\ | v_{o} | t \sin θ_{o} + p_{o, y} \end{matrix}] (11) \end{array}$

式中:θvt為AUV到目標點(diǎn)的方向角;θo為移動(dòng)障礙物的方向角;t為時(shí)間變量;pv,x、pv,y和po,x、po,y分別為當前時(shí)刻AUV和障礙物的位置,兩者之間的距離為

$f (t) = | V (t) - Ο (t) |_{2} (12)$

式中: $| \cdot |$ 為歐幾里德范數.

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

圖5 AUV避障過(guò)程下載原圖

Fig.5 Process of AUV collision avoidance

2.2 任務(wù)分解

為了使基于NSB法的避障策略能夠處理靜態(tài)和動(dòng)態(tài)障礙物,并根據前文所述的避障策略選擇繞過(guò)障礙物的方向,按照不同優(yōu)先級將避障任務(wù)分解為任務(wù)1(避障)、任務(wù)2(繞過(guò)障礙物)、任務(wù)3(駛向目標點(diǎn))3個(gè)子任務(wù),同時(shí)根據AUV與障礙物之間的相對距離將障礙物周?chē)譃榘踩珔^、常規避障區和緊急避障區3個(gè)區域(見(jiàn)圖6).在安全區內,認為障礙物對AUV無(wú)威脅,AUV正常駛向目標點(diǎn),即為任務(wù)3;在常規避障區內,AUV探測到障礙物,啟動(dòng)任務(wù)2,關(guān)閉任務(wù)1、3,以便AUV選擇繞過(guò)障礙物的方向和虛擬切點(diǎn)位置;在緊急避障區內未繞過(guò)障礙物前,AUV啟動(dòng)任務(wù)1、2,關(guān)閉任務(wù)3,確保AUV以佳路徑順利繞過(guò)障礙物;確認繞過(guò)障礙物后,在避障區內任務(wù)1、3同時(shí)啟動(dòng),關(guān)閉任務(wù)2;駛出避障區后,關(guān)閉任務(wù)1、2,開(kāi)啟任務(wù)3,AUV重新規劃路線(xiàn),選擇短路徑駛向目標點(diǎn).

圖6 不同避障區下載原圖

Fig.6 Different zones of collision avoidance

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

$\begin{array}{l} σ_{1} = | Ρ_{v} - Ρ_{o} |_{1} (13) \end{array}$

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

$\begin{array}{l} σ_{1, d} = d (14) \end{array}$

對應的雅可比矩陣為

$J_{1} = \frac{Ρ_{v} - Ρ_{o}}{| Ρ_{v} - Ρ_{o} |_{1}} = \hat{r}^{Τ} (15)$

式中: $\hat{r}^{Τ}$ 為由AUV指向障礙物的單位向量,當且僅當Pv≠Po時(shí), $\hat{r}^{Τ}$ 存在.

任務(wù)1的輸出速度為

$\begin{array}{l} v_{1} = J_{1}^{†} λ_{1} (d - σ_{1}) (16) \end{array}$

式中:λ1為任務(wù)1的增益矩陣.其零空間向量為

$\begin{array}{l} Ν_{1} = Ι - J_{1}^{†} J_{1} = Ι - \hat{r} \hat{r}^{Τ} (17) \end{array}$

任務(wù)2 繞過(guò)障礙物.作為第2優(yōu)先級的控制目標任務(wù)2,引導AUV按照順時(shí)針或者逆時(shí)針?lè )较蚶@過(guò)障礙物,其任務(wù)變量為

$σ_{2} = | Ρ_{v} |_{1}$ (18)

$σ_{2, d} = | t_{o} |_{1} (19)$

式中:to為障礙物上的虛擬切點(diǎn).

首先定義起始點(diǎn)Ps分別到障礙物中心點(diǎn)Po和切點(diǎn)t1(見(jiàn)圖7)的向量Pso和Pst1:

圖7 虛擬切點(diǎn)示意圖下載原圖

Fig.7 Schematic diagram of virtual point of tangency

Pso=Po-Ps (20)

Pst1=t1-Ps (21)

其長(cháng)度分別為

$\begin{array}{l} d_{s o} = | Ρ_{o} - Ρ_{s} |_{2} (22) \\ d_{s t_{1}} = \sqrt{d_{s o}^{2} - r_{o}^{2}} (23) \end{array}$

式中:ro為障礙物半徑.

然后確定Pst1與xOy平面的夾角θt1:

$\begin{array}{l} θ_{t_{1}} = θ_{t_{1}, 1} + θ_{t_{1}, 2} (24) \\ θ_{t_{1}, 1} = \arcsin \frac{r_{o}}{d_{s o}} (25) \\ θ_{t_{1}, 2} = \arctan 2 (Ρ_{s o, y}, Ρ_{s o, x}) (26) \end{array}$

式中:θt1,1為Pso和Pst1之間的夾角;θt1,2為Pso與xOy平面的夾角.

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

$R (θ) = [\begin{matrix} \cos θ & - \sin θ \\ \sin θ & \cos θ \end{matrix}] (27)$

則切點(diǎn)t1為

$t_{1} = R (θ_{t_{1}}) [\begin{matrix} 1 \\ 0 \end{matrix}] d_{t} + Ρ_{s} (28)$

式中:dt為當前時(shí)刻AUV距離障礙物的距離.

同理,當AUV逆時(shí)針繞過(guò)障礙物時(shí),

$θ_{t_{2}} = θ_{t_{2}, 2} - θ_{t_{2}, 1} (29)$

切點(diǎn)t2為

任務(wù)2的變量為一條直線(xiàn),其雅可比矩陣為

$J_{2} = Ι (31)$

輸出速度為

$v_{2} = λ_{2} (t_{o} - Ρ_{v}) (32)$

式中:λ2為任務(wù)2的增益矩陣.

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

任務(wù)3 駛向目標點(diǎn).在本文中低優(yōu)先級為駛向目標點(diǎn)的控制目標任務(wù)3,在A(yíng)UV避障任務(wù)完成前后,確保AUV沿短路徑到達目標點(diǎn).考慮到AUV沿直線(xiàn)到達目標,其任務(wù)變量為

$\begin{array}{l} σ_{3} = | Ρ_{v} |_{1} (33) \\ σ_{3, d} = | Ρ_{t} |_{1} (34) \end{array}$

同任務(wù)2類(lèi)比,其雅可比矩陣、輸出速度和零空間分別為

$\begin{array}{l} J_{3} = Ι (35) \\ v_{3} = λ_{3} (Ρ_{t} - Ρ_{v}) (36) \\ Ν_{3} = 0 (37) \end{array}$

式中:Pt為目標點(diǎn)位置;λ3為任務(wù)3的增益矩陣.

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

$v_{d} = v_{1} + Ν_{1} v_{2} + Ν_{1} Ν_{2} v_{3} (38)$

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

圖8 NSB任務(wù)管理器控制邏輯圖下載原圖

Fig.8 Control logic diagram of NSB task manager

圖9 NSB任務(wù)管理器工作示意圖下載原圖

Fig.9 Working diagram of NSB task manager

3 模擬分析

本文選取文獻[22]中的AUV模型為算例對象,在以上分析的基礎上,模擬計算NSB法對動(dòng)靜態(tài)障礙物的避障效果.AUV的詳細參數為:m1=1 116 kg,m2=2 133 kg,m3=2 133 kg;阻尼系數 d1=25.5 kg/s,d2=138 kg/s,d3=138 kg/s.

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

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

ηt=[70 m 70 m -]T

P′o=[30 30 15]T m

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

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

Fig.10 Contrast figure of obstacle avoidance effect by different methods

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

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

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

ηt=[160 m 130 m -]T

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

$\begin{array}{l} Ρ^{'}_{o 1} = [\begin{matrix} 50 & 40 & 15 \end{matrix}]^{Τ} m \\ Ρ^{'}_{o 2} = [\begin{matrix} 110 & 70 & 20 \end{matrix}]^{Τ} m \\ Ρ^{'}_{o 3} = [\begin{matrix} 60 & 100 & 18 \end{matrix}]^{Τ} m \\ Ρ^{'}_{o 4} = [\begin{matrix} 120 & 30 & 10 \end{matrix}]^{Τ} m \end{array}$

模擬結果如圖11所示.

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

情況2 對于動(dòng)態(tài)障礙物,本文設置了2個(gè)沿不同方向逼近AUV的動(dòng)態(tài)障礙物,以考察NSB法對動(dòng)態(tài)障礙物的應對能力.AUV在駛向目標點(diǎn)的過(guò)程中,需要實(shí)時(shí)探測相對于障礙物的距離和行駛方向,選擇避開(kāi)障礙物的佳路線(xiàn).

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

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

Fig.11 AUV collision avoidance strategy for static obstacle

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

ηt=[120 m 140 m -]T

兩個(gè)障礙物的位置和安全半徑分別為

$\begin{array}{l} Ρ^{'}_{o 1} = [\begin{matrix} 180 & - 20 & 25 \end{matrix}]^{Τ} m \\ Ρ^{'}_{o 2} = [\begin{matrix} 0 & 80 & 15 \end{matrix}]^{Τ} m \end{array}$

障礙物的運動(dòng)速度分別為

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法的避障策略對于動(dòng)態(tài)障礙物同樣有效.如圖12(a)所示為AUV在駛向目標點(diǎn)的過(guò)程中發(fā)現動(dòng)態(tài)障礙物、繞過(guò)動(dòng)態(tài)障礙物并終駛向目標點(diǎn)的完整路線(xiàn)圖.如圖12(b)所示,在10.7 s時(shí)AUV探測到動(dòng)態(tài)障礙物2,根據障礙物運動(dòng)方向,選擇以順時(shí)針?lè )较蚶@過(guò)障礙物,并在51.7 s時(shí)繞過(guò)障礙物2(見(jiàn)圖12(c));如圖12(d)所示,在63.8 s時(shí)AUV在繼續行駛途中發(fā)現動(dòng)態(tài)障礙物1,同時(shí)啟動(dòng)避障算法;由圖12(e)可以看出,在110.8 s時(shí)AUV以逆時(shí)針?lè )较蚶@過(guò)障礙物1之后繼續駛向目標點(diǎn);圖12(f)所示為AUV駛向目標點(diǎn).

情況3 當障礙物沿AUV正面運動(dòng)方向相向運動(dòng)時(shí),被視為正面障礙物.當AUV在駛向目標點(diǎn)的過(guò)程中遇到正面障礙物時(shí),需要判斷相對于障礙物的行駛角度,并及時(shí)選擇避開(kāi)障礙物的方向(逆時(shí)針或順時(shí)針).

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

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

ηt=[150 m 200 m -]T

障礙物的初始位置和安全半徑為

$Ρ^{'}_{o} = [\begin{matrix} 150 & 200 & 20 \end{matrix}]^{Τ} m$

速度參數為

vo=[-1.0 m/s -1.2 m/s 0°]T

模擬結果圖13所示.

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

圖12 動(dòng)態(tài)障礙物AUV避障策略圖下載原圖

Fig.12 AUV collision avoidance strategy for dynamic obstacle

圖13 AUV正面障礙物避障策略圖下載原圖

Fig.13 AUV collision avoidance strategy for head-on obstacle

4 結語(yǔ)

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

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