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

梁家海

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

0 引 言

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

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

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

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

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

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

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

局部最小值問題是基于人工勢場法路徑規(guī)劃不可回避的問題。所謂的局部最小值問題就是在沒有到達目標點但合力為零，從而使機器人在該點上來回 “抖動”，無法前行。本研究中，當移動機器人進行合力為零的點時，保持前一時刻的前進方向，直到合力不為零時為止。

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

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

2 環(huán)境的柵格化和運行費用評估

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

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

式（1）中，M，N為柵格的大?。ㄏ峦琱ij為柵格內(nèi)點（i，j）的高程（下同），HOBJ為目標點的高程。

式（3）中，xij，yij為點（i，j）的坐標，xOBJ，yOBJ為目標點的坐標。

（4）經(jīng)過該柵格的綜合運行費用C：其評價模型如下

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

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

3 人工勢場的建立和路徑規(guī)劃

3.1 人工勢場的建立

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

圖2 運行環(huán)境的柵格劃分及運行費用評價結(jié)果

圖3 人工勢場力關(guān)系

每個柵格的斥力勢場函數(shù)定義為

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

目標點的引力場定義為

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

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

式中：Frepi——第i個影響范圍內(nèi)的柵格的所產(chǎn)生的斥力；合力的方向為

利用上述模型，對圖1所標的三維地形建立的人工勢力場，如圖4所示，圖中黑點為目標點，箭頭所指為該點處的合力方向，箭頭的長度表示合力的大小，箭頭的長則合力大，反之亦然。

圖4 三維環(huán)境中的人工勢力場

3.2 路徑規(guī)劃

對研究的三維地形進行柵格化，并對每個柵格進行了運行費用的評估后，利用上述的人工勢場的計算模型規(guī)劃一條從起始點到目標點的運行費用較低的路徑，規(guī)劃過程如圖5所示。

4 仿真實驗與結(jié)果分析

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

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

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

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

5 結(jié)束語

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

圖7 3組實驗的路徑高程變化對比

表1 仿真實驗結(jié)果對照

［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í)移動機器人在未知環(huán)境中的路徑規(guī)劃 ［J］，機械設(shè)計，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算法及其在多機器人路徑規(guī)劃中的應(yīng)用 ［J］.東北大學(xué)學(xué)報（自然科學(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）.［禹建麗，程思雅，孫增圻.一種移動機器人三維路徑規(guī)劃優(yōu)化算法 ［J］.中南大學(xué)學(xué)報（自然科學(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）.［徐璐，曹亮，居鶴華，等.基于三維通行性的月球車自主導(dǎo)航［J］.系統(tǒng)仿真學(xué)報，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）. ［于振中，閆繼宏，趙杰.改進人工勢場法的移動機器人路徑規(guī)劃 ［J］.哈爾濱工業(yè)大學(xué)學(xué)報，2011，43（1）：51－55.］