龔志力， 谷玉海*， 朱騰騰， 任 斌

(1.北京信息科技大學(xué)現(xiàn)代測(cè)控技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室， 北京 100192；2.北京航天長(zhǎng)征飛行器研究所， 北京 100076)

移動(dòng)機(jī)器人自主導(dǎo)航技術(shù)作為各項(xiàng)高新技術(shù)綜合應(yīng)用的載體，在各個(gè)行業(yè)均展現(xiàn)了巨大的應(yīng)用價(jià)值[1]。在移動(dòng)機(jī)器人進(jìn)行自主導(dǎo)航前，還需要依賴同步定位與建圖(simultaneous localization and mapping，SLAM)技術(shù)完成機(jī)器人在未知環(huán)境中的地圖構(gòu)建。目前主要應(yīng)用在移動(dòng)機(jī)器人自主導(dǎo)航的地圖類型主要分為拓?fù)涞貓D[2]、柵格地圖[3]和語義地圖[4]，三種方法各有其優(yōu)劣和不同的適用范圍。

其中柵格地圖易于實(shí)現(xiàn)建模、更新與處理，具有結(jié)構(gòu)簡(jiǎn)單、便于存儲(chǔ)計(jì)算等優(yōu)點(diǎn)，是目前移動(dòng)機(jī)器人自主導(dǎo)航中研究和應(yīng)用最廣泛的地圖類型。柵格地圖把環(huán)境信息離散劃分成一系列柵格，用概率值表征該柵格位置下的障礙信息。一般情況下只含簡(jiǎn)單的布爾信息，因此在復(fù)雜環(huán)境下尋求更優(yōu)路徑有一定局限性。為改善柵格地圖的缺陷，李天成等[5]提出了一種極坐標(biāo)下扇形柵格地圖的建立方法，構(gòu)建了一種新型高效的工作空間。岳偉韜等[6]提出了“有義地圖率”的表征柵格地圖準(zhǔn)確度的概念，提供了以準(zhǔn)確度和信息量評(píng)估柵格地圖獲得最佳柵格大小方法。余文凱等[7]基于K-means聚類算法對(duì)柵格地圖進(jìn)行分區(qū)并提出了量化各局部區(qū)域的復(fù)雜度的方法。張福海等[8]改善代價(jià)地圖與室內(nèi)環(huán)境的匹配性，提升了地圖更新的速度。

以上研究成果均針對(duì)傳統(tǒng)柵格地圖建立方式進(jìn)行優(yōu)化，使柵格地圖能包含更多環(huán)境信息，以提高自主導(dǎo)航效率。在移動(dòng)機(jī)器人導(dǎo)航中對(duì)柵格地圖的優(yōu)化由代價(jià)地圖完成。代價(jià)地圖對(duì)傳統(tǒng)柵格劃分新的區(qū)域，根據(jù)傳感器數(shù)據(jù)實(shí)時(shí)更新障礙物信息，機(jī)器人移動(dòng)時(shí)在障礙物周圍設(shè)置一層安全緩沖區(qū)。相比于其他種類地圖，代價(jià)地圖更有助于機(jī)器人的定位與導(dǎo)航[9]。如今隨著移動(dòng)機(jī)器人自主導(dǎo)航應(yīng)用范圍的不斷擴(kuò)大，代價(jià)地圖也逐漸表露出了應(yīng)對(duì)復(fù)雜環(huán)境時(shí)障礙物劃分不靈活、路徑規(guī)劃不理想等不足。

現(xiàn)提出一種基于機(jī)器人操作系統(tǒng)(robot operating system, ROS)的代價(jià)地圖自適應(yīng)膨脹半徑設(shè)置方法及一種改進(jìn)A*路徑規(guī)劃算法，優(yōu)化移動(dòng)機(jī)器人自主導(dǎo)航中的地圖構(gòu)建與路徑規(guī)劃，以提升移動(dòng)機(jī)器人自主導(dǎo)航過程中應(yīng)對(duì)復(fù)雜環(huán)境的安全性和魯棒性。

1 代價(jià)地圖構(gòu)建

獲取地圖信息是ROS移動(dòng)機(jī)器人進(jìn)行自主導(dǎo)航的第一步。代價(jià)地圖是由機(jī)器人接受各類傳感器采集到的環(huán)境數(shù)據(jù)而創(chuàng)建、更新的二維或三維地圖。通常情況下ROS中的代價(jià)地圖采用柵格形式，根據(jù)與障礙物的碰撞情況可以分為三種狀態(tài)：無障礙(Free)、有障礙(Occupied)和未知區(qū)域(Unknown)[10]。

1.1 代價(jià)地圖的分層

代價(jià)地圖由多個(gè)層組成，每個(gè)地圖層(Layer)都有其特定的功能，各個(gè)地圖層的信息疊加生成最終的代價(jià)地圖信息。如圖1所示，靜態(tài)地圖層(Static Layer)是第一層，障礙物層(Obstacle Layer)是第二層，膨脹層(Inflation Layer)是第三層。這三層組合成了疊加各個(gè)地圖層后的代價(jià)地圖(Master map)，為路徑規(guī)劃模塊提供地圖信息。

圖1 Master map及基礎(chǔ)地圖層Fig.1 Master map and basic layers

1.2 代價(jià)地圖的更新

代價(jià)地圖以一定的周期進(jìn)行更新，每個(gè)周期中都要依據(jù)更新的傳感器數(shù)據(jù)對(duì)代價(jià)地圖底層結(jié)構(gòu)進(jìn)行標(biāo)記(mark)和清除(clear)操作，再將各層信息投影到master map上得到相應(yīng)的代價(jià)值。mark操作將地圖上障礙物坐標(biāo)對(duì)應(yīng)的柵格設(shè)置為致命代價(jià)值，clear操作將清除雷達(dá)向外發(fā)射線穿過柵格的代價(jià)值設(shè)置為無障礙[11]。如存儲(chǔ)的障礙物信息為3D，則需要將每一列障礙物信息投影成2D后才能放入代價(jià)地圖中。

更新過程主要分為兩個(gè)階段：階段一為各個(gè)Layer的更新，階段二為Master map的更新。階段一中，擁有自身實(shí)體地圖信息的地圖層會(huì)更新從傳感器獲得的新增區(qū)域的代價(jià)值，如Static Layer和Obstacle Layer；自身不維護(hù)地圖的地圖層則不會(huì)更新，如Inflation Layer。階段二中，會(huì)將各個(gè)地圖層的數(shù)據(jù)逐一添加至Master map。

1.3 代價(jià)地圖柵格概率值

代價(jià)地圖中的一個(gè)柵格在計(jì)算機(jī)內(nèi)存中占用一個(gè)字節(jié)，可以表示0～255中的任意數(shù)字，稱為柵格代價(jià)值。根據(jù)Bresenham算法對(duì)障礙物點(diǎn)進(jìn)行標(biāo)記[12]，填充機(jī)器人傳感器位置到障礙物之間的柵格概率。獲得障礙點(diǎn)的坐標(biāo)后，需要對(duì)障礙點(diǎn)做膨脹處理，即根據(jù)柵格代價(jià)值函數(shù)填充障礙物周圍膨脹區(qū)柵格的代價(jià)值。

令一柵格qn到距離它最短歐式距離的障礙點(diǎn)的距離為d(qn)，柵格qn的柵格代價(jià)值記為c(qn)，其柵格代價(jià)值函數(shù)為

c(qn)=

(1)

式(1)中：ra為每個(gè)柵格的邊長(zhǎng)；ri為機(jī)器人地面投影輪廓的內(nèi)切圓半徑；rc為外接圓半徑；rm為人為規(guī)定的代價(jià)地圖膨脹半徑；w為代價(jià)值下降權(quán)重。由式(1)可得如圖2的柵格代價(jià)值分布示意圖。

center cell為機(jī)器人輪廓幾何中心柵格

若ra≤d(qn)

若ri≤d(qn)

若ri≤d(qn)

若rm≤d(qn)，珊格代價(jià)記為自由空間(Free space)，屬于沒有障礙物的空間。

據(jù)式(1)可知，在對(duì)障礙物膨脹處理中，膨脹半徑為定值，選擇的膨脹半徑是否合適在一定程度上決定了避障效果的好壞。

2 自適應(yīng)膨脹半徑算法

在室內(nèi)環(huán)境中，利用目前較為成熟的SLAM算法如gmapping，cartographer等能實(shí)現(xiàn)較好的導(dǎo)航效果[14]。但在室外環(huán)境處于非結(jié)構(gòu)化環(huán)境中的密集障礙物群時(shí)，代價(jià)地圖中膨脹半徑設(shè)置過大會(huì)阻礙局部路徑規(guī)劃，導(dǎo)致不能靈活地通過障礙物群而被困在障礙物之中。膨脹半徑設(shè)置過小則會(huì)導(dǎo)致規(guī)劃出與障礙物距離過近的危險(xiǎn)路徑，增大碰撞的風(fēng)險(xiǎn)。由此，根據(jù)機(jī)器人與障礙物的距離，提出一種代價(jià)地圖自適應(yīng)膨脹半徑的設(shè)置方法。

2.1 距離自適應(yīng)膨脹半徑算法

設(shè)柵格地圖分辨率為Δ，機(jī)器人與任一柵格的距離l轉(zhuǎn)換成柵格距離d的計(jì)算公式為

(2)

(3)

每次柵格地圖更新時(shí)記錄距機(jī)器人最遠(yuǎn)障礙點(diǎn)的距離Dmax，則可計(jì)算On的衰減系數(shù)coef。

(4)

結(jié)合式(1)可得到距離自適應(yīng)膨脹半徑柵格代價(jià)值函數(shù)s(qn)為

s(qn)=

代價(jià)地圖對(duì)障礙物邊緣的膨脹是柵格地圖上的障礙點(diǎn)依據(jù)與機(jī)器人的柵格距離，對(duì)其子節(jié)點(diǎn)迭代填充代價(jià)值的過程。

如圖3所示，首先把某一障礙點(diǎn)On作為父節(jié)點(diǎn)，計(jì)算子節(jié)點(diǎn)(O1、O2、O3、O4)與On的距離d(qn)，然后由d(qn)計(jì)算子節(jié)點(diǎn)的代價(jià)值。最后判斷d(qn)

圖3 迭代節(jié)點(diǎn)關(guān)系圖Fig.3 Iteration node diagram

算法步驟如下。

(1)構(gòu)造膨脹點(diǎn)優(yōu)先隊(duì)列inflation cells，將所有障礙點(diǎn)依次放入隊(duì)列中，得到Dmax。

(3)判斷d(qn)

(4)由s(qn)計(jì)算出當(dāng)前節(jié)點(diǎn)的代價(jià)值，與原始代價(jià)值比較，取最大值。

(5)從隊(duì)列inflation cells中移除當(dāng)前節(jié)點(diǎn)，返回步驟(2)。

2.2 輪廓自適應(yīng)膨脹半徑算法

為提高機(jī)器人的靈活性，在距離自適應(yīng)膨脹半徑算法基礎(chǔ)上，針對(duì)機(jī)器人緊貼障礙物邊緣行駛的情況，提出一種根據(jù)輪廓位置自適應(yīng)調(diào)整膨脹半徑的方法：在未緊貼障礙物行駛時(shí)保留更多的安全行駛空間，在緊貼障礙物行駛時(shí)再給出最低限度安全行駛空間。

如圖4所示，七宮格為一個(gè)7行7列的平面方陣。算法開始時(shí)，把當(dāng)前子?xùn)鸥穹湃肫邔m格的中心位置[15]，即第4行第4列，記為Pn，然后以這一點(diǎn)為中心，檢測(cè)其余柵格的概率值。

圖4 七宮格法Fig.4 Seven-grid method

無人車靠近某個(gè)障礙物點(diǎn)Pn時(shí)，周圍非占據(jù)的柵格數(shù)量減少，被占據(jù)柵格數(shù)量增加。記nF為柵格值為FREE SPACE的柵格數(shù)量，nL為柵格值為L(zhǎng)ETHAL OBSTACLE的柵格數(shù)量。則可計(jì)算衰減因子sp。

(6)

結(jié)合式(5)得到自適應(yīng)膨脹半徑柵格代價(jià)值計(jì)算函數(shù)f(qn)。

f(qn)=

(7)

計(jì)算步驟如下。

(1)在第2.1節(jié)步驟(2)的基礎(chǔ)上判斷當(dāng)前處理節(jié)點(diǎn)是否為障礙點(diǎn)，如果是，則將其作為七宮格的中心；如不符合，則繼承其On的衰減系數(shù)sp，進(jìn)行步驟(4)。

(2)依次得到七宮格中的其余48個(gè)柵格值，累計(jì)得出值為FREE SPACE的柵格數(shù)量nF和值為L(zhǎng)ETHAL OBSTACLE的柵格數(shù)量nL。

(3)計(jì)算衰減因子sp，并將其繼承到所有子節(jié)點(diǎn)。

(4)繼續(xù)進(jìn)行第2.1節(jié)中的步驟(3)。

2.3 ROS導(dǎo)航中的應(yīng)用效果

為實(shí)現(xiàn)本文自適應(yīng)膨脹半徑算法，在Gazebo中搭建仿真環(huán)境，改寫ROS中代價(jià)地圖功能包進(jìn)行測(cè)試。采用ROS Navigation導(dǎo)航框架，使用的SLAM算法為gmapping，不使用靜態(tài)地圖，在未知環(huán)境中進(jìn)行實(shí)時(shí)建圖導(dǎo)航。全局規(guī)劃算法選取經(jīng)典的A*算法，局部規(guī)劃算法選取動(dòng)態(tài)窗口法(dynamic window approach，DWA)。在相同的障礙物分布情況下對(duì)比膨脹半徑設(shè)置效果以及路徑規(guī)劃情況。所使用的程序運(yùn)行在Ubuntu18.04操作系統(tǒng)中，運(yùn)行內(nèi)存為8 GB，主頻為2.3 GHz，ROS版本為Melodic。

Gazebo中自適應(yīng)膨脹半徑設(shè)置方法使用前后效果如圖5所示。

圖5 自適應(yīng)膨脹半徑方法效果對(duì)比Fig.5 Comparison of effect of adaptive inflation radius

Gazebo下設(shè)置單一障礙物情景，相同起始點(diǎn)和目標(biāo)點(diǎn)下使用膨脹半徑算法的路徑導(dǎo)航效果如圖6所示。

圖6 Gazebo中簡(jiǎn)單情境導(dǎo)航效果對(duì)比Fig.6 Comparison of the effects of simple situation navigation in Gazebo

Gazebo下設(shè)置復(fù)雜障礙物情景，相同起始點(diǎn)和目標(biāo)點(diǎn)下使用膨脹半徑算法的路徑導(dǎo)航效果如圖7所示。

圖7 Gazebo中復(fù)雜情境導(dǎo)航效果對(duì)比Fig.7 Comparison of the effects of complex situation navigation in Gazebo

仿真結(jié)果表明，障礙物的膨脹半徑隨與機(jī)器人的距離遠(yuǎn)近而自適應(yīng)的調(diào)整大小，證明了自適應(yīng)膨脹半徑算法的有效性。機(jī)器人緊靠障礙物行駛時(shí)，機(jī)器人在保證安全的前提下能預(yù)留出更多的局部規(guī)劃空間。復(fù)雜障礙物情景下，使用傳統(tǒng)的膨脹半徑算法，全局規(guī)劃路徑從障礙物之中穿過，最終機(jī)器人進(jìn)入障礙物群中，與障礙物發(fā)生碰撞被困，導(dǎo)航失??；使用改進(jìn)后的膨脹半徑算法，機(jī)器人在遇到復(fù)雜障礙物情況時(shí)，全局路徑規(guī)劃成功繞過障礙物進(jìn)行導(dǎo)航，最終到達(dá)目標(biāo)點(diǎn)，導(dǎo)航成功。

3 結(jié)合環(huán)境信息的改進(jìn)A*算法

結(jié)合代價(jià)地圖自適應(yīng)膨脹半徑算法，設(shè)計(jì)了A*路徑規(guī)劃算法的估價(jià)函數(shù)f(n)，根據(jù)環(huán)境信息設(shè)置動(dòng)態(tài)衡量啟發(fā)函數(shù)，在無障礙物環(huán)境適當(dāng)提高搜索效率。對(duì)子節(jié)點(diǎn)的選擇進(jìn)行進(jìn)一步優(yōu)化，避免路徑通過障礙物頂點(diǎn)。

3.1 動(dòng)態(tài)衡量啟發(fā)函數(shù)

A*算法是一種啟發(fā)式搜索算法，結(jié)合了Dijkstra和廣度優(yōu)先搜索(breadth first search，BFS)兩種算法的優(yōu)勢(shì)[16]，在搜索效率和搜索廣度做到平衡。算法中以距離值表征路徑代價(jià)。其代價(jià)函數(shù)f(n)計(jì)算公式為

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

(8)

式(8)中：g(n)為起始點(diǎn)到當(dāng)前節(jié)點(diǎn)n實(shí)際路程的代價(jià)函數(shù)，由累計(jì)當(dāng)前節(jié)點(diǎn)與父節(jié)點(diǎn)距離值得到；h(n)為當(dāng)前節(jié)點(diǎn)到目標(biāo)點(diǎn)的估計(jì)路程代價(jià)的啟發(fā)函數(shù)，通常使用當(dāng)前節(jié)點(diǎn)與目標(biāo)點(diǎn)的曼哈頓距離或歐式距離，此算法采用歐式距離。針對(duì)自適應(yīng)膨脹半徑方法使得機(jī)器人避開復(fù)雜環(huán)境的情況，結(jié)合環(huán)境信息對(duì)啟發(fā)函數(shù)設(shè)置了權(quán)重函數(shù)w(n)，公式為

(9)

(10)

式中：ds為當(dāng)前節(jié)點(diǎn)與起始點(diǎn)的距離；dg為當(dāng)前節(jié)點(diǎn)與目標(biāo)點(diǎn)的距離；t為同級(jí)子節(jié)點(diǎn)中障礙節(jié)點(diǎn)的數(shù)量。

由式(10)可知，權(quán)重函數(shù)w(n)∈[1,e]。在無障礙物環(huán)境中，隨接近目標(biāo)點(diǎn)的過程，啟發(fā)函數(shù)權(quán)重逐漸增大，提高了搜索效率。

3.2 子節(jié)點(diǎn)選擇的優(yōu)化

傳統(tǒng)A*算法在柵格地圖上選擇子節(jié)點(diǎn)時(shí)，欠缺了對(duì)路徑與障礙物位置關(guān)系的考慮，使得部分路徑穿過障礙物頂點(diǎn)，增大了發(fā)生碰撞的風(fēng)險(xiǎn)[8]。針對(duì)這一問題對(duì)子節(jié)點(diǎn)的選擇條件進(jìn)行優(yōu)化，提高路徑規(guī)劃的安全性。

子節(jié)點(diǎn)和父節(jié)點(diǎn)的位置關(guān)系如圖8所示，將處在頂點(diǎn)位置上的4個(gè)節(jié)點(diǎn)歸為危險(xiǎn)子節(jié)點(diǎn)(子節(jié)點(diǎn)1、3、5、7)。

圖8 節(jié)點(diǎn)位置關(guān)系圖Fig.8 Node location relationship diagram

在選子節(jié)點(diǎn)時(shí)，若該子節(jié)點(diǎn)為危險(xiǎn)子節(jié)點(diǎn)，則檢測(cè)其臨近的兩個(gè)子節(jié)點(diǎn)是否存在障礙節(jié)點(diǎn)，若存在則放棄該危險(xiǎn)子節(jié)點(diǎn)，若不存在則將其作為子節(jié)點(diǎn)。

優(yōu)化子節(jié)點(diǎn)選擇條件后，解決規(guī)劃后的路徑穿過障礙物頂點(diǎn)的問題，避免機(jī)器人在運(yùn)動(dòng)過程中與障礙物發(fā)生碰撞。

4 仿真實(shí)驗(yàn)與結(jié)果分析

在MATLAB中建立60×60大小的柵格地圖作為移動(dòng)機(jī)器人工作環(huán)境，將自適應(yīng)膨脹半徑算法配合改進(jìn)A*算法與傳統(tǒng)膨脹半徑算法配合傳統(tǒng)A*算法進(jìn)行對(duì)比仿真測(cè)試。

為方便此次對(duì)比實(shí)驗(yàn)，設(shè)定移動(dòng)機(jī)器人的工作環(huán)境柵格邊長(zhǎng)為單位1 m，設(shè)傳統(tǒng)膨脹半徑設(shè)置方法的膨脹半徑為2 m，最低限度安全行駛半徑為1 m。

柵格中左下角為起點(diǎn)，右上角為目標(biāo)點(diǎn)。黑色網(wǎng)格為障礙物，藍(lán)色網(wǎng)格為障礙物膨脹區(qū)域。模擬密集障礙物分布，隨機(jī)改變障礙物位置進(jìn)行6組對(duì)照實(shí)驗(yàn)。其中一組傳統(tǒng)膨脹半徑算法配合傳統(tǒng)A*算法路徑規(guī)劃效果如圖9所示，自適應(yīng)膨脹半徑算法配合改進(jìn)A*算法路徑規(guī)劃效果如圖10所示。使用兩種算法的平均實(shí)驗(yàn)路徑參數(shù)如表1所示。

圖9 傳統(tǒng)膨膨脹半徑算法配合傳統(tǒng)A*算法路徑規(guī)劃Fig.9 Conventional inflation map algorithm combined with traditional A* algorithm

圖10 自適應(yīng)膨脹半徑算法配合改進(jìn)A*算法路徑規(guī)劃Fig.10 Adaptive expansion radius algorithm and improved A* algorithm path planning

表1 實(shí)驗(yàn)路徑參數(shù)表Table 1 Experimental path parameters

由表1數(shù)據(jù)可知，6組對(duì)照實(shí)驗(yàn)中，相比于傳統(tǒng)算法，使用自適應(yīng)膨脹半徑算法配合改進(jìn)A*算法后能避開密集障礙物群，轉(zhuǎn)向次數(shù)和轉(zhuǎn)角總和減少，路徑不再穿過障礙物頂點(diǎn)。由于需要避免進(jìn)入密集障礙物群，平均路程增加4.9%。轉(zhuǎn)向次數(shù)和總和分別減少33.6%和37%。

實(shí)驗(yàn)表明，結(jié)合自適應(yīng)膨脹半徑算法和改進(jìn)A*算法規(guī)劃后的路徑，犧牲小部分最短路徑，避開了密集障礙物群，在路徑長(zhǎng)度相差不大的情況下，轉(zhuǎn)角次數(shù)和總和減少，根據(jù)環(huán)境信息不同提升了搜索效率。路徑規(guī)劃更平滑，路徑通過的障礙物密度下降，易于機(jī)器人行駛通過。

5 結(jié)論

通過對(duì)基于ROS的代價(jià)地圖和A*路徑規(guī)劃算法研究，得到以下結(jié)論。

(1)提出一種代價(jià)地圖自適應(yīng)膨脹半徑設(shè)置方法，可用于移動(dòng)機(jī)器人全局路徑規(guī)劃時(shí)避開復(fù)雜障礙物群，局部規(guī)劃時(shí)增加可行駛空間。

(2)提出一種改進(jìn)A*路徑規(guī)劃算法，用于移動(dòng)機(jī)器人導(dǎo)航全局規(guī)劃，增加搜索效率，平滑路徑，降低碰撞風(fēng)險(xiǎn)。

(3)改寫ROS中代價(jià)地圖功能包，在Gazebo仿真平臺(tái)上對(duì)自適應(yīng)膨脹半徑算法進(jìn)行導(dǎo)航測(cè)試，證明能避開復(fù)雜障礙物群。在MATLAB中對(duì)自適應(yīng)膨脹算法配合改進(jìn)A*算法進(jìn)行驗(yàn)證。通過實(shí)驗(yàn)數(shù)據(jù)，證明該方法能提升機(jī)器人在復(fù)雜環(huán)境中導(dǎo)航的安全性和魯棒性。

(4)自適應(yīng)膨脹半徑算法計(jì)算衰減系數(shù)增加了一定空間復(fù)雜度，導(dǎo)航安全較高的路徑增加了部分路程長(zhǎng)度。存在權(quán)衡上的不足需在未來做進(jìn)一步研究。