基于元胞聚焦搜索的移動機器人路徑規(guī)劃方法

陳志軍，鐘陽晶

（1.義烏工商職業(yè)技術學院，浙江 義烏 322000；2.廣東農工商職業(yè)技術學院，廣東 廣州 510507）

路徑規(guī)劃是移動機器人領域中的重點研究問題，［1-6］它使機器人在工作區(qū)域內沿著一條最優(yōu)路徑進行移動。目前移動機器人的路徑規(guī)劃主要分傳統(tǒng)算法和智能算法，傳統(tǒng)的算法如：可視圖法、自由空間法、柵格法等，更有一些結合自然界生物群體行為和生態(tài)機制為運行機理的智能算法，效率通常也比傳統(tǒng)算法要更高一些。移動機器人由于傳感器的限制以及周圍環(huán)境的不確定性，很難預先對機器人的移動路徑進行動態(tài)規(guī)劃，對于復雜多變的環(huán)境及動態(tài)障礙缺乏智能化的避障策略。同時障礙物的形狀及其個數(shù)也對路徑規(guī)劃產生了較大影響，并且通常還需要滿足一系列約束條件，［7-11］會出現(xiàn)最短路徑、移動速度、轉向、動態(tài)避障等不同的問題。

目前，國內外學者對移動機器人的路徑規(guī)劃問題提出了大量算法和模型。張慧等［12］為了提高四足機器人在復雜環(huán)境下的適應性，采用高斯過程回歸模型對TOF 相機的距離數(shù)據(jù)誤差進行校正，并通過計算各柵格的坡度、起伏度、粗糙度對地形進行識別，提出了滑窗和增量式A*算法結合的路徑規(guī)劃方法，該方法解決了采用A* 算法進行重規(guī)劃時效率低的問題。宋暉等［13］通過設計障礙物旋轉勢場來解決勢場法局部極小值問題，進而提出基于旋轉勢場法的避障路徑規(guī)劃方法，用來實現(xiàn)雙足機器人的實時避障運動。馬歡等［14］利用粒子鄰域限定來實現(xiàn)粒子隨機重置和粒子誤差序列性評價，從而建立一類多自由度空間機器人衛(wèi)星慣性參數(shù)在軌辨識算法。陳彥杰等［15］結合地圖學習規(guī)劃算法來解決室內服務機器人躲避碰撞問題，建立了一種改進型路徑規(guī)劃算法，克服了地圖學習規(guī)劃在臨近目標點收斂性缺陷。Bazeille 等［16］通過建立ARA*算法來研究復雜條件下的環(huán)境感知和路徑規(guī)劃問題。KIM等［17］通過概率建模將障礙物對周圍環(huán)境的影響狀況量化為連續(xù)危險度概率值，機器人利用給出的概率值判斷周圍環(huán)境的危險程度。Zhang 等［18］提出通過已知障礙物信息來分析未知障礙物影響概率，并計算全地圖位置信息中權值矩陣的障礙物概率影響程度。

在上述工作的基礎上，筆者首先建立了移動環(huán)境的生成指標及評價參數(shù)，基于人工勢場方法對移動路徑進行規(guī)劃，并利用元胞聚焦搜索算法對路徑實現(xiàn)優(yōu)化，通過仿真實驗，深入研究影響該方法的關鍵因素。

一、基于人工勢場法的路徑規(guī)劃

移動機器人路徑規(guī)劃首先需要建立機器人移動的二維空間環(huán)境，在建立機器人移動空間環(huán)境時，常采用的方法有柵格法和人工勢場法等。采用柵格法來創(chuàng)建地圖較為容易，可以根據(jù)柵格的大小和數(shù)目建立二維空間環(huán)境。由于柵格大小和數(shù)目會對機器人環(huán)境信息存儲量的大小和規(guī)劃時間的長短造成影響，往往會通過比較移動機器人、空間環(huán)境大小和障礙物三者之間的關系來確定柵格尺寸和數(shù)目，這樣機器人不僅能在柵格中自由活動，還能減少環(huán)境信息的存儲量，這有利于對移動機器人進行路徑規(guī)劃。為研究方便，通常將機器人視作質點，并且障礙物不滿一個柵格時也算作一個柵格，可以占據(jù)多個柵格。柵格表示方法采用坐標法和序列法結合，即任意柵格z 在二維空間R 中都有唯一的坐標點g（x，y）和序列號S={1，2，…，n}。通常設左下角第一個柵格為坐標起點，其對應的坐標為0，序列號由左至右由下至上遞增。

柵格法能很好地對路徑進行標示，但在路徑規(guī)劃時存在著效率低的問題，因此筆者結合人工勢場法對移動路徑進行重新規(guī)劃。人工勢場法針對工作環(huán)境定義抽象勢場，該勢場由障礙物斥力場和位置引力場疊加而成，障礙物對機器人施加排斥力形成斥力場，目標點向機器人施加吸引力形成引力場，在合力的作用下機器人向目標點移動。假設機器人在二維空間R 中的坐標向量Xbegin=（xbegin，ybegin），目標點向量Xend=（xend，yend），勢場U 和合力F 滿足下式：

其中，U（X）為機器人運行環(huán)境的總勢場，UF（X）為引力場，Uf（X）為阻力場，分別定義為：

其中，ω1和ω2分別為位置增益系數(shù)和斥力增益系數(shù)，X 為機器人當前的位置，Xend為目標點的位置，d0為障礙物的影響距離即安全距離，d 為機器人當前位置與目標點的距離，Xo為障礙物的位置。在移動過程中，每個采樣周期都以機器人的勢場強度之和最小值作為子目標點，經(jīng)過多個采樣周期后就能得到多個子目標點，所有的子目標點按順序連接起來就構成了全局優(yōu)化路徑，由此得出最短移動路徑，提高了機器人運動的平穩(wěn)性和執(zhí)行的效率。

結合隨機聚焦搜索算法中，這里將單個搜索個體作為D 維空間中的一個點。設s 為該機器人的個數(shù)，則每個個體機器人有如下屬性：第n次迭代時該個體在搜索空間中的位置（j表示變量的第j維分量）；所有個體到目前為止獲得的全局極值的位置為Xbest，個體的移動速度。待解決的優(yōu)化問題為最短路徑問題，在隨機聚焦搜索算法中個體的速度和位置更新公式如下式：

二、元胞聚焦搜索求解方法

針對上述最優(yōu)路徑規(guī)劃模型，傳統(tǒng)的尋優(yōu)算法存在陷入局部最優(yōu)和不易收斂等問題，為了提高尋優(yōu)結果的可靠性和收斂性，以便快速高效地獲得最優(yōu)結果，筆者提出基于元胞自動機和聚焦搜索的機器人路徑規(guī)劃，旨在找出機器人運行空間中的最優(yōu)移動路徑。隨機聚焦搜索算法的本質是模擬人類的搜索行為及人類行為的隨機性，選取適當?shù)南噜徔臻g參數(shù)，避免陷入局部最優(yōu)值且高效快速實現(xiàn)搜索全局最優(yōu)值。這里給出具體算法（Cellular Stochastic Focusing Search Algorithm，CSFS）的實現(xiàn)步驟：

Step 1 初始化一組群體和最大迭代次數(shù)N，每個種群中具有n-2 個參數(shù){（xi，yi）}，標出每個機器人個體的初始位置坐標及每個機器人個體的目標坐標點，Xbest初始值為個體初始坐標，設機器人移動步長為R，K 用于尋優(yōu)計數(shù)器，T=0 表示當前尋得的路徑數(shù)目，K[]存放當前最優(yōu)路徑并初始化。

Step 2 每個群體中，利用人工勢場法在相鄰兩個點（xi，yi）和（xi+1，yi+1）之間計算出一條連接兩個點的無碰撞路徑，這樣便得到一條從起始點到目標點的無碰撞路徑。

Step 3 根據(jù)公式（1）和公式（2），計算當前位置的適應度函數(shù)值，暫時將其作為最優(yōu)適應度值：

Step 4 在初始節(jié)點Xij上搜索下一節(jié)點Xi（j+1）時，通過公式（6）和公式（7）計算出下一節(jié)點的位置和速度。

Step 5 判斷機器人是否碰到障礙物，如果碰到障礙物則退回原位，重新選取后續(xù)節(jié)點。

Step 6 通過公式（7）和公式（8）更新個體的位置和速度，并比較其適應度函值。

Step 7 對機器人狀態(tài)進行判定，如果機器人曾經(jīng)經(jīng)過目標節(jié)點或已到達目標節(jié)點，則將K[]中的最優(yōu)路徑取出，如果不是初始路徑，則將其與最優(yōu)路徑進行比較，如果該適應度函值小于最優(yōu)適應度值結果，則替代最優(yōu)路徑。

Step 8 令K=K+1，如果K＞N，則停止路徑規(guī)劃，確定最優(yōu)解，將路徑鏈輸出；反之跳轉到Step 2。

Step 9 算法結束。

同時，為了提交算法的收斂速度，這里在元胞自動機的基礎上進一步引入交叉變異操作，將步驟（4）進行改進。

這里將待移動的位置看作元胞，根據(jù)公式（11）計算元胞適應度值Fit，若Fit值越大，那么對應位置被選中的可能性就越高。將上述選擇出來的元胞作為父體，按照一定概率η，并根據(jù)元胞演化規(guī)則在中心元胞和相臨元胞之間進行交叉操作，獲得下一時刻元胞狀態(tài)。同時，為避免在進化過程中陷入局部極值或停止進化，這里以變異概率ξ 來隨機改變元胞基因值。令τ 為（0，1］之間的隨機數(shù)，F(xiàn)it'為變異后的狀態(tài)，F(xiàn)ita和Fitb為狀態(tài)中最大和最小適應度值，變異概率閾值為Ω。根據(jù)式（16）對交叉操作產生的新個體進行變異操作：

同時，結合機器人的到達狀態(tài)（即位置和速度）對交叉和變異過程中采用的元胞演化規(guī)則進行定義：

在某時刻t，如果機器人i的到達位置超出邊界閾值X，則重新計算最優(yōu)適應度值；

如果機器人i的到達位置未超出邊界閾值X，進一步判斷到達速率與當前速度閾值V之間關系：

機器人i在t+1 時刻的到達速度更新為：

三、仿真實驗

為了驗證CSFS 算法的有效性，筆者將CSFS 算法和蟻群算法進行對比分析。這里設置相關參數(shù)：ε=1，最大迭代次數(shù)N=300，權重值ω1=ω2=ω3=ω4=0.25，kF=0.5，kf=0.5。在200×200 的二維空間中尋找一條從起點到目標節(jié)點的最優(yōu)路徑，在該二維空間中存在4 個障礙物，他們的各頂點的坐標分別是（40，140；60，160；100，140；60，120），（50，30；30，40；80，80；100，40），（120，160；140，100；180，170；165，180），（120，40；170，40；140，80），其中機器人的起始移動坐標為（20，180），目標點坐標為（160，90）。假定每條路徑可經(jīng)過的鏈路數(shù)為6 個，每條鏈路均離散化為10 個小路段，種群個體數(shù)為10 個，允許的最大迭代次數(shù)N=500，則迭代過程中規(guī)劃出的路徑如圖1。圖1 為在200×200 二維空間環(huán)境中的路徑規(guī)劃圖，從圖可知，機器人從初始位置到目標位置的路徑不是唯一的，且不同的路徑長度不等，圖中虛線部分為最優(yōu)路徑，相比其它而言，它的路徑長度最短，且每條分鏈路也比其他路徑的分鏈路短，能很好地避開障礙物走向目標點，移動中也能較好地與障礙物保持安全距離，并提高機器人移動的速率。

圖1 路徑規(guī)劃結果

此外，圖2 給出了蟻群算法與CSFS 算法的適應度隨著迭代次數(shù)的變化曲線，從圖中可以看出，兩種算法的適應度值隨著迭代次數(shù)的增加先減小后呈現(xiàn)穩(wěn)定的趨勢，但CSFS 算法的適應度值始終比蟻群算法的適應度值小。而適應度值越小，巡游能力也就越好，由此可以看出CSFS 算法比蟻群算法尋優(yōu)效果更好。此外還能從圖中得出，只有當?shù)螖?shù)超越某一值時尋優(yōu)能力才能得到更好的體現(xiàn)，因而在做實驗時，不能為了簡單而減小迭代次數(shù)。

圖2 適應度變化比較

同時，圖3 給出了CSFS 算法隨迭代次數(shù)變化下路徑總長度的變化情況。由于本文主要是對路徑尋優(yōu)，因而追求的是從起始節(jié)點到目標節(jié)點的最短路徑，以保證高效快速地到達目的地，從公式（11）中可以得出路徑越短適應度值越小，圖3 也間接地說明了適應度的變化趨勢。從圖中可以看出在迭代次數(shù)小于100 時，路徑總長度相對較長，適應度值較大，但隨著迭代次數(shù)的不斷增加，路徑總長度不斷減小，使得適應度值基本穩(wěn)定在較小的值。

圖3 路徑總長度變化情況

圖4 路徑長度變化圖

為了有效解決移動機器人路徑規(guī)劃存在的效率較低問題，結合元胞聚焦搜索算法建立了一種移動機器人路徑規(guī)劃方法CSFS。該方法首先基于人工勢場法對移動路徑進行規(guī)劃，建立移動環(huán)境的生成指標和評價參數(shù)。然后利用元胞聚焦搜索算法對路徑實施優(yōu)化。最后通過仿真實驗，深入研究影響該方法的關鍵因素（如適應度和路徑長度）。與蟻群算法相比，實驗結果表明CSFS 具有更好適應性。在今后的研究中，將進一步考慮結合障礙物形狀和個數(shù)、傳感器限制和周圍環(huán)境的不確定性，來提高移動機器人的定位精度和實時性，建立和完善復雜環(huán)境中的機器人路徑規(guī)劃模型。