辛 鵬，馬希青

(河北工程大學(xué)機(jī)械與裝備工程學(xué)院，邯鄲 056038)

0 引言

移動(dòng)機(jī)器人具備自主移動(dòng)、運(yùn)動(dòng)靈活等特點(diǎn)，在工業(yè)生產(chǎn)以及家政服務(wù)等領(lǐng)域有廣泛的應(yīng)用和開發(fā)前景。移動(dòng)機(jī)器人開發(fā)的核心部分是機(jī)器人路徑規(guī)劃，即針對(duì)當(dāng)前環(huán)境規(guī)劃出從起始位姿到目標(biāo)位姿的安全無碰撞路徑[1-3]。高效的路徑規(guī)劃算法能使移動(dòng)機(jī)器人迅速規(guī)劃出最優(yōu)路徑。目前較為常用的算法有以下幾種：快速搜索隨機(jī)樹[4]、A*算法[5]、動(dòng)態(tài)窗口法[6]、蟻群算法[7]、粒子群算法[8]等。

A*算法是經(jīng)典的啟發(fā)式路徑規(guī)劃算法，但算法存在規(guī)劃的路徑拐點(diǎn)和冗余路段較多等諸多問題。目前，針對(duì)A*算法已經(jīng)做了較多研究和改進(jìn)?；眲?chuàng)鋒等[9]提出將A*算法領(lǐng)域擴(kuò)展為7×7與優(yōu)化評(píng)價(jià)函數(shù)后的動(dòng)態(tài)窗口法相結(jié)合的融合算法，但在搜索過程中需判定父節(jié)點(diǎn)與子節(jié)點(diǎn)的連線中是否經(jīng)過障礙物，路徑搜索效率在復(fù)雜環(huán)境中會(huì)降低?？桌^利等[10]對(duì)路徑進(jìn)行雙向搜索，優(yōu)化了評(píng)價(jià)函數(shù)，并將文中的改進(jìn)算法與Dijkstra算法和蟻群算法進(jìn)行比對(duì)，驗(yàn)證算法高效性。陳嬌等[11]通過提取關(guān)鍵節(jié)點(diǎn)剔除冗余子節(jié)點(diǎn)，并按照相鄰節(jié)點(diǎn)分段使用動(dòng)態(tài)窗口法規(guī)劃路徑，但是未能有效減少A*算法搜索時(shí)間。王中玉等[12]改進(jìn)了代價(jià)函數(shù)權(quán)重，并引入了一種障礙物擴(kuò)展策略，使路徑更加合理。張志文等[13]利用JPS算法跳躍擴(kuò)展子節(jié)點(diǎn)，縮短搜索時(shí)間，并對(duì)文中改進(jìn)A*算法規(guī)劃的路徑使用貝塞爾曲線進(jìn)行平滑處理。

針對(duì)A*算法搜索效率不高，規(guī)劃路徑較長(zhǎng)且不利于機(jī)器人行駛，將優(yōu)化改進(jìn)A*算法規(guī)劃的路徑按照節(jié)點(diǎn)分段使用動(dòng)態(tài)窗口法。在全局規(guī)劃方面，擴(kuò)大搜索鄰域，調(diào)節(jié)代價(jià)函數(shù)，剔除冗余子節(jié)點(diǎn)、減少路徑長(zhǎng)度，提高搜索效率。在局部規(guī)劃方面，將優(yōu)化后路徑分段使用改進(jìn)動(dòng)態(tài)窗口法，避免陷入全局最優(yōu)，提高機(jī)器人移動(dòng)安全性。

1 優(yōu)化改進(jìn)A*算法

A*算法在搜索過程中利用目前所知的狀態(tài)信息生成全局路徑。傳統(tǒng)A*算法思想是引入代價(jià)函數(shù)判定靜態(tài)環(huán)境下當(dāng)前節(jié)點(diǎn)代價(jià)值，在開放列表中找到代價(jià)最小值進(jìn)行擴(kuò)展直到目標(biāo)節(jié)點(diǎn)也在開放列表中，完成搜索。但A*算法搜索速度較慢，路徑拐點(diǎn)較多，針對(duì)這些問題從以下3個(gè)方面進(jìn)行改進(jìn)。

1.1 改進(jìn)代價(jià)函數(shù)

傳統(tǒng)A*算法的代價(jià)函數(shù)如式(1)所示。

f(n)=g(n)+h(n)

(1)

式中，g(n)為起始節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)的代價(jià)值，使用起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)歐幾里得距離表示；h(n)是當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的代價(jià)值?？紤]到實(shí)際環(huán)境中障礙物較多，機(jī)器人在多數(shù)情況下并不能從當(dāng)前節(jié)點(diǎn)直線到達(dá)目標(biāo)點(diǎn)，且使用歐幾里得距離會(huì)增加計(jì)算量降低路徑規(guī)劃效率，因此采用曼哈頓距離表示，如式(2)所示。

h(n)=|xg-xn|+|yg-yn|

(2)

式中，(xn,yn)為當(dāng)前節(jié)點(diǎn)坐標(biāo)；(xg,yg)為目標(biāo)節(jié)點(diǎn)坐標(biāo)。

針對(duì)傳統(tǒng)A*算法搜索路徑時(shí)間較長(zhǎng)的問題，優(yōu)化代價(jià)函數(shù)，增大前期h(n)值，提高搜索效率，使得規(guī)劃路徑更快向目標(biāo)節(jié)點(diǎn)靠近。改進(jìn)代價(jià)函數(shù)為式(3)。其中R為初始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的歐氏距離。

(3)

1.2 擴(kuò)展搜索鄰域

傳統(tǒng)A*算法搜索鄰域過小導(dǎo)致規(guī)劃的路徑中拐點(diǎn)以及路徑節(jié)點(diǎn)較多，不利于機(jī)器人的移動(dòng)和控制。為此，應(yīng)適當(dāng)擴(kuò)大搜索鄰域去除多余子節(jié)點(diǎn)。當(dāng)擴(kuò)大搜索鄰域時(shí)，需判定父節(jié)點(diǎn)與子節(jié)點(diǎn)的連線是否會(huì)經(jīng)過障礙物，若搜索鄰域擴(kuò)展過大，算法在復(fù)雜環(huán)境搜索路徑時(shí)會(huì)增加計(jì)算量導(dǎo)致搜索效率較低，因此采用擴(kuò)充為5×5搜索領(lǐng)域。如圖1所示，左側(cè)為A*算法的搜索鄰域，右側(cè)為改進(jìn)A*算法的搜索鄰域。

(a) A*算法的搜索鄰域 (b) 改進(jìn)A*算法的搜索鄰域圖1 傳統(tǒng)A*算法與改進(jìn)A*算法搜索鄰域?qū)Ρ?/p>

1.3 路徑優(yōu)化

改進(jìn)算法規(guī)劃的路徑中存在大量冗余路段，應(yīng)采用一種關(guān)鍵點(diǎn)提取的算法，提取出路徑關(guān)鍵轉(zhuǎn)折點(diǎn)，剔除冗余路段，重新規(guī)劃路徑。重新規(guī)劃后的路徑長(zhǎng)度更短，拐點(diǎn)更少，具體實(shí)現(xiàn)如下：

(1)首先得到規(guī)劃路徑中的所有節(jié)點(diǎn){x1,x2,…,xn}，x1為起始節(jié)點(diǎn)；xn為目標(biāo)節(jié)點(diǎn)；V為關(guān)鍵點(diǎn)集合。

(2)以x1為出發(fā)點(diǎn)，依次直線連接x2,x3…直到當(dāng)與xm的連線中存在障礙物，則將xm-1加入V。x1～xm-1之間的節(jié)點(diǎn)都為冗余節(jié)點(diǎn)，此時(shí)將xm-1設(shè)置為出發(fā)點(diǎn)，依次直線連接xm,xm+1…直到存在出發(fā)點(diǎn)與目標(biāo)點(diǎn)直線連接。

(3)目標(biāo)節(jié)點(diǎn)加入到V，依次直線連接V中所有節(jié)點(diǎn)完成優(yōu)化，優(yōu)化步驟如圖2所示。

(a) 改進(jìn)A*算法 (b) 提取關(guān)鍵轉(zhuǎn)折點(diǎn) (c) 優(yōu)化后路徑圖2 優(yōu)化步驟

優(yōu)化后，路徑長(zhǎng)度從39.253 6減少到了36.980 5，優(yōu)化了5.79%，節(jié)點(diǎn)從25個(gè)減少到10個(gè)，優(yōu)化了60%。路徑拐點(diǎn)從17個(gè)減少到8個(gè)，優(yōu)化了52.94%。優(yōu)化后的路徑仍存在移動(dòng)機(jī)器人易與障礙物發(fā)生碰撞且不能動(dòng)態(tài)避障，路徑不夠平滑等問題。

2 改進(jìn)動(dòng)態(tài)窗口法

動(dòng)態(tài)窗口法主要是在速度空間中提取多組速度，模擬出提取速度下一段時(shí)間內(nèi)機(jī)器人運(yùn)行軌跡，評(píng)價(jià)所有提取到的軌跡并選取最優(yōu)作為當(dāng)前軌跡。評(píng)價(jià)函數(shù)中加權(quán)比例固定，不利于機(jī)器人快速向目標(biāo)點(diǎn)靠近，將傳統(tǒng)加權(quán)系數(shù)改進(jìn)為動(dòng)態(tài)加權(quán)系數(shù)，實(shí)現(xiàn)動(dòng)態(tài)調(diào)整，提高搜索效率。

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

現(xiàn)假設(shè)機(jī)器人運(yùn)動(dòng)過程中在Δt時(shí)間內(nèi)是沿直線運(yùn)動(dòng)且機(jī)器人全向運(yùn)動(dòng)，速度為v；角速度為ω。此時(shí)機(jī)器人的運(yùn)動(dòng)軌跡可根據(jù)式(4)得到：

(4)

屆時(shí)根據(jù)速度分辨率在速度空間中采集多組速度，得出移動(dòng)機(jī)器人相應(yīng)運(yùn)動(dòng)軌跡。

2.2 速度采樣

在速度空間中存在多組數(shù)據(jù)，但考慮到機(jī)器人組成結(jié)構(gòu)以及運(yùn)行環(huán)境，限制采樣速度。機(jī)器人自身速度限制如式(5)所示。

Vm={(v,ω)|v∈[vmin,vmax],ω∈[ωmin,ωmax]}

(5)

機(jī)器人在運(yùn)動(dòng)過程中由于電機(jī)力矩有限而存在速度約束，因此機(jī)器人運(yùn)動(dòng)中實(shí)際運(yùn)動(dòng)速度為：

(6)

為保證機(jī)器人的安全應(yīng)使機(jī)器人碰撞到障礙物之前停止，因此需要對(duì)其速度進(jìn)行限制：

(7)

式中，dist(v,ω)為當(dāng)前速度對(duì)應(yīng)軌跡中距離障礙物最小的值。

2.3 改進(jìn)評(píng)價(jià)函數(shù)

在所采樣的速度中有多組可行軌跡，此時(shí)需要評(píng)價(jià)選取最優(yōu)路徑即使用評(píng)價(jià)函數(shù)。在局部路徑規(guī)劃中，此時(shí)評(píng)價(jià)函數(shù)為:

G(v,ω)=σ(αhead(v,ω)+βdist(v,ω)+γvel(v,ω))

(8)

式中，head(v,ω)是用來評(píng)估在當(dāng)前速度下，目標(biāo)方向與模擬出的軌跡末端方向偏差；vel(v,ω)是對(duì)機(jī)器人當(dāng)前運(yùn)轉(zhuǎn)速度大小的評(píng)價(jià)；σ為平滑函數(shù)；α、β、γ為加權(quán)系數(shù)。

式(8)中α與γ比例值固定，不利于機(jī)器人快速向目標(biāo)點(diǎn)靠近。對(duì)加權(quán)系數(shù)做出調(diào)整，當(dāng)移動(dòng)機(jī)器人距離障礙物較遠(yuǎn)時(shí)，使機(jī)器人方向占比更高，向目標(biāo)點(diǎn)方向靠近，當(dāng)機(jī)器人靠近障礙物時(shí)，提高移動(dòng)速度占比，快速繞過障礙物。根據(jù)移動(dòng)機(jī)器人與障礙物的距離動(dòng)態(tài)調(diào)節(jié)系數(shù)。改進(jìn)評(píng)價(jià)函數(shù)如式(9)所示。

(9)

式中，R表示障礙物半徑，在計(jì)算中將dist(v,ω)設(shè)定一個(gè)最大值2R，如果不設(shè)定，當(dāng)路徑中沒有障礙物時(shí)將占比過重，因此dist(v,ω)∈(0,2R)。

傳統(tǒng)動(dòng)態(tài)窗口法如圖3所示，改進(jìn)動(dòng)態(tài)窗口法路徑如圖4所示，兩種方法對(duì)比如表1所示。

圖3 傳統(tǒng)動(dòng)態(tài)窗口法 圖4 改進(jìn)動(dòng)態(tài)窗口法路徑

表1 傳統(tǒng)動(dòng)態(tài)窗口法與改進(jìn)動(dòng)態(tài)窗口法時(shí)間對(duì)比

通過對(duì)比路徑，兩種算法路徑基本保持一致。通過對(duì)比規(guī)劃時(shí)間，改進(jìn)算法時(shí)間減少了13.41%，改進(jìn)評(píng)價(jià)函數(shù)有效地減少了搜索時(shí)間，提高搜索效率。

3 融合算法

傳統(tǒng)的動(dòng)態(tài)窗口法參照點(diǎn)只有目標(biāo)點(diǎn)，因此尋找路徑中容易出現(xiàn)局部最優(yōu)，致使找不到全局路徑。改進(jìn)的A*算法優(yōu)化路徑后依然存在路徑不夠平滑，機(jī)器人移動(dòng)過程中存在安全隱患等情況。

針對(duì)這兩個(gè)問題，將優(yōu)化后的路徑按相鄰節(jié)點(diǎn)分段。在每段中使用改進(jìn)動(dòng)態(tài)窗口法。以相鄰節(jié)點(diǎn)構(gòu)成向量的角度作為機(jī)器人每段移動(dòng)的初始角度，增加搜索效率。將路徑分段使用改進(jìn)動(dòng)態(tài)窗口法時(shí)，分段路徑中沒有過多障礙物，以相鄰節(jié)點(diǎn)為目標(biāo)點(diǎn)，可以較為快速的找到路徑，實(shí)現(xiàn)局部最優(yōu)。

圖5 融合算法實(shí)現(xiàn)流程圖

4 仿真與分析

在MATLAB中構(gòu)建柵格地圖驗(yàn)證算法可行性。使用經(jīng)典的二維柵格地圖表示機(jī)器人行駛時(shí)的周圍環(huán)境，黑色柵格表示障礙物，空白柵格表示可通行區(qū)域。起始點(diǎn)(23,28)，目標(biāo)點(diǎn)(2,3)。分兩組實(shí)驗(yàn)對(duì)融合算法進(jìn)行驗(yàn)證。

實(shí)驗(yàn)1：驗(yàn)證優(yōu)化改進(jìn)A*算法的效率以及融合算法的實(shí)現(xiàn)。如圖6～圖9所示。4種算法性能對(duì)比如表2所示。

圖6 傳統(tǒng)A*算法路徑 圖7 優(yōu)化改進(jìn)A*算法路徑

圖8 傳統(tǒng)動(dòng)態(tài)窗口法路徑 圖9 融合算法路徑

表2 4種算法對(duì)比

如表2所示，相較于傳統(tǒng)A*算法，優(yōu)化改進(jìn)A*算法路徑縮短了2.17%，路徑規(guī)劃時(shí)間減少了45.48%，路徑中拐點(diǎn)減少了27.3%。相較于傳統(tǒng)動(dòng)態(tài)窗口法陷入局部最優(yōu)未能找到路徑，融合算法成功找到路徑并且路徑長(zhǎng)度比優(yōu)化改進(jìn)A*算法規(guī)劃的路徑更短。

實(shí)驗(yàn)2：在優(yōu)化改進(jìn)A*算法生成的路徑中加入障礙物用于驗(yàn)證本文算法動(dòng)態(tài)避障性。紅色小塊為加入的障礙物，如圖10所示。

在路徑中加入動(dòng)態(tài)障礙物后，移動(dòng)機(jī)器人的路徑避開障礙物如圖11～圖13所示，機(jī)器人在運(yùn)行過程中的線速度、角速度和機(jī)器人姿態(tài)變化分別如圖14～圖16所示。

圖10 在優(yōu)化改進(jìn)A*算法路徑中加入障礙物 圖11 移動(dòng)機(jī)器人避開第一個(gè)障礙物

圖12 移動(dòng)機(jī)器人避開第二個(gè)障礙物 圖13 機(jī)器人整體運(yùn)行軌跡

圖14 機(jī)器人運(yùn)行線速度 圖15 機(jī)器人運(yùn)行角速度

在優(yōu)化路徑中加入障礙物后，移動(dòng)機(jī)器人依然可從起始點(diǎn)到達(dá)目標(biāo)點(diǎn)且成功避開了障礙物，達(dá)到了動(dòng)態(tài)避障的目的。

5 結(jié)論

為使機(jī)器人路徑規(guī)劃實(shí)現(xiàn)全局最優(yōu)以及在復(fù)雜環(huán)境中動(dòng)態(tài)避障，本文將優(yōu)化改進(jìn)A*算法按照關(guān)鍵節(jié)點(diǎn)分段使用改進(jìn)動(dòng)態(tài)窗口法進(jìn)行路徑規(guī)劃。由仿真結(jié)果得，相較于A*算法，優(yōu)化改進(jìn)A*算法規(guī)劃的路徑更短，路徑節(jié)點(diǎn)更少，規(guī)劃效率更高。融合算法不會(huì)陷入全局最優(yōu)，且路徑更短。在原有規(guī)劃路徑中加入障礙物，機(jī)器人能夠成功避開，達(dá)到了動(dòng)態(tài)避障的目的。