動態環境下矢量化路徑規劃算法＊

徐啟蕾，唐功友，蓋紹婷，楊 雪

（1.中國海洋大學信息科學與工程學院，山東 青島266100；2.青島科技大學自動化與電子工程學院，山東 青島266042）

路徑規劃問題是機器人自主導航研究的一個重要問題。機器人從初始位置到目標位置的路徑規劃要求實現：（1）具有可達性和無碰撞；（2）規劃的線路應滿足某種指標的最優（如時間最優、能耗最優等）。

針對完全未知環境下路徑規劃，常用方法有人工勢場法[1－3]、模糊邏輯算法[4－5]、BUG系列算法[6－10]等。人工勢場法結構簡單，便于控制，能得到平滑的路徑軌跡，多種研究方法可避免其陷入局部最優，但該類方法對狹窄空間的路徑規劃及不規則障礙物的處理方面還有待研究。模糊邏輯算法模擬生物的避障思想，對機器人定位精度、環境檢測精度不敏感，具有較好的實時性和穩定性，但規劃結果、運算量受模糊隸屬函數和模糊控制規則的影響十分嚴重。Bug系列算法中，機器人利用觸覺或視覺傳感器數據，按朝向目標前進、沿障礙物邊界繞行障礙物和停止3種運動狀態行進，此類算法簡單，但在選取最優障礙物繞行方向時運算量較大，且所有的Bug系列算法均未考慮機器人行進的安全半徑，僅是貼合障礙物邊緣行進。

對于移動障礙物，文獻[11]提出軌道變形算法，根據移動障礙物信息實時修改已知環境下的預定路線，當移動障礙超過算法約束時，則需要由1個帶時間約束的實時動態規劃器[12]重新規劃路徑。此方法可以解決已知環境下的移動障礙物，但對未知的動態環境無能為力。

本文針對完全未知動態環境，提出了矢量化路徑描述方法，并將此方法與Bug系列算法思想相結合，研究全方位移動式機器人的運動規劃及其避障策略。機器人根據其感知的局部信息獨立規劃并更新其路徑，實現可達、無碰撞的路徑規劃。起初根據初始位置和目標位置生成初始路徑，路徑以矢量形式進行描述及保存。機器人根據規劃路徑進行移動，先根據路徑轉過相應的角度，其次沿直線向前運動至路徑規定距離，然后依規劃路徑進行下一輪角度旋轉和直線運動。運動過程中以一定的間隔對環境信息采樣、分析，不斷根據局部障礙物的位置及其移動速度和方向信息，在一定范圍內產生路徑中間點，根據設定的規則選擇可行的中間點，并將中間點插入原有路徑以實現路徑實時更新，達到避障的目的。提出的矢量化路徑描述方法是以一組首尾相連的矢量來表示路徑，通過該種方式來記錄和保存路徑可降低路徑存儲的空間需求，并在一定程度上簡化對機器人的控制。路徑規劃過程中，考慮到機器人行進過程中與障礙物間的安全距離，增強了算法的實用性和安全性。

1 問題描述

1.1 規劃問題描述

將全方位移動式機器人簡化為1個質點，考慮其在二維狀態空間R2內從初始位置qstart到目標位置qgoal的運動規劃問題。

假設機器人配備的測距設備可掃描機器人前方360°的以最大測距Rsensor為半徑的圓形范圍。在行進過程中機器人每走過一定距離就會對周圍環境進行檢測，且在前一段路徑結束，轉向進入下一段路徑后必須立即進行環境檢測。在檢測到障礙物時，機器人將在一定時間間隔tsensor內停止運動，并不斷檢測障礙物，以計算出障礙物的移動速度及方向。假設障礙物沿直線勻速移動，且障礙物的移動速度vo滿足，n≥5，以保證在機器人停駐檢測障礙物時不會發生碰撞，且機器人有足夠時間和距離繞過障礙物。然后綜合當前行進路徑向量以及檢測到的障礙物所在方位及其移動方向和速度來判斷該障礙物是否會阻擋當前行進路線。一旦發現該障礙物阻擋當前路徑，則計算中間插入點，并更新規劃路徑以實現避障。

1.2 矢量化路徑描述

本文提出以一組有序的矢量來表示任意路徑規劃結果，該方法稱為矢量化路徑描述方法。

令Pi表示任意第i次規劃的路徑，且有

式（1）中各個向量依次首尾相接，其中αik為構成路徑Pi的第k個向量，αik以向量αi（k－1）的終點為起點，記作αik＝ （θik，‖αik‖ ），式（1）中θik是向量αik與向量αi（k－1）的夾角，取順時針方向為正，逆時針方向為負，－π≤θik≤π，‖αik‖為向量的模，即機器人在轉動θik方向后應行進的距離。

機器人首先根據初始位置和目標位置創建初始路徑，然后按照路徑行進，并在行進過程中，不斷檢測環境中障礙物信息，并根據檢測到的障礙物信息和中間點生成法則來生成中間點，最后插入中間點、實時更新路徑，從而實時規劃出無碰撞路徑。其初始路徑P0由以起始位置為起點、目標位置為終點構成的向量組成，有

其中：以機器人的初始方向為基準，轉向目標位置所轉過的角度記為θ01，起始位置到目標位置間的直線距離記為‖α01‖。

機器人沿初始路徑開始移動，并不斷對周邊環境進行檢測，一旦發現有障礙物，則需要增加路徑中間點以更新規劃路徑，從而實現避障。

圖1給出了機器人按照矢量化描述的路徑移動、并插入中間點更新路徑的過程。機器人首先按照路徑矢量組Pi行進，在沿αik運動的途中，檢測到障礙物Obstacle阻擋路線，則在路徑矢量組Pi上添加點qinsert（i＋1）得到更新路線后的路線矢量組Pi＋1，則有

因α（i＋1）n是機器人在αin上行進過的路線，所以θ（i＋1）n＝θin，‖α（i＋1）n‖和‖αin‖－‖α（i＋1）n‖已知。又根據規則1可知‖α（i＋1）（n＋1）‖和θ（i＋1）（n＋1），則由余弦定理可得

2 規劃算法

2.1 障礙物檢測

機器人按規劃路徑行進過程中，需要不斷根據檢測到的環境信息，判斷前方是否有障礙物阻擋或將要阻擋當前路徑。圖2給出了障礙物檢測的4種情況。圖中淺色矩形為第一次檢測到障礙物時障礙物所在位置；深色矩形為經過tsensor時間后，障礙物所在位置；q表示機器人當前所在位置；大圓形表示檢測范圍Rsensor；小圓形表示安全距離Rsafe；pi表示檢測到的障礙物點，p1是第一次檢測到的障礙物點，p2和p3分別是tsensor時間后檢測到的左右2個邊緣的障礙物點；φi表示當前路徑間與q到pi之間的連線的夾角，以當前路徑方向為起始方向，逆時針方向為正，順時針方向為負。

情況（a）時，經過時間tsensor后，無法檢測到障礙物，此時判定該障礙物無法阻擋當前路徑，機器人仍沿原來路徑行進。

情況（b），（c）和（d）時，須按下列不等式組進行判斷：

若滿足不等式組（9），則認為此障礙物正向遠離障礙物的方向移動，機器人可沿原路徑行進；否則，則認為障礙物正朝向阻擋路徑方向移動，或保持不動，需計算中間插入點更新路徑。

圖2 障礙物檢測示意圖Fig.2 Obstacle detection

2.2 插入點生成

當檢測到有障礙物阻擋當前路徑時，首先計算出最大插入點距離dmax，然后按照本文提出的下列規則計算中間插入點。

規則1 取tsensor時間后檢測到的機器人與障礙物間最長距離為dmax，Rsafe≤dmax≤Rsensor，以dmax為向量長度，機器人當前位置為向量起始點，以機器人當前行進方向為基準，按角度θ做向量，若該向量無障礙阻擋，則將該向量添加到新的路徑中，且該向量的終點為插入的中間點；若有障礙物阻擋，則取θ＝θ＋Δθ，Δθ為規定的角度值，重復上述步驟做向量；若直至θ＝2π時仍未有向量滿足上述要求，則取dmax＝α·dmax，0＜α＜1，重新以角度θ做向量，重復以上過程，其中

2.3 更新路徑

當計算出中間插入點之后，需要根據1.2章節中給出的路徑更新計算方法，計算出更新后的路徑，然后令機器人按照更新后的路徑繼續行進。

3 仿真實驗及分析

為驗證算法，在配置為i5－M430（2.27GHz）、2G內存的筆記本電腦上以C＃語言編寫仿真軟件進行實驗。圖4是對不同地圖進行仿真所得到的結果。每幅地圖大小均為1 024×768像素，以1像素為1長度單位，模擬未知的實際環境。初始位置到目標位置的距離稱為初始距離。取機器人測距范圍Rsensor＝300，安全半徑Rsafe＝20。圖中“●”是初始位置，“█”是目標位置，“□”表示檢測到障礙物，原地等待tsensor的位置。實線是機器人的行進路徑。圖中最淺色實體為第一次檢測到障礙物時障礙物所在位置，由淺入深的幾個實體依次分別為機器人等待tsensor時間后檢測到的障礙物所在位置。為了使圖看起來更加清晰、簡潔，對于那些已經被繞行過、不會對當前行進路徑造成阻擋、或已離開移動智能體感知范圍的障礙物，圖中將不再加以顯示。

圖3 不同地圖仿真結果Fig.3 Simulation results of different maps

從圖3的仿真結果中可以看出，本文提出的方法可以預測出移動障礙物的運動趨勢，實現提前根據其運動方向選擇是否繞行障礙物、或在繞行障礙物時選擇與障礙物運動方向相反的方向繞行，最終使機器人自主地從初始位置安全、無碰撞的到達目標位置。

表1給出了實驗數據。假設機器人移動及距離檢測的時間為零，表中總耗時僅為計算中間點及規劃路徑的耗用時間。結果證明本算法實時性可滿足要求。

表1 不同地圖規劃結果對比Table 1 Result data of different maps

4 結語

本文提出了矢量化路徑描述方法，以一組有序的首尾相接的矢量對路徑進行描述與存儲，該方式易于實現路徑的執行，并可降低對路徑存儲空間的要求，尤其適合大范圍、復雜動態環境下機器人運動路徑規劃。文中將矢量化路徑描述方法與Bug思想相結合，解決未知動態環境下機器人實時在線路徑規劃問題。規劃過程中充分考慮了機器人與障礙物間的安全距離，提高了算法的實用性及安全性。提出的中間點選擇策略只需進行少量簡單計算，有利于機器人實時根據障礙物移動信息更新路徑。經仿真試驗證明，本文提出的方法可實現完全未知動態環境下機器人實時路徑規劃。

[1] Masoud Ahmad A.Decentralized self－organizing potential fieldbased control for individually motivated mobile agents in a cluttered environment：A vector－harmonic potential field approach[J].IEEE Trans on Systems，Man，and Cybernetics（TSMC），2007，37（3）：372－390.

[2]Mabrouk M H，McInnes C R.Solving the potential field local minimum problem using internal agent states[J].Robotics and Autonomous Systems，2008，56（12）：1050－1060.

[3]Zhang Tao，Zhu Yi，Song Jingyan.Real－time motion planning for mobile robots by means of artificial potential field method in unknown environment[J].Industrial Robot：An International Journal，2010，37（4）：384－400.

[4]Tarokh Mahmoud.Hybrid intelligent path planning for articulated rovers in rough terrain [J].Fuzzy Sets and Systems，2008，159（21）：2927－2937.

[5]Min Byung Cheol，Lee Moon－Su，Kim Donghan.Fuzzy logic path planner and motion controller by evolutionary programming for mobile robots[J].International Journal of Fuzzy System，2009，11（3）：154－163.

[6]Lumelsky V J，Stepanov A A.Path－Planning strategies for a point mobile automaton moving amidst unknown obstacles for arbitrary shape[J].Algorithnica，1987，2（3）：403－430.

[7]Lumelsky V J，Mukhopadhyay S，Sun K.Dynamic path planning in sensor－based terrain acquisition [J].IEEE Trans on Robotics and Automation，1990，6（4）：462－472.

[8]Kamon I，Rimon E，Rivlin E.TangentBug：A range－sensor－based navigation algorithm [J].The Inteernational Journal of Robotics Research，1998，17（9）：934－953.

[9]Buniamin N，Wan N W A J，Sariff N，et al.A simple local path planning algorithm for autonomous mobile robots [J].International Journal of Systems Applications，Engineering and Development，2011，5（2）：151－159.

[10]Guruprasad K R.Egressbug：a real time path planning algorithm for a mobile robot in an unknown environment[J].Lecture Notes in Computer Science，2012，7135：228－236.

[11]Delsart V，Fraichard T.Navigating Dynamic Environments Using Trajectory Deformation[C].France：IEEE－RSJ Int Conf on Intelligent Robots and Systems，2008：226－233.

[12]Delsart V，Fraichard T，Martinez L.Real－time Trajectory Generation for Car－like Vehicles Navigating Dynamic Environments[C].Kobe：IEEE Int Conf on Robotics and Automation，2009：3401－3406.