基于微分進(jìn)化算法的室內(nèi)移動(dòng)機(jī)器人路徑規(guī)劃

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

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

0 引 言

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

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

1 TurtleBot2移動(dòng)機(jī)器人運(yùn)動(dòng)模型

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

1.1 移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型

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

圖 1 移動(dòng)機(jī)器人底座運(yùn)動(dòng)學(xué)模型Fig.1 Kinematics model of mobile robot base

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

(1)

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

(2)

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

1.2 里程計(jì)模型

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

(3)

(4)

(5)

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

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

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

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

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

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

(a) 搜索相鄰柵格

(b) 搜索中心變化

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

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

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

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

(6)

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

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

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

(7)

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

2.2.2 交叉操作 通過(guò)變異向量vi和目標(biāo)向量xi各維度的隨機(jī)重組以提高種群個(gè)體的多樣性,交叉向量[ui,1,ui,2,…,ui,D]:

(8)

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

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

(9)

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

威脅代價(jià)最小性能指標(biāo)為

(10)

耗電代價(jià)最小性能指標(biāo)為

(11)

移動(dòng)機(jī)器人路徑規(guī)劃的總性能指標(biāo)為

minJ=kJt+(1-k)Jf

(12)

總威脅代價(jià)為

(13)

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

圖 5 微分進(jìn)化算法流程圖

Fig.5 Flow chart of differential evolution algorithm

3 結(jié)果與分析

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

設(shè)定實(shí)驗(yàn)仿真室內(nèi)客廳環(huán)境含3個(gè)沙發(fā),1架鋼琴以及1個(gè)落地?zé)?房間內(nèi)家具靜止定點(diǎn)分布,移動(dòng)機(jī)器人打掃路線規(guī)劃是指滿足某種性能指標(biāo)的最優(yōu)行走路線[17]。假設(shè)在得到威脅家具信息的時(shí)刻,移動(dòng)機(jī)器人勻速保持當(dāng)前方向。設(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é)果和進(jìn)化曲線。

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

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

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

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

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

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

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

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

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

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

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

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

圖7中,箭頭表示消息傳遞的方向,圓表示話題,矩形表示節(jié)點(diǎn)[19]。利用Gazebo進(jìn)行機(jī)器人動(dòng)力學(xué)的仿真,使用Rviz驅(qū)動(dòng)TurtleBot2轉(zhuǎn)動(dòng)得到可視化環(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

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

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

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

4 結(jié) 語(yǔ)

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