融合改進(jìn)A*算法和優(yōu)化動態(tài)窗口法的路徑規(guī)劃

鄒 文,韓丙辰,李鵬飛,田劍鋒

(1.太原師范學(xué)院 計算機(jī)系,山西 晉中 030619;2.太原師范學(xué)院 物理系,山西 晉中 030619)

0 引言

移動機(jī)器人因其自主導(dǎo)航、靈活移動等特點(diǎn),被廣泛用于物流運(yùn)輸、日常服務(wù)、工業(yè)制造等多個領(lǐng)域。路徑規(guī)劃是移動機(jī)器人諸多技術(shù)中不可或缺的重要組成部分,目的是在有障礙物的環(huán)境中從起點(diǎn)到目標(biāo)點(diǎn)規(guī)劃出一條無碰撞且較優(yōu)的路徑。按照機(jī)器人移動過程中對環(huán)境感知的不同,可分為全局規(guī)劃和局部規(guī)劃兩大類。全局路徑規(guī)劃主要應(yīng)用于已知的靜態(tài)環(huán)境,常用的算法有A*算法[1]、D*算法[2]、人工勢場法[3]等;局部路徑規(guī)劃主要解決機(jī)器人移動過程中遇到的未知障礙物,常用的算法有動態(tài)窗口法[4]、粒子群算法[5]、遺傳算法[6]、蟻群算法[7]等。

A*算法是一種經(jīng)典的啟發(fā)式搜索算法,具有計算過程簡單、規(guī)劃路徑短等優(yōu)點(diǎn)。但是傳統(tǒng)的A*算法存在計算量大、耗時長,以及路徑平滑度低、拐點(diǎn)多等缺點(diǎn)。對此,一些研究對A*算法的啟發(fā)式搜索過程進(jìn)行優(yōu)化。例如:MIN等[8]通過設(shè)置冗余安全空間來避免輪廓碰撞,并在啟發(fā)式函數(shù)設(shè)計中加入了路徑曲率的代價,提高了路徑的平滑性,得到一條更加滿足車輛運(yùn)動約束的路徑?；眲?chuàng)鋒等[9]將傳統(tǒng)A*算法3×3的搜索領(lǐng)域擴(kuò)展為7×7,優(yōu)化搜索角度,減少了折點(diǎn)提高了平滑度?；蛘邔*算法已經(jīng)規(guī)劃出的路徑進(jìn)行優(yōu)化處理,如文獻(xiàn)[10]～文獻(xiàn)[13]不同程度地結(jié)合了Floyd算法思想優(yōu)化路徑,刪除了冗余的節(jié)點(diǎn),減少轉(zhuǎn)折。這些研究都提高了路徑的平滑度,且保證了路徑長度最優(yōu),但都只是針對規(guī)劃的路徑效果,而沒有考慮到A*算法啟發(fā)式搜索時的計算效率,均未減小全局路徑規(guī)劃的計算量和消耗時間。王洪斌等[14]提出多目標(biāo)點(diǎn)和自適應(yīng)圓弧優(yōu)化算法得到多目標(biāo)路徑并對其進(jìn)行平滑處理,有效提高了搜索效率和路徑的平滑度,但是未說明多個目標(biāo)點(diǎn)如何確定。王云峰等[15]在A*算法的啟發(fā)函數(shù)中加入“方向函數(shù)”來減少倉儲自動引導(dǎo)車(Automated Guided Vehicle,AGV)的轉(zhuǎn)彎次數(shù)和沖突節(jié)點(diǎn)數(shù)量,有效地降低了總完成時間,但該方法僅適用于倉儲大規(guī)模AGV環(huán)境下。

動態(tài)窗口法(Dynamic Window Approach,DWA)可以使機(jī)器人在移動過程中及時避讓未知的障礙物,提高路徑平滑度,但存在運(yùn)動狀態(tài)單一、容易陷入局部最優(yōu)陷阱等缺點(diǎn)。為此,一些研究者將A*算法和改進(jìn)動態(tài)窗口法相融合,封碩等[16]在動態(tài)窗口法的評價函數(shù)中擴(kuò)展動態(tài)障礙物的速度信息,并結(jié)合優(yōu)化A*算法,獲得全局路徑的同時提高了局部實(shí)時避障能力。文獻(xiàn)[17]將A*算法規(guī)劃的路徑作為DWA算法的全局路徑指引,既保證了全局路徑最優(yōu),又避免機(jī)器人陷入局部最優(yōu)陷阱?；蛘呓Y(jié)合環(huán)境信息來優(yōu)化DWA算法,MAI等[18]在動態(tài)窗口法路徑評價函數(shù)中引入與密度變化相關(guān)的評價因子,提前避免進(jìn)入密集區(qū)域,改進(jìn)后的動態(tài)窗口法具有相對穩(wěn)定的速度和加速度。這些研究都避免機(jī)器人陷入局部最優(yōu),增強(qiáng)局部規(guī)劃能力,但都未改變DWA算法僅有單一運(yùn)動狀態(tài)的缺點(diǎn),機(jī)器人遇到障礙物僅會避讓,而不會選擇其他運(yùn)動狀態(tài),可能會使機(jī)器人的移動路徑過長、轉(zhuǎn)折角度過大。

上述文獻(xiàn)不同程度地對A*算法和動態(tài)窗口法進(jìn)行了優(yōu)化,但都未解決A*算法搜索節(jié)點(diǎn)計算量大、耗時長以及動態(tài)窗口法運(yùn)動狀態(tài)單一、靈活性差的問題。對此,本文通過建立拓?fù)鋵拥貓D來減少A*算法搜索時的節(jié)點(diǎn)數(shù)量和耗費(fèi)時間,并在柵格地圖中對路徑進(jìn)行優(yōu)化得到關(guān)鍵折點(diǎn),再融合增加運(yùn)動狀態(tài)的優(yōu)化DWA算法完成機(jī)器人的局部動態(tài)規(guī)劃,最終為機(jī)器人規(guī)劃出長度較優(yōu),又能對環(huán)境情況作出靈活反應(yīng)的路徑。

1 改進(jìn)A*算法

1.1 A*算法基礎(chǔ)

A*算法是一種求解最短路徑很有效的啟發(fā)式搜索方法之一,其融合了Dijkstra算法和Best First(路徑優(yōu)先)算法的優(yōu)點(diǎn)。它的核心代價函數(shù)為:

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

(1)

式中:f(n)表示當(dāng)前節(jié)點(diǎn)柵格n的總移動代價值;g(n)表示當(dāng)前節(jié)點(diǎn)柵格n到初始點(diǎn)的移動代價值;h(n)表示當(dāng)前節(jié)點(diǎn)柵格n到目標(biāo)點(diǎn)的移動代價值。每次選取f(n)值最小的柵格作為下一次拓展的父節(jié)點(diǎn),h(n)是代價函數(shù)最重要的部分,本文算法注重減少計算量,故選取歐式距離作為計算h(n)移動價值的代價函數(shù),該函數(shù)表示為:

(2)

式中:(xn,yn)表示當(dāng)前節(jié)點(diǎn)柵格所在的坐標(biāo);(xg,yg)表示目標(biāo)點(diǎn)所在柵格的坐標(biāo)。

1.2 雙層優(yōu)化A*算法

1.2.1 拓?fù)涞貓D

當(dāng)在較大的場景下且需要精度高的環(huán)境模型時,柵格數(shù)量會呈現(xiàn)指數(shù)型增長,從而導(dǎo)致計算時間的增加。本文首先在柵格地圖之上建立一層拓?fù)涞貓D,并在拓?fù)鋵拥貓D中使用傳統(tǒng)A*算法得到初步全局路徑。在柵格地圖上,以固定間隔為單位距離取出柵格點(diǎn)作為拓?fù)潼c(diǎn),建立一個拓?fù)鋵拥貓D。間隔距離可以根據(jù)不同情況進(jìn)行設(shè)置。本文取間隔距離為10,建立的拓?fù)鋵拥貓D如圖1所示。

圖1 建立拓?fù)鋵拥貓D

1.2.2 最優(yōu)中繼點(diǎn)選取

在雙層地圖中,柵格層中的初始點(diǎn)和目標(biāo)點(diǎn)往往不在拓?fù)潼c(diǎn)上,因此在路徑規(guī)劃前需要先找到離初始點(diǎn)和目標(biāo)點(diǎn)最優(yōu)的拓?fù)潼c(diǎn)分別作為拓?fù)鋵勇窂揭?guī)劃的起點(diǎn)和終點(diǎn)。被選中的拓?fù)潼c(diǎn)分別命名為初始中繼點(diǎn)和目標(biāo)中繼點(diǎn)。

首先將拓?fù)潼c(diǎn)命名,如圖2所示,將初始點(diǎn)周圍的4個拓?fù)潼c(diǎn)順時針依次命名為P01,P02,P03,P04,目標(biāo)點(diǎn)周圍的4個拓?fù)潼c(diǎn)順時針依次命名為P11,P12,P13,P14。然后用目標(biāo)點(diǎn)坐標(biāo)減去初始點(diǎn)坐標(biāo),根據(jù)坐標(biāo)差值的正負(fù)分別選出初始中間點(diǎn)和目標(biāo)中繼點(diǎn),判別公式如下:

(3)

圖2 拓?fù)潼c(diǎn)命名

式中:(Δx,Δy)表示坐標(biāo)差值;(xg,yg)表示目標(biāo)點(diǎn)坐標(biāo);(xo,yo)表示初始點(diǎn)坐標(biāo)。差值分類及相應(yīng)選擇如表1所示。

表1 中繼拓?fù)潼c(diǎn)選擇

1.2.3 全局優(yōu)化路徑

在建立好大柵格層地圖以及確定最優(yōu)中繼點(diǎn)后,本文先在大柵格層地圖中使用傳統(tǒng)A*算法進(jìn)行路徑規(guī)劃,得到初步全局路徑,之后再結(jié)合Floyd算法思想在小柵格地圖中進(jìn)一步對全局路徑優(yōu)化。在減少計算量、縮短計算時間的同時又得到長度更短、轉(zhuǎn)折點(diǎn)更少的全局路徑。具體步驟歸納如下:

步驟1在大柵格層地圖中,以初始中繼點(diǎn)為起點(diǎn),目標(biāo)中繼點(diǎn)為終點(diǎn),用傳統(tǒng)A*算法進(jìn)行路徑規(guī)劃得到路徑1,并記錄路徑1經(jīng)過的拓?fù)潼c(diǎn)的坐標(biāo),將其存入列表1中。使用拓?fù)鋵拥玫匠醪铰窂綍O大減少A*算法在啟發(fā)式搜索時需要計算的節(jié)點(diǎn)數(shù)量,較原本柵格層節(jié)點(diǎn)數(shù)量減少的比例計算公式如下:

(4)

式中:m表示柵格總長度;d表示拓?fù)潼c(diǎn)間隔的柵格數(shù)量。如圖1所示在柵格層中取間隔為10,柵格總數(shù)量由圖1a中的50×50減少到圖1b中拓?fù)潼c(diǎn)量為5×5,節(jié)點(diǎn)數(shù)量減少比例為99%。

步驟2將列表1中的拓?fù)潼c(diǎn)依次轉(zhuǎn)換到小柵格地圖中的坐標(biāo),并在列表第一行和最后一行分別加上初始點(diǎn)坐標(biāo)和目標(biāo)點(diǎn)坐標(biāo),存入關(guān)鍵點(diǎn)列表2中。

表2 改進(jìn)算法性能對比

步驟3判斷列表2中相鄰兩個點(diǎn)的連線上有無障礙物,若有則在相關(guān)兩個點(diǎn)之間用傳統(tǒng)A*算法進(jìn)行路徑規(guī)劃,并得到該段路徑的關(guān)鍵折點(diǎn)。將關(guān)鍵折點(diǎn)存入關(guān)鍵折點(diǎn)列表Kpoint中,并同時將關(guān)鍵折點(diǎn)加入列表2中相應(yīng)兩個點(diǎn)之間,得到新的列表3。若無則繼續(xù)下一步,此步驟得到路徑2。

表3 改進(jìn)DWA算法性能對比

步驟4將關(guān)鍵折點(diǎn)列表3中的第一個點(diǎn)取出,作為起點(diǎn)與列表3中比該點(diǎn)更靠近終點(diǎn)的拓?fù)潼c(diǎn)依次連接,并判斷連線上有無障礙物,直到連線上有障礙物為止。若與第n個點(diǎn)連線有障礙物,則將第n-1個點(diǎn)提取出來存入列表4中。

表4 融合改進(jìn)算法性能對比

步驟5將第n-1個點(diǎn)作為新的起點(diǎn),執(zhí)行步驟4,直到連接到目標(biāo)點(diǎn)為止。將列表4中所有點(diǎn)連接起來,得到最終全局路徑3。

改進(jìn)算法全局規(guī)劃的步驟路徑如圖3所示。

圖3 改進(jìn)A*算法規(guī)劃路徑步驟

2 優(yōu)化動態(tài)窗口算法

2.1 傳統(tǒng)動態(tài)窗口法

動態(tài)窗口法是常用的機(jī)器人局部規(guī)劃算法,其既能有效地避開路徑中出現(xiàn)的未知障礙物,又能提升路徑的平滑性。該算法主要是在速度空間(線速度、角速度)中采樣多組速度,并模擬這些速度組在下一段時間間隔內(nèi)的運(yùn)動軌跡,將與障礙物碰撞的軌跡剔除,最后通過評價函數(shù)選取出最優(yōu)的模擬軌跡所對應(yīng)的速度組來驅(qū)動機(jī)器人完成相應(yīng)的運(yùn)動。

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

動態(tài)窗口法針對機(jī)器人在窗口區(qū)域內(nèi)的速度空間進(jìn)行采樣,并根據(jù)機(jī)器人運(yùn)動學(xué)模型,模擬出下一時間間隔Δt內(nèi)的運(yùn)動軌跡,在一個最小單位時間內(nèi),假定機(jī)器人做勻速直線運(yùn)動。速度空間的線速度v和角速度w反饋出的便是機(jī)器人的運(yùn)動狀態(tài)。其運(yùn)動模型可表示為:

xt=xt-1+vx×Δtcosθt-vy×Δtsinθt;

yt=yt-1+vx×Δtsinθt-vy×Δtcosθt;

θt=θt-1+wt×Δt。

(5)

2.1.2 機(jī)器人速度采樣

在速度空間(v,w)的二維平面中存在無窮組速度,根據(jù)機(jī)器人自身性能和環(huán)境限制,對采樣速度的范圍進(jìn)行約束:

vm={v∈[vmin,vmax],w∈[wmin,wmax]}。

(6)

因加速度受電機(jī)性能約束,在一個時間間隔Δt內(nèi),機(jī)器人實(shí)際能到達(dá)的速度為:

(7)

式中:(vc,wc)表示當(dāng)前速度組,(va,wa)表示機(jī)器人最大加速度組,(vb,wb)表示機(jī)器人最大減速度組。

2.1.3 評價函數(shù)

采樣的各速度組的模擬軌跡中,除去與障礙物發(fā)生碰撞的軌跡外,還存在很多組可行軌跡,因此通過以下評價函數(shù)篩選出最優(yōu)的軌跡:

G(v,w)=σ(α×heading(v,w)+

β×dist(v,w)+γ×velocity(v,w))。

(8)

式中:heading(v,w)為方位角評價子函數(shù),表示對應(yīng)速度組模擬軌跡末端時的朝向與目標(biāo)位置航向角之間的角度偏差;dist(v,w)為障礙物評價子函數(shù),表示對應(yīng)速度組模擬軌跡與障礙物最近的距離;velocity(v,w)為速度評價子函數(shù),表示模擬軌跡的速度大小;σ為平滑系數(shù),α,β,γ為加權(quán)系數(shù)。

2.2 優(yōu)化運(yùn)動狀態(tài)的動態(tài)窗口法

傳統(tǒng)的動態(tài)窗口法僅在已知的全局靜態(tài)障礙物的情況下,有良好的避障性能,但當(dāng)路徑中出現(xiàn)較多的移動障礙物時,傳統(tǒng)動態(tài)窗口法存在靈活性差、避障路徑較長和轉(zhuǎn)折角度過大等缺點(diǎn)。針對上述問題,本文通過增加行為狀態(tài)來優(yōu)化動態(tài)窗口法。

2.2.1 增加剎車等待運(yùn)動狀態(tài)

針對傳統(tǒng)動態(tài)窗口法運(yùn)動過程中僅有單一運(yùn)動狀態(tài),即簡單地朝目標(biāo)點(diǎn)移動以及持續(xù)地避開障礙物,只要未到達(dá)目標(biāo)點(diǎn),機(jī)器人會持續(xù)不斷地運(yùn)動。本文提出增加剎車等待和保持跟隨兩種運(yùn)動狀態(tài)。

當(dāng)機(jī)器人遇到動態(tài)的L型陷阱時,傳統(tǒng)動態(tài)窗口法會規(guī)劃出與移動障礙物運(yùn)動方向相近的朝向或轉(zhuǎn)折角度過大的路徑,此時移動距離過長,路徑不是最優(yōu)或較優(yōu)。故本文提出增加剎車等待的運(yùn)動狀態(tài),當(dāng)同時滿足以下情況時:移動障礙物處于機(jī)器人前進(jìn)方向;與機(jī)器人距離小于d1;障礙物移動速度大于v1;障礙物移動方向與機(jī)器人規(guī)劃當(dāng)前朝向夾角大于π/4,如圖4所示。此時,機(jī)器人將等待移動障礙物消失在規(guī)劃的路徑前方,再向前運(yùn)動。d1與v1參數(shù)可根據(jù)機(jī)器人自身不同情況進(jìn)行設(shè)置。

圖4 剎車等待示意圖

2.2.2 增加保持跟隨運(yùn)動狀態(tài)

當(dāng)移動障礙物處于機(jī)器人前方且運(yùn)動方向與機(jī)器人相同或相近時,傳統(tǒng)動態(tài)窗口法僅會向左或向右繞開障礙物,但實(shí)際運(yùn)動過程中,往往機(jī)器人跟隨障礙物移動才是最優(yōu)的路徑。故本文提出增加保持跟隨運(yùn)動狀態(tài)。當(dāng)同時滿足以下情況時:移動障礙物處于機(jī)器人前方的全局路徑之上;與機(jī)器人距離小于d2;障礙物移動速度大于機(jī)器人最大速度約束的2/3;障礙物移動方向與機(jī)器人規(guī)劃當(dāng)前朝向夾角小于π/12,如圖5所示。此時,機(jī)器人將保持原本航線不變,并將最大速度約束設(shè)置為障礙物移動速度。d2參數(shù)可根據(jù)機(jī)器人自身不同情況進(jìn)行設(shè)置。

圖5 保持跟隨示意圖

3 融合算法

經(jīng)過本文改進(jìn)后的A*算法能快速地規(guī)劃出一條路徑短,折點(diǎn)少的全局路徑,但不能及時地避讓環(huán)境中出現(xiàn)的移動障礙物,且存在路徑轉(zhuǎn)折角度較大、與障礙物距離過近等不足;經(jīng)本文優(yōu)化后的動態(tài)窗口法不能做到全局最優(yōu)路徑,且容易陷入局部最優(yōu)陷阱中。

針對兩種優(yōu)化算法中存在的不足,本文將改進(jìn)后的A*算法和優(yōu)化后的動態(tài)窗口法進(jìn)一步融合。將改進(jìn)A*算法規(guī)劃出的全局路徑關(guān)鍵點(diǎn)提取出來,作為優(yōu)化動態(tài)窗口法各個階段的臨時目標(biāo)點(diǎn)。融合算法在確保全局路徑最優(yōu)的同時又保證了局部規(guī)劃時的良好避障和移動效果。融合算法流程如圖6所示。

圖6 融合算法流程圖

4 仿真實(shí)驗(yàn)及分析

為了驗(yàn)證本文改進(jìn)A*算法和優(yōu)化動態(tài)窗口法的改進(jìn)性能,以及融合算法的有效性,在MATLAB 2016a環(huán)境下進(jìn)行仿真對比試驗(yàn),以及進(jìn)行實(shí)物實(shí)驗(yàn),并對各實(shí)驗(yàn)結(jié)果進(jìn)行分析。

4.1 改進(jìn)A*算法仿真實(shí)驗(yàn)及分析

改進(jìn)A*算法仿真環(huán)境為二維柵格地圖,其中黑色柵格表示障礙物,白色柵格表示可通過路徑點(diǎn)。為驗(yàn)證改進(jìn)A*算法對于規(guī)劃路徑長度、耗費(fèi)時間以及折點(diǎn)數(shù)方面性能的提升,分別在50×50、75×75以及100×100大小的柵格地圖中對A*算法改進(jìn)前后進(jìn)行仿真對比試驗(yàn),結(jié)果分別如圖7～圖9所示。上述仿真實(shí)驗(yàn)結(jié)果統(tǒng)計如表2所示。

圖7 50×50柵格地圖場景下仿真結(jié)果

圖8 75×75柵格地圖場景下仿真結(jié)果

圖9 100×100柵格地圖場景下仿真結(jié)果

結(jié)合圖7～圖10以及表2可知,在相同的靜態(tài)環(huán)境中,相比于傳統(tǒng)A*算法,改進(jìn)A*算法因?yàn)槭紫冗M(jìn)行拓?fù)鋵拥貓D的路徑規(guī)劃,所以極大地減少了啟發(fā)式搜索時需要計算的柵格數(shù)量,使耗費(fèi)時間大幅減少;并刪除了路徑中冗余的折點(diǎn),使路徑不但長度減少而且更加平滑。聯(lián)合圖11和表2可知,當(dāng)?shù)貓D大小由50×50增加到100×100時,各項(xiàng)性能指標(biāo)優(yōu)化效果更加明顯,尤其是耗費(fèi)時間減少比例由38.6%增加至71.8%。由此可見,地圖越大、柵格數(shù)量越多的場景越適合用改進(jìn)A*算法。

圖10 算法改進(jìn)前后各性能對比

圖11 改進(jìn)A*算法不同大小地圖中性能優(yōu)化趨勢

4.2 優(yōu)化動態(tài)窗口法仿真實(shí)驗(yàn)及分析

針對不同的移動障礙物場景,分別對改進(jìn)動態(tài)窗口法和傳統(tǒng)動態(tài)窗口法進(jìn)行仿真實(shí)驗(yàn)。仿真環(huán)境為15×15二維地圖,障礙物及影響范圍用圓形表示,設(shè)置多個靜態(tài)障礙物和兩個移動障礙物。

針對動態(tài)L型陷阱仿真對比結(jié)果如圖12和圖13所示。針對同向動態(tài)障礙物場景仿真結(jié)果如圖14和圖15所示。

圖13 優(yōu)化動態(tài)窗口法路徑

圖14 傳統(tǒng)動態(tài)窗口法路徑

圖15 優(yōu)化動態(tài)窗口法路徑

由圖12、圖13和表3可看出,傳統(tǒng)動態(tài)窗口法遇到動態(tài)的L型障礙時,規(guī)劃出的路徑弧度過大導(dǎo)致最終路徑長度較大,而優(yōu)化動態(tài)窗口法能合理地剎車等待,規(guī)劃出的路徑長度由27.82減少到18.30,花費(fèi)時間由43.93 s減少到35.16 s。由圖14和圖15可看出,遇到移動障礙擋在航線前方時,傳統(tǒng)動態(tài)窗口法只會單一地繞過障礙物,最終的路徑可能偏離原本的航線,導(dǎo)致路徑長度過長;而動態(tài)窗口法能根據(jù)航線前方的障礙物速度選擇跟隨障礙物,規(guī)劃出的路徑長度由28.57減少到15.37,花費(fèi)時間由43.52 s減少到34.76 s,改進(jìn)動態(tài)窗口法規(guī)劃出的路徑更短、更合理。

4.3 融合算法仿真實(shí)驗(yàn)及分析

融合算法的仿真實(shí)驗(yàn)設(shè)置兩個仿真環(huán)境,第一個大小為50×50的柵格地圖,第二個大小為75×60的柵格地圖。障礙物為黑色柵格和移動的紅色圓圈,黑色小圓圈表示機(jī)器人當(dāng)前位置,藍(lán)色實(shí)線表示路徑。為驗(yàn)證融合算法的合理性,在第一個仿真環(huán)境中的靜態(tài)環(huán)境中用改進(jìn)A*算法、優(yōu)化動態(tài)窗口法以及改進(jìn)后的融合算法進(jìn)行路徑規(guī)劃仿真實(shí)驗(yàn);在動態(tài)環(huán)境中用改進(jìn)A*算法分別與傳統(tǒng)動態(tài)窗口法和本文優(yōu)化后的動態(tài)窗口法融合作對比仿真實(shí)驗(yàn)。其中靜態(tài)環(huán)境仿真結(jié)果如圖16所示,動態(tài)環(huán)境仿真結(jié)果如圖17～圖20和表4所示。

圖16 靜態(tài)環(huán)境

圖17 傳統(tǒng)融合算法用于動態(tài)環(huán)境

由圖16可知,改進(jìn)A*算法能快速有效地規(guī)劃出從起點(diǎn)到終點(diǎn)的最優(yōu)路徑,但是改進(jìn)A*算法無法實(shí)時規(guī)劃路徑,不能有效躲避移動障礙物;優(yōu)化動態(tài)窗口法沒有全局目標(biāo)點(diǎn)的指引,可能會陷入局部最優(yōu),尋路失敗;融合改進(jìn)A*算法和動態(tài)窗口法的融合算法不但能有效地規(guī)劃出路徑,而且路徑平滑度更高,曲率連續(xù),且能和障礙物保持合適的安全距離。

第二個仿真環(huán)境中,除在區(qū)域1和區(qū)域4設(shè)置移動速度較快的動態(tài)L型障礙和同向移動障礙外,還在區(qū)域2和區(qū)域3設(shè)置移動速度緩慢的動態(tài)L型障礙和同向移動障礙。

由圖17～圖20可知,傳統(tǒng)融合算法和改進(jìn)融合算法都能有效地避開動態(tài)障礙物到達(dá)終點(diǎn),但是傳統(tǒng)融合算法遇到不同的動態(tài)環(huán)境時,都只有單一的避讓狀態(tài),而改進(jìn)融合算法能根據(jù)障礙物情況選擇不同的運(yùn)動狀態(tài)。對比圖17、圖18及表4可知,在遇到動態(tài)L型障礙和同向移動障礙時,改進(jìn)算法會分別進(jìn)入剎車等待和保持跟隨狀態(tài),路徑長度由79.65減少到66.45,遇到動態(tài)L型障礙路徑曲率由20.17減少到3.25,遇到同向移動障礙時曲率由12.74減少到7.91。對比圖19和圖20及表4可知,改進(jìn)融合算法能根據(jù)環(huán)境中移動速度緩慢的障礙物選擇避讓狀態(tài),而對移動速度快的障礙物情況靈活地選擇剎車等待、保持跟隨狀態(tài),整體路徑長度由103.72減少到97.67,曲率變化更小。

圖18 改進(jìn)融合算法用于動態(tài)環(huán)境

圖19 傳統(tǒng)融合算法用于動態(tài)環(huán)境

圖20 改進(jìn)融合算法用于動態(tài)環(huán)境

聯(lián)合圖17～圖20及表4可得,改進(jìn)融合算法能根據(jù)不同的環(huán)境情況靈活地選擇不同的運(yùn)動狀態(tài),既減少了整體路徑長度,又減少了經(jīng)過障礙物時的轉(zhuǎn)折角度,增強(qiáng)了機(jī)器人路徑規(guī)劃時的靈活性。

4.4 實(shí)驗(yàn)驗(yàn)證及分析

為驗(yàn)證本文算法在實(shí)際應(yīng)用中的有效性,除仿真實(shí)驗(yàn)外,還增加實(shí)際實(shí)驗(yàn)。實(shí)驗(yàn)選用搭載機(jī)器人操作系統(tǒng)ROS(Kinetic)的移動機(jī)器人,實(shí)驗(yàn)場地與融合算法的第二個仿真環(huán)境類似,環(huán)境中以行人構(gòu)建動態(tài)L型和同向移動障礙,實(shí)驗(yàn)場地和用Gmapping算法構(gòu)建的代價地圖如圖21所示,實(shí)驗(yàn)結(jié)果如圖22和圖23所示,圖中紅色箭頭反映轉(zhuǎn)折角度。

圖21 實(shí)驗(yàn)場地及代價地圖

圖22 傳統(tǒng)融合算法

圖23 改進(jìn)融合算法

由圖22和圖23可知,兩種融合算法都能避讓障礙物,使機(jī)器人安全到達(dá)終點(diǎn)。對比圖22和圖23中的區(qū)域a和區(qū)域b可證明,當(dāng)機(jī)器人在遇到動態(tài)L型障礙和同向移動障礙時,相比于傳統(tǒng)融合算法,選用改進(jìn)融合算法規(guī)劃出的路徑不僅長度更短,轉(zhuǎn)折角度也更平緩,規(guī)劃出的路徑更合理、更有利于機(jī)器人移動。

5 結(jié)束語

為提升傳統(tǒng)A*算法在柵格數(shù)量較多時的路徑規(guī)劃效率以及增強(qiáng)傳統(tǒng)動態(tài)窗口法在不同場景下的靈活性。本文通過建立拓?fù)鋵拥貓D并刪除多余折點(diǎn)來改進(jìn)A*算法,通過增加運(yùn)動狀態(tài)來優(yōu)化動態(tài)窗口法,并融合兩種改進(jìn)算法。融合算法不僅能快速地規(guī)劃出一條折點(diǎn)數(shù)少路徑較優(yōu)的全局路徑,還能在機(jī)器人移動過程中有效地對各種場景下的移動障礙物作出不同的狀態(tài)變化。通過對比試驗(yàn)驗(yàn)證了改進(jìn)后的融合算法的有效性和合理性。但所有實(shí)驗(yàn)都是在已經(jīng)獲得障礙物運(yùn)動信息的假定前提條件下,未來將研究移動障礙物的準(zhǔn)確感知、目標(biāo)跟蹤和實(shí)時預(yù)測,探索在不同復(fù)雜場景下的移動機(jī)器人路徑規(guī)劃算法。