網(wǎng)絡(luò)出版地址：http://www.cnki.net/kcms/detail/23.1538.tp.20150601.0940.001.html

實(shí)時(shí)避碰的無(wú)人水面機(jī)器人在線路徑規(guī)劃方法

冷靜1,2，劉健1，徐紅麗1

(1.中國(guó)科學(xué)院 沈陽(yáng)自動(dòng)化研究所，遼寧 沈陽(yáng) 110016；2.中國(guó)科學(xué)院大學(xué)，北京 100049)

摘要：針對(duì)動(dòng)態(tài)環(huán)境下，無(wú)人水面機(jī)器人(USV)必須遵守國(guó)際海上避碰規(guī)則公約且實(shí)時(shí)在線路徑規(guī)劃的難題，提出一種無(wú)人水面機(jī)器人在動(dòng)態(tài)環(huán)境下進(jìn)行在線路徑規(guī)劃的方法。此方法將國(guó)際海上避碰規(guī)則公約融入速度避障法，將速度避障法中的相對(duì)速度線性化，使其能作為約束融入混合整數(shù)線性規(guī)劃器。同時(shí)將USV的本體動(dòng)力學(xué)約束與環(huán)境約束結(jié)合，采用多目標(biāo)函數(shù)作為優(yōu)化函數(shù)，根據(jù)任務(wù)的要求選擇距離優(yōu)化函數(shù)、速度優(yōu)化函數(shù)和遵守COLREGs目標(biāo)函數(shù)等多個(gè)優(yōu)化目標(biāo)函數(shù)。最后，通過(guò)對(duì)USV在2種會(huì)遇情況規(guī)劃路徑的仿真實(shí)驗(yàn)分析，驗(yàn)證了此規(guī)劃算法的有效性。

關(guān)鍵詞：無(wú)人水面機(jī)器人；在線路徑規(guī)劃；速度避障法；國(guó)際海上避碰規(guī)則公約；混合整數(shù)線性規(guī)劃；動(dòng)態(tài)環(huán)境

DOI:10.3969/j.issn.1673-4785.201405012

中圖分類號(hào)：TP242 文獻(xiàn)標(biāo)志碼：A

收稿日期：2014-05-06. 網(wǎng)絡(luò)出版日期：2015-06-01.

基金項(xiàng)目：中國(guó)科學(xué)院重點(diǎn)部署項(xiàng)目子課題(KGFZD-125-014).

作者簡(jiǎn)介：

中文引用格式：冷靜，劉健，徐紅麗. 實(shí)時(shí)避碰的無(wú)人水面機(jī)器人在線路徑規(guī)劃方法[J]. 智能系統(tǒng)學(xué)報(bào), 2015, 10(3): 343-348.

英文引用格式：LENG Jing, LIU Jiang, XU Hongli. Online path planning of an unmanned surface vehicle for real-time collision avoidance[J]. CAAI Transactions on Intelligent Systems, 2015, 10(3): 343-348.

Online path planning of an unmanned surface vehicle for

real-time collision avoidance

LENG Jing1,2, LIU Jiang1, XU Hongli1

(1. Shenyang Institute of Automation, Chinese Academy of Sciences, Shenyang 110016, China; 2. University of Chinese Academy of Sciences, Beijing 100049, China)

Abstract：A new method of online path planning is proposed in order to solve the problem of online real-time path planning of the unmanned surface vehicle (USV) in a dynamic environment by complying with the Convention on the International Regulations for Preventing Collisions at Sea (COLREGs). This method merges the International Regulations for Preventing Collisions at Sea with velocity obstacle (VO) to linearize relative speed of VO and to make VO merge with the mixed-integer linear programming (MILP) as a set of velocity constraints. The USV body dynamics constraints and environmental constraints are combined using multi-objective function as the optimization function and choosing distance, speed and abeyance COLREGS as target functions. Finally, the simulation for the path planning in the two situations of encounter is analyzed and the results show the effectiveness of this planning algorithm.

Keywords：unmanned surface vehicle (USV); online path planning; velocity obstacle; Convention on the International Regulations for Preventing Collisions at Sea (COLREGs); mixed-integer linear programming (MILP); dynamic environment

通信作者：冷靜. E-mail: jingleng77@163.com.

無(wú)人水面機(jī)器人(unmanned surface vehicle, USV)在海上航行時(shí)需要具備自主規(guī)避障礙的能力，在線路徑規(guī)劃提供了一種遠(yuǎn)程障礙規(guī)避的手段。在線路徑規(guī)劃是指基于雷達(dá)獲得的船舶、島嶼等障礙信息，實(shí)時(shí)規(guī)劃自身的速度和航向以產(chǎn)生避開(kāi)障礙、到達(dá)目標(biāo)的最優(yōu)或次最優(yōu)路徑(無(wú)障礙物路徑)。根據(jù)對(duì)環(huán)境的獲知程度，路徑規(guī)劃分為全局路徑規(guī)劃和局部路徑規(guī)劃，在線路徑規(guī)劃是一種局部路徑規(guī)劃，因?yàn)椴糠汁h(huán)境是未知的。

無(wú)人水面機(jī)器人在海洋中在線路徑規(guī)劃的特點(diǎn)是：1)海洋環(huán)境具有動(dòng)態(tài)、突發(fā)性、不可預(yù)測(cè)性。2)USV具有6個(gè)自由度、偏航、前進(jìn)、橫移，也會(huì)出現(xiàn)橫傾、縱傾，體積小、速度快、靈活性高、瞬時(shí)的失誤可能造成無(wú)法彌補(bǔ)的損失等運(yùn)動(dòng)特性。3)USV預(yù)先定義好的航線和駕駛規(guī)則。常用的在線路徑規(guī)劃算法采用局部避障算法結(jié)合全局規(guī)劃算法，如障礙物邊界追蹤結(jié)合融入A*搜索算法[1]。然而這種方法并沒(méi)有考慮國(guó)際海上避碰規(guī)則公約(convention on the international regulations for preventing collisions at sea, COLREGs)[2]。COLREGs指出了當(dāng)存在碰撞的危險(xiǎn)時(shí)，應(yīng)該采取的避碰行為。當(dāng)USV與其他船舶相遇時(shí)，其導(dǎo)航算法必須遵守COLREGs，才能安全躲避其他船舶，這樣其他船舶上的駕駛?cè)司湍茴A(yù)測(cè)它所采取的安全行為。許多學(xué)者提出了遵守COLREGs的導(dǎo)航方法，如模糊邏輯[3]、區(qū)間優(yōu)化[4]。然而這些算法對(duì)多個(gè)COLREGs規(guī)則的應(yīng)用不是很好，而且在實(shí)時(shí)性需求方面并不能達(dá)到理想。Yoshiaki等[5]將COLREGs規(guī)定的可行速度區(qū)與速度障礙區(qū)同時(shí)作為約束條件，以交叉檢測(cè)的碰撞時(shí)間和下個(gè)路徑點(diǎn)的期望速度作為代價(jià)函數(shù)，在速度-航向(v-θ)空間搜索最好的速度矢量。這種方法能快速有效地避開(kāi)障礙物，然而他們也指出，使用的速度避障法只是一種局部規(guī)劃器，要想實(shí)現(xiàn)遠(yuǎn)程距離的路徑規(guī)劃必須加入全局規(guī)劃器。COLREGs本身就包含有避碰行動(dòng)信息，與速度避障法能非常好的融合，同時(shí)使速度避障法更好地運(yùn)用于USV與船舶會(huì)遇的避碰行動(dòng)中，使避碰行動(dòng)完全合理合法。祖迪等[6]采用MILP (mixed-integer linear programming)進(jìn)行移動(dòng)機(jī)器人的路徑規(guī)劃，將移動(dòng)機(jī)器人的本體動(dòng)力學(xué)和速度避障法角度約束結(jié)合，取得比較好的效果。

本文對(duì)USV在動(dòng)態(tài)環(huán)境下進(jìn)行在線路徑規(guī)劃進(jìn)行研究，采用基于速度避障法的局部規(guī)劃器規(guī)避障礙，基于MILP的全局規(guī)劃器實(shí)現(xiàn)距離時(shí)間的優(yōu)化，使規(guī)劃的路徑既能遵守COLREGs規(guī)則，安全地避開(kāi)障礙，同時(shí)獲得全局路徑優(yōu)化。

1改進(jìn)速度避障法

1.1問(wèn)題描述

本文研究的USV在線路徑規(guī)劃問(wèn)題如下：

1)目標(biāo)：1個(gè)遠(yuǎn)程靜態(tài)路徑點(diǎn)。

2)障礙物：視一定范圍內(nèi)的水面船舶為運(yùn)動(dòng)障礙物，通過(guò)傳感器獲得其速度、航向和位置。

3)USV運(yùn)動(dòng)模型：包含運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)，應(yīng)用其在仿真中可實(shí)時(shí)模擬USV的運(yùn)動(dòng)狀態(tài)。

4)假設(shè)條件：因?yàn)榇w本身具有的慣性比較大，所以假設(shè)極短的規(guī)劃時(shí)間Δt內(nèi)，障礙物保持勻速運(yùn)動(dòng)，即USV相對(duì)運(yùn)動(dòng)障礙物的速度改變量等于USV自身的速度改變量。

由此，在線路徑規(guī)劃的任務(wù)即是在每個(gè)規(guī)劃時(shí)間間隔搜索出最優(yōu)的速度矢量增量，使其既能遵守COLREGs規(guī)則躲避運(yùn)動(dòng)的障礙物，同時(shí)能搜索到達(dá)目標(biāo)的全局最優(yōu)或次最優(yōu)(無(wú)碰撞)的優(yōu)化路徑。

1.2融入COLREGs規(guī)則的改進(jìn)速度避障法

圖1　速度避障法二維原理圖 Fig. 1　Schematics of velocity obstacle

1.2.1相關(guān)定義

1)膨脹半徑：R=Vr/γ，碰撞半徑等于會(huì)遇船舶的安全距離。安全距離取決于相對(duì)速度和轉(zhuǎn)向率,見(jiàn)文獻(xiàn)[8]。

2)避碰區(qū)(collision cone, CC)：

即所有導(dǎo)致USV與障礙物碰撞的相對(duì)速度vUO，它本質(zhì)為相對(duì)速度的速度集。

3)避碰角：

(1)

即在規(guī)劃時(shí)間內(nèi)，相對(duì)速度夾角的改變量。本文避碰策略是首先將相對(duì)速度增量線性化為避碰角，然后通過(guò)避碰角選擇方向來(lái)脫離避碰區(qū)，最終實(shí)現(xiàn)避碰過(guò)程。式(1)的線性化推導(dǎo)流程見(jiàn)文獻(xiàn)[9]。

5)碰撞危險(xiǎn)度：ρ=(a×DCPA)2+(b×TCPA)2，其中a, b分別表示DCPA和TCPA的權(quán)值。碰撞危險(xiǎn)度通過(guò)DCPA和TCPA加權(quán)確定。

6)規(guī)劃時(shí)間：T=w1ρ+w2/τ，即規(guī)劃時(shí)間與碰撞危險(xiǎn)度成正比，與碰撞時(shí)間成反比。因?yàn)槭艿経SV本體運(yùn)動(dòng)性能約束，其轉(zhuǎn)角加速度和縱向加速度都有限制，所以碰撞危險(xiǎn)度越大，碰撞時(shí)間越短，規(guī)劃時(shí)間就越短，否則無(wú)法及時(shí)進(jìn)行航速和方向的規(guī)劃。

1.2.2避碰策略

2混合線性規(guī)劃方法

速度避障法解決的是局部實(shí)時(shí)避碰的問(wèn)題，但容易陷入局部最小值，所以在對(duì)USV進(jìn)行在線路徑規(guī)劃的時(shí)候，必須加入全局規(guī)劃器。在基本LP中，各約束條件之間形成“與”的邏輯關(guān)系，對(duì)于具有復(fù)雜邏輯關(guān)系的優(yōu)化問(wèn)題不能直接求解。用MILP方法解決動(dòng)態(tài)環(huán)境下的路徑規(guī)劃問(wèn)題，可以很容易地把機(jī)器人本體動(dòng)力學(xué)、運(yùn)動(dòng)學(xué)以及環(huán)境不確定性等約束考慮進(jìn)去，并且能解決障礙物轉(zhuǎn)向的“或”的邏輯問(wèn)題，得到一條關(guān)于目標(biāo)函數(shù)最優(yōu)且滿足所有約束條件的最優(yōu)路徑。由于MILP要求目標(biāo)函數(shù)和約束均為線性，因此將這些約束和目標(biāo)函數(shù)線性化。

2.1目標(biāo)函數(shù)

1)第1類目標(biāo)函數(shù)：距離收斂。

USV通過(guò)調(diào)整速度大小和方向，每規(guī)劃一個(gè)時(shí)間間隔都使下一時(shí)刻USV離目標(biāo)越來(lái)越近，即

(2)

式中：j表示維數(shù)，即將到目標(biāo)的距離分解為X、Y軸，這樣才可以用于線性規(guī)劃器中。Δvj是所要規(guī)劃的變量，即為每個(gè)規(guī)劃時(shí)間，相對(duì)目標(biāo)的速度改變量。

2)第2類目標(biāo)函數(shù)：速度最優(yōu)。

將相對(duì)目標(biāo)的速度分解為指向目標(biāo)的分量VC和垂直目標(biāo)的分量VT。要想使到達(dá)障礙物的速度最優(yōu)，只需最大化VC和最小化VT即可。即

(3)

由泰勒公式可得

min:g+Δg

因此推理公式(3)轉(zhuǎn)化為

(4)

綜上所述可得線性規(guī)劃器的目標(biāo)函數(shù):

式中:m1、m2、m3、m4分別代表上述4個(gè)目標(biāo)函數(shù)的權(quán)值。

2.2USV本體動(dòng)力學(xué)自身約束

1)加速度的約束：-ΔmaxΔvjΔmax。

2)速度的約束： -VmaxΔvj+vjVmax。

因?yàn)榫€性規(guī)劃只有一個(gè)上下限，所以速度變量的上下限整合為

2.3障礙物約束

根據(jù)上一節(jié)對(duì)速度避障法的避碰策略可知：

(5)

式中：ei+fi=1，ei和fi為二進(jìn)制變量(取0或1)，ξ為正實(shí)數(shù)，且遠(yuǎn)大于式(5)中不等式左側(cè)可取的數(shù)值。

式(5)確保在同一規(guī)劃時(shí)間段[tk,tk+Δt]之內(nèi)，有且僅有1組(前2個(gè)或后2個(gè))約束成立。此處，為了遵守COLREGs規(guī)則，當(dāng)右交叉相遇和對(duì)遇時(shí)，ei=0,fi=1,式(5)中前2不等式被激活，后2個(gè)不等式失效；當(dāng)左交叉相遇時(shí)，ei=1,fi=0，前2個(gè)不等式失效，后2個(gè)不等式被激活。

2.4算法流程

總結(jié)上述體系結(jié)構(gòu)、并融合COLREGs的在線路徑規(guī)劃流程圖如圖2。具體的算法流程如下：

1)通過(guò)APAR雷達(dá)獲得自身的經(jīng)緯度和航向，通過(guò)AIS接收器獲得對(duì)方船舶的距離和航向。

2)通過(guò)公式計(jì)算出每艘船舶與USV之間的最近會(huì)遇時(shí)間TCPA和最近會(huì)遇距離DCPA。

3)通過(guò)危險(xiǎn)度判斷模型判斷是否處于危險(xiǎn)狀態(tài)。如果危險(xiǎn)則開(kāi)啟避碰行為進(jìn)行第4)步，否則不實(shí)施避碰行為。

4)進(jìn)行船舶與USV的會(huì)遇局面劃分及行為決策。

5)對(duì)本體運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)、探測(cè)范圍、障礙物約束和目標(biāo)函數(shù)進(jìn)行建模。

6)將約束的建模加入MILP規(guī)劃器中，由規(guī)劃器可獲得這個(gè)規(guī)劃間隔時(shí)間里USV期望的轉(zhuǎn)向角及速度。

7)將期望的航向角和速度輸入到航行控制器中，通過(guò)航行控制器輸出發(fā)動(dòng)機(jī)的轉(zhuǎn)速和舵角作用于USV實(shí)際模型中，使USV能達(dá)到期望的航向和航速。

8)檢測(cè)是否達(dá)到目標(biāo)，如果達(dá)到則結(jié)束，否則進(jìn)行下一個(gè)規(guī)劃周期，重復(fù)步驟1)~8)，直到目標(biāo)到達(dá)才結(jié)束。

圖2　在線路徑規(guī)劃流程 Fig. 2　Flowchart of online path planning

3仿真驗(yàn)證

為了說(shuō)明此規(guī)劃方法的有效性，采用實(shí)際的USV動(dòng)力學(xué)模型進(jìn)行實(shí)驗(yàn)仿真，采用的USV模型是Ribcraft模型[10]，其回轉(zhuǎn)速度為15.8 °/s，采用PID控制器對(duì)其進(jìn)行航行控制。下面給出對(duì)遇局面和交叉局面的仿真實(shí)驗(yàn)結(jié)果，如圖3和4。圖3所示為對(duì)遇局面，其初始環(huán)境具體參數(shù)見(jiàn)表1。圖4所示為右交叉局面，其初始環(huán)境具體參數(shù)見(jiàn)表2。

仿真開(kāi)發(fā)環(huán)境在MATLAB環(huán)境下。在仿真中用大圓代表USV，小圓代表運(yùn)動(dòng)障礙物，最上面的圓代表目標(biāo)點(diǎn)。USV通過(guò)雷達(dá)進(jìn)行探測(cè)，最大探測(cè)距離為30 nm。圖3(a)和4(a)圖表示初始狀態(tài)，圖3(b)和4(b)表示USV進(jìn)行實(shí)時(shí)在線路徑規(guī)劃，每一個(gè)時(shí)間段規(guī)劃出最優(yōu)的速度矢量，圖3(c)和4(c)表示最終到達(dá)目標(biāo)點(diǎn)，圖3(d)和4(d)表示在規(guī)劃中的規(guī)劃(期望)與USV實(shí)際的比較。圖3(b)和4(b)中，USV在此種情況下右轉(zhuǎn)向避碰，符合COLREGs規(guī)則的規(guī)定。通過(guò)圓圈稀疏可知當(dāng)危險(xiǎn)解除，規(guī)劃時(shí)間變短，并符合COLREGs規(guī)定的從后方繞過(guò)障礙物的規(guī)定。

表 1　對(duì)遇局面態(tài)勢(shì)仿真環(huán)境

表 2　交叉局面態(tài)勢(shì)仿真環(huán)境

(a)初始會(huì)遇狀態(tài)，發(fā)現(xiàn)障礙物

(b)開(kāi)始轉(zhuǎn)向

(c)到達(dá)目標(biāo)點(diǎn)

(d)規(guī)劃的航向角與實(shí)際的航向角 圖3　對(duì)遇局面態(tài)勢(shì) Fig. 3　Situation of head-on

(a)初始會(huì)遇狀態(tài)，發(fā)現(xiàn)障礙物

(b)開(kāi)始轉(zhuǎn)向

(c)到達(dá)目標(biāo)點(diǎn)

(d)規(guī)劃的航向角與實(shí)際的航向角 圖4　交叉局面態(tài)勢(shì) Fig. 4　Situation of cross

4結(jié)束語(yǔ)

本文通過(guò)對(duì)在線路徑規(guī)劃算法進(jìn)行實(shí)驗(yàn)仿真可知，新算法在遵守COLREGs規(guī)則的同時(shí)，能獲得較好的規(guī)劃路徑。然而本文并沒(méi)有對(duì)最優(yōu)化與COREGs規(guī)則存在的矛盾進(jìn)行進(jìn)一步討論，因?yàn)樵谶x擇航向的過(guò)程中，最優(yōu)化的結(jié)果有可能與COLREGs規(guī)則沖突，在這種情況下，需要引進(jìn)一個(gè)選擇算法，遵循優(yōu)先級(jí)的高低來(lái)選擇每種會(huì)遇局面最合適的規(guī)劃。

參考文獻(xiàn):

[1]CASALINO G, TURETTA A, SIMETTI E. A three-layered architecture for real time path planning and obstacle avoidance for surveillance USVs operating in harbour fields[C]//IEEE OCEANS 2009-Europe. Bremen, Germany, 2009: 1-8.

[2]International Maritime Organization. International regulations for prevention of collisions at sea, 1972 (COLREGs)[EB/OL]. [2014-04-25]. http://www.imo.org/About/Conventions/ListOfConventions/Pages/COLREG.aspx.

[3]PERERA L P, CARVALHO J P, SOARES C G. Autonomous guidance and navigation based on the COLREGs rules and regulations of collision avoidance [C]//Proceedings of the International Workshop “Advanced Ship Design for Pollution Prevention”. Split, Croatia, 2009: 205-216.

[4]BENJAMIN M R, CURCIO J A, LEONARD J J, et al. Navigation of unmanned marine vehicles in accordance with the rules of the road[C]//2006 IEEE International Conference on Robotics and Automation. Orlando, USA, 2006: 3581-3587.

[5]KUWATA Y, WOLF M T, ZARZHITSKY D, et al. Safe maritime navigation with COLREGS, using velocity obstacles[C]//2011 IEEE/RSJ International Conference on Intelligent Robots and Systems. San Francisco, USA, 4728-4734.

[6]FIORINI P, SHILLER Z. Motion planning in dynamic environments using velocity obstacles[J]. The International Journal of Robotics Research, 1998, 17(7): 760-772.

[7]祖迪, 韓建達(dá), 談大龍. 加速度空間中基于線性規(guī)劃的移動(dòng)機(jī)器人路徑規(guī)劃方法[J]. 自動(dòng)化學(xué)報(bào), 2007, 33(10): 1036-1042.

ZU Di, HAN Jianda, TAN Dalong. LP-based path planning method in acceleration space for mobile robot[J]. Acta Automatica Sinica, 2007, 33(10): 1036-1042.

[8]COLLEY B A, CURTIS R G, STOCKEL C T. Manoeuvring times, domains and arenas[J]. Journal of Navigation, 1983, 36(2): 324-328.

[9]程大軍, 劉開(kāi)周. 基于MILP的AUV實(shí)時(shí)優(yōu)化行為方法研究[J]. 機(jī)械設(shè)計(jì)與制造, 2012(4): 91-93.

CHENG Dajun, LIU Kaizhou. Research on real-time optimization behavior of AUV based on MILP[J]. Machinery Design & Manufacture, 2012(4): 91-93.

[10]SONNENBURG C R, WOOLSEY C A. Modeling, identification, and control of an unmanned surface vehicle[J]. Journal of Field Robotics, 2013, 30(3): 371-398.

冷靜，女，1990年生，碩士研究生，主要研究方向?yàn)闊o(wú)人水面機(jī)器人的在線路徑規(guī)劃。曾獲中國(guó)科學(xué)院大學(xué)“三好學(xué)生”和國(guó)家獎(jiǎng)學(xué)金。

劉健，男，1962年生，研究員，主要研究方向?yàn)楣I(yè)控制、圖像跟蹤、水下機(jī)器人控制和組合導(dǎo)航技術(shù)，主持完成國(guó)家重大項(xiàng)目多項(xiàng)。

徐紅麗，女，1979年生，副研究員，工學(xué)博士，主要研究方向?yàn)樗聶C(jī)器人控制、水下聲吶圖像處理、多傳感器數(shù)據(jù)融合與環(huán)境建模。曾主持國(guó)家自然科學(xué)基金項(xiàng)目1項(xiàng)、國(guó)家“863”計(jì)劃項(xiàng)目1項(xiàng)，并參與國(guó)家及省部級(jí)重點(diǎn)項(xiàng)目多項(xiàng)，發(fā)表學(xué)術(shù)論文10余篇。