一種動態(tài)不確定環(huán)境下的機(jī)器人路徑規(guī)劃算法

武心安，孫 堯，莫宏偉

(哈爾濱工程大學(xué)自動化學(xué)院，哈爾濱150001，wxashow@126.com)

目前，未知動態(tài)環(huán)境下機(jī)器人的實(shí)時路徑規(guī)劃的研究工作主要集中在對可用環(huán)境信息的分析處理和在此基礎(chǔ)上的機(jī)器人動作規(guī)劃上.對環(huán)境信息的分析和處理主要有:根據(jù)機(jī)器人和障礙物的安全距離直接規(guī)劃機(jī)器人的運(yùn)動動作［1-2］，在預(yù)定運(yùn)動集合中選擇機(jī)器人的運(yùn)動動作［3-4］，運(yùn)用關(guān)鍵特征信息規(guī)劃機(jī)器人的運(yùn)動動作［5］3種.在未知動態(tài)不確定環(huán)境下，機(jī)器人所在位置到目標(biāo)點(diǎn)之間的路徑是由連續(xù)的運(yùn)動動作構(gòu)成的.有很多文獻(xiàn)描述了此類方法，例如基于柵格的A*算法［6］、voronoi diagrams and visibility graphs算法［7］、人工勢場法［8］.有些算法只是應(yīng)用全局的方法并計算靜態(tài)障礙物，有些算法則由于面臨未考慮到的環(huán)境從而使得機(jī)器人陷入死區(qū).為了選擇合理的運(yùn)動動作，文獻(xiàn)［9］將啟發(fā)式函數(shù)引入進(jìn)來.啟發(fā)式函數(shù)通過損失函數(shù)從可能的動作集中選擇最優(yōu)化動作，實(shí)現(xiàn)機(jī)器人運(yùn)動動作的連續(xù)性和最優(yōu)性.

本文提出了一種新的實(shí)時路徑規(guī)劃算法，在環(huán)境信息中提取用于規(guī)劃的雙安全邊緣信息，減少環(huán)境信息的處理量，并運(yùn)用啟發(fā)式函數(shù)規(guī)劃機(jī)器人的運(yùn)動動作.

1 實(shí)時環(huán)境信息描述

1.1 搜索雙安全邊緣信息

1.1.1 搜索傳感器邊緣

傳感器邊緣是根據(jù)傳感器信息直接搜索到的障礙物邊緣，根據(jù)探測到的障礙物位置可以判斷其距離和方向.如圖1(a)所示，o點(diǎn)為機(jī)器人所在質(zhì)點(diǎn)(傳感器也視為和機(jī)器人質(zhì)點(diǎn)重合的質(zhì)點(diǎn))，機(jī)器人質(zhì)點(diǎn)為圓心的自身安全區(qū)域，半徑為rs;傳感器簇的最大探測角度是π，最大探測半徑為Rmax，障礙物為ob1和ob2;移動機(jī)器人與障礙物邊緣點(diǎn)的距離為di.將傳感器探測范圍分為180個等份，相鄰之間角度為1°，該半圓的直徑與機(jī)器人軸線垂直.根據(jù)判定規(guī)則參數(shù)SEi，可以快速搜索到傳感器邊緣點(diǎn)集{a，b，c，d}.其中SEi= Rmax-di，i∈［0，180］.判定存在障礙物邊緣點(diǎn)的原則為:若SEi≥2rs，則存在傳感器邊緣點(diǎn)Pi;判定傳感器邊緣點(diǎn)方向的原則為:假設(shè)從機(jī)器人左側(cè)開始搜索傳感器邊緣點(diǎn)，若min{SEi-1，SEi+1}= SEi-1，則Pi的方向相對于機(jī)器人自身為左，記為L，若min{SEi-1，SEi+1}=SEi+1，則Pi的方向相對于機(jī)器人自身為右，記為R.如圖1(b)所示為某一時刻SE的狀態(tài)曲線，根據(jù)傳感器邊緣點(diǎn)判定規(guī)則可以得到傳感器邊緣點(diǎn)集{a，b，c，d}及相應(yīng)的方向集{R，L，R，L}.在圖1(a)中可以直觀地看到傳感器邊緣點(diǎn)所處情況，它們即為當(dāng)前環(huán)境信息下障礙物的傳感器邊緣點(diǎn).

圖1 搜索傳感器邊緣點(diǎn)

1.1.2 搜索機(jī)器人雙安全邊緣

在上節(jié)中搜索的邊緣點(diǎn)是基于傳感器的障礙物邊緣點(diǎn)，也就是把機(jī)器人看成是一個質(zhì)點(diǎn).但是，實(shí)際的機(jī)器人有自己特定的外形和動力學(xué)特性，簡化成質(zhì)點(diǎn)的結(jié)果會導(dǎo)致路徑規(guī)劃的失敗，因此，將機(jī)器人自身的安全區(qū)域和動力學(xué)影響范圍融入到環(huán)境信息中是十分必要的.本文將結(jié)合傳感器邊緣點(diǎn)和機(jī)器人自身的安全區(qū)域與動力學(xué)影響范圍，將當(dāng)前時刻的環(huán)境信息抽象為雙安全邊緣信息，為下一步規(guī)劃運(yùn)動動作做好前提工作.

機(jī)器人雙安全邊緣定義:在搜索到傳感器邊緣點(diǎn)的前提下，結(jié)合機(jī)器人自身的安全區(qū)域與動力學(xué)影響范圍，從傳感器邊緣點(diǎn)開始，按照其方向搜索障礙物上的點(diǎn)，使得通過這一點(diǎn)向機(jī)器人安全區(qū)域的切線同時又與障礙物相切，障礙物和機(jī)器人處在切線的異側(cè)，這些點(diǎn)稱之為雙安全邊緣點(diǎn)集，這些切線為雙安全邊緣集.

如圖2(b)所示為某一時刻SE的狀態(tài)曲線，根據(jù)雙安全邊緣點(diǎn)的定義可以搜索到雙安全邊緣點(diǎn)集{a'，b'，c'，d'}和相應(yīng)的方向集{R，L，R，L}.在圖2(a)中可以直觀地看到雙安全邊緣點(diǎn)情況，它們就是本文用來規(guī)劃機(jī)器人運(yùn)動動作的關(guān)鍵環(huán)境信息.

成功搜索到機(jī)器人雙安全邊緣的意義在于有效地分析和處理了某時刻傳感器所探測到的環(huán)境信息，簡化了環(huán)境信息，提高了實(shí)時性能，并兼顧了移動機(jī)器人自身的安全區(qū)域和動力學(xué)影響范圍，為能更加高效地規(guī)劃機(jī)器人的運(yùn)動動作做準(zhǔn)備.

圖2 搜索雙安全邊緣點(diǎn)

1.2 雙安全邊緣相關(guān)的物理量

如圖3所示，以雙安全邊緣點(diǎn)b'和c'為例. ob'oc'為機(jī)器人質(zhì)點(diǎn)到雙安全邊緣點(diǎn)的距離，m和n為移動機(jī)器人自身安全圓上的點(diǎn)，om=on=rs. mb'和nc'為雙安全邊緣點(diǎn)到機(jī)器人自身安全圓的切線長度，mb'=(ob'2-om2)1/2，nc'=(oc'2-on2)1/2.op平行mb'，oq平行nc'，向量p和q為當(dāng)前信息下移動機(jī)器人的備選規(guī)劃航向，mb'和nc'為當(dāng)前信息下的備選安全距離.向量p和q之間的夾角為α，此α角用來判斷機(jī)器人在當(dāng)前航向下能否在兩個障礙物之間通過.

圖3 雙安全邊緣的安全距離和方向

1.3 雙安全邊緣的類型

在對復(fù)雜的環(huán)境信息分析和處理并搜索到了用于規(guī)劃機(jī)器人運(yùn)動動作的雙安全邊緣后，對這些雙安全邊緣的類型進(jìn)行劃分，分別為單雙邊緣、多雙邊緣和無雙邊緣情況，見表1.

1.3.1 單雙邊緣

如表1中a所示，目標(biāo)點(diǎn)g和機(jī)器人安全圓上的點(diǎn)m連線為mg，只要mg與障礙物信息有交點(diǎn)k，則移動機(jī)器人必須要繞開障礙物ob.根據(jù)當(dāng)前環(huán)境信息搜索到安全邊緣點(diǎn)b'，安全邊緣線mb'，op平行mb'，則移動機(jī)器人的運(yùn)動方向?yàn)椤蟬=∠op，安全航行距離為de-e=mb'.

表1 雙安全邊緣類型

1.3.2 多雙邊緣

如表1中b所示，目標(biāo)點(diǎn)g和機(jī)器人安全圓上的點(diǎn)m2和n1連線為m2g和n1g，只要m2g和n1g與障礙物信息有交點(diǎn)k1和k2，則移動機(jī)器人必須要繞開障礙物ob2.根據(jù)當(dāng)前環(huán)境信息搜索到雙安全點(diǎn)集{a，b，c，d，e，f}，與障礙物ob2相關(guān)的安全邊緣集{m1b，n1c，m2d，n2e}，可行的路線集為{p1，q1，p2，q2}，α1和α2分別為向量p1和q1以及p2和q2的夾角.

1.3.3 無雙邊緣

根據(jù)機(jī)器人的當(dāng)前傳感器信息搜索不到雙安全邊緣.一種情況是沒有探測到障礙物，則機(jī)器人可以在所探測的區(qū)域內(nèi)向任何方向運(yùn)動;另一種情況是障礙物探測范圍部分或者全部被遮蓋.如表1中c所示部分遮蓋情況，此情況下沒有傳感器邊緣點(diǎn)，搜索不到雙安全邊緣點(diǎn)，則機(jī)器人進(jìn)入游弋狀態(tài)進(jìn)行搜索雙安全邊緣點(diǎn).游弋原則為:若傳感器探測范圍)cd部分被遮蓋，則選取與障礙物交接點(diǎn)集{a，b}，將點(diǎn)集合{a，b}按照雙安全邊緣點(diǎn)處理方法處理得到方向集為{oa'，ob'}，并用啟發(fā)式算法選取方向，即方向oa'，進(jìn)行搜索雙安全邊緣，直到搜索到雙安全雙邊緣為止;若傳感器探測范圍)cd全部被遮蓋，則選取傳感器端點(diǎn)集{c，d}，可行方向集為{oc，od}，并用啟發(fā)式算法選取方向，即方向oc，進(jìn)行搜索雙安全邊緣，直到搜索到雙安全雙邊緣為止.

2 雙安全邊緣算法設(shè)計

2.1 局部雙安全子目標(biāo)的選取

選取局部子目標(biāo)的目的是確定機(jī)器人當(dāng)前狀態(tài)下的安全運(yùn)動方向和距離，本文確定子目標(biāo)點(diǎn)的方法是跟蹤雙安全邊緣點(diǎn)，采用啟發(fā)式函數(shù)來完成局部子目標(biāo)S的選擇.單雙安全邊緣點(diǎn)情況下機(jī)器人對運(yùn)行的方向和安全距離沒有選擇，局部子目標(biāo)即為雙安全邊緣點(diǎn)，只需按照當(dāng)前的安全距離和安全方向進(jìn)行運(yùn)動動作的規(guī)劃.所以，啟發(fā)式函數(shù)的應(yīng)用，主要是針對多雙安全邊緣和無雙安全邊緣情況，根據(jù)從環(huán)境信息中提取的雙安全邊緣信息，定義啟發(fā)式函數(shù)形式如式(1)所示:

其中:i∈{1，2，…，n}，j∈{1，2，…，m}.

為達(dá)到優(yōu)化路徑的目的，選取的子目標(biāo)點(diǎn)必須要滿足函數(shù)f(si)的最小化.其中λ(αj)為雙安全邊緣點(diǎn)閾值參數(shù)(開關(guān)函數(shù)).它是判斷在當(dāng)前環(huán)境信息下，機(jī)器人是否有足夠的空間在兩障礙物之間穿越，能穿越，其值為1，否則為0.g(si)為機(jī)器人在當(dāng)前環(huán)境信息下可移動的安全距離函數(shù).它是當(dāng)前環(huán)境信息下備選安全雙邊緣線的長度，可以由機(jī)器人質(zhì)點(diǎn)到安全邊緣點(diǎn)的距離di(o-e)和機(jī)器人的自身安全圓半徑rs求得.h(si)為機(jī)器人在當(dāng)前環(huán)境信息下備選安全子目標(biāo)點(diǎn)到目標(biāo)點(diǎn)的距離，可以由備選子目標(biāo)點(diǎn)和目標(biāo)點(diǎn)的位置求得.通過啟發(fā)式函數(shù)即可選擇用于規(guī)劃機(jī)器人運(yùn)動動作的最優(yōu)雙安全邊緣點(diǎn)，即為局部子目標(biāo)點(diǎn).

2.2 對新障礙物和動態(tài)障礙物的探測

根據(jù)雙安全邊緣的搜索原理可知，只需跟蹤雙安全邊緣點(diǎn)數(shù)量和位置變化，就能準(zhǔn)確地判斷障礙物的出現(xiàn)或消失.當(dāng)雙安全邊緣點(diǎn)數(shù)量和位置變化時，首先需要判斷此障礙物是否是動態(tài)障礙物，即判斷連續(xù)兩時刻機(jī)器人同一位置雙安全邊緣是否變化，若無變化則為靜態(tài)障礙物，按照規(guī)劃好的路徑運(yùn)動;若有變化則為動態(tài)障礙物，需要預(yù)測雙安全邊緣的運(yùn)動規(guī)律來近似動態(tài)障礙物的運(yùn)動規(guī)律，并對機(jī)器人的運(yùn)動方向和安全距離進(jìn)行調(diào)整.

機(jī)器人通過跟蹤雙安全邊緣點(diǎn)數(shù)量和位置變化探測到新障礙物后，需要在原地探測至少兩個時刻{t1，t2}的傳感器信息，并分別進(jìn)行機(jī)器人雙安全邊緣的搜索，通過兩時刻的雙安全邊緣信息確定機(jī)器人安全航向和安全航行距離.如圖4所示，在t1時刻，移動機(jī)器人根據(jù)當(dāng)前環(huán)境信息搜索

到的雙安全邊緣點(diǎn)為 a't1，安全航行距離為d(e-e)t1=mt1a't1，安全運(yùn)動方向?yàn)椤蟬t1=∠opt1;同理在t2時刻可搜索到安全雙邊緣點(diǎn)為a't2，安全航行距離為 d(e-e)t2=mt2a't2，安全運(yùn)動方向?yàn)椤蟬t2=∠opt2.于是可以估計出一側(cè)雙安全邊緣點(diǎn)在t=t1-t2內(nèi)的運(yùn)動距離^se、速度^ve和運(yùn)動方向∠^ve，分別為

同理可以估計出另一側(cè)雙安全邊緣點(diǎn)的運(yùn)動規(guī)律.運(yùn)用這種方法可以對動態(tài)障礙物狀態(tài)進(jìn)行實(shí)時監(jiān)控，從而提高對動態(tài)障礙物運(yùn)動規(guī)律預(yù)測的精度.本文通過對雙安全邊緣的跟蹤來近似動態(tài)障礙物的運(yùn)動規(guī)律.在此基礎(chǔ)上就可對移動機(jī)器人和動態(tài)障礙物在未來時刻的相對位置進(jìn)行預(yù)測.

圖4 探測新的障礙物和動態(tài)障礙物

2.3 對動態(tài)障礙物的躲避算法

如圖4所示，機(jī)器人在當(dāng)前環(huán)境信息下的安全航行距離和方向分別為d(e-e)t1和∠st1，動態(tài)障礙物(雙安全邊緣)的運(yùn)動速度和方向分別為^ve和∠^ve，則可進(jìn)行對動態(tài)障礙物運(yùn)動規(guī)律的預(yù)測及指導(dǎo)機(jī)器人采取相應(yīng)的躲避方法.令 lse= mt1a't1為機(jī)器人靠近動態(tài)障礙物一側(cè)的安全邊界向量，lve=a't1a't2為動態(tài)障礙物雙安全邊緣點(diǎn)運(yùn)動向量，d⊥為兩個向量的距離，如圖4所示.

根據(jù)具體情況分為如下4種處理方法:

(a)若lse平行l(wèi)ve，且d⊥≥rs，則動態(tài)障礙物不會影響機(jī)器人當(dāng)前的安全運(yùn)動方向和安全運(yùn)動距離.

(b)若lse平行l(wèi)ve，且d⊥＜rs，則根據(jù)機(jī)器人安全邊界和雙安全邊緣點(diǎn)的相對速度計算是否能相碰.若能相碰，則需要預(yù)測雙安全邊緣點(diǎn)在未來時刻的位置，重新確定機(jī)器人運(yùn)動的安全方向和安全距離.

(c)若lse不平行l(wèi)ve，且兩向量預(yù)測不會相交，則動態(tài)障礙物不會影響機(jī)器人當(dāng)前的安全運(yùn)動方向和安全運(yùn)動距離.

(d)若lse不平行l(wèi)ve，且兩向量預(yù)測會相交，則預(yù)測交點(diǎn)時刻移動機(jī)器人能否通過交點(diǎn)，若能通過，則視為動態(tài)障礙物不會影響機(jī)器人當(dāng)前的安全運(yùn)動方向和安全運(yùn)動距離.若不能通過，則根據(jù)動態(tài)障礙物兩側(cè)安全雙邊緣點(diǎn)的運(yùn)動規(guī)律重新制定移動機(jī)器人的航向和安全距離.

3 算法流程

圖5所示為移動機(jī)器人安全雙邊緣點(diǎn)路徑規(guī)劃算法決策樹模型.可以看出算法針對動態(tài)不確定環(huán)境下靜態(tài)和動態(tài)障礙物進(jìn)行分類描述和處理，最終規(guī)劃出每一時刻的機(jī)器人運(yùn)動動作，其中原則1、原則2和原則3分別為第2.2、1.3和2.3敘述的原則，具體算法流程如下:

圖5 DSE算法決策樹

Step1 根據(jù)當(dāng)前狀態(tài)傳感器信息按照原則1檢測是否有新障礙物出現(xiàn)并判斷障礙物是靜態(tài)還是動態(tài)障礙物.若靜態(tài)障礙物，轉(zhuǎn)到step2;若動態(tài)障礙物，轉(zhuǎn)到step3.

Step2 根據(jù)原則2判斷雙安全邊緣點(diǎn)所屬類型，轉(zhuǎn)到step4.

Step3 根據(jù)原則3判斷動態(tài)障礙物所屬4種類型分別處理后，根據(jù)原則2判斷雙安全邊緣點(diǎn)所屬類型，轉(zhuǎn)到step4.

Step4 3種雙安全邊緣點(diǎn)類型及規(guī)劃方法:

1)按照單雙安全邊緣方法規(guī)劃出安全運(yùn)動方向和安全距離后，轉(zhuǎn)到step5;

2)按照多雙安全邊緣方法規(guī)劃出安全運(yùn)動方向和安全距離后，轉(zhuǎn)到step5;

3)按照無雙安全邊緣方法規(guī)劃出安全運(yùn)動方向和安全距離后，轉(zhuǎn)到step5.

Step5.機(jī)器人按照step4規(guī)劃的機(jī)器人運(yùn)動動作運(yùn)動，若子目標(biāo)點(diǎn)是目標(biāo)點(diǎn)，則規(guī)劃結(jié)束，否則轉(zhuǎn)到step1.

4 算法在不同環(huán)境下的仿真

圖6是對本文提出的算法在U型(死區(qū))環(huán)境信息和復(fù)雜靜態(tài)環(huán)境下進(jìn)行的仿真結(jié)果.仿真圖中黑色圓點(diǎn)為機(jī)器人每一時刻所處位置，在每一時刻機(jī)器人通過雙安全邊緣算法規(guī)劃當(dāng)前時刻的安全方向和距離，圓點(diǎn)之間的連線即為機(jī)器人的實(shí)際路徑.在圖6(a)中，機(jī)器人首先進(jìn)入小U型環(huán)境并擺脫U型環(huán)境，然后進(jìn)入連續(xù)的狹長U型環(huán)境，最終到達(dá)目標(biāo)點(diǎn).圖6(b)中，機(jī)器人處在復(fù)雜環(huán)境下，通過傳感器探測到的局部環(huán)境信息最終到達(dá)目標(biāo)點(diǎn).

圖6 靜態(tài)環(huán)境下的仿真結(jié)果

表2是對本文提出的算法在含有動態(tài)障礙物的未知環(huán)境下進(jìn)行的仿真結(jié)果，機(jī)器人在起始點(diǎn)到達(dá)目標(biāo)點(diǎn)過程中，需要面臨3個動態(tài)障礙物，它們的運(yùn)動速度和方向分別為v1，v2，v3和相對地圖向左，向右和向上.其中(1)～(3)分別是對3個不同相對位置的動態(tài)障礙物進(jìn)行的避碰，(a)～(d)為機(jī)器人與動態(tài)障礙物之間4個相對位置狀態(tài).可以看到在(1)中，機(jī)器人在探測并且預(yù)測了動態(tài)障礙物的運(yùn)動規(guī)律后，采取躲避策略成功避開動態(tài)障礙物;在(2)中，機(jī)器人在探測并且預(yù)測了動態(tài)障礙物的運(yùn)動規(guī)律后，采取跟隨策略，直到有新的空間出現(xiàn)，從而脫離動態(tài)障礙物;在(3)中，機(jī)器人在探測并且預(yù)測了動態(tài)障礙物的運(yùn)動規(guī)律后，計算出機(jī)器人無需采取任何措施，直接到達(dá)目標(biāo)點(diǎn).從仿真結(jié)果可以看出，本文提出的動態(tài)不確定環(huán)境下的路徑規(guī)劃算法在復(fù)雜的靜態(tài)和動態(tài)環(huán)境下都能夠成功的完成規(guī)劃任務(wù)，體現(xiàn)了此算法的強(qiáng)實(shí)時、強(qiáng)適應(yīng)和較優(yōu)化的特點(diǎn).

表2 動態(tài)環(huán)境下的仿真結(jié)果

5 結(jié)論

1)本文提出的雙安全邊緣算法分析和處理傳感器探測的有限實(shí)時環(huán)境信息，在保證機(jī)器人運(yùn)動安全的前提下，搜索可用于規(guī)劃機(jī)器人運(yùn)動動作的雙安全邊緣點(diǎn)信息，簡化了復(fù)雜的環(huán)境信息，降低了運(yùn)算量，并結(jié)合了具有優(yōu)化路徑能力的啟發(fā)式思想，增強(qiáng)了在動態(tài)不確定環(huán)境下的適應(yīng)性和實(shí)時性.

2)仿真證實(shí)了本文算法的可行性.該方法中局部子目標(biāo)的選取，也為同步地圖創(chuàng)建和定位中機(jī)器人運(yùn)動方式提供了新的方案.

［1］AZARM K，SCHMIDT G.Integrated mobile robot motion planning and execution in changing indoor environment［C］//IEEE International conference on Intelligent Robot and System.Munich:IEEE，1994:298-305.

［2］WILLMS A R，YANG S X.An efficient dynamic system for real-time robot-path planning［J］.IEEE Transactions on System，Man and Cybernetics，Part B(Cybernetics)，2006，36(4):755-766.

［3］FEITEN W，BAUER R，LAWITZKY G.Robust obstacle avoidance in unknown and crmped environment［C］//IEEE Int Conference on Robotics and Automation.San Diego:IEEE，1994:2412-2417.

［4］FOX D，BURGARD W.The dynamic window approach to collision avoidance［J］.IEEE Robotics＆Automation Magazine，1997，4(1):23-33.

［5］WU L，HORI Y.Real-time collision-free path planning for robot manipulator based on octree model［C］//9thIEEE International Worshop on Advanced Motion Control.Istanbul:IEEE，2006:284-288.

［6］KOENING S，LIHACHEV M.Fast replanning for navigation in unknown terrain［J］.IEEE Transactions on Robotics，2005，21(3):354-363.

［7］HAMMOURI O M，MUSTAFA M.Voronoi Path Planning Technique for Recovering Communication in UAVs［C］// IEEE/ACS International Conference in Computer Systems and Applications.Doha:IEEE，2008:403-406.

［8］JUAN A F，JAVIER G.Hierarchical Graph Search for Mobile Robot Path Planning［C］//Proceeding of the 1998 IEEE International Conference on Robotics＆Automation.Leuven:IEEE，1998:656-661.

［9］CHA Y Y.Navigation of a free-ranging mobile robotusing heuristic local path-planning algorithm［J］.Robotics and Computer-Integrated Manufacturing，1997，13 (2):145-156.