未知環(huán)境下基于橢圓約束的機器人路徑規(guī)劃

張琴麗，吳懷宇，陳 洋

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

0 引 言

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

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

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

1 問題描述

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

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

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

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

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

（2）確定機器人在該橢圓內的運動方向；

（3）計算機器人在該橢圓內運動方向上的運動距離。

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

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

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

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

1.2 機器人運動方向模型

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

若目標點在橢圓邊界上，即滿足Φt（xg）＝0，橢圓內部機器人運動方向xn為當前位置指向目標位置的單位向量；若目標點在橢圓外，即Φt（xg）＞0，則根據以下方法計算出橢圓內機器人的運動方向。

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

（2）判斷目標點與橢圓x 軸的位置關系，確定橢圓內機器人的運動方向

設t時刻橢圓圓心為C（xtc，ytc），目標點坐標G（xg，yg），機器人坐標R（xtr，ytr）。根據式 （2）判斷目標點與橢圓x 軸的位置關系

若l≥0，則目標點位于橢圓x軸左側，如圖2 （a）所示。根據式 （3）、式 （4）求橢圓上一點B（xb，yb）

圖2 橢圓內機器人運動方向

1.3 機器人在橢圓內的運動步長

計算出橢圓內機器人的運動方向xn，則機器人沿該方向的運動步長ln＝min（δ1，δ2），其中δ1是根據機器人的運動速度給定的一個標量值，若目標點在橢圓上，則δ2＝，若目標點在橢圓外，則

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

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

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

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

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

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

步驟5 根據式（2）、式（3）、式（4）求橢圓上一點B；

步驟6 計算機器人在橢圓內的運動方向xn；

步驟7 計算機器人沿橢圓內運動方向上的運動步長ln＝min（δ1，δ2）；

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

步驟9 輸出機器人的運動軌跡。

2 仿真實驗及分析

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

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

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

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

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

2.2 算法對比研究

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

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

2.2.1 算法性能的對比

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

圖5 不同算法仿真結果對比

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

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

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

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

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

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

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

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

3 結束語

本文提出了一種未知環(huán)境下基于橢圓約束的移動機器人路徑規(guī)劃算法。該算法提高了復雜環(huán)境下移動機器人避障規(guī)劃的能力，在規(guī)劃路徑時，引入機器人運動步長及運動方向的約束，有效的提高了算法的規(guī)劃效率，提高了路徑規(guī)劃的成功率。利用凸優(yōu)化理論的數(shù)學建模方法進行路徑規(guī)劃，與以往利用智能算法進行路徑規(guī)劃相比，不需要經過多次反復迭代來尋找最優(yōu)解，其建立的數(shù)學模型即為最優(yōu)解模型。當環(huán)境規(guī)模較大，機器人對復雜環(huán)境中障礙物的信息一無所知時，該算法具有很強的自適應能力，和其他算法相比，更能體現(xiàn)其優(yōu)越性。在機器人的實際應用中也有很好的發(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）.［朱毅，張濤，宋靖雁.非完整移動機器人的人工勢場法路徑規(guī)劃 ［J］.控制理論與應用，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)境下基于有先驗知識的滾動Q學習機器人路徑規(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）. ［陳雄，趙一路，韓建達.一種改進的機器人路徑規(guī)劃的蟻群算法［J］.控制理論與應用，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)境下改進的基于RRT 算法的移動機器人路徑規(guī)劃 ［J］.模式識別與人工智能，2009，22 （3）：337－343.］