機(jī)器人在已知三維自然環(huán)境中的路徑規(guī)劃算法

梁家海

（欽州學(xué)院 數(shù)學(xué)與計(jì)算機(jī)科學(xué)學(xué)院，廣西 欽州535000）

0 引 言

移動(dòng)機(jī)器人路徑規(guī)劃是指在有障礙物環(huán)境中規(guī)劃出一條從指定起始位置到指定目標(biāo)位置、安全、無(wú)碰撞、最短（或運(yùn)行費(fèi)用最低）的路徑。移動(dòng)機(jī)器人路徑規(guī)劃依據(jù)機(jī)器人對(duì)環(huán)境的認(rèn)識(shí)分為環(huán)境已知的全局路徑規(guī)劃、環(huán)境未知的局部路徑規(guī)劃［1－2］。按環(huán)境的空間特征分為二維空間的路徑規(guī)劃、三維空間的路徑規(guī)劃［3－4］，對(duì)于二維空間的路徑規(guī)劃的主要方法有路線(xiàn)圖法（roadmap）、單元分解法（cell decomposition）、 人 工 勢(shì) 場(chǎng) 法（artificial potential field ，APF）、粒群算法等［5－7］，這些方法都能較好的解決二維空間的路徑規(guī)劃問(wèn)題。對(duì)于三維空間的路徑規(guī)劃，文獻(xiàn) ［8］給出一種基于神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)能量函數(shù)的移動(dòng)機(jī)器人三維路徑規(guī)劃算法，但該方法主要是針對(duì)規(guī)則的障礙物。文獻(xiàn)［9］提出了一種基于障礙物分類(lèi)的通行性方法，但缺乏全局性規(guī)劃。本文針對(duì)移動(dòng)機(jī)器人在已知三維環(huán)境中的路徑規(guī)劃問(wèn)題，對(duì)人工勢(shì)場(chǎng)法進(jìn)行改進(jìn)，提出了一種新的路徑規(guī)劃的算法，該算法較好解決了移動(dòng)機(jī)器人在已知的三維自然環(huán)境中的路徑規(guī)劃問(wèn)題。

1 三維環(huán)境中路徑規(guī)劃的原理

1.1 傳統(tǒng)人工勢(shì)場(chǎng)的路徑規(guī)劃原理

傳統(tǒng)的人工勢(shì)場(chǎng)理論指出：對(duì)于目標(biāo)導(dǎo)向的移動(dòng)機(jī)器人，無(wú)論其身處的環(huán)境包含靜止的障礙物還是動(dòng)態(tài)移動(dòng)障礙物，都可以定義并計(jì)算出一個(gè)人工勢(shì)場(chǎng)。該人工勢(shì)場(chǎng)中移動(dòng)機(jī)器人的目標(biāo)為一個(gè)吸引極，產(chǎn)生吸引力，每個(gè)障礙物為一個(gè)斥力極，產(chǎn)生排斥力，所有斥力和引力的合力決定了機(jī)器人的運(yùn)動(dòng)方向。移動(dòng)機(jī)器人沿合力方向從起始位置一直移動(dòng)到目標(biāo)位置，機(jī)器人的運(yùn)行軌跡即為人工勢(shì)場(chǎng)作用下規(guī)劃的一條路徑［10］。

1.2 人工勢(shì)場(chǎng)在三維自然環(huán)境中的路徑規(guī)劃

利用人工勢(shì)場(chǎng)法進(jìn)行二維環(huán)境中的路徑規(guī)劃時(shí)，其障礙物都是不可跨越的，所以二維環(huán)境中的路徑規(guī)劃的主要目標(biāo)就是找出一條能繞過(guò)障礙物的最短路徑。而在自然的三維環(huán)境中，移動(dòng)機(jī)器人所面對(duì)的主要是起伏變化的地形，這樣的地形對(duì)于移動(dòng)機(jī)器人來(lái)說(shuō)，并不是完全不可跨越的，只是跨越這些不同的地形需要付出不同的運(yùn)行費(fèi)用。所謂的運(yùn)行費(fèi)用，本文定義為移動(dòng)機(jī)器人運(yùn)行過(guò)程中所付出的能源、操作成本、安全風(fēng)險(xiǎn)等因素的綜合。在三維環(huán)境中的路徑規(guī)劃就是規(guī)劃出一條從起始點(diǎn)到目標(biāo)點(diǎn)的運(yùn)行費(fèi)用最低的路徑。

本文所研究的對(duì)象是已知或大致已知的三維自然環(huán)境，在此環(huán)境中的路徑規(guī)劃的原理是：對(duì)所研究的三維自然環(huán)境進(jìn)行柵格化，即將所研究的環(huán)境劃分為若干個(gè)柵格。根據(jù)移動(dòng)機(jī)器人的對(duì)于不同地形所表現(xiàn)出的性能，建立柵格的運(yùn)行費(fèi)用評(píng)價(jià)模型，利用此模型計(jì)算每個(gè)柵格的運(yùn)行費(fèi)用。設(shè)計(jì)以運(yùn)行費(fèi)用為自變量的斥力場(chǎng)函數(shù)，運(yùn)行費(fèi)用與斥力強(qiáng)度正相關(guān)。以此函數(shù)分別計(jì)算每個(gè)柵格的斥力場(chǎng)。同時(shí)建立以目標(biāo)點(diǎn)為中心的引力場(chǎng)，計(jì)算環(huán)境中各點(diǎn)的斥力場(chǎng)和引力場(chǎng)所組成的合力場(chǎng)，并將合力場(chǎng)的方向作為移動(dòng)機(jī)器人在該點(diǎn)的路徑走向。顯然，斥力的方向?yàn)檫\(yùn)行費(fèi)用負(fù)梯度的方向，引力方向是目標(biāo)對(duì)于移運(yùn)機(jī)器人的最短路徑方向，因此，從起始點(diǎn)出發(fā)沿著合力場(chǎng)的方向通往目標(biāo)點(diǎn)的路徑為一條運(yùn)行費(fèi)用較低的路徑。

局部最小值問(wèn)題是基于人工勢(shì)場(chǎng)法路徑規(guī)劃不可回避的問(wèn)題。所謂的局部最小值問(wèn)題就是在沒(méi)有到達(dá)目標(biāo)點(diǎn)但合力為零，從而使機(jī)器人在該點(diǎn)上來(lái)回 “抖動(dòng)”，無(wú)法前行。本研究中，當(dāng)移動(dòng)機(jī)器人進(jìn)行合力為零的點(diǎn)時(shí)，保持前一時(shí)刻的前進(jìn)方向，直到合力不為零時(shí)為止。

基于人工勢(shì)場(chǎng)的三維自然環(huán)境中的路徑規(guī)劃技術(shù)路線(xiàn)如圖1所示。

圖1 路徑規(guī)劃技術(shù)路線(xiàn)

2 環(huán)境的柵格化和運(yùn)行費(fèi)用評(píng)估

基于人工勢(shì)場(chǎng)法的三維環(huán)境路徑規(guī)劃的第一個(gè)環(huán)節(jié)是對(duì)三維自然環(huán)境進(jìn)行運(yùn)行費(fèi)用的評(píng)估，而自然的真實(shí)環(huán)境理論上可以看作由無(wú)數(shù)個(gè)地形點(diǎn)組成的，是一種自然的模擬量，所以需要將自然的真實(shí)環(huán)境進(jìn)行數(shù)字化。地形的柵格化就是將自然地形數(shù)字化的過(guò)程，具體的過(guò)程是將自然地形的物理空間在邏輯劃分成M×N的柵格（單元）。每一個(gè)柵格（單元）是某一區(qū)域物理環(huán)境的抽象，從而使無(wú)限的物理空間轉(zhuǎn)換成有限的邏輯單元。

由于移動(dòng)機(jī)器人對(duì)不同的地形的運(yùn)行代價(jià)的不同，所以在評(píng)估運(yùn)行費(fèi)用時(shí)，必須綜合柵格內(nèi)的平均高程、地形的變化、與目標(biāo)的距離等方面的因素。根據(jù)移動(dòng)機(jī)器人的特點(diǎn)或通過(guò)實(shí)驗(yàn)的方法測(cè)算出不同種類(lèi)地形的對(duì)運(yùn)行費(fèi)用影響的系數(shù)，在本研究中移動(dòng)機(jī)器人對(duì)于柵格的運(yùn)行費(fèi)用評(píng)價(jià)模型如下：

式（1）中，M，N為柵格的大小（下同），hij為柵格內(nèi)點(diǎn)（i，j）的高程（下同），HOBJ為目標(biāo)點(diǎn)的高程。

式（3）中，xij，yij為點(diǎn)（i，j）的坐標(biāo)，xOBJ，yOBJ為目標(biāo)點(diǎn)的坐標(biāo)。

（4）經(jīng)過(guò)該柵格的綜合運(yùn)行費(fèi)用C：其評(píng)價(jià)模型如下

式（4）中，α，β，δ為地形影響系數(shù)。

本研究所用三維環(huán)境由計(jì)算機(jī)隨機(jī)生成，該環(huán)境大小為1100×600個(gè)長(zhǎng)度單位，如圖1所示。圖中顏色的深淺分別表示高程的低和高。該環(huán)境劃分為6×11的柵格，每個(gè)柵格大小為100×100個(gè)單位。利用上述的運(yùn)行費(fèi)用評(píng)價(jià)函數(shù)，對(duì)每個(gè)柵格進(jìn)行評(píng)價(jià)，評(píng)價(jià)的參數(shù)選定為：α＝6，β＝1.5，δ＝1.0，目標(biāo)點(diǎn)坐標(biāo)為：（1000，200）。評(píng)價(jià)結(jié)果如圖2所示，圖中柵格的數(shù)字為該柵格的運(yùn)行費(fèi)用。

3 人工勢(shì)場(chǎng)的建立和路徑規(guī)劃

3.1 人工勢(shì)場(chǎng)的建立

本研究中人工勢(shì)場(chǎng)包括斥力場(chǎng)和引力場(chǎng)，斥力場(chǎng)由柵格產(chǎn)生，設(shè)定柵格的幾何中心為質(zhì)心，斥力的方向背向質(zhì)心；引力場(chǎng)由目標(biāo)點(diǎn)產(chǎn)生，引力場(chǎng)方向指向目標(biāo)點(diǎn)。環(huán)境的任意點(diǎn)所受的力為該點(diǎn)所受的斥力和引力的總和，如圖3所示。

圖2 運(yùn)行環(huán)境的柵格劃分及運(yùn)行費(fèi)用評(píng)價(jià)結(jié)果

圖3 人工勢(shì)場(chǎng)力關(guān)系

每個(gè)柵格的斥力勢(shì)場(chǎng)函數(shù)定義為

式中：η——一個(gè)正的斥力比例因子，C——柵格的運(yùn)行費(fèi)用，ρ（q，qg）——機(jī)器人到柵格中心的距離，ρ0——障礙物的影響距離，超出了這個(gè)距離，障礙物對(duì)機(jī)器人的斥力為0。相應(yīng)的斥力函數(shù)如下

目標(biāo)點(diǎn)的引力場(chǎng)定義為

式中：ε——一個(gè)正的引力比例因子，K——目標(biāo)點(diǎn)吸引力強(qiáng)度，ρ（q，qgoal）——機(jī)器人q和目標(biāo)qgoal之間的距離。相應(yīng)的斥力函數(shù)可表示如下

三維環(huán)境中的任意一點(diǎn)的作用力為是目標(biāo)點(diǎn)對(duì)該點(diǎn)的引力與所有影響范圍內(nèi)的柵格對(duì)該點(diǎn)的斥力之和，這個(gè)合力決定了該點(diǎn)總的受力方向。該點(diǎn)的合力為

式中：Frepi——第i個(gè)影響范圍內(nèi)的柵格的所產(chǎn)生的斥力；合力的方向?yàn)?/p>

利用上述模型，對(duì)圖1所標(biāo)的三維地形建立的人工勢(shì)力場(chǎng)，如圖4所示，圖中黑點(diǎn)為目標(biāo)點(diǎn)，箭頭所指為該點(diǎn)處的合力方向，箭頭的長(zhǎng)度表示合力的大小，箭頭的長(zhǎng)則合力大，反之亦然。

圖4 三維環(huán)境中的人工勢(shì)力場(chǎng)

3.2 路徑規(guī)劃

對(duì)研究的三維地形進(jìn)行柵格化，并對(duì)每個(gè)柵格進(jìn)行了運(yùn)行費(fèi)用的評(píng)估后，利用上述的人工勢(shì)場(chǎng)的計(jì)算模型規(guī)劃一條從起始點(diǎn)到目標(biāo)點(diǎn)的運(yùn)行費(fèi)用較低的路徑，規(guī)劃過(guò)程如圖5所示。

4 仿真實(shí)驗(yàn)與結(jié)果分析

本研究的仿真程序設(shè)計(jì)采用Visual C＃2005進(jìn)行開(kāi)發(fā)，仿真過(guò)程是：首先用隨機(jī)的方法生成三維的地形，然后在該三維地形上進(jìn)行柵格化和運(yùn)行費(fèi)用的評(píng)價(jià)，如圖1所示；最后設(shè)定移動(dòng)機(jī)器人的初始點(diǎn)，移動(dòng)機(jī)器人從初始點(diǎn)沿合力方向一直到達(dá)目標(biāo)點(diǎn)，其所經(jīng)過(guò)的路徑即為該點(diǎn)到目標(biāo)點(diǎn)規(guī)劃路徑。

本研究進(jìn)行了3組對(duì)照仿真實(shí)驗(yàn)，每一組起始點(diǎn)和目標(biāo)點(diǎn)相同，每一組分別進(jìn)行利用本研究的方法進(jìn)行路徑規(guī)劃的實(shí)驗(yàn)和不進(jìn)行規(guī)劃直接到達(dá)目標(biāo)點(diǎn)的實(shí)驗(yàn)，3組實(shí)驗(yàn)起始點(diǎn)的坐標(biāo)分別為：S1（100，550）、S2（600，550）、S3（200，100），目標(biāo)點(diǎn)的坐標(biāo)皆為：E（1000，200）；運(yùn)行費(fèi)用評(píng)價(jià)參數(shù)：α＝6，β＝1.5，δ＝1.0。采用了圖1所示的地形，3組實(shí)驗(yàn)的路徑規(guī)劃結(jié)果在偽等高線(xiàn)圖上表示，如圖6所示。圖6中D1、D2、D3分別在3組實(shí)驗(yàn)中未經(jīng)規(guī)劃的路徑，L1、L2、L3分別為3組實(shí)驗(yàn)中使用本算法規(guī)劃出和路徑。從圖6中看出，經(jīng)過(guò)規(guī)劃的路徑比較較圓滑，并都能繞開(kāi)了運(yùn)行費(fèi)用較高的區(qū)域準(zhǔn)確到達(dá)目標(biāo)點(diǎn)。

3組實(shí)驗(yàn)所規(guī)劃出的路徑的高程變化過(guò)程如圖7所示，從圖中看出，經(jīng)過(guò)規(guī)劃的路徑其高差比未經(jīng)過(guò)規(guī)劃的路徑的高差明顯減小，說(shuō)明經(jīng)過(guò)規(guī)劃后路徑變平穩(wěn)，提高了移動(dòng)機(jī)器人運(yùn)行的安全性。

3組仿真實(shí)驗(yàn)的結(jié)果見(jiàn)表1，從表1中可以看出，使用本研究的方法對(duì)移動(dòng)機(jī)器人的運(yùn)行路徑進(jìn)行規(guī)劃后，移動(dòng)機(jī)器人在運(yùn)行過(guò)程中雖然運(yùn)行的距離變長(zhǎng)了，但上、下坡的次數(shù)明顯減少，在不同程度上降低的運(yùn)行費(fèi)用。

5 結(jié)束語(yǔ)

本研究提出并實(shí)現(xiàn)了一種移動(dòng)機(jī)器人在已知三維自然環(huán)境中的路徑規(guī)劃算法。該算法通過(guò)對(duì)三維自然環(huán)境的柵格化，引入柵格運(yùn)行費(fèi)用的定義，建立柵格的運(yùn)行費(fèi)用評(píng)估模型，利用運(yùn)行費(fèi)用建立勢(shì)力場(chǎng)等措施實(shí)現(xiàn)了移動(dòng)機(jī)器人在已知三維環(huán)境中的路徑規(guī)劃，為移動(dòng)機(jī)器人在已知三維自然環(huán)境中的路徑規(guī)劃提供了一種非常有效的方法。仿真實(shí)驗(yàn)結(jié)果表明本文所提的算法可行、有效，非常適合移動(dòng)機(jī)器人在已知或大致已知的三維環(huán)境中的路徑規(guī)劃。同時(shí)，應(yīng)當(dāng)指出，本文的算法對(duì)柵格的劃分并沒(méi)有嚴(yán)格的限制，需根據(jù)具體的需要而定，劃分的柵格越多路徑規(guī)劃效果越好，但計(jì)算量越多、實(shí)時(shí)性越差，反之亦然。

圖7 3組實(shí)驗(yàn)的路徑高程變化對(duì)比

表1 仿真實(shí)驗(yàn)結(jié)果對(duì)照

［1］ZHU Qingbao.Ant Algorithm for navigation of multi－robot movement in unknown environment［J］.Journal of Software，2006，17（9）：1890－1898.

［2］YANG Chuanhua，YANG Ping.Path planning of self－learning mobile robot in unknown circumstances ［J］.Journal of Machine Design，2006，23（2）：16－18（in Chinese）. ［楊傳華，楊萍.自學(xué)習(xí)移動(dòng)機(jī)器人在未知環(huán)境中的路徑規(guī)劃 ［J］，機(jī)械設(shè)計(jì)，2006，23（2）：16－18.］

［3］GAO Bo，YAN Weisheng，ZHANG Fubin.A method of constructing complete graph for multi－objects path planning in complex environment ［C］.Proceedings of the IEEE International Conference on Information and Automation，2009：403－407.

［4］Venkateswaran Nagarajan，Prahasaran raja.Path planning for space robots：Based on know extrapolation and risk factors［C］.Proceedings of the IEEE International Conference on Automation and Logistics，2010：373－378.

［5］GAO Wei，ZHAO Hai，LUO Guilan.A APF algorithm and its application to the path planning problems of multiple mobile－robots［J］.Journal of Northeastern University（Natural Science），2009，30（5）：644－647（in Chinese）. ［高巍，趙海，羅桂蘭.一種AAPF算法及其在多機(jī)器人路徑規(guī)劃中的應(yīng)用 ［J］.東北大學(xué)學(xué)報(bào)（自然科學(xué)版），2009，30（5）：644－647.］

［6］Michael Brand，Nicole Wehne，YU Xiaohua.Ant colony optimization algorithm for robot path planning ［C］.International Conference on Computer Design and Appliations，2010：436－410.

［7］Fethi Belkhouche.Reactive path planning dynamic environment［J］.IEEE Transactions on Robotics，2009，25（4）：902－911.

［8］YU Jianli，CHENG Siya，SUN Zengqi.An optimal algorithm of 3Dpath planning for mobile robots ［J］.Journal of Central South University（Science and Technology），2009，40（2）：471－476（in Chinese）.［禹建麗，程思雅，孫增圻.一種移動(dòng)機(jī)器人三維路徑規(guī)劃優(yōu)化算法 ［J］.中南大學(xué)學(xué)報(bào)（自然科學(xué)版），2009，40（2）：471－476.］

［9］XU Lu，CAO Liang，JU Hehua.Traversability－based lunar rover autonomous navigation in three－dimensional terrain ［J］.Journal of System Simulation，2007，19（12）：2852－2856（in Chinese）.［徐璐，曹亮，居鶴華，等.基于三維通行性的月球車(chē)自主導(dǎo)航［J］.系統(tǒng)仿真學(xué)報(bào)，2007，19（12）：2852－2856.］

［10］YU Zhenzhong，YAN Jihong，ZHAO Jie.Mobile robot path planning based on improved artificial potential field method［J］.Journal of Harbin Institute of Technology，2011，43（1）：51－55（in Chinese）. ［于振中，閆繼宏，趙杰.改進(jìn)人工勢(shì)場(chǎng)法的移動(dòng)機(jī)器人路徑規(guī)劃 ［J］.哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2011，43（1）：51－55.］