移動(dòng)機(jī)器人路徑規(guī)劃融合算法研究

楊偉煌,王容霞

(廣州南洋理工職業(yè)學(xué)院,廣東 廣州 510900)

0 引言

路徑規(guī)劃是機(jī)器人研究領(lǐng)域不可或缺的技術(shù)。所謂路徑規(guī)劃,就是讓機(jī)器人在遵循一定的評(píng)價(jià)指標(biāo)函數(shù)的前提下,在充滿障礙物的環(huán)境中順利避開障礙物,從而規(guī)劃出從起始位置到目標(biāo)位置的最優(yōu)路徑。

由于標(biāo)準(zhǔn)A*算法需要從起點(diǎn)開始,不斷擴(kuò)展相鄰節(jié)點(diǎn),并使用定義的代價(jià)估計(jì)函數(shù)尋找使函數(shù)得到最小的節(jié)點(diǎn)F(m)值,所以算法的時(shí)間消耗主要在于計(jì)算相鄰節(jié)點(diǎn)的代價(jià)估計(jì)函數(shù)和選擇最小的節(jié)點(diǎn)F(m)。這樣一來,標(biāo)準(zhǔn)的A*算法就會(huì)有一些不可避免的缺陷:隨著機(jī)器人二維空間的增加,算法擴(kuò)展出來的無用節(jié)點(diǎn)的數(shù)量越來越多,導(dǎo)致計(jì)算數(shù)據(jù)量和搜索時(shí)間增加算法的增加,路徑規(guī)劃效率不高。

本文將基于跳躍點(diǎn)搜索的jump-A*算法與全局最優(yōu)的動(dòng)態(tài)窗口方法相結(jié)合,提出了一種混合路徑規(guī)劃算法。一個(gè)新的參數(shù)JPHeader(v,ω),它測(cè)量模擬軌跡的末端和距離當(dāng)前軌跡最近的跳躍點(diǎn)之間的方位角偏差,被設(shè)計(jì)來構(gòu)造一個(gè)新的評(píng)估函數(shù)。該融合算法基于動(dòng)態(tài)窗口方法,采用跳躍點(diǎn)搜索方法,規(guī)劃速度更快,避障路徑更平滑。

1 jump-A*算法

A*算法是一種啟發(fā)式搜索算法,用于在靜態(tài)二維配置空間中計(jì)算最優(yōu)路徑,與A*算法不同的是,jump-A*算法引入了跳轉(zhuǎn)點(diǎn)的概念(見圖1)。

圖1 跳轉(zhuǎn)點(diǎn)搜索算法

在A*算法中,由于評(píng)估了大量無意義的節(jié)點(diǎn),如灰色網(wǎng)格,無形中增加了計(jì)算量,降低了搜索效率。因此,假設(shè)目標(biāo)位置n是當(dāng)前節(jié)點(diǎn)x的8個(gè)鄰域中的任意一個(gè)節(jié)點(diǎn)n,則通過滿足約束方程,確定從起點(diǎn)p到目標(biāo)點(diǎn)n直線延伸的最優(yōu)路徑。當(dāng)擴(kuò)展的相鄰網(wǎng)格節(jié)點(diǎn)遇到障礙物時(shí),除強(qiáng)制鄰居節(jié)點(diǎn)外,其余的灰色網(wǎng)格都應(yīng)該被剪掉。節(jié)點(diǎn)X是算法中要找到的跳轉(zhuǎn)點(diǎn)。切掉無意義的節(jié)點(diǎn)后,就可以得到一系列的跳躍點(diǎn)。然后,可以通過在這些跳躍點(diǎn)上應(yīng)用標(biāo)準(zhǔn)的A*算法來獲得全局最優(yōu)路徑。該算法稱之為jump-A*算法[1]。

對(duì)于jump-A*算法中的距離測(cè)量方法,使用較多的是歐幾里得幾何距離或曼哈頓距離。為了便于控制機(jī)器人,結(jié)合以上兩種測(cè)量方法,設(shè)計(jì)出更接近實(shí)際估計(jì)成本h(m)的新距離函數(shù):

假設(shè)起點(diǎn)和目標(biāo)點(diǎn)的坐標(biāo)為(pi,qi)和(pj,qj),那么dx(m)=|pi-pj|,dy(m)=|qi-qj|。

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

2.1 傳統(tǒng)的動(dòng)態(tài)窗口方法

動(dòng)態(tài)窗口算法的核心是將路徑規(guī)劃問題轉(zhuǎn)化為約束速度向量空間優(yōu)化問題。

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

實(shí)現(xiàn)動(dòng)態(tài)窗口法的第一步是獲得機(jī)器人的運(yùn)動(dòng)學(xué)模型,這也是模擬機(jī)器人運(yùn)動(dòng)軌跡的前提。假設(shè)機(jī)器人的軌跡由一條微小的弧形軌跡組成,每個(gè)軌跡對(duì)應(yīng)一個(gè)唯一的速度向量(vt,ωt)。由于每條圓弧軌跡產(chǎn)生的時(shí)間間隔Δt很短,可以近似看成一條直線軌跡。因此,可以假設(shè)機(jī)器人在Δt的時(shí)間間隔內(nèi)做勻速直線運(yùn)動(dòng),得到機(jī)器人的運(yùn)動(dòng)學(xué)模型如下:

其中xt,yt,θt為t時(shí)刻機(jī)器人的坐標(biāo)位置和方位角；xt+1,yt+1,θt+1為t+1時(shí)刻機(jī)器人的坐標(biāo)位置和方位角；vt和ωt是機(jī)器人在時(shí)間t的平移速度和旋轉(zhuǎn)角速度。

2.1.2 速度采樣

速度采樣是模擬軌跡所必需的。在動(dòng)態(tài)窗口方法中,定義了3種不同的約束來限制采樣速度的范圍。

第一個(gè)約束是受機(jī)器人所處環(huán)境和物理結(jié)構(gòu)的限制。機(jī)器人可以達(dá)到的速度范圍是:

其中,vmax和vmin是機(jī)器人可以達(dá)到的最大和最小線速度,ωmax和ωmin是機(jī)器人可以達(dá)到的最大和最小角速度。

第二個(gè)約束考慮遇到障礙的情況。當(dāng)機(jī)器人采用最大減速度時(shí),v′b,ω′b緊急制動(dòng),為了使機(jī)器人在碰撞前停止并確保其安全,必須設(shè)置允許的速度范圍:

其中,dist(v,ω)表示速度向量模擬的軌跡之間的距離(v,ω)和最近的障礙物。

第三個(gè)約束是在模擬時(shí)間Δt間隔內(nèi)可以達(dá)到的速度范圍,由電機(jī)允許的最大加速度v′a,ω′a和最大減速度v′b,ω′b限制:

其中vc,ωc表示機(jī)器人當(dāng)前的線速度和角速度。

2.1.3 評(píng)價(jià)函數(shù)

在多組采樣速度的集合中,通過仿真可以得到多組可行的軌跡。要從這些可行軌跡中選擇最優(yōu)軌跡,需要從多個(gè)維度進(jìn)行評(píng)估,使機(jī)器人能夠沿著最優(yōu)軌跡安全到達(dá)目標(biāo)位置。評(píng)價(jià)函數(shù)如下:

其中,head(v,ω)用于測(cè)量在當(dāng)前選擇的采樣速度下,模擬軌跡終點(diǎn)方向與目標(biāo)位置方向之間的角度偏差。dist(v,ω)為速度向量(v,ω)模擬的軌跡與最近障礙物的距離,得分越低,機(jī)器人與障礙物碰撞的概率越大；σ為平滑函數(shù)。α,β,γ為3個(gè)評(píng)價(jià)指標(biāo)的加權(quán)系數(shù)[2]。

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

傳統(tǒng)的動(dòng)態(tài)窗口方法具有良好的動(dòng)態(tài)避障性能,但由于陷入局部區(qū)域無法到達(dá)目標(biāo)的致命缺陷,難以根據(jù)全局環(huán)境信息規(guī)劃全局最優(yōu)路徑。為了解決這個(gè)問題,筆者充分考慮了jump-A*算法得到的全局路徑信息,改進(jìn)了原動(dòng)態(tài)窗口方法的評(píng)價(jià)函數(shù)。改進(jìn)后的動(dòng)態(tài)窗口方法可以動(dòng)態(tài)避開障礙物并確保其全局最優(yōu)性。改進(jìn)后的評(píng)估函數(shù)表達(dá)式寫為:

方程中的head(v,ω)替換為JPHead(v,ω),它測(cè)量模擬軌跡的末端和距離當(dāng)前軌跡最近的跳躍點(diǎn)之間的方位角偏差。優(yōu)化的動(dòng)態(tài)窗口方法保證了在動(dòng)態(tài)路徑規(guī)劃中沿著全局最優(yōu)路徑的輪廓平滑的全局最優(yōu)路徑。改進(jìn)的動(dòng)態(tài)窗口方法的流程如下:

首先初始化,獲取機(jī)器人的運(yùn)動(dòng)學(xué)模型,判斷是否達(dá)到了目標(biāo)位置,如果達(dá)到了目標(biāo)位置,那么機(jī)器人速度采樣,模擬運(yùn)動(dòng)軌跡,使用全局改進(jìn)的評(píng)估函數(shù),通過jump-A*算法得到的路徑信息,選擇最優(yōu)軌跡,機(jī)器人按照最佳軌跡移動(dòng)；以至于得到最優(yōu)路徑[3]。

3 實(shí)驗(yàn)與結(jié)果

為了驗(yàn)證上面提到的jump-A*算法和改進(jìn)的動(dòng)態(tài)窗口方法的有效性,MATLAB中建立網(wǎng)格環(huán)境,對(duì)jump-A*算法結(jié)合跳躍點(diǎn)搜索和集成全局路徑信息的改進(jìn)動(dòng)態(tài)窗口方法進(jìn)行了2次靜態(tài)仿真實(shí)驗(yàn),如圖2所示。

圖2 兩種不同算法在簡(jiǎn)單和復(fù)雜靜態(tài)實(shí)驗(yàn)環(huán)境下的仿真結(jié)果

通過仿真結(jié)果,無論是簡(jiǎn)單環(huán)境還是復(fù)雜境與jump-A*算法相比,具有全局路徑信息的改進(jìn)動(dòng)態(tài)窗口方法可以解決jump-A*算法在相同網(wǎng)格大小和相同障礙物的轉(zhuǎn)折點(diǎn)處曲率不連續(xù)的問題分布環(huán)境,生成的路徑比jump-A*算法更平滑,融合算法也具有全局最優(yōu)性能。無論障礙物的位置、形狀和大小如何變化,融合算法都能在滿足全局優(yōu)化的基礎(chǔ)上規(guī)劃出更平滑的路徑。此外,改進(jìn)的動(dòng)態(tài)窗口方法融合了全局路徑信息,克服了原有動(dòng)態(tài)窗口方法陷入局部最優(yōu)的缺點(diǎn),達(dá)到了該融合算法的設(shè)計(jì)預(yù)期。