動(dòng)態(tài)環(huán)境下矢量化路徑規(guī)劃算法＊

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

（1.中國(guó)海洋大學(xué)信息科學(xué)與工程學(xué)院，山東 青島266100；2.青島科技大學(xué)自動(dòng)化與電子工程學(xué)院，山東 青島266042）

路徑規(guī)劃問題是機(jī)器人自主導(dǎo)航研究的一個(gè)重要問題。機(jī)器人從初始位置到目標(biāo)位置的路徑規(guī)劃要求實(shí)現(xiàn)：（1）具有可達(dá)性和無碰撞；（2）規(guī)劃的線路應(yīng)滿足某種指標(biāo)的最優(yōu)（如時(shí)間最優(yōu)、能耗最優(yōu)等）。

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

對(duì)于移動(dòng)障礙物，文獻(xiàn)[11]提出軌道變形算法，根據(jù)移動(dòng)障礙物信息實(shí)時(shí)修改已知環(huán)境下的預(yù)定路線，當(dāng)移動(dòng)障礙超過算法約束時(shí)，則需要由1個(gè)帶時(shí)間約束的實(shí)時(shí)動(dòng)態(tài)規(guī)劃器[12]重新規(guī)劃路徑。此方法可以解決已知環(huán)境下的移動(dòng)障礙物，但對(duì)未知的動(dòng)態(tài)環(huán)境無能為力。

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

1 問題描述

1.1 規(guī)劃問題描述

將全方位移動(dòng)式機(jī)器人簡(jiǎn)化為1個(gè)質(zhì)點(diǎn)，考慮其在二維狀態(tài)空間R2內(nèi)從初始位置qstart到目標(biāo)位置qgoal的運(yùn)動(dòng)規(guī)劃問題。

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

1.2 矢量化路徑描述

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

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

式（1）中各個(gè)向量依次首尾相接，其中αik為構(gòu)成路徑Pi的第k個(gè)向量，αik以向量αi（k－1）的終點(diǎn)為起點(diǎn)，記作αik＝ （θik，‖αik‖ ），式（1）中θik是向量αik與向量αi（k－1）的夾角，取順時(shí)針方向?yàn)檎?，逆時(shí)針方向?yàn)樨?fù)，－π≤θik≤π，‖αik‖為向量的模，即機(jī)器人在轉(zhuǎn)動(dòng)θik方向后應(yīng)行進(jìn)的距離。

機(jī)器人首先根據(jù)初始位置和目標(biāo)位置創(chuàng)建初始路徑，然后按照路徑行進(jìn)，并在行進(jìn)過程中，不斷檢測(cè)環(huán)境中障礙物信息，并根據(jù)檢測(cè)到的障礙物信息和中間點(diǎn)生成法則來生成中間點(diǎn)，最后插入中間點(diǎn)、實(shí)時(shí)更新路徑，從而實(shí)時(shí)規(guī)劃出無碰撞路徑。其初始路徑P0由以起始位置為起點(diǎn)、目標(biāo)位置為終點(diǎn)構(gòu)成的向量組成，有

其中：以機(jī)器人的初始方向?yàn)榛鶞?zhǔn)，轉(zhuǎn)向目標(biāo)位置所轉(zhuǎn)過的角度記為θ01，起始位置到目標(biāo)位置間的直線距離記為‖α01‖。

機(jī)器人沿初始路徑開始移動(dòng)，并不斷對(duì)周邊環(huán)境進(jìn)行檢測(cè)，一旦發(fā)現(xiàn)有障礙物，則需要增加路徑中間點(diǎn)以更新規(guī)劃路徑，從而實(shí)現(xiàn)避障。

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

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

2 規(guī)劃算法

2.1 障礙物檢測(cè)

機(jī)器人按規(guī)劃路徑行進(jìn)過程中，需要不斷根據(jù)檢測(cè)到的環(huán)境信息，判斷前方是否有障礙物阻擋或?qū)⒁钃醍?dāng)前路徑。圖2給出了障礙物檢測(cè)的4種情況。圖中淺色矩形為第一次檢測(cè)到障礙物時(shí)障礙物所在位置；深色矩形為經(jīng)過tsensor時(shí)間后，障礙物所在位置；q表示機(jī)器人當(dāng)前所在位置；大圓形表示檢測(cè)范圍Rsensor；小圓形表示安全距離Rsafe；pi表示檢測(cè)到的障礙物點(diǎn)，p1是第一次檢測(cè)到的障礙物點(diǎn)，p2和p3分別是tsensor時(shí)間后檢測(cè)到的左右2個(gè)邊緣的障礙物點(diǎn)；φi表示當(dāng)前路徑間與q到pi之間的連線的夾角，以當(dāng)前路徑方向?yàn)槠鹗挤较?，逆時(shí)針方向?yàn)檎?，順時(shí)針方向?yàn)樨?fù)。

情況（a）時(shí)，經(jīng)過時(shí)間tsensor后，無法檢測(cè)到障礙物，此時(shí)判定該障礙物無法阻擋當(dāng)前路徑，機(jī)器人仍沿原來路徑行進(jìn)。

情況（b），（c）和（d）時(shí)，須按下列不等式組進(jìn)行判斷：

若滿足不等式組（9），則認(rèn)為此障礙物正向遠(yuǎn)離障礙物的方向移動(dòng)，機(jī)器人可沿原路徑行進(jìn)；否則，則認(rèn)為障礙物正朝向阻擋路徑方向移動(dòng)，或保持不動(dòng)，需計(jì)算中間插入點(diǎn)更新路徑。

圖2 障礙物檢測(cè)示意圖Fig.2 Obstacle detection

2.2 插入點(diǎn)生成

當(dāng)檢測(cè)到有障礙物阻擋當(dāng)前路徑時(shí)，首先計(jì)算出最大插入點(diǎn)距離dmax，然后按照本文提出的下列規(guī)則計(jì)算中間插入點(diǎn)。

規(guī)則1 取tsensor時(shí)間后檢測(cè)到的機(jī)器人與障礙物間最長(zhǎng)距離為dmax，Rsafe≤dmax≤Rsensor，以dmax為向量長(zhǎng)度，機(jī)器人當(dāng)前位置為向量起始點(diǎn)，以機(jī)器人當(dāng)前行進(jìn)方向?yàn)榛鶞?zhǔn)，按角度θ做向量，若該向量無障礙阻擋，則將該向量添加到新的路徑中，且該向量的終點(diǎn)為插入的中間點(diǎn)；若有障礙物阻擋，則取θ＝θ＋Δθ，Δθ為規(guī)定的角度值，重復(fù)上述步驟做向量；若直至θ＝2π時(shí)仍未有向量滿足上述要求，則取dmax＝α·dmax，0＜α＜1，重新以角度θ做向量，重復(fù)以上過程，其中

2.3 更新路徑

當(dāng)計(jì)算出中間插入點(diǎn)之后，需要根據(jù)1.2章節(jié)中給出的路徑更新計(jì)算方法，計(jì)算出更新后的路徑，然后令機(jī)器人按照更新后的路徑繼續(xù)行進(jìn)。

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

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

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

從圖3的仿真結(jié)果中可以看出，本文提出的方法可以預(yù)測(cè)出移動(dòng)障礙物的運(yùn)動(dòng)趨勢(shì)，實(shí)現(xiàn)提前根據(jù)其運(yùn)動(dòng)方向選擇是否繞行障礙物、或在繞行障礙物時(shí)選擇與障礙物運(yùn)動(dòng)方向相反的方向繞行，最終使機(jī)器人自主地從初始位置安全、無碰撞的到達(dá)目標(biāo)位置。

表1給出了實(shí)驗(yàn)數(shù)據(jù)。假設(shè)機(jī)器人移動(dòng)及距離檢測(cè)的時(shí)間為零，表中總耗時(shí)僅為計(jì)算中間點(diǎn)及規(guī)劃路徑的耗用時(shí)間。結(jié)果證明本算法實(shí)時(shí)性可滿足要求。

表1 不同地圖規(guī)劃結(jié)果對(duì)比Table 1 Result data of different maps

4 結(jié)語(yǔ)

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

[1] Masoud Ahmad A.Decentralized self－organizing potential fieldbased control for individually motivated mobile agents in a cluttered environment：A vector－h(huán)armonic 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，F(xiàn)raichard T.Navigating Dynamic Environments Using Trajectory Deformation[C].France：IEEE－RSJ Int Conf on Intelligent Robots and Systems，2008：226－233.

[12]Delsart V，F(xiàn)raichard 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.