改進(jìn)A*算法的移動(dòng)機(jī)器人路徑規(guī)劃

楊明亮,李寧

(太原科技大學(xué) 機(jī)械工程學(xué)院,太原 030024)

移動(dòng)機(jī)器人在生產(chǎn)建設(shè)、國(guó)防科技、生活服務(wù)等重要領(lǐng)域具有廣闊的發(fā)展應(yīng)用前景。隨著移動(dòng)機(jī)器人不斷被普及應(yīng)用,機(jī)器人實(shí)現(xiàn)智能化和自主化需要完成3個(gè)目標(biāo):第一個(gè)是同步定位,第二個(gè)是地圖生成,第三個(gè)是路徑規(guī)劃。前兩個(gè)任務(wù)是機(jī)器人自身定位與地圖的構(gòu)建(Simultaneous localization and mapping,SLAM),指在未知的環(huán)境下,移動(dòng)機(jī)器人通過(guò)鍵盤(pán)發(fā)布底盤(pán)速度指令控制機(jī)器人低速移動(dòng),通過(guò)激光雷達(dá)等傳感器反饋的數(shù)據(jù),讓移動(dòng)機(jī)器人逐步建立出該環(huán)境下的地圖。第三個(gè)任務(wù)是路徑規(guī)劃,指在地圖建立的情況下,在有靜態(tài)和動(dòng)態(tài)障礙物環(huán)境中快速規(guī)劃出一條無(wú)碰撞最優(yōu)路徑,使移動(dòng)機(jī)器人能平滑、穩(wěn)定地從起始點(diǎn)到達(dá)目標(biāo)點(diǎn)[1]。

路徑規(guī)劃問(wèn)題是移動(dòng)機(jī)器人領(lǐng)域研究的熱點(diǎn),盡管許多學(xué)者都對(duì)此做了大量研究,仍存在很多不足。例如文獻(xiàn)[2]提出在方格地圖中使用定向搜索的方法減少A*算法生成并擴(kuò)展的節(jié)點(diǎn)數(shù)量,不需要提前處理任何信息,提高搜索效率;文獻(xiàn)[3]對(duì)人工勢(shì)場(chǎng)法進(jìn)行改進(jìn),提出安全距離概念,解決在沒(méi)有動(dòng)態(tài)障礙物環(huán)境下傳統(tǒng)人工勢(shì)場(chǎng)法路徑規(guī)劃中斷問(wèn)題;文獻(xiàn)[4]提出混合路徑規(guī)劃方法,改進(jìn)BP算法的收斂性,實(shí)時(shí)規(guī)劃出局部臨時(shí)目標(biāo)點(diǎn)避開(kāi)動(dòng)態(tài)障礙物。文獻(xiàn)[5]將傳統(tǒng)A*算法擴(kuò)大可搜索鄰域,降低轉(zhuǎn)折點(diǎn)個(gè)數(shù),快速實(shí)現(xiàn)無(wú)碰撞軌跡規(guī)劃;文獻(xiàn)[6]提出基于3階Bezier曲線用于設(shè)計(jì)兩點(diǎn)的轉(zhuǎn)彎,獲得非完整約束的平滑路徑;文獻(xiàn)[7]提出采用預(yù)瞄偏差角追蹤法追擊動(dòng)態(tài)目標(biāo)點(diǎn),提高路徑規(guī)劃效率。

基于以上研究,本文提出了一種混合下改進(jìn)A*和DWA算法,同時(shí)具備全局路徑和局部路徑下的規(guī)劃優(yōu)點(diǎn),并且計(jì)算量低,能有效避免陷入局部最優(yōu),提升算法效率。

1 主要研究方法

1.1 Cartographer的柵格地圖創(chuàng)建

Cartographer[8-12]主要由前端和后端兩部分組成:前端主要負(fù)責(zé)子圖匹配和回環(huán)檢測(cè),后端負(fù)責(zé)對(duì)當(dāng)前位姿圖序列進(jìn)行優(yōu)化。激光雷達(dá)的數(shù)據(jù)幀與子圖進(jìn)行匹配,使得當(dāng)前幀的激光數(shù)據(jù)插入到子圖上最優(yōu)位置,當(dāng)沒(méi)有新的掃描被插入時(shí),子地圖會(huì)停止創(chuàng)建并進(jìn)行局部回環(huán)檢測(cè)。子圖創(chuàng)建完成后,激光雷達(dá)數(shù)據(jù)幀匹配到最近子地圖,找到估計(jì)位姿最佳的匹配點(diǎn),將其添加到回環(huán)約束中。后端每隔幾秒會(huì)進(jìn)行一次優(yōu)化保證新的激光雷達(dá)數(shù)據(jù)被添加進(jìn)來(lái),提高實(shí)時(shí)回環(huán)檢測(cè)效率,采用分支定界和預(yù)先計(jì)算的網(wǎng)絡(luò)保證系統(tǒng)的實(shí)時(shí)性。

1.1.1 局部?jī)?yōu)化

將激光雷達(dá)數(shù)據(jù)記做H={hk}k=1,…,khk∈R2,初始激光點(diǎn)為0∈R2。激光掃描幀映射到子地圖的位姿變換描述為T(mén)ξ,將掃描點(diǎn)從掃描幀映射到子圖坐標(biāo)系下,即

(1)

幾個(gè)迭代的數(shù)據(jù)幀生成一個(gè)子圖,子圖使用概率網(wǎng)格地圖。每當(dāng)一個(gè)新的掃描數(shù)據(jù)插入概率網(wǎng)絡(luò),概率網(wǎng)絡(luò)的狀態(tài)會(huì)被計(jì)算,每個(gè)網(wǎng)格點(diǎn)概率都有命中(hit)和不相交(miss)兩種狀態(tài)。對(duì)沒(méi)有觀測(cè)到的網(wǎng)格掃描點(diǎn)集中起來(lái)并分配概率值,已經(jīng)觀測(cè)到的相關(guān)點(diǎn),進(jìn)行概率更新,則

(2)

Mnew(x)=clamp(odds-1(odds(Mold(x)godds(Phit)))

(3)

式中clamp是區(qū)間限定函數(shù)。

在把獲得的最新的掃描幀插入到子地圖最優(yōu)位置之前,掃描幀姿態(tài)跟當(dāng)前子地圖通過(guò)非線性最小二乘法進(jìn)行優(yōu)化。

(4)

式中Msmooth是線性評(píng)價(jià)函數(shù)。

1.1.2 全局優(yōu)化

由于每個(gè)激光雷達(dá)掃描幀僅與近期子圖進(jìn)行匹配容易產(chǎn)生累計(jì)誤差,采用稀疏位姿消除誤差,進(jìn)行優(yōu)化。激光掃描點(diǎn)插入子地圖時(shí)的位姿會(huì)被緩存到內(nèi)存中,創(chuàng)建完成子地圖后會(huì)進(jìn)行閉環(huán)檢測(cè)。插入子圖的激光雷達(dá)數(shù)據(jù)幀匹配后都在后端優(yōu)化位姿,發(fā)現(xiàn)好的閉環(huán)匹配則加入全局優(yōu)化中來(lái)。Cartographer算法使用分支定界方法確定ξ*和搜索窗口構(gòu)建回環(huán)。

1.2 移動(dòng)機(jī)器人路徑規(guī)劃

1.2.1 全局路徑規(guī)劃

采用A*算法[13],設(shè)置柵格單元的位置坐標(biāo)(x,y),并把估價(jià)函數(shù)f(n)初始化為零,生成Open表和Close表,通過(guò)全面評(píng)估各擴(kuò)展節(jié)點(diǎn)的代價(jià)值,選擇最低代價(jià)的節(jié)點(diǎn)進(jìn)行擴(kuò)展使其能快速地導(dǎo)向目標(biāo)點(diǎn)。

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

(5)

式中:f(n)是估價(jià)函數(shù);g(n)表示的是起始點(diǎn)到當(dāng)前點(diǎn)實(shí)際付出的代價(jià)值;h(n)代表當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的預(yù)計(jì)代價(jià)值。

在A*算法中,啟發(fā)函數(shù)的選取至關(guān)重要,本文中以柵格作為地圖環(huán)境,且地圖中存在障礙物,故采用曼哈頓距離作為啟發(fā)函數(shù)，即

f(n)=g(n)+(|dx-nx|)+(|dy-ny|)

(6)

f(n)在g(n)確定的情況下h(n)的選取極為重要。當(dāng)前擴(kuò)展點(diǎn)距離目標(biāo)點(diǎn)越近,h越小,f也就越小,搜索的節(jié)點(diǎn)多,效率低,反之,f越大效率高,難以找到最優(yōu)路徑。圖1為A*算法示意圖。

圖1 A*算法示意圖

1.2.2 移動(dòng)機(jī)器人局部路徑規(guī)劃

采用動(dòng)態(tài)窗口法(Dynamic window approach,DWA)[14],在速度空間[v,w]中對(duì)線速度v和角速度w進(jìn)行多組采樣,并根據(jù)機(jī)器人的自身限制及環(huán)境因素,對(duì)最大減速度和最大加速度進(jìn)行約束,在Δt時(shí)間間隔內(nèi),計(jì)算移動(dòng)機(jī)器人實(shí)際能夠達(dá)到的速度vd的范圍為

(7)

預(yù)測(cè)在采樣速度條件下機(jī)器人在一段時(shí)間內(nèi)的運(yùn)行軌跡,在預(yù)測(cè)軌跡上有障礙物時(shí),為了保證機(jī)器人安全,實(shí)現(xiàn)避障功能,保證當(dāng)前速度能在撞擊障礙物前降為0,速度的最大值為

(8)

式中distmin(v,w)代表在相應(yīng)預(yù)測(cè)軌跡上與障礙物之間的最短距離。

最后引入評(píng)價(jià)函數(shù)對(duì)預(yù)測(cè)的運(yùn)行軌跡進(jìn)行評(píng)估,選取移動(dòng)機(jī)器人采樣好速度樣本后,評(píng)價(jià)每對(duì)采樣速度對(duì)應(yīng)的模擬運(yùn)行軌跡,根據(jù)評(píng)價(jià)得分選取最優(yōu)采樣速度并引入目標(biāo)函數(shù)去控制機(jī)器人的速度、方向和距離之間的關(guān)系,目標(biāo)函數(shù)為

G(v,w)=σ(α·heading(v,w)+β·dist(v,w)+

γ·velocity(v,w))

(9)

式中:heading(v,w)代表目標(biāo)方向;dist(v,w)代表與障礙物的最近距離;velocity(v,w)代表采樣速度;σ、β、γ為權(quán)重比。

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

采用改進(jìn)A*算法進(jìn)行全局路徑規(guī)劃,融合DWA算法來(lái)局部避障。通過(guò)增加參數(shù)Weigth設(shè)定A*算法中h(n)權(quán)重系數(shù),并增加參數(shù)Corner-amend進(jìn)行拐角修正,不僅可以實(shí)時(shí)路徑規(guī)劃還避免陷入局部最優(yōu)問(wèn)題,保證動(dòng)態(tài)規(guī)劃路徑的全局最優(yōu)性。本文算法流程如圖2所示。

圖2 本文算法流程圖

本文采用傳統(tǒng)A*算法與改進(jìn)后A*算法與DWA算法對(duì)路徑進(jìn)行規(guī)劃,得出兩條路徑坐標(biāo)點(diǎn)隊(duì)列。試驗(yàn)圖像結(jié)果顯示分別如圖3和圖4所示。以運(yùn)行時(shí)間、路徑長(zhǎng)度與平滑性作為評(píng)判指標(biāo),對(duì)比算法的優(yōu)劣性。

圖3 傳統(tǒng)A*搜索算法路徑規(guī)劃結(jié)果

圖4 改進(jìn)A*算法和DWA算法路徑規(guī)劃結(jié)果

表1 改進(jìn)前后算法性能指標(biāo)的對(duì)比

由表1中可以看出,改進(jìn)后A*算法和DWA算法后,運(yùn)行時(shí)間縮短14%,提高了運(yùn)行效率,克服了傳統(tǒng)路徑規(guī)劃中轉(zhuǎn)折點(diǎn)多和轉(zhuǎn)折角度大等缺點(diǎn),證明了本文算法的有效性。

2 移動(dòng)機(jī)器人硬軟件平臺(tái)設(shè)計(jì)

2.1 移動(dòng)平臺(tái)硬件系統(tǒng)設(shè)計(jì)

本文建立的自主移動(dòng)機(jī)器人系統(tǒng)簡(jiǎn)易運(yùn)動(dòng)底盤(pán)、2D激光傳感器和計(jì)算機(jī)運(yùn)算平臺(tái)組成。采用激光雷達(dá)傳感器獲取的環(huán)境點(diǎn)云信息和AMCL算法來(lái)更新粒子分布,計(jì)算出當(dāng)前位姿。接受里程計(jì)發(fā)布的Odom和雷達(dá)傳感器發(fā)布的sensor-msgs,利用谷歌開(kāi)源Cartographer算法建立基于圖優(yōu)化的柵格地圖。設(shè)定好路徑點(diǎn),采用A*和動(dòng)態(tài)窗口法,獲得最優(yōu)路徑曲線。根據(jù)路徑曲線,考慮運(yùn)動(dòng)約束,生成控制量,控制移動(dòng)機(jī)器人運(yùn)動(dòng)。系統(tǒng)架構(gòu)與工作流程如圖5所示。

圖5 系統(tǒng)架構(gòu)與工作流程

在計(jì)算機(jī)預(yù)裝Ubuntu16.04操作系統(tǒng),安裝配置機(jī)器人操作系統(tǒng)ROS Kinetic、RVIZ以及樂(lè)視激光雷達(dá)傳感器的ROS驅(qū)動(dòng)等基礎(chǔ)工具包。并根據(jù)系統(tǒng)功能要求,開(kāi)發(fā)移動(dòng)機(jī)器人運(yùn)動(dòng)控制、定位建圖與路徑規(guī)劃等算法[15-16]。

2.2 ROS移動(dòng)機(jī)器人平臺(tái)

ROS系統(tǒng)封裝及測(cè)試需要在以下方面進(jìn)行配置和改進(jìn)[17-18]:通過(guò)TF和URDF功能包定義機(jī)器人坐標(biāo)變換關(guān)系和坐標(biāo)參數(shù);通過(guò)ROS發(fā)布IMU數(shù)據(jù)和里程計(jì)預(yù)處理節(jié)點(diǎn)信息;以及從激光雷達(dá)傳感器和下位機(jī)控制板獲取信息通過(guò)ROS發(fā)布數(shù)據(jù)流,如圖6所示。

圖6 ROS功能包在SLAM和路徑規(guī)劃上的應(yīng)用

建立URDF功能包后運(yùn)行l(wèi)aunch文件啟動(dòng)joint-state-publisher節(jié)點(diǎn)發(fā)布每個(gè)joint的狀態(tài);啟動(dòng)robot-state-publisher節(jié)點(diǎn)將整個(gè)機(jī)器人各個(gè)link、joint之間的關(guān)系,通過(guò)TF的形式變換成三維姿態(tài)信息發(fā)布出去。主要步驟:創(chuàng)建底層控制節(jié)點(diǎn)、IMU節(jié)點(diǎn)以及URDF模型節(jié)點(diǎn)編寫(xiě)啟動(dòng)launch文件,并在Linux終端編譯運(yùn)行。

通過(guò)ROS發(fā)布nav-msgs/odometry里程計(jì)信息,該消息存儲(chǔ)了機(jī)器人速度信息和位姿信息的估計(jì)值,然后使用TransformBroadcaster發(fā)送變化給TF以便導(dǎo)航包可以從中獲取速度消息。

通過(guò)ROS發(fā)布sensor-msgs傳感器數(shù)據(jù)流,該消息存放激光信息(LaserScan)以實(shí)現(xiàn)機(jī)器人動(dòng)態(tài)避障,stamp對(duì)應(yīng)存儲(chǔ)時(shí)間信息,frame-id字段存儲(chǔ)與數(shù)據(jù)相關(guān)聯(lián)的參考坐標(biāo)系。

利用ROS現(xiàn)有導(dǎo)航框架逐個(gè)編寫(xiě)上述代碼,采用基于圖優(yōu)化SLAM算法在ROS下改進(jìn)的cartographer功能包,機(jī)器人2D地圖移動(dòng)時(shí)采用的定位系統(tǒng)選用Amcl定位功能包,訂閱初始位姿來(lái)估計(jì)移動(dòng)機(jī)器人姿態(tài),在已知地圖信息的情況下能實(shí)現(xiàn)全局路徑導(dǎo)航。在未知環(huán)境下,根據(jù)A*算法計(jì)算地圖中的最優(yōu)且安全路徑,結(jié)合DWA算法使移動(dòng)機(jī)器人能夠?qū)崿F(xiàn)實(shí)時(shí)的動(dòng)態(tài)避障。

3 試驗(yàn)結(jié)果與分析

在SLAM建圖中,回環(huán)檢測(cè)是一個(gè)重要的研究對(duì)象。在移動(dòng)機(jī)器人建圖過(guò)程中,機(jī)器人的定位與建圖會(huì)隨著傳感器和環(huán)境因素的影響而不斷積累誤差,通過(guò)對(duì)回形的走廊式環(huán)境建圖可以對(duì)類(lèi)似環(huán)境和回環(huán)檢測(cè)兩項(xiàng)進(jìn)行判定建圖的效果。在未知環(huán)境下移動(dòng)機(jī)器人自主建圖結(jié)果如圖7所示。從回環(huán)檢測(cè)試驗(yàn)的結(jié)果可以看出,在地圖矩形兩側(cè)墻壁長(zhǎng)度與實(shí)際環(huán)境差距較小,進(jìn)行了很好的回環(huán)檢測(cè),建圖效果較好,能準(zhǔn)確反映環(huán)境信息。

圖7 移動(dòng)機(jī)器人室內(nèi)試驗(yàn)

自主探索的局部路徑規(guī)劃使用DWA算法,運(yùn)動(dòng)過(guò)程中根據(jù)自身半徑計(jì)算障礙物膨脹半徑進(jìn)行避障。圖8為在設(shè)置多障礙物下通過(guò)navigation功能包實(shí)現(xiàn)路徑規(guī)劃,從表2中的數(shù)據(jù)可以看出,移動(dòng)機(jī)器人能成功的按照預(yù)期目標(biāo)完成未知環(huán)境下避障建圖等任務(wù),機(jī)器人在兩個(gè)障礙物之間時(shí)產(chǎn)生了抖動(dòng)并在障礙物前停滯了幾秒后立即做出決策,整體下來(lái)運(yùn)行平穩(wěn)。通過(guò)實(shí)驗(yàn)數(shù)據(jù)可以得出融合改進(jìn)的A*算法和DWA算法規(guī)劃的路徑相對(duì)長(zhǎng)度最小,路徑更平滑,距離更優(yōu)。

圖8 多障礙物路徑規(guī)劃試驗(yàn)

表2 試驗(yàn)數(shù)據(jù)

4 結(jié)束語(yǔ)

本文利用運(yùn)動(dòng)底盤(pán)、激光雷達(dá)傳感器和運(yùn)算平臺(tái),搭建自主移動(dòng)機(jī)器人系統(tǒng)。研究了移動(dòng)機(jī)器人的SLAM問(wèn)題和路徑規(guī)劃問(wèn)題。采用改進(jìn)Cartograhper-SLAM算法與激光雷達(dá)構(gòu)建環(huán)境地圖;在路徑規(guī)劃方面,通過(guò)A*算法和改良DWA算法可以繞開(kāi)障礙物到達(dá)目標(biāo)點(diǎn)并規(guī)劃的路徑保持全局最優(yōu)。通過(guò)設(shè)計(jì)實(shí)驗(yàn)檢測(cè)系統(tǒng)的自主定位、建圖、路徑規(guī)劃與運(yùn)動(dòng)控制的能力。根據(jù)實(shí)驗(yàn)結(jié)果驗(yàn)證了機(jī)器人導(dǎo)航系統(tǒng)具有較高的可靠性,效果達(dá)到預(yù)期目標(biāo)。