馬麗萍,吳丹丹,姚 鑫,李 珣

(西安工程大學(xué) 電子信息學(xué)院,陜西 西安 710048)

0 引 言

路徑規(guī)劃是機器人研究中最基本的關(guān)鍵問題,目的是尋找一條從起點到終點的最短安全路徑[1]。對于時間的長短參數(shù)、能量的消耗參數(shù)以及很多生態(tài)性的問題均要找出最優(yōu)的解決方法。國內(nèi)外學(xué)者就移動機器人路徑規(guī)劃問題做了大量的工作[2]，如可視圖法[3]、人工勢場法[4]、動態(tài)窗口算法[5]、模糊邏輯算法[6]以及Dijkstra算法[7]等。除此之外,智能算法是近十年解決移動機器人路徑規(guī)劃問題的主流方法之一,微分進化算法則是其中一種較為新穎仿生智能算法,其通過“優(yōu)勝略汰”的自然進化法則,解決路徑規(guī)劃問題,現(xiàn)有仿真實驗表明,相較于其他算法,微分進化算法的全局優(yōu)化能力強,是一種可以被廣泛應(yīng)用的優(yōu)化方法[8]。

上述研究,大多依靠靜態(tài)避障思維[9]進行移動機器人軌跡規(guī)劃研究,根據(jù)障礙物進行既定的動作。控制移動機器人脫離險境,使用遍歷法達到目的地,對于室內(nèi)有限環(huán)境而言這種方式機器人的運行效率較低。因為,室內(nèi)環(huán)境中有限次數(shù)的環(huán)境探知或是地圖的輸入,能夠提供移動機器人路徑規(guī)劃的優(yōu)化依據(jù),所以根據(jù)這一假設(shè)進行室內(nèi)機器人路徑優(yōu)化研究,能夠有效提高移動機器人在特定環(huán)境中的工作效率。本文針對室內(nèi)環(huán)境并在假設(shè)地圖已知情況下,在Matlab仿真實驗中基于微分進化算法對文中移動機器人的路徑進行規(guī)劃研究,在ROS的Mapping仿真環(huán)境進行了代碼的移植和實驗。

1 TurtleBot2移動機器人運動模型

本文實驗物理對象是TurtleBot2移動機器人平臺,為了能夠?qū)⒈疚难芯康穆窂揭?guī)劃算法進行ROS系統(tǒng)的可靠移植,需要進行運動學(xué)模型的建立。

1.1 移動機器人運動學(xué)模型

TurtleBot2 輪式移動機器人由底座、 驅(qū)動輪和從動輪組成, 從動輪僅起支撐作用。 忽略路面情況變化等影響, 得出移動機器人底座的運動學(xué)模型[10], 如圖1所示。

圖 1 移動機器人底座運動學(xué)模型Fig.1 Kinematics model of mobile robot base

圖1中,l為兩驅(qū)動輪的間距,v和w分別為機器人底座的線速度和角速度,vl和vr分別為機器人底座左右輪速。由圖1可得到左右兩輪速度的關(guān)系式為

(1)

假設(shè)兩輪差速驅(qū)動移動機器人質(zhì)心c在兩驅(qū)動輪軸線中心位置,(xc,yc)為機器人的質(zhì)心坐標,r為驅(qū)動輪半徑,底座的位姿向量為p=(xc,yc,θ)T,兩輪差速驅(qū)動底座模型,如圖2所示。 運動學(xué)方程為

(2)

圖 2 差速驅(qū)動移動機器人模型Fig.2 Model of differentially driven mobile robot

1.2 里程計模型

設(shè)ω為機器人的轉(zhuǎn)向角速度;v為機器人質(zhì)心處的線速度;vl和vr分別為機器人的2個驅(qū)動輪的線速度;θ為機器人運動方向與x軸的夾角即方向角;l為機器人驅(qū)動輪的間距,對于圖2所示的兩輪差速驅(qū)動移動機器人,當其運動滿足純滾動而無滑動條件時,機器人的運動有如下約束:

(3)

(4)

(5)

以上被稱為僅適用于移動機器人直線行駛也就是起始和終止位姿方向角差值為0的里程計模型。

2 微分進化算法的路徑規(guī)劃

2.1 A*算法的路徑規(guī)劃

A*算法[11]是ROS系統(tǒng)仿真環(huán)境中,內(nèi)置于移動機器人路徑規(guī)劃的基礎(chǔ)算法,使用柵格法將移動機器人的室內(nèi)工作環(huán)境模擬成為柵格地圖[12],每個柵格的信息直接與地圖中每個區(qū)域相匹配,根據(jù)柵格地圖來定位導(dǎo)航。在柵格地圖中,機器人的行進方向不再是任意方向,而是固定的8個行進方向,分別為左(L)、左前(F-L)、左后(B-L)、右(R)、右前(F-R)、右后(B-R)、前(F)、后(B)。以簡單的機器人從客廳到餐廳的室內(nèi)路線規(guī)劃問題為例(見圖3)，做室內(nèi)2D空間仿真環(huán)境(見圖4)。

圖 3 室內(nèi)平面環(huán)境示意圖Fig.3 Sketch of indoor environment

移動機器人在前進的過程中通過自身所帶的傳感器掃描感應(yīng)周圍的環(huán)境和自身狀態(tài),其搜索過程如圖4(a) (b) (c)所示,即首先起點由第一個搜索中心開始,相鄰柵格標注A，B，C，D,如圖4(a)所示;然后將相鄰柵格做為t+1時刻的搜索備選中心,如圖4(b)所示,判斷當中心點是否為終點坐標;最后連接各個點位的搜索結(jié)果,獲得規(guī)劃好的路徑如圖4(c)所示。

(a) 搜索相鄰柵格

(b) 搜索中心變化

(c) A*算法Matlab路徑仿真圖圖 4 A*搜索算法示意圖Fig.4 Sketch of A* search algorithm

從Matlab仿真環(huán)境中的實驗明顯看出,依靠A*算法進行的路徑規(guī)劃,由于其在臨近柵格中搜索以距離為指標的路徑,所以存在一定的“視野”局限性,最終仿真結(jié)果如圖4(c)所示的路徑,不能與圖3中的理想路徑匹配。因此,為改進實驗中發(fā)現(xiàn)的問題,引入微分進化算法進行室內(nèi)環(huán)境的移動機器人路徑規(guī)劃研究。

2.2 微分進化算法的路徑規(guī)劃

仿生優(yōu)化算法中,蟻群算法易于實現(xiàn),具有良好的全局優(yōu)化能力,但是隨著環(huán)境的擴大,計算量急速增長,易陷入局部最優(yōu)[13];遺傳算法最大的優(yōu)點是易于和其他算法相結(jié)合,并充分發(fā)揮自身迭代的優(yōu)勢,缺點是適應(yīng)度函數(shù)選擇不當?shù)那闆r下有可能收斂于局部,由于沒有反饋信息,到后期之后運算效率將大大降低[14];同遺傳算法相比,粒子群算法具有簡潔、易于實現(xiàn)、魯棒性好、路徑短、收斂速度快等優(yōu)點,但易陷入局部最優(yōu)[15]。

(6)

式中:xj,max和xj,min分別為解空間第j維的上下界。隨機選擇2個不同的個體向量得到差分向量,將差分向量賦予權(quán)值后與另一個隨機選出的向量相減得到變異個體,變異個體和目標個體進行參數(shù)混合交叉得到交叉?zhèn)€體,擇優(yōu)生成新一代種群。微分進化算法的基本操作包括變異,交叉及選擇3種。

2.2.1 變異操作 對于父代種群中任意目標向量xi,利用微分進化算法，按式(7)生成變異向量vi:

vi=xr1+F·(xr2-xr3),i=1,2,…,N

(7)

式中:{xr1,xr2,xr3}是隨機選擇的3個不同個體,且r1≠r2≠r3≠i,種群規(guī)模滿足N≥4；F用于控制差分向量(xr1-xr2)的影響,稱為放縮因子，F(xiàn)值介于[0，2]之間。

2.2.2 交叉操作 通過變異向量vi和目標向量xi各維度的隨機重組以提高種群個體的多樣性,交叉向量[ui,1,ui,2,…,ui,D]:

(8)

微分進化算法主要控制參數(shù)和種群規(guī)模N、放縮因子F以及交叉常量C,若C為0則沒有交叉,rand(b)保證ui至少從vi中獲得一個元素用來確保有新的個體生成,防止種群出現(xiàn)進化停滯不變。

2.2.3 選擇操作 新的向量個體ui的適應(yīng)度值比目標向量個體xi的適應(yīng)度值更好時,ui才會被種群所接受,否則xi仍然保存在下一代種群中,并且迭加計算時繼續(xù)作為目標向量執(zhí)行變異和交叉操作。設(shè)待優(yōu)化問題minf(x)，則

(9)

本文中移動機器人路徑規(guī)劃的性能指標主要包括完成任務(wù)的安全和耗電指標,即威脅代價最小和耗電代價最小指標。

威脅代價最小性能指標為

(10)

耗電代價最小性能指標為

(11)

移動機器人路徑規(guī)劃的總性能指標為

minJ=kJt+(1-k)Jf

(12)

總威脅代價為

(13)

式中:n為威脅源個數(shù)；wt為路徑上各點威脅代價;wf為路徑上各點耗電代價;Lij為起始點到節(jié)點之間的長度;tk為威脅點的威脅等級,耗電代價和路徑有關(guān)；k∈[0,1]表示安全性能和耗電性能的權(quán)衡系數(shù),任務(wù)重視安全則k較大,若任務(wù)需要快速則k較小。本文微分進化算法流程如圖5所示。

圖 5 微分進化算法流程圖

Fig.5 Flow chart of differential evolution algorithm

3 結(jié)果與分析

3.1 室內(nèi)環(huán)境描述及初始化

設(shè)定實驗仿真室內(nèi)客廳環(huán)境含3個沙發(fā),1架鋼琴以及1個落地?zé)?房間內(nèi)家具靜止定點分布,移動機器人打掃路線規(guī)劃是指滿足某種性能指標的最優(yōu)行走路線[17]。假設(shè)在得到威脅家具信息的時刻,移動機器人勻速保持當前方向。設(shè)定初始參數(shù)如表1所示:種群規(guī)模N=30,優(yōu)化維數(shù)D=20,最大迭加次數(shù)Nmax=250,變異因子F=20,交叉因子C=0.6,之后改變種群規(guī)模N和迭加次數(shù)Nmax得到不同的路徑規(guī)劃結(jié)果和進化曲線。

表 1 移動機器人執(zhí)行任務(wù)時的環(huán)境參數(shù)Tab.1 Environmental parameters for mobile robots in task execution

3.2 仿真及結(jié)果分析

根據(jù)上述初始化參數(shù)構(gòu)建的仿真環(huán)境,需要特別說明的是,為證明規(guī)劃方法的有效性和實時性,對仿真環(huán)境中的物品位置進行隨意設(shè)置。

獲得仿真實驗結(jié)果如圖6所示。圖6(a)為A*算法與微分進化算法路徑對比圖，圖6(b)為A*算法與微分進化算法迭代曲線圖。

從仿真的結(jié)果來看,采用微分進化算法規(guī)劃的移動機器人,將在有障礙環(huán)境下的路線規(guī)劃問題轉(zhuǎn)化為求D維函數(shù)極值的優(yōu)化問題,收斂速度快而且性能良好,這一優(yōu)點得到了很好的驗證。移動機器人規(guī)劃的前進路線成功繞過了各種障礙點,順利到達任務(wù)終點。在微分進化算法中根據(jù)障礙點的大小,障礙等級的不同而產(chǎn)生不同的規(guī)劃路線,在相同參數(shù)條件下由于微分進化算法本身收斂情況的不穩(wěn)定也會導(dǎo)致規(guī)劃路徑的不同。實驗不僅驗證了微分進化算法的可行性,同時也驗證了微分進化算法的高效性。從圖6(b)的算法尋優(yōu)迭代曲線可以看出,相對于傳統(tǒng)啟發(fā)算法,微分進化算法在復(fù)雜環(huán)境的路徑規(guī)劃過程中,于第12次迭代達到最優(yōu)值,A*算法需要14余次迭代才能達到最優(yōu)值。

(a) A*算法與微分進化算法路徑對比

(b) A*算法與微分進化算法迭代曲線對比圖 6 微分進化與A*算法仿真實驗對比Fig.6 Simulation experiment comparison of differential evolution and A* algorithm

雖然在迭代次數(shù)上微分進化算法弱優(yōu)于A*算法，但是，微分進化算法在最差路徑長度、最優(yōu)路徑長度、平均路徑長度上都優(yōu)于A*算法，這是因為A*算法在搜索時“視野”有一定局限性，導(dǎo)致算法容易陷入局中最優(yōu)解，規(guī)劃的路徑精度不高。微分進化算法的整體性能要遠遠優(yōu)于A*算法路徑規(guī)劃,收斂時間短,具有很好的全局優(yōu)化能力。

3.3 ROS的路徑規(guī)劃實現(xiàn)

根據(jù)西安工程大學(xué)電子信息學(xué)院207實驗室布局創(chuàng)建地圖，利用A*算法和微分進化算法對仿真環(huán)境中移動機器人TurtleBot2進行路徑規(guī)劃。

在ROS中Map-server是ROS的地圖服務(wù)器[18],其能夠在虛擬環(huán)境中根據(jù)實際室內(nèi)特征建立地圖,本文地圖創(chuàng)建如圖7所示。

圖 7 ROS系統(tǒng)地圖構(gòu)建示意圖Fig.7 Schematic map of ROS systematic map construction

圖7中,箭頭表示消息傳遞的方向,圓表示話題,矩形表示節(jié)點[19]。利用Gazebo進行機器人動力學(xué)的仿真,使用Rviz驅(qū)動TurtleBot2轉(zhuǎn)動得到可視化環(huán)境[20],如圖8(a)所示;建立室內(nèi)環(huán)境仿真地如圖8(b)所示。

(a) Rviz可視化環(huán)境 (b) 室內(nèi)環(huán)境仿真圖圖 8 Gazebo中路徑規(guī)劃仿真地圖Fig.8 Simulation map of path planning in Gazebo

機器人首先規(guī)劃出全局路徑,然后沿著全局路徑前進,到達障礙物的影響距離以內(nèi)時,機器人由于受到影響向遠離障礙物的方向前進,直到過了障礙物,最后得到安全有效的規(guī)劃路徑。對于室內(nèi)可能存在較復(fù)雜的環(huán)境,設(shè)定了跨度較復(fù)雜的起始點進行實驗,如圖9所示。其中圖9(a)為A*算法規(guī)劃的路徑,圖9(b)為微分進化算法規(guī)劃的路徑。

(a) A*算法規(guī)劃的路徑 (b) 微分進化算法規(guī)劃的路徑圖 9 基于ROS的仿真實驗圖Fig.9 Simulation experiment diagram based on ROS

在ROS的Rviz中可視化環(huán)境中可以看到,雖然A*算法機器人安全有效的通過障礙物,但是機器人繞過障礙物時沒有選擇最短路徑規(guī)劃線路并不是最優(yōu)。多次實驗結(jié)果表明,本文方法與ROS內(nèi)核中的A*算法都能夠使機器人平穩(wěn)、流暢的越過障礙物,并且最終再次回到規(guī)劃的路徑上面,直到到達目標點。但從圖9中可以看出,當存在2個障礙物之間的距離略大于機器人的通過空間時,由于A*算法是利用遍歷避障的方式,因此存在丟失最優(yōu)路徑的可能;但微分進化算法是一種全局優(yōu)化算法,故機器人會選擇到目標點距離最短的路徑,即可獲得障礙物之間的路徑。因此,在地圖已提供的基礎(chǔ)上,微分進化算法在路徑規(guī)劃尋優(yōu)的能力上具有更大的優(yōu)勢。

4 結(jié) 語

為能夠針對室內(nèi)環(huán)境特點給予移動機器人更優(yōu)的規(guī)劃路徑,重點介紹了A*算法和微分進化算法的原理模型,通過Matlab軟件仿真實驗說明算法的可行性。由于A*算法存在多個最小值時不能保證搜索的路徑最優(yōu),容易陷入局部最優(yōu)解的時候無法跳出,搜索的點數(shù)多,搜索范圍大,效率較低。為此,文中采用微分進化算法進行移動機器人路徑規(guī)劃研究,并進行了Matlab和ROS環(huán)境中的仿真實驗驗證。實驗結(jié)果說明,基于微分進化算法的規(guī)劃路徑,由于算法的收斂性較強因此尋優(yōu)效率較高,同時能夠較A*算法具有更好的全局尋優(yōu)能力。