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

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

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

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

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

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

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

DOI:10.3969/j.issn.1673-4785.201405012

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

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

基金項目：中國科學(xué)院重點部署項目子課題(KGFZD-125-014).

作者簡介：

中文引用格式：冷靜，劉健，徐紅麗. 實時避碰的無人水面機器人在線路徑規(guī)劃方法[J]. 智能系統(tǒng)學(xué)報, 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.

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

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

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

1改進速度避障法

1.1問題描述

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

1)目標(biāo)：1個遠程靜態(tài)路徑點。

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

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

4)假設(shè)條件：因為船體本身具有的慣性比較大，所以假設(shè)極短的規(guī)劃時間Δt內(nèi)，障礙物保持勻速運動，即USV相對運動障礙物的速度改變量等于USV自身的速度改變量。

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

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

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

1.2.1相關(guān)定義

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

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

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

3)避碰角：

(1)

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

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

6)規(guī)劃時間：T=w1ρ+w2/τ，即規(guī)劃時間與碰撞危險度成正比，與碰撞時間成反比。因為受到USV本體運動性能約束，其轉(zhuǎn)角加速度和縱向加速度都有限制，所以碰撞危險度越大，碰撞時間越短，規(guī)劃時間就越短，否則無法及時進行航速和方向的規(guī)劃。

1.2.2避碰策略

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

速度避障法解決的是局部實時避碰的問題，但容易陷入局部最小值，所以在對USV進行在線路徑規(guī)劃的時候，必須加入全局規(guī)劃器。在基本LP中，各約束條件之間形成“與”的邏輯關(guān)系，對于具有復(fù)雜邏輯關(guān)系的優(yōu)化問題不能直接求解。用MILP方法解決動態(tài)環(huán)境下的路徑規(guī)劃問題，可以很容易地把機器人本體動力學(xué)、運動學(xué)以及環(huán)境不確定性等約束考慮進去，并且能解決障礙物轉(zhuǎ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通過調(diào)整速度大小和方向，每規(guī)劃一個時間間隔都使下一時刻USV離目標(biāo)越來越近，即

(2)

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

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

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

(3)

由泰勒公式可得

min:g+Δg

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

(4)

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

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

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

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

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

因為線性規(guī)劃只有一個上下限，所以速度變量的上下限整合為

2.3障礙物約束

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

(5)

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

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

2.4算法流程

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

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

2)通過公式計算出每艘船舶與USV之間的最近會遇時間TCPA和最近會遇距離DCPA。

3)通過危險度判斷模型判斷是否處于危險狀態(tài)。如果危險則開啟避碰行為進行第4)步，否則不實施避碰行為。

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

5)對本體運動學(xué)、動力學(xué)、探測范圍、障礙物約束和目標(biāo)函數(shù)進行建模。

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

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

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

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

3仿真驗證

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

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

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

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

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

(b)開始轉(zhuǎn)向

(c)到達目標(biāo)點

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

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

(b)開始轉(zhuǎn)向

(c)到達目標(biāo)點

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

4結(jié)束語

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

參考文獻:

[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]祖迪, 韓建達, 談大龍. 加速度空間中基于線性規(guī)劃的移動機器人路徑規(guī)劃方法[J]. 自動化學(xué)報, 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]程大軍, 劉開周. 基于MILP的AUV實時優(yōu)化行為方法研究[J]. 機械設(shè)計與制造, 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年生，碩士研究生，主要研究方向為無人水面機器人的在線路徑規(guī)劃。曾獲中國科學(xué)院大學(xué)“三好學(xué)生”和國家獎學(xué)金。

劉健，男，1962年生，研究員，主要研究方向為工業(yè)控制、圖像跟蹤、水下機器人控制和組合導(dǎo)航技術(shù)，主持完成國家重大項目多項。

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