陳志軍，鐘陽晶

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

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

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

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

一、基于人工勢(shì)場(chǎng)法的路徑規(guī)劃

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

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

其中，U（X）為機(jī)器人運(yùn)行環(huán)境的總勢(shì)場(chǎng)，UF（X）為引力場(chǎng)，Uf（X）為阻力場(chǎng)，分別定義為：

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

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

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

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

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

Step 2 每個(gè)群體中，利用人工勢(shì)場(chǎng)法在相鄰兩個(gè)點(diǎn)（xi，yi）和（xi+1，yi+1）之間計(jì)算出一條連接兩個(gè)點(diǎn)的無碰撞路徑，這樣便得到一條從起始點(diǎn)到目標(biāo)點(diǎn)的無碰撞路徑。

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

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

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

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

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

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

Step 9 算法結(jié)束。

同時(shí)，為了提交算法的收斂速度，這里在元胞自動(dòng)機(jī)的基礎(chǔ)上進(jìn)一步引入交叉變異操作，將步驟（4）進(jìn)行改進(jìn)。

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

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

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

如果機(jī)器人i的到達(dá)位置未超出邊界閾值X，進(jìn)一步判斷到達(dá)速率與當(dāng)前速度閾值V之間關(guān)系：

機(jī)器人i在t+1 時(shí)刻的到達(dá)速度更新為：

三、仿真實(shí)驗(yàn)

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

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

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

圖2 適應(yīng)度變化比較

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

圖3 路徑總長(zhǎng)度變化情況

圖4 路徑長(zhǎng)度變化圖

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