未知環(huán)境下室內(nèi)輪式機(jī)器人路徑規(guī)劃算法

李 軒，劉曉東，石祥濱

(沈陽航空航天大學(xué) a.電子信息工程學(xué)院； b.計(jì)算機(jī)學(xué)院，沈陽 110136)

隨著移動(dòng)機(jī)器人技術(shù)的不斷發(fā)展，其應(yīng)用領(lǐng)域已經(jīng)拓展到人們的日常生活中，這對移動(dòng)機(jī)器人自主運(yùn)動(dòng)的要求不斷提高。作為自主運(yùn)動(dòng)的基礎(chǔ)，路徑規(guī)劃成為移動(dòng)機(jī)器人研究領(lǐng)域的一個(gè)重要分支[1-3]。路徑規(guī)劃是指在移動(dòng)機(jī)器人的工作空間中搜索出一條以初始狀態(tài)為起點(diǎn)、目標(biāo)狀態(tài)為終點(diǎn)，按照某種性能指標(biāo)(如時(shí)間、距離、能量等)最優(yōu)或次優(yōu)的無碰撞路徑[4-5]。目前，路徑規(guī)劃根據(jù)環(huán)境信息的掌握程度分為兩種，一是在環(huán)境信息已知情況下的靜態(tài)路徑規(guī)劃，無法在具有動(dòng)態(tài)因素的環(huán)境中進(jìn)行路徑規(guī)劃；二是在環(huán)境信息未知或者部分未知情況下，通過傳感器獲取環(huán)境信息從而進(jìn)行動(dòng)態(tài)路徑規(guī)劃[6-8]，其可有效地躲避環(huán)境中的動(dòng)態(tài)因素。

快速擴(kuò)展隨機(jī)樹(Rapidly-exploring Random Tree，RRT)算法是靜態(tài)路徑規(guī)劃主流方法之一，其避免了靜態(tài)路徑規(guī)劃需要對環(huán)境進(jìn)行建模的過程，具有計(jì)算量小且速度快等優(yōu)點(diǎn)[9]，但是RRT算法依據(jù)隨機(jī)采樣進(jìn)行擴(kuò)展導(dǎo)致其具有較大的隨機(jī)性。研究者針對RRT算法隨機(jī)性大的缺點(diǎn)對其進(jìn)行改進(jìn)[10-12]，有效降低了算法的隨機(jī)性。人工勢場法(Artificial Potential Field，APF)屬于動(dòng)態(tài)路徑規(guī)劃，其是一種通過構(gòu)建虛擬力學(xué)勢場進(jìn)行路徑規(guī)劃的方法，在規(guī)劃路徑過程中易陷入局部最小值[13]，為克服此缺點(diǎn)有較多的改進(jìn)方法[14-15]。

針對傳統(tǒng)RRT算法，本文提出一種R-QRRT(Regional Quantitative RRT)算法，通過區(qū)域量化和方向引導(dǎo)降低RRT算法隨機(jī)性，改善了節(jié)點(diǎn)分布，提高了算法效率。針對傳統(tǒng)APF算法易陷入局部最小值的問題，本文提出了一種通過距離評估障礙物對機(jī)器人作用，選擇性忽略某障礙物影響的D-APF(Distance APF)算法，使機(jī)器人快速逃離局部最小值點(diǎn)。對本文研究的室內(nèi)未知環(huán)境下輪式機(jī)器人路徑規(guī)劃問題，提出R-QRRT算法和D-APF算法相結(jié)合的混合算法，通過傳感器信息對環(huán)境中障礙物進(jìn)行類型區(qū)分，當(dāng)檢測到存在動(dòng)態(tài)障礙物時(shí)，通過D-APF算法進(jìn)行局部路徑規(guī)劃，達(dá)到躲避動(dòng)態(tài)障礙物目的；未檢測到動(dòng)態(tài)障礙物時(shí)，通過R-QRRT算法進(jìn)行全局路徑規(guī)劃，最后在真實(shí)環(huán)境中驗(yàn)證了混合算法具有良好的路徑規(guī)劃效果。

1 機(jī)器人相關(guān)模型

本文研究內(nèi)容主要針對基于二輪差速運(yùn)動(dòng)模型的室內(nèi)輪式機(jī)器人，通過二維激光雷達(dá)獲取環(huán)境信息。

1.1 運(yùn)動(dòng)學(xué)模型

二輪差速輪式機(jī)器人在運(yùn)動(dòng)控制方面有較高的靈活性，在進(jìn)行路徑規(guī)劃時(shí)便于控制。二輪差速運(yùn)動(dòng)學(xué)模型如圖1所示。

圖1 二輪差速運(yùn)動(dòng)學(xué)模型

機(jī)器人在空間中一點(diǎn)的描述包括其位姿信息和運(yùn)動(dòng)狀態(tài)，假設(shè)時(shí)刻t0機(jī)器人處于圖中的A點(diǎn)，位姿為(xA,yA,θA)，(xA,yA)是當(dāng)前的位置，θA是當(dāng)前的航向角，線速度v、角速度ω為當(dāng)前的運(yùn)動(dòng)狀態(tài)。B點(diǎn)為時(shí)刻t0+△t機(jī)器人位置。根據(jù)圖中幾何關(guān)系，當(dāng)△t趨向零時(shí)，得到機(jī)器人的運(yùn)動(dòng)學(xué)模型表達(dá)式，如式(1)所示。

(1)

其中，vL和vR為機(jī)器人左右輪子的速度。以A點(diǎn)為機(jī)器人的起始點(diǎn)，通過速度v和ω可以求得任意時(shí)刻t機(jī)器人的位姿如式(2)所示。

(2)

1.2 障礙物模型

路徑規(guī)劃任務(wù)的本質(zhì)是躲避環(huán)境中的障礙物，因此需要對環(huán)境信息進(jìn)行探測，建立障礙物模型。首先通過二維激光雷達(dá)獲取障礙物點(diǎn)的距離與角度信息，并判斷障礙物點(diǎn)是否屬于同一障礙物以及統(tǒng)計(jì)障礙物個(gè)數(shù)，采用聚類的方法描述各障礙物信息，包括障礙物類型、障礙物可探測邊緣的長度以及中心點(diǎn)，構(gòu)建障礙物模型。通過構(gòu)建關(guān)聯(lián)性函數(shù)判斷相鄰時(shí)刻障礙物是否為同一障礙物以及類型。

二維激光雷達(dá)探測到障礙物點(diǎn)的角度信息為當(dāng)前雷達(dá)轉(zhuǎn)動(dòng)角度，根據(jù)三角測距原理得到其距離信息，三角測距原理如圖2所示。

圖2 三角測距原理

根據(jù)圖2中相似三角形，有f/x=q/s，則點(diǎn)C與機(jī)器人的距離計(jì)算方法如式(3)所示。

(3)

在激光雷達(dá)探測過程中，設(shè)置自適應(yīng)閾值σ，比較連續(xù)障礙點(diǎn)A與B的距離與該閾值的關(guān)系，判斷點(diǎn)A與B是否屬于同一障礙物。自適應(yīng)閾值σ的取值表達(dá)式如式(4)所示。

σ=dsin(0.5°)

(4)

其中，0.5°為激光雷達(dá)的角度分辨率，d為當(dāng)前測得障礙點(diǎn)的距離。當(dāng)A與B的距離大于σ時(shí)判定出現(xiàn)新的障礙物，否則A與B屬于同一障礙物。

采用聚類方法描述機(jī)器人當(dāng)前時(shí)刻所探測到障礙物的信息，如式(5)所示。

(5)

其中，IDk(t)表示障礙物類型；Ck(t)表示探測到障礙物邊緣線段的中心點(diǎn)；Sk(t)表示障礙物探測到邊緣的長度。通過式(6)所示的關(guān)聯(lián)性函數(shù)，判斷相鄰時(shí)刻障礙物是否為同一個(gè)障礙物以及其類型。

(6)

其中，λF、λG為系數(shù)，F(xiàn)為邊緣線段中心點(diǎn)之間的距離，G為前后位置測到障礙物不重合部分長度占總長度的比例，計(jì)算方式如式(7)所示。

(7)

設(shè)置閾值c，進(jìn)行相鄰時(shí)刻探測到障礙物匹配工作，當(dāng)C的取值大于該閾值時(shí)，判定障礙物OkI(t1)和Ok2(t2)為同一個(gè)障礙物。匹配工作之后，設(shè)置閾值csta、cdyn和ηk1對同一個(gè)障礙物的類型進(jìn)行判斷，判斷規(guī)則如表1。

表1 障礙物類型判斷規(guī)則

2 靜態(tài)路徑規(guī)劃

以RRT算法為基礎(chǔ)，針對其存在的隨機(jī)性大的問題，提出R-QRRT算法， R-QRRT算法通過區(qū)域量化與方向引導(dǎo)，降低RRT算法的隨機(jī)性、改善了樹節(jié)點(diǎn)的分布，提高了算法的規(guī)劃效率。在環(huán)境中不存在動(dòng)態(tài)障礙物時(shí)，通過R-QRRT算法進(jìn)行靜態(tài)路徑規(guī)劃。

2.1 區(qū)域量化模型

由于RRT算法采樣過程的隨機(jī)性，使得算法在擴(kuò)展過程中對已探索區(qū)域重復(fù)探索，產(chǎn)生較多的無用的樹節(jié)點(diǎn)。本文對樹節(jié)點(diǎn)周圍空間進(jìn)行量化，改善隨機(jī)樹節(jié)點(diǎn)分布情況，降低算法的隨機(jī)性。量化模型如圖3所示，隨機(jī)樹擴(kuò)展點(diǎn)選取的規(guī)則如表2，根據(jù)量化模型和擴(kuò)展點(diǎn)選取規(guī)則，隨機(jī)樹的節(jié)點(diǎn)之間的距離均大于擴(kuò)展步長，在相同區(qū)域不會(huì)出現(xiàn)多個(gè)節(jié)點(diǎn)的情況。

圖3 區(qū)域量化模型

劃分區(qū)域區(qū)域范圍選取擴(kuò)展點(diǎn)Ⅰ0～60度aⅡ60～120度bⅢ120～180度cⅣ180～240度dⅤ240～300度fⅥ300～360度e

2.2 R-QRRT算法原理

假設(shè)機(jī)器人工作空間為二維空間C，其中可自由移動(dòng)空間為Cfree。在自由空間中選取路徑規(guī)劃的起始點(diǎn)xstart和目標(biāo)點(diǎn)xgoal，滿足條件xstart，xgoal∈Cfree。以起始點(diǎn)為根節(jié)點(diǎn)構(gòu)建隨機(jī)樹Tk，其有k個(gè)節(jié)點(diǎn)且均位于自由空間。xi為隨機(jī)樹上的節(jié)點(diǎn)，即xi∈Tk，i=1,2,3,…，k。隨機(jī)樹擴(kuò)展過程如圖4所示。

圖4 隨機(jī)樹擴(kuò)展過程示意圖

在工作空間中隨機(jī)選取點(diǎn)xrand，判斷xrand∈Cfree是否成立，如果其不成立則重新選取xrand。在樹Tk上選取xnear，xnear滿足Dis(xnear,xrand)≤Dis(xi,xrand)，式中Dis(a,b)函數(shù)表示空間中點(diǎn)a和b的幾何距離，計(jì)算方法如式(8)所示。

(8)

3 動(dòng)態(tài)路徑規(guī)劃

APF算法有較好的實(shí)時(shí)性，且計(jì)算量相對于其他算法有明顯優(yōu)勢，但易于陷入局部最小值點(diǎn)。本文提出D-APF算法，通過評估在局部最小值點(diǎn)對機(jī)器人影響最小的障礙物，已到達(dá)逃離最小值點(diǎn)的效果。在環(huán)境中具有動(dòng)態(tài)障礙物時(shí)，采用D-APF算法進(jìn)行動(dòng)態(tài)路徑規(guī)劃，其原理如下。

D-APF算法將物理學(xué)中的勢場理論引入到路徑規(guī)劃問題中，構(gòu)建虛擬力學(xué)勢場，虛擬力學(xué)勢場分為以目標(biāo)點(diǎn)為中心的引力場和以障礙物為中心的斥力場，機(jī)器人根據(jù)合力勢場中受到的合力進(jìn)行運(yùn)動(dòng)。引力場和斥力場函數(shù)如式(9)和(10)所示。

(9)

Urep(x)=

(10)

式中x表示機(jī)器人的位置，η和λ為引力尺度系數(shù)，d0為斥力場的有效范圍。機(jī)器人在點(diǎn)x受到的引力和斥力如式(11)和(12)所示。

Fatt(x)=-Uatt(x)=ηDis(x,xgoal)

(11)

Frep(x)=-

(12)

引力和斥力分別指向目標(biāo)點(diǎn)和機(jī)器人。上式中，▽d(x,xobs)為障礙物指向機(jī)器人的向量單位化，如式(13)所示。

(13)

機(jī)器人在某一位置所受的合力為斥力與引力的矢量之和，合力計(jì)算公式如式(14)所示。

(14)

圖5為機(jī)器人在人工構(gòu)造勢場的受力情況示意圖。

圖5 機(jī)器人受力情況示意圖

當(dāng)機(jī)器人陷入局部最小值時(shí)，選擇對機(jī)器人影響最小的第k個(gè)障礙物進(jìn)行舍棄，該障礙物機(jī)器人的距離為d，舍棄后的合力表達(dá)式如式(15)所示。

(15)

機(jī)器人按照上述合力進(jìn)行運(yùn)動(dòng)，機(jī)器人的運(yùn)動(dòng)方向指向第k個(gè)障礙物。當(dāng)機(jī)器人運(yùn)動(dòng)距離到d/2時(shí)，恢復(fù)第k個(gè)障礙物的勢力場對機(jī)器人的影響，保證機(jī)器人逃離局部最小值。

4 混合路徑規(guī)劃算法

通過傳感器獲取環(huán)境信息，當(dāng)環(huán)境中不存在動(dòng)態(tài)障礙物時(shí)，機(jī)器人根據(jù)R-QRRT算法規(guī)劃出的靜態(tài)路徑進(jìn)行移動(dòng)，當(dāng)機(jī)器人當(dāng)前探測到環(huán)境中具有動(dòng)態(tài)障礙物時(shí)，根據(jù)D-APF算法規(guī)劃出路徑進(jìn)行移動(dòng)，直至到達(dá)目標(biāo)點(diǎn)?；旌下窂揭?guī)劃策略如圖6所示。

圖6 機(jī)器人混合路徑規(guī)劃流程圖

5 路徑規(guī)劃算法實(shí)驗(yàn)

實(shí)驗(yàn)部分分為仿真與真實(shí)環(huán)境實(shí)驗(yàn)兩方面，仿真方面通過MATLAB軟件分別對R-QRRT算法和D-APF算法進(jìn)行仿真，真實(shí)環(huán)境實(shí)驗(yàn)使用二輪差速移動(dòng)實(shí)驗(yàn)平臺對混合算法的可行性和有效性進(jìn)行驗(yàn)證。

5.1 R-QRRT算法MATLAB仿真

如圖7所示，為三次不同起始點(diǎn)和目標(biāo)點(diǎn)時(shí)傳統(tǒng)RRT算法、RRT-Edge算法和R-QRRT算法MATLAB仿真結(jié)果。圖中標(biāo)注起始點(diǎn)和目標(biāo)點(diǎn)，藍(lán)色分支表示該次路徑規(guī)劃時(shí)生成的隨機(jī)樹，黑色線段為最終路徑。表3為相應(yīng)的隨機(jī)樹節(jié)點(diǎn)數(shù)目和規(guī)劃時(shí)間。

圖7 傳統(tǒng)RRT算法RRT-Edge改進(jìn)RRT算法路徑規(guī)劃MATLAB仿真比較

組別傳統(tǒng)RRT節(jié)點(diǎn)數(shù)運(yùn)行時(shí)間RRT-Edge節(jié)點(diǎn)數(shù)運(yùn)行時(shí)間R-QRRT節(jié)點(diǎn)數(shù)運(yùn)行時(shí)間12757.181376.51154.5922088.421274.211093.4832334.551203.761183.63

由圖6和表3可以看出，R-QRRT算法與傳統(tǒng)RRT算法相比，隨機(jī)樹節(jié)點(diǎn)的數(shù)量減少了50%左右，且改善了樹節(jié)點(diǎn)在空間中的分布，不存在較多樹節(jié)點(diǎn)位于相同區(qū)域的情況；提高了路徑的搜索效率，提高算法的實(shí)時(shí)性。與RRT-Edge算法相比較，節(jié)點(diǎn)數(shù)量有小幅減少，但是在路徑規(guī)劃的效率上有明顯的提高，證明本文提出的R-QRRT算法有效減少了節(jié)點(diǎn)數(shù)量，改善了節(jié)點(diǎn)分布并且提高了路徑規(guī)劃的效率。

5.2 D-APF算法MATLAB仿真

如圖8為機(jī)器人根據(jù)人工勢場法進(jìn)行路徑規(guī)劃的仿真示意圖，其中起始點(diǎn)為點(diǎn)(0,0)，目標(biāo)點(diǎn)為(10,10)，圖中存在障礙物A、B、C和D，障礙物A為動(dòng)態(tài)障礙物，其余障礙物為靜態(tài)障礙物，曲線為機(jī)器人移動(dòng)軌跡。從圖7可以看出，D-APF可以有效地解決在具有動(dòng)態(tài)因素環(huán)境中的局部路徑規(guī)劃問題。

圖8 人工勢場法路徑規(guī)劃MATLAB仿真效果圖

5.3 混合路徑規(guī)劃算法真實(shí)環(huán)境實(shí)驗(yàn)

本文主要研究室內(nèi)環(huán)境未知情況下的路徑規(guī)劃問題，搭建如圖9中的室內(nèi)環(huán)境，采用本文提出的R_QRRT算法與D-APF算法相結(jié)合的混合算法進(jìn)行起始點(diǎn)到目標(biāo)點(diǎn)的路徑規(guī)劃。在初始位置，機(jī)器人探測到環(huán)境中的動(dòng)態(tài)障礙物，采用R-QRRT算法進(jìn)行路徑規(guī)劃，所得路徑如圖9路徑1中曲線；機(jī)器人按照路徑1運(yùn)動(dòng)到位置1，探測到環(huán)境中的動(dòng)態(tài)障礙物，采用D-APF算法進(jìn)行路徑規(guī)劃，所得路徑如圖9路徑2中曲線；當(dāng)機(jī)器人繼續(xù)移動(dòng)到達(dá)位置2時(shí)，動(dòng)態(tài)障礙物占據(jù)機(jī)器人所規(guī)劃路徑，機(jī)器人重新規(guī)劃路徑，如圖9路徑3所示；機(jī)器人移動(dòng)到位置3時(shí)，動(dòng)態(tài)障礙物停止移動(dòng)，機(jī)器人按照R-QRRT算法重新規(guī)劃路徑，如路徑4所示；之后機(jī)器人經(jīng)過位置4到達(dá)目標(biāo)點(diǎn)，該過程中規(guī)劃路徑如路徑5所示。

圖9 真實(shí)環(huán)境下混合規(guī)劃算法實(shí)驗(yàn)圖

實(shí)驗(yàn)表明，在具有動(dòng)態(tài)障礙物的室內(nèi)未知環(huán)境中，混合路徑規(guī)劃算法能夠?qū)崟r(shí)通過傳感器判斷環(huán)境中是否存在動(dòng)態(tài)障礙物，進(jìn)而選取R-QRRT或D-APF算法進(jìn)行路徑規(guī)劃，機(jī)器人按照規(guī)劃路徑安全無碰撞地從起始點(diǎn)移動(dòng)到目標(biāo)點(diǎn)。證明混合路徑規(guī)劃算法在具有動(dòng)態(tài)障礙物的室內(nèi)未知環(huán)境中具有有效性和可行性。

6 結(jié)論

對未知室內(nèi)環(huán)境下輪式機(jī)器人路徑規(guī)劃問題的研究，其目的在于豐富輪式機(jī)器人路徑規(guī)劃算法，為輪式機(jī)器人在室內(nèi)環(huán)境中提供一種行之有效的方法。本文提出的基于R-QRRT的靜態(tài)路徑規(guī)劃和基于D-APF的動(dòng)態(tài)路徑規(guī)劃相結(jié)合的混合算法不需要復(fù)雜的計(jì)算，易于實(shí)現(xiàn)。根據(jù)探測到的環(huán)境信息進(jìn)行路徑規(guī)劃，在遇到不同類型障礙物時(shí)，采用不同的路徑規(guī)劃方法，能有效躲避障礙物并到達(dá)目標(biāo)點(diǎn)。通過實(shí)驗(yàn)表明，該方法在實(shí)際環(huán)境中能夠?qū)ふ业揭粭l從起點(diǎn)到終點(diǎn)的安全路徑，具有有效性和可行性。