張琴麗，吳懷宇，陳 洋

（武漢科技大學(xué) 信息科學(xué)與工程學(xué)院，湖北 武漢430081）

0 引 言

目前，路徑規(guī)劃主要分為對已知環(huán)境路徑規(guī)劃和對未知環(huán)境路徑規(guī)劃。在已知環(huán)境中進(jìn)行路徑規(guī)劃的方法已經(jīng)比較成熟，如：人工勢場法［1，2］、A＊算法［3］、遺傳算法［4］等。在未知環(huán)境中，由于機(jī)器人無法準(zhǔn)確判斷環(huán)境中是否有障礙物存在及障礙物的具體位置信息，只能通過傳感器獲知其探測范圍內(nèi)的環(huán)境信息來進(jìn)行局部的路徑規(guī)劃。因此，環(huán)境信息的不完整性是未知環(huán)境中路徑規(guī)劃的關(guān)鍵問題［5］。

近年來，很多學(xué)者針對未知環(huán)境下的機(jī)器人路徑規(guī)劃做了大量研究，并取得了一定成果［2－10］。其中具有代表性的成果是將多種規(guī)劃方法相結(jié)合來進(jìn)行混合路徑規(guī)劃。如文獻(xiàn) ［6］中提出的利用Voronoi圖和勢場法對未知環(huán)境進(jìn)行路徑規(guī)劃。該方法采用兩級規(guī)劃算法：頂層規(guī)劃采用廣義Voronoi圖法產(chǎn)生從起始點(diǎn)到目標(biāo)點(diǎn)的路徑；底層規(guī)劃采用勢場法對未知障礙物進(jìn)行避障。該方法有效解決了有大量障礙物存在的未知環(huán)境下機(jī)器人路徑規(guī)劃問題。但該方法主要適用于大量密集型障礙物環(huán)境中的路徑規(guī)劃，算法復(fù)雜度高、花費(fèi)時(shí)間長。為解決算法復(fù)雜度問題，有學(xué)者提出一種基于橢圓約束的機(jī)器人導(dǎo)航 （ellipsoidal constrained agent navigation，ECAN）算法［8］。該算法的主要思想是通過規(guī)劃具有相關(guān)約束的橢圓來尋找機(jī)器人運(yùn)動路徑，將機(jī)器人路徑規(guī)劃問題轉(zhuǎn)換為橢圓優(yōu)化的問題。該方法能有效應(yīng)用于2維和3維的未知環(huán)境，且具有路徑規(guī)劃實(shí)時(shí)性高、復(fù)雜度低等優(yōu)點(diǎn)。但是機(jī)器人在規(guī)劃過程中，未考慮機(jī)器人運(yùn)動角度且未檢測到障礙物對規(guī)劃橢圓的影響，易使機(jī)器人陷入局部震蕩，從而導(dǎo)致規(guī)劃失敗。

針對上述研究現(xiàn)狀及不足，本文通過引入機(jī)器人運(yùn)動步長及運(yùn)動方向的約束，根據(jù)凸二次約束二次優(yōu)化［7］理論，建立具有凸約束的目標(biāo)函數(shù)模型，有效解決了ECAN 算法中機(jī)器人目標(biāo)不可達(dá)問題，并且大幅提高了算法的收斂速度，降低了復(fù)雜度和規(guī)劃時(shí)間。使用該方法能夠快速準(zhǔn)確地在未知環(huán)境下進(jìn)行路徑規(guī)劃。

1 問題描述

設(shè)機(jī)器人在二維平面的有限區(qū)域內(nèi)運(yùn)動，該區(qū)域內(nèi)任意分布著有限個(gè)障礙物。路徑規(guī)劃的任務(wù)是在障礙物信息未知的情況下，機(jī)器人規(guī)劃出一條從起始點(diǎn)S到目標(biāo)點(diǎn)G與障礙物無碰撞的路徑。本文提出的基于橢圓約束的路徑規(guī)劃方法，將障礙物與機(jī)器人統(tǒng)一等效為點(diǎn)。規(guī)劃的基本原理如圖1所示。

圖1 橢圓約束路徑規(guī)劃基本原理

設(shè)機(jī)器人的起始位置是S（0，0），目標(biāo)位置是G（4，1）。環(huán)境中障礙物信息如圖1 （a）所示。機(jī)器人在運(yùn)動過程中，當(dāng)運(yùn)動到坐標(biāo)（1，1）時(shí)，通過激光傳感器掃描檢測到在坐標(biāo)（2.2，1）處存在一個(gè)障礙物。此時(shí)，機(jī)器人根據(jù)運(yùn)動的當(dāng)前位置，檢測到的障礙物位置以及目標(biāo)點(diǎn)的位置，規(guī)劃出一個(gè)滿足一定約束條件的橢圓，并且確定機(jī)器人在該橢圓內(nèi)與障礙物無碰撞的運(yùn)動方向，如圖1 （b）所示。當(dāng)機(jī)器人在橢圓內(nèi)運(yùn)動到指定位置時(shí)，又檢測到坐標(biāo)（3.5，2）處存在另一障礙物。機(jī)器人則根據(jù)當(dāng)前檢測到的信息規(guī)劃出滿足約束條件的另一個(gè)橢圓。此時(shí)目標(biāo)點(diǎn)在該規(guī)劃橢圓的邊界上，機(jī)器人直接從當(dāng)前位置運(yùn)動到目標(biāo)位置，完成路徑規(guī)劃，如圖1 （c）所示。

根據(jù)上述路徑規(guī)劃的基本原理，在障礙物信息未知的二維環(huán)境內(nèi)，進(jìn)行無碰撞路徑規(guī)劃可分為三步：

（1）機(jī)器人在檢測到障礙物存在的t時(shí)刻，規(guī)劃滿足一定約束條件的橢圓；

（2）確定機(jī)器人在該橢圓內(nèi)的運(yùn)動方向；

（3）計(jì)算機(jī)器人在該橢圓內(nèi)運(yùn)動方向上的運(yùn)動距離。

上述過程需要求解滿足凸二次約束的二次優(yōu)化［7］問題。假設(shè)機(jī)器人通過激光傳感器掃描，檢測到有限區(qū)域內(nèi)環(huán)境障礙物信息，將該區(qū)域稱為機(jī)器人視野域。視野域由激光掃描參數(shù)（r，θ）決定，其中r∈（0，R］，θ∈［－，］。t時(shí)刻，機(jī)器人、障礙物及目標(biāo)點(diǎn)信息分別由坐標(biāo)，和xg表 示； 規(guī) 劃 的 橢 圓 方 程 表 示 為 Φt（P，q，r） ＝，即橢圓內(nèi)部滿足intΦt＝｛x｜Φt（P，q，r）＜0｝ ，橢圓邊界值滿足。其中P 為二維正定矩陣，

1.1 橢圓約束的路徑規(guī)劃模型

機(jī)器人運(yùn)動過程中，在t時(shí)刻檢測到有障礙物存在，則規(guī)劃出滿足以下約束條件的橢圓：①機(jī)器人在橢圓內(nèi)；②所檢測到的障礙物在橢圓外；③目標(biāo)點(diǎn)在橢圓邊界上或在橢圓外。滿足這3個(gè)約束條件的數(shù)學(xué)模型為

為了保證機(jī)器人能夠保持向目標(biāo)位置運(yùn)動，所規(guī)劃的橢圓要盡量靠近目標(biāo)點(diǎn)。式 （1）中數(shù)學(xué)模型的目標(biāo)函數(shù)h1最小化機(jī)器人當(dāng)前位置離橢圓邊界的距離；目標(biāo)函數(shù)h2保證在障礙物存在的情況下，所規(guī)劃的橢圓最大；目標(biāo)函數(shù)h3表示目標(biāo)點(diǎn)離橢圓圓心的距離，保證在滿足約束的情況下橢圓離目標(biāo)點(diǎn)最近，當(dāng)目標(biāo)點(diǎn)在橢圓邊界時(shí)，h3為零。

1.2 機(jī)器人運(yùn)動方向模型

t時(shí)刻規(guī)劃出可行橢圓后，機(jī)器人在橢圓內(nèi)的運(yùn)動必須選擇合適的方向使機(jī)器人在向目標(biāo)點(diǎn)運(yùn)動的同時(shí)避開障礙物。本文采用橢圓的特征向量和特征值及代數(shù)幾何的方法，計(jì)算機(jī)器人在橢圓內(nèi)部的運(yùn)動方向xn。

若目標(biāo)點(diǎn)在橢圓邊界上，即滿足Φt（xg）＝0，橢圓內(nèi)部機(jī)器人運(yùn)動方向xn為當(dāng)前位置指向目標(biāo)位置的單位向量；若目標(biāo)點(diǎn)在橢圓外，即Φt（xg）＞0，則根據(jù)以下方法計(jì)算出橢圓內(nèi)機(jī)器人的運(yùn)動方向。

（1）建立橢圓坐標(biāo)系：定義橢圓的長短軸分別為矩陣P 最大最小特征值所對應(yīng)的特征向量xp，xo。xo在與xp成±90°的方向。若xp方向左側(cè)障礙物的數(shù)量多于右側(cè)障礙物的數(shù)量，則xo選擇為矢量xp的－90°方向；反之，xo選擇為矢量xp的＋90°方向。

（2）判斷目標(biāo)點(diǎn)與橢圓x 軸的位置關(guān)系，確定橢圓內(nèi)機(jī)器人的運(yùn)動方向

設(shè)t時(shí)刻橢圓圓心為C（xtc，ytc），目標(biāo)點(diǎn)坐標(biāo)G（xg，yg），機(jī)器人坐標(biāo)R（xtr，ytr）。根據(jù)式 （2）判斷目標(biāo)點(diǎn)與橢圓x 軸的位置關(guān)系

若l≥0，則目標(biāo)點(diǎn)位于橢圓x軸左側(cè)，如圖2 （a）所示。根據(jù)式 （3）、式 （4）求橢圓上一點(diǎn)B（xb，yb）

圖2 橢圓內(nèi)機(jī)器人運(yùn)動方向

1.3 機(jī)器人在橢圓內(nèi)的運(yùn)動步長

計(jì)算出橢圓內(nèi)機(jī)器人的運(yùn)動方向xn，則機(jī)器人沿該方向的運(yùn)動步長ln＝min（δ1，δ2），其中δ1是根據(jù)機(jī)器人的運(yùn)動速度給定的一個(gè)標(biāo)量值，若目標(biāo)點(diǎn)在橢圓上，則δ2＝，若目標(biāo)點(diǎn)在橢圓外，則

1.4 具有橢圓約束的路徑規(guī)劃算法

機(jī)器人路徑規(guī)劃算法主要步驟可描述如下：

步驟1 初始化相關(guān)變量α，β，γ；初始化機(jī)器人的起始點(diǎn)及目標(biāo)點(diǎn)；初始化激光所掃描的二維有限區(qū)域范圍；

步驟2 如果機(jī)器人未到達(dá)目標(biāo)點(diǎn) （與目標(biāo)點(diǎn)的距離大于0.01m），執(zhí)行步驟3；否則判斷機(jī)器人已到達(dá)目標(biāo)位置，執(zhí)行步驟9；

步驟3 在激光傳感器掃描的二維環(huán)境范圍內(nèi)，根據(jù)式（1）規(guī)劃滿足約束條件的橢圓；

步驟4 如果目標(biāo)點(diǎn)位于步驟3中所規(guī)劃的橢圓之外時(shí)，執(zhí)行步驟5；否則執(zhí)行步驟6；

步驟5 根據(jù)式（2）、式（3）、式（4）求橢圓上一點(diǎn)B；

步驟6 計(jì)算機(jī)器人在橢圓內(nèi)的運(yùn)動方向xn；

步驟7 計(jì)算機(jī)器人沿橢圓內(nèi)運(yùn)動方向上的運(yùn)動步長ln＝min（δ1，δ2）；

步驟8 機(jī)器人按照上述規(guī)劃路徑運(yùn)動，并不斷更新機(jī)器人的位置信息；返回步驟2重復(fù)執(zhí)行。

步驟9 輸出機(jī)器人的運(yùn)動軌跡。

2 仿真實(shí)驗(yàn)及分析

利用凸優(yōu)化工具箱CVX［7］在Matlab環(huán)境下進(jìn)行仿真實(shí)驗(yàn)。仿真中，基本參數(shù)設(shè)置如下：α＝0.1，β＝5×10－5，γ＝1，r＝2m，θ＝120°。首先利用本文算法在復(fù)雜未知環(huán)境下得到路徑規(guī)劃結(jié)果，然后將其與文獻(xiàn) ［8］中已有方法進(jìn)行比較。

2.1 未知環(huán)境中的仿真

根據(jù)本文提出的算法進(jìn)行路徑規(guī)劃。設(shè)置仿真實(shí)驗(yàn)環(huán)境為10m×10m 的區(qū)域，并隨機(jī)生成靜態(tài)障礙物坐標(biāo)。機(jī)器人起始位置s（2，1），目標(biāo)位置G（8，6）。路徑規(guī)劃仿真結(jié)果及每次橢圓規(guī)劃所需時(shí)間如圖3、圖4所示。

圖3 本文算法路徑規(guī)劃仿真結(jié)果

由圖3分析可知，利用該算法在障礙物未知的環(huán)境中進(jìn)行路徑規(guī)劃，能夠使機(jī)器人避開障礙物，到達(dá)目標(biāo)點(diǎn)。在規(guī)劃過程中，機(jī)器人根據(jù)檢測到的當(dāng)前環(huán)境信息所規(guī)劃的橢圓選擇運(yùn)動步長，通過控制機(jī)器人每次規(guī)劃運(yùn)動的長度和方向來尋找正確的路徑。該算法在障礙物未知的情況下，能夠快速進(jìn)行路徑規(guī)劃，減少規(guī)劃時(shí)間（如圖4 所示），提高了機(jī)器人路徑規(guī)劃的有效性和實(shí)時(shí)性。

2.2 算法對比研究

圖4 橢圓規(guī)劃所需時(shí)間

將本文提出的算法與文獻(xiàn) ［8］中提出的ECAN 算法進(jìn)行對比分析。分別用2種不同的算法進(jìn)行路徑規(guī)劃，觀察規(guī)劃結(jié)果。主要進(jìn)行兩方面的對比。第一方面比較機(jī)器人是否順利到達(dá)目標(biāo)點(diǎn)，第二方面比較在利用2種算法都能到達(dá)目標(biāo)點(diǎn)的情況下，所規(guī)劃路徑的優(yōu)劣情況。

2.2.1 算法性能的對比

設(shè)置仿真實(shí)驗(yàn)環(huán)境為10m×10m 的區(qū)域，隨機(jī)生成靜態(tài)障礙物坐標(biāo)。機(jī)器人初始位置s（0，0），目標(biāo)位置G（9，9）。分別采用本文算法和ECAN 算法進(jìn)行路徑規(guī)劃，規(guī)劃結(jié)果如圖5所示。

圖5 不同算法仿真結(jié)果對比

由圖5知，使用ECAN 算法時(shí)，機(jī)器人在復(fù)雜環(huán)境中規(guī)劃路徑容易陷入局部震蕩的情況，從而導(dǎo)致機(jī)器人路徑規(guī)劃失敗 （如圖5 （b）所示）。本文所提出的改進(jìn)算法能有效解決規(guī)劃過程中機(jī)器人陷入局部震蕩，目標(biāo)不可達(dá)的問題 （如圖5 （a）所示）。

為了進(jìn)一步驗(yàn)證該算法解決局部震蕩問題的有效性，分別使用該算法和文獻(xiàn) ［8］中的算法進(jìn)行路徑規(guī)劃100次，將規(guī)劃成功次數(shù)進(jìn)行對比分析。隨機(jī)設(shè)置100 個(gè)二維有限區(qū)域，隨機(jī)生成靜態(tài)障礙物點(diǎn)。對每個(gè)區(qū)域分別用2種不同的算法進(jìn)行路徑規(guī)劃，記錄每種算法成功次數(shù)。對比實(shí)驗(yàn)結(jié)果見表1。

表1 路徑規(guī)劃成功率

由表1可知，本文提出的算法規(guī)劃成功次數(shù)要高于文獻(xiàn) ［8］中所提出的算法規(guī)劃成功次數(shù)。對文獻(xiàn) ［8］算法在障礙物密集環(huán)境下路徑規(guī)劃存在的目標(biāo)不可達(dá)問題，本文提出的算法有很好的規(guī)劃效果。該算法對復(fù)雜環(huán)境的適應(yīng)性更高。

2.2.2 規(guī)劃的路徑對比

在2種算法均能規(guī)劃成功時(shí)，比較所規(guī)劃路徑的優(yōu)劣情況。仿真實(shí)驗(yàn)環(huán)境為10m×10m 隨機(jī)生成障礙物坐標(biāo)的區(qū)域。機(jī)器人起始位置s（0，0），目標(biāo)位置G（8，6）。2種算法進(jìn)行規(guī)劃的規(guī)劃結(jié)果如圖6所示。

圖6 不同算法規(guī)劃成功路徑的優(yōu)劣對比

在均能規(guī)劃正確路徑到達(dá)指定的目標(biāo)點(diǎn)，并且完成避障行為的情況下，本文提出的算法所規(guī)劃的路徑 （圖6 （a）所示）要比ECAN 算法 （圖6 （b）所示）所規(guī)劃的路徑長度更短，連續(xù)性更好，路徑更平滑。本文所提出的基于ECAN 的改進(jìn)算法可在線運(yùn)行，實(shí)時(shí)規(guī)劃路徑，對于障礙物未知的環(huán)境，具有很好的環(huán)境適應(yīng)能力。采用該算法能快速有效的在未知環(huán)境下規(guī)劃出平滑的，無碰撞路徑。

3 結(jié)束語

本文提出了一種未知環(huán)境下基于橢圓約束的移動機(jī)器人路徑規(guī)劃算法。該算法提高了復(fù)雜環(huán)境下移動機(jī)器人避障規(guī)劃的能力，在規(guī)劃路徑時(shí)，引入機(jī)器人運(yùn)動步長及運(yùn)動方向的約束，有效的提高了算法的規(guī)劃效率，提高了路徑規(guī)劃的成功率。利用凸優(yōu)化理論的數(shù)學(xué)建模方法進(jìn)行路徑規(guī)劃，與以往利用智能算法進(jìn)行路徑規(guī)劃相比，不需要經(jīng)過多次反復(fù)迭代來尋找最優(yōu)解，其建立的數(shù)學(xué)模型即為最優(yōu)解模型。當(dāng)環(huán)境規(guī)模較大，機(jī)器人對復(fù)雜環(huán)境中障礙物的信息一無所知時(shí)，該算法具有很強(qiáng)的自適應(yīng)能力，和其他算法相比，更能體現(xiàn)其優(yōu)越性。在機(jī)器人的實(shí)際應(yīng)用中也有很好的發(fā)展前景。

［1］LI G，Tamura Y，Yamashita A，et al.Effective improved artificial potential field－based regression search method for autonomous mobile robot path planning ［J］.International Journal of Mechatronics and Automation，2013，3 （3）：141－170.

［2］ZHU Yi，ZHANG Tao，SONG Jingyan.Path planning for nonholonomic mobile robots using artificial potential field method ［J］.Control Theory ＆ Applications，2010，27 （2）：152－158 （in Chinese）.［朱毅，張濤，宋靖雁.非完整移動機(jī)器人的人工勢場法路徑規(guī)劃 ［J］.控制理論與應(yīng)用，2010，27（2）：152－158.］

［3］ZENG C，ZHANG Q，WEI X.GA－based global path planning for mobile robot employing A＊ algorithm ［J］.Journal of Computers，2012，7 （2）：470－474.

［4］Ismail ALT，Sheta A，AL－Weshah M.A mobile robot path planning using genetic algorithm in static environment ［J］.Journal of Computer Science，2008，4 （4）：341－344.

［5］HU Jun，ZHU Qingbao.Path planning of robot for unknown environment based on prior knowledge rolling Q－leaning ［J］.Control and Decision，2010，25 （9）：1364－1368 （in Chinese）.［胡俊，朱慶保.未知環(huán)境下基于有先驗(yàn)知識的滾動Q學(xué)習(xí)機(jī)器人路徑規(guī)劃 ［J］.控制與決策，2010，25 （9）：1364－1368.］

［6］OK K，Ansari S，Gallagher B，et al.Path planning with uncertainty：Voronoi uncertainty fields［C］／／IEEE International Conference on Robotics and Automation，2013：4596－4601.

［7］Boyd SP，Vandenberghe L.Convex optimization ［M］.U.K：Cambridge University Press，2004.

［8］Sharma S.QCQP－tunneling：Ellipsoidal constrained agent navigation［C］／／Proceedings of the 2nd IASTED International Conference on Robotics Robo.Calgary：Acta Press，2011：294－301.

［9］CHEN Xiong，ZHAO Yilu，HAN Jianda.An improved ant colony optimization algorithm for robotic path planning［J］.Control Theory ＆Applications，2010，27 （6）：821－825 （in Chinese）. ［陳雄，趙一路，韓建達(dá).一種改進(jìn)的機(jī)器人路徑規(guī)劃的蟻群算法［J］.控制理論與應(yīng)用，2010，27 （6）：821－825.］

［10］KANG Liang，ZHAO Chunxia，GUO Jianhui.Improved path planning based on rapidly－exploring random tree for mobile robot in unknown environment［J］.Pattern Recognition and Artificial Intelligence，2009，22 （3）：337－343 （in Chinese）. ［康亮，趙春霞，郭劍輝.未知環(huán)境下改進(jìn)的基于RRT 算法的移動機(jī)器人路徑規(guī)劃 ［J］.模式識別與人工智能，2009，22 （3）：337－343.］