基于改進(jìn)A＊算法的煤礦救援機(jī)器人路徑規(guī)劃

張偉民，張 月，張 輝

(1.中國(guó)地質(zhì)大學(xué)(武漢) 機(jī)械與電子信息學(xué)院，湖北 武漢 430074；2.武漢重型機(jī)床集團(tuán)有限公司，湖北 武漢 430074)

煤礦救援機(jī)器人(簡(jiǎn)稱救援機(jī)器人)是在礦井災(zāi)后救援活動(dòng)中進(jìn)行情報(bào)收集和判斷，根據(jù)災(zāi)害情況進(jìn)行救助的一種機(jī)器人。它可以代替救援人員進(jìn)入礦井實(shí)施環(huán)境監(jiān)測(cè)、人員搜索等任務(wù)。為了使煤礦救援機(jī)器人快速、順利地到達(dá)災(zāi)害現(xiàn)場(chǎng)并實(shí)施救援，規(guī)劃出一條安全、無碰撞的路徑非常重要[1]。

救援機(jī)器人的路徑規(guī)劃是指機(jī)器人按照某一種方法在分布有障礙物的地圖中搜索出一條從指定起始點(diǎn)到指定目標(biāo)點(diǎn)的無碰撞最優(yōu)路徑[2]。救援機(jī)器人的路徑規(guī)劃包含全局路徑規(guī)劃以及在移動(dòng)過程中應(yīng)對(duì)局部環(huán)境變化的局部路徑規(guī)劃。為快速地規(guī)劃出效果好的全局路徑，減少救援機(jī)器人的整體移動(dòng)時(shí)間，需要采用一個(gè)合適的全局規(guī)劃算法。

目前常用的全局規(guī)劃算法有Dijkstra 算法[3]、A*算法[4]、粒子群算法(Particle Swarm Optimizations，PSO)[5]、遺傳算法[6]等。其中A*算法是在靜態(tài)路網(wǎng)中求解最短路最有效的搜索方法，目前使用較為普遍。但傳統(tǒng)A*算法在柵格環(huán)境地圖下最多允許機(jī)器人向周圍8 個(gè)柵格(稱為“鄰接點(diǎn)”)移動(dòng)，導(dǎo)致傳統(tǒng)A*算法規(guī)劃的路徑對(duì)于可沿任意方向運(yùn)動(dòng)的救援機(jī)器人來說存在路徑冗余點(diǎn)過多、累計(jì)轉(zhuǎn)折角度大等問題，并非是最優(yōu)路徑[7-8]。

目前已有相關(guān)文獻(xiàn)針對(duì)傳統(tǒng)A*算法的上述問題進(jìn)行了改進(jìn)。陶德俊等[9]提出一種基于改進(jìn)A*算法的煤礦救援機(jī)器人路徑平滑算法，該算法利用Douglas-Peucker 算法剔除路徑中的冗余節(jié)點(diǎn)，并對(duì)提取出的關(guān)鍵節(jié)點(diǎn)進(jìn)行平滑處理；雖然路徑平滑問題有所改善，但還有剩余的路徑冗余點(diǎn)。劉夢(mèng)杰等[10]提出了一種基于雙向A*算法的礦井水災(zāi)逃生路徑規(guī)劃方法，該算法可獲得節(jié)點(diǎn)更少、轉(zhuǎn)折角度更小的逃生路徑，但路徑仍有較為明顯的平滑問題。郭江等[11]提出了一種將Bezier 曲線與A*算法融合的改進(jìn)算法，該算法對(duì)路徑具有良好的平滑效果，但若改變路徑節(jié)點(diǎn)位置，則會(huì)影響整條路徑的平滑效果。單偉等[12]提出一種使用極多項(xiàng)式曲線進(jìn)行路徑平滑的改進(jìn)A*算法，這有利于實(shí)現(xiàn)機(jī)器人的路徑跟蹤，但未對(duì)冗余路徑點(diǎn)進(jìn)行去除。趙曉等[13]提出一種結(jié)合跳點(diǎn)搜索法的改進(jìn)A*算法，該算法通過減少對(duì)某些節(jié)點(diǎn)的搜索可提高搜索效率、降低路徑轉(zhuǎn)折角度，但是由于缺少對(duì)某些必要節(jié)點(diǎn)的計(jì)算導(dǎo)致路徑代價(jià)有所增加。

上述文獻(xiàn)對(duì)傳統(tǒng)A*算法進(jìn)行了部分改進(jìn)，但是最終路徑仍然存在較為明顯的問題。為能同時(shí)對(duì)路徑的冗余點(diǎn)及路徑的轉(zhuǎn)折角進(jìn)行改進(jìn)，筆者在標(biāo)準(zhǔn)A*算法的基礎(chǔ)上，提出一種改進(jìn)A*算法。改進(jìn)A*算法為快速獲得初始路徑，增加救援機(jī)器人可能的移動(dòng)方向，將救援機(jī)器人的鄰接點(diǎn)從一層8 個(gè)擴(kuò)展為兩層24 個(gè)，即允許機(jī)器人最多向周圍24 個(gè)柵格移動(dòng)；通過擴(kuò)展機(jī)器人的移動(dòng)方向，可以減少算法迭代次數(shù)，快速生成初始路徑。獲得初始路徑后，再利用本文采用的去除冗余點(diǎn)方法對(duì)路徑去除冗余點(diǎn)，并采用5 次B 樣條曲線對(duì)路徑進(jìn)行平滑處理，以獲得路徑代價(jià)更小、累計(jì)轉(zhuǎn)折角度更小的優(yōu)化路徑。

1 傳統(tǒng)A＊算法

1.1 地圖環(huán)境描述

柵格法由于操作簡(jiǎn)單、易于實(shí)現(xiàn)，被廣泛用來描述機(jī)器人環(huán)境地圖[14]。柵格法需要計(jì)算機(jī)利用傳感器收集環(huán)境信息，創(chuàng)建環(huán)境的二維平面圖M，救援機(jī)器人RR看作在M上移動(dòng)的點(diǎn)狀物體；若M為不規(guī)則圖形，則將M填補(bǔ)為規(guī)則圖形，并將填補(bǔ)的區(qū)域設(shè)為障礙物區(qū)域。經(jīng)過柵格化處理的地圖如圖1 所示，設(shè)圖中左下角為坐標(biāo)原點(diǎn)，水平方向?yàn)閤軸，豎直方向?yàn)閥軸；白色柵格為“可通行柵格(點(diǎn))”，允許機(jī)器人通過；黑色柵格為“不可通行柵格(環(huán)境中的障礙物)”，不允許機(jī)器人通過[15]。

圖1 柵格地圖Fig.1 Grid map

記a為M中任意柵格，設(shè)柵格集合A={ai}(i=1,2,···,n)構(gòu)成地圖M，集合Nobs={oj}(j=1,2,···,m)(m＜n)?A組成障礙物柵格，?a∈A在M中的對(duì)應(yīng)坐標(biāo)為(x,y)。記左下角第一個(gè)柵格坐標(biāo)為[1,1]。在柵格地圖環(huán)境下進(jìn)行路徑規(guī)劃是指在M上使得救援機(jī)器人RR從指定起始節(jié)點(diǎn)node_S沿著一條安全、無碰撞的最優(yōu)路徑到達(dá)指定目標(biāo)節(jié)點(diǎn)node_G。

為便于后續(xù)內(nèi)容展開，本文采用以下設(shè)定：

(1)障礙物已基于機(jī)器人外形尺寸進(jìn)行擴(kuò)張，故機(jī)器人可作為一個(gè)點(diǎn)在柵格地圖范圍內(nèi)移動(dòng)。

(2)機(jī)器人不能穿過障礙物柵格的四角。

(3)機(jī)器人允許的移動(dòng)方向用鄰接矩陣表示。A*算法通常允許機(jī)器人向周圍最多8 個(gè)柵格移動(dòng)，其鄰接矩陣如下式所示。矩陣中數(shù)字“2”表示機(jī)器人RR當(dāng)前所在位置；“1”表示救援機(jī)器人允許前往的位置。

1.2 傳統(tǒng)A＊算法

傳統(tǒng)A*算法(以下簡(jiǎn)稱A*算法)結(jié)合Dijkstra 算法和最佳優(yōu)先算法(Best First Search，BFS)的思想，在保證可以得到最優(yōu)路徑的基礎(chǔ)上，同時(shí)采用啟發(fā)式搜索[16]，以提高算法搜索效率。A*算法假設(shè)任意節(jié)點(diǎn)均能通過一個(gè)代價(jià)估計(jì)函數(shù)計(jì)算出該節(jié)點(diǎn)的代價(jià)值，并在已計(jì)算出代價(jià)值的節(jié)點(diǎn)中選出代價(jià)值最小的節(jié)點(diǎn)作為算法的下一個(gè)擴(kuò)展點(diǎn)，若目標(biāo)點(diǎn)被選為下一個(gè)擴(kuò)展點(diǎn)，則表示搜索到最優(yōu)路徑。

為便于表述，對(duì)以下參數(shù)進(jìn)行說明(此處的節(jié)點(diǎn)指某一個(gè)柵格的中心，其坐標(biāo)為所在柵格坐標(biāo))：

O、C分別為存放未擴(kuò)展節(jié)點(diǎn)、已擴(kuò)展節(jié)點(diǎn)相關(guān)信息的集合，即開集、閉集；n為當(dāng)前擴(kuò)展節(jié)點(diǎn)；n’為當(dāng)前擴(kuò)展節(jié)點(diǎn)n的某一個(gè)鄰接點(diǎn)；c(n’,n) 為當(dāng)前擴(kuò)展節(jié)點(diǎn)n移動(dòng)到其鄰接點(diǎn)n’的代價(jià)；gx、gy分別為點(diǎn)g在x、y方向的坐標(biāo)；F(n)、f分別為當(dāng)前擴(kuò)展節(jié)點(diǎn)n的估計(jì)代價(jià)值函數(shù)(簡(jiǎn)稱估價(jià)函數(shù))及函數(shù)值；G(n)、g分別為從起始節(jié)點(diǎn)node_S到當(dāng)前節(jié)點(diǎn)n的實(shí)際代價(jià)計(jì)算函數(shù)及函數(shù)值；g’為當(dāng)前擴(kuò)展節(jié)點(diǎn)n已經(jīng)在O中時(shí)的實(shí)際代價(jià)值；H(n)、h為從當(dāng)前擴(kuò)展節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)node_G的估計(jì)代價(jià)函數(shù)(啟發(fā)式函數(shù))及函數(shù)值；H*(n)、h* 為從當(dāng)前擴(kuò)展節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)node_G的實(shí)際代價(jià)函數(shù)及函數(shù)值；Par() 為某節(jié)點(diǎn)的父節(jié)點(diǎn)在閉集C中的索引；Size() 為獲得某集合行數(shù)的函數(shù)。

A*算法的搜索過程如下：

(1)初始化開集O、閉集C。

(2)將起始節(jié)點(diǎn)node_S加入O，并計(jì)算代價(jià)F(node_S)。

(3)若O為空集，則表示尋路失敗，算法結(jié)束，輸出結(jié)果；若O不為空集，則取出O中f值最小的節(jié)點(diǎn)n，并把n從O中移到C中。

(4)若n為目標(biāo)點(diǎn)，則尋路成功，算法結(jié)束，輸出結(jié)果；否則，處理n的鄰接點(diǎn)：若當(dāng)前鄰接點(diǎn)n’已經(jīng)在C中，則直接處理下一個(gè)鄰接點(diǎn)；若n’已經(jīng)在O中，則比較n’當(dāng)前的實(shí)際代價(jià)值g和開集O中已保存的實(shí)際代價(jià)值g’；若g＜g’，則更新O中n’的相關(guān)信息；若g≥g’，則不對(duì)n’作處理；若n’不在O和C中，則計(jì)算相關(guān)信息，并將n’加入O中；當(dāng)所有鄰接點(diǎn)均處理完畢，返回執(zhí)行第(3)步。

1.3 估價(jià)函數(shù)F(n)

合適的估價(jià)函數(shù)F(n)，會(huì)使A*算法保證能找到最優(yōu)路徑。A*算法的估價(jià)函數(shù)F(n)表示為：

式中：F(n)為從起始節(jié)點(diǎn)node_S經(jīng)過當(dāng)前節(jié)點(diǎn)n到達(dá)目標(biāo)點(diǎn)node_G的估價(jià)函數(shù)；G(n)為從起始節(jié)點(diǎn)node_S到當(dāng)前節(jié)點(diǎn)n的實(shí)際代價(jià)函數(shù)；H(n)為從當(dāng)前節(jié)點(diǎn)n到達(dá)目標(biāo)節(jié)點(diǎn)node_G的啟發(fā)式函數(shù)[17]。

H(n)與H*(n)的關(guān)系會(huì)對(duì)A*算法的搜索速度和搜索結(jié)果產(chǎn)生重要影響，故H(n)的選擇是A*算法的關(guān)鍵內(nèi)容之一。H(n)與H*(n)的關(guān)系通常存在以下情況：

(1)若啟發(fā)式函數(shù)恒為0，則F(n)=G(n)，A*算法變?yōu)镈ijkstra 算法。此時(shí)算法總能搜索到最優(yōu)路徑，但需擴(kuò)展大量節(jié)點(diǎn)，算法效率低。

(2)若H(n)≤H*(n)，則A*算法總能搜索到一條最優(yōu)路徑。當(dāng)柵格地圖中無障礙物或障礙物對(duì)最優(yōu)路徑無影響時(shí)，存在H(n)=H*(n)，此時(shí)A*算法只擴(kuò)展最優(yōu)路徑點(diǎn)，這時(shí)算法搜索效率很高；當(dāng)?shù)貓D中障礙物對(duì)最優(yōu)路徑產(chǎn)生影響時(shí)，存在H(n)＜H*(n)，此時(shí)A*需要擴(kuò)展其他節(jié)點(diǎn)以搜索最優(yōu)路徑。

(3)若H(n)＞H*(n)，A*算法不能保證搜索到最優(yōu)路徑，但算法搜索節(jié)點(diǎn)少、搜索效率高。

(4)若H(n)＞＞H*(n)，此時(shí)G(n)對(duì)F(n)的影響可忽略，則F(n)=H(n)，A*算法變?yōu)锽FS 算法。此時(shí)算法不能保證會(huì)搜索到最優(yōu)路徑，但搜索節(jié)點(diǎn)少，搜索效率高。

常見的代價(jià)計(jì)算方式有曼哈頓距離、對(duì)角距離和歐幾里得距離[18]。采用歐幾里得距離時(shí)有：

由于地圖中障礙物的存在，總是會(huì)有H(n)≤H*(n)，故A*算法總是可以搜索到最優(yōu)路徑。本文路徑代價(jià)計(jì)算均采用歐幾里得距離。

2 改進(jìn)A＊算法

對(duì)于可沿任意方向運(yùn)動(dòng)的救援機(jī)器人而言，由于A*算法最多允許機(jī)器人向周圍8 個(gè)柵格移動(dòng)，故A*算法在柵格地圖環(huán)境下規(guī)劃的路徑是非最優(yōu)的。以無障礙地圖為例，A*算法規(guī)劃出的路徑如圖2 所示。若機(jī)器人最多只能朝周圍8 個(gè)柵格移動(dòng)，則圖2 中的路徑可以稱為“最優(yōu)的”；但若機(jī)器人可沿任意方向移動(dòng)，則其最優(yōu)路徑應(yīng)是起始節(jié)點(diǎn)node_S和目標(biāo)節(jié)點(diǎn)node_G的連線。故需要對(duì)A*算法進(jìn)行改進(jìn)，以獲得對(duì)可沿任意方向移動(dòng)的救援機(jī)器人而言的“最優(yōu)路徑”。

圖2 傳統(tǒng)A*算法搜索結(jié)果Fig.2 Search result of conventional A* algorithm

2.1 改進(jìn)A＊算法方案

在改進(jìn)A*算法中，定義了RemovePoint函數(shù)、Spline函數(shù)和SmoothPath函數(shù)。RemovePoint函數(shù)移除路徑點(diǎn)集合中的冗余點(diǎn)；Spline函數(shù)將路徑點(diǎn)按照步長(zhǎng)α進(jìn)行分割；SmoothPath函數(shù)采用5 次B 樣條曲線對(duì)路徑進(jìn)行平滑處理。

改進(jìn)A*算法總體分為5 步：

(1)設(shè)置A*算法中救援機(jī)器人最多可向周圍24個(gè)柵格移動(dòng)，并生成初始路徑點(diǎn)集合Pori。

(2)調(diào)用RemovePoint函數(shù)，去除初始路徑點(diǎn)集合Pori中的冗余點(diǎn)，獲得優(yōu)化路徑點(diǎn)集合Pnew。

(3)調(diào)用Spline函數(shù)，將路徑點(diǎn)集合Pnew中前后兩路徑點(diǎn)的連線按照一定步長(zhǎng)進(jìn)行分割，獲得分割后的路徑點(diǎn)集合Pdiv。

(4)調(diào)用RemovePoint函數(shù)，去除路徑點(diǎn)集合Pdiv中的冗余點(diǎn)，獲得優(yōu)化路徑點(diǎn)集合Pre。

(5)調(diào)用SmoothPath函數(shù)，對(duì)路徑點(diǎn)集合Pre形成的路徑進(jìn)行平滑處理，獲得最終路徑點(diǎn)集合Pfinal。

2.2 鄰接點(diǎn)擴(kuò)展

為使移動(dòng)機(jī)器人在使用改進(jìn)A*算法進(jìn)行路徑規(guī)劃時(shí)，更快地獲得初始路徑，本文將改進(jìn)A*算法允許機(jī)器人最多可移動(dòng)的柵格數(shù)量擴(kuò)展為24 個(gè)，此時(shí)機(jī)器人的鄰接矩陣如下式所示。

采用24 鄰接點(diǎn)的改進(jìn)A*算法生成的路徑轉(zhuǎn)折角度更小且能減少搜索路徑時(shí)擴(kuò)展的節(jié)點(diǎn)數(shù)量，從而降低內(nèi)存占用，如圖3 所示。

圖3 鄰接點(diǎn)數(shù)量為8 和24 時(shí)A*算法規(guī)劃結(jié)果Fig.3 Planning results of A* algorithm with 8 and 24 adjacency points

對(duì)比采用8 鄰接點(diǎn)的A*算法和采用24 鄰接點(diǎn)的改進(jìn)A*算法規(guī)劃出的路徑，可看出改進(jìn)A*算法規(guī)劃的路徑路徑點(diǎn)數(shù)量更少，路徑轉(zhuǎn)折次數(shù)更少，并且規(guī)劃路徑過程中擴(kuò)展的節(jié)點(diǎn)數(shù)量(閉集中節(jié)點(diǎn)數(shù)量)也更少。故采用24 鄰接點(diǎn)可以快速獲得效果更好的初始路徑。

2.3 去除路徑冗余點(diǎn)

去除冗余點(diǎn)流程如圖4 所示，其主要原理是重連路徑點(diǎn)。設(shè)重連路徑點(diǎn)時(shí)的起始連接點(diǎn)為s、目標(biāo)連接點(diǎn)為g、新路徑點(diǎn)集合為R(R代表2.1 節(jié)中的Pnew或Pre)。初始時(shí)，獲取路徑點(diǎn)集合P(P代表2.1 節(jié)中的Pori或Pdiv)，并將P中第一個(gè)路徑點(diǎn)設(shè)為s、第二個(gè)路徑點(diǎn)設(shè)為g，通過函數(shù)CheckPath檢查兩點(diǎn)連線lsg是否經(jīng)過障礙物，若直線lsg未通過障礙物，則將下一路徑點(diǎn)設(shè)為g；若經(jīng)過障礙物，則將起始連接點(diǎn)s加入新路徑點(diǎn)集合R，并將目標(biāo)連接點(diǎn)g的前一個(gè)連接點(diǎn)設(shè)為起始連接點(diǎn)s；重復(fù)這一過程直到目標(biāo)連接點(diǎn)為目標(biāo)節(jié)點(diǎn)node_G。最終輸出新路徑點(diǎn)集合R。

圖4 去除路徑冗余點(diǎn)流程Fig.4 Flow chart of removing redundant points in path

CheckPath函數(shù)是去除冗余點(diǎn)時(shí)的關(guān)鍵步驟，其輸入?yún)?shù)分別是δ、σ、s、g、M，δ是距離閾值，限制周圍柵格與直線lsg的最小距離，σ是采樣步長(zhǎng)，即從起始連接點(diǎn)開始，按照r×σ(r=0,1,2,···)距離取點(diǎn)n，并判斷l(xiāng)sn是否通過障礙物。

針對(duì)不同的運(yùn)動(dòng)方向，CheckPath函數(shù)需要檢查的節(jié)點(diǎn)不同：

(1)路徑方向?yàn)樽笊系接蚁聲r(shí)，需要檢查當(dāng)前節(jié)點(diǎn)所在柵格的左側(cè)NL和上側(cè)NT柵格。

(2)路徑方向?yàn)橛疑系阶笙聲r(shí)，需要檢查當(dāng)前節(jié)點(diǎn)所在柵格的右側(cè)NR和上側(cè)NT柵格。

(3)路徑方向?yàn)橛蚁碌阶笊蠒r(shí)，需要檢查當(dāng)前節(jié)點(diǎn)所在柵格的右側(cè)NR和下側(cè)NU柵格。

(4)路徑方向?yàn)樽笙碌接疑蠒r(shí)，需要檢查當(dāng)前節(jié)點(diǎn)所在柵格的左側(cè)NL和下側(cè)NU柵格。

若有一個(gè)待檢查柵格為障礙物柵格，則需計(jì)算2個(gè)距離參數(shù)；若2 個(gè)待檢查柵格均為障礙物柵格，表示當(dāng)前連接點(diǎn)g和起始連接點(diǎn)s的連線經(jīng)過障礙物，需對(duì)g的前一個(gè)路徑點(diǎn)進(jìn)行處理。

路徑方向?yàn)樽笊系接蚁聲r(shí)CheckPath函數(shù)的偽代碼如圖5 所示。圖5 中dmin為與直線lsg斜率相同且經(jīng)過柵格NT左下角(NL右上角)的直線與柵格NT左上角(NL右下角)的最小距離。

當(dāng)柵格NT為障礙物柵格時(shí)，dll為直線lsg到柵格NT左下角的最小距離，dul為直線lsg到柵格NT左上角的最小距離；只有同時(shí)滿足dll＞ δ和dul＞ dmin兩個(gè)條件，才能保證直線沒有經(jīng)過柵格NT且與柵格NT的距離滿足要求(代碼第15—第20 行)。

柵格NL原理相同，dur為直線lsg到柵格NL右上角的最小距離，dlr為直線lsg到柵格NL右下角的最小距離；只有同時(shí)滿足dur＞δ和dlr＞dmin兩個(gè)條件，才能保證直線lsg沒有經(jīng)過柵格NL且與柵格NL的距離滿足要求(代碼第21—第26 行)。

以圖6 中路徑為例，此時(shí)CheckPath函數(shù)的輸入點(diǎn)為s、g，當(dāng)前檢查節(jié)點(diǎn)為n，當(dāng)前檢查柵格為(2,2)。通過三角形面積公式，計(jì)算得：

式中：θ為此時(shí)的連接起點(diǎn)和目標(biāo)點(diǎn)所成直線的斜率。

當(dāng)前柵格的左側(cè)柵格NL為障礙物，則需計(jì)算NL右上角及右下角到直線lsn(即直線lsg)的距離dur、dlr。若dur＞δ，則能保證直線與柵格右上角點(diǎn)的距離滿足算法要求，但是無法判斷直線lsg是否穿過柵格NL；只有dlr滿足dlr＞dmin，才能保證直線lsg不在柵格NL內(nèi)。

采用CheckPath進(jìn)行去除冗余點(diǎn)后的路徑如圖7所示。與原路徑相比，去除冗余點(diǎn)后的路徑代價(jià)和路徑累計(jì)轉(zhuǎn)折角度均有所減小。

圖7 去除路徑冗余點(diǎn)后的路徑Fig.7 The path after removing redundant points

2.4 路徑平滑

路徑經(jīng)過兩次去除冗余點(diǎn)后，其平滑問題得到初步改善，但還需對(duì)路徑轉(zhuǎn)角進(jìn)行平滑處理。B 樣條曲線能夠?qū)β窂竭M(jìn)行布局修改而不影響整條路徑的基本形狀特征，被廣泛應(yīng)用于路徑平滑和軌跡平滑中[18-20]。同時(shí)B 樣條曲線可人為調(diào)整曲線階數(shù)，通過改變階數(shù)可有效改善曲線的平滑效果。在仿真實(shí)驗(yàn)過程中，將B 樣條曲線的階數(shù)設(shè)置依次設(shè)置為3～7 階，其平滑效果如圖8 所示。從圖8 中可知，采用3 次B 樣條曲線平滑后的路徑有較為明顯的轉(zhuǎn)折；將樣條曲線階數(shù)從三階增加到五階時(shí)，其路徑平滑效果有明顯改善；將樣條曲線階數(shù)從五階增加到七階時(shí)，雖然路徑平滑效果有所改善，但是效果沒有從三階到五階的改善效果明顯。故本文選用5 次B 樣條曲線進(jìn)行路徑平滑。

圖8 不同階數(shù)的B 樣條曲線平滑效果Fig.8 Smoothing results of B-spline curves of different orders

B 樣條曲線的總方程為：

式中：Pi為給定的控制點(diǎn)坐標(biāo)；Fi,k(t)階B 樣條基函數(shù)，其表達(dá)式為：

將基函數(shù)代入到式(5)中，即可得到5 次B 樣條曲線方程：

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

3.1 仿真實(shí)驗(yàn)

為了驗(yàn)證本文提出的改進(jìn)A*算法的有效性，使用傳統(tǒng)A*算法和本文提出的改進(jìn)A*算法分別在5 種尺寸的單位大小柵格組成的隨機(jī)地圖環(huán)境進(jìn)行仿真，柵格地圖障礙物密度為20%，實(shí)驗(yàn)環(huán)境為：CPU Intel Core i5-4200U，內(nèi)存12 GB，實(shí)驗(yàn)工具M(jìn)ATLAB 2020a。

圖9 為尺寸為20×20 的隨機(jī)柵格地圖環(huán)境下，傳統(tǒng)A*算法和改進(jìn)A*算法的路徑搜索結(jié)果對(duì)比。通過對(duì)比，可看出相同環(huán)境下改進(jìn)A*算法搜索到的路徑相比于傳統(tǒng)A*算法搜索到的路徑代價(jià)更小、轉(zhuǎn)折角度更小。

圖9 傳統(tǒng)A*算法和改進(jìn)A*算法的仿真結(jié)果Fig.9 Simulation result of conventional A* algorithm and improved A* algorithm

仿真實(shí)驗(yàn)在5 種尺寸的隨機(jī)地圖分別連續(xù)成功運(yùn)行100 次(可能存在無可行路徑的情況)，并記錄傳統(tǒng)A*算法和改進(jìn)A*算法運(yùn)行時(shí)的搜索節(jié)點(diǎn)數(shù)量、路徑代價(jià)及路徑轉(zhuǎn)折角度等3 個(gè)數(shù)據(jù)。

為驗(yàn)證不同路徑方向的CheckPath函數(shù)，不同尺寸地圖的起始點(diǎn)和目標(biāo)點(diǎn)不同：

(1)地圖尺寸為20×20 時(shí)，起始節(jié)點(diǎn)為(2,2)，目標(biāo)節(jié)點(diǎn)為(18,18)。

(2)地圖尺寸為30×30 時(shí)，起始節(jié)點(diǎn)為(27,3)，目標(biāo)節(jié)點(diǎn)為(3,27)。

(3)地圖尺寸為50×50 時(shí)，起始節(jié)點(diǎn)為(45,45)，目標(biāo)節(jié)點(diǎn)為(5,5)。

(4)地圖尺寸為80×80 時(shí)，起始節(jié)點(diǎn)為(8,72)，目標(biāo)節(jié)點(diǎn)為(72,8)。

(5)地圖尺寸為100×100 時(shí)，起始節(jié)點(diǎn)為(10,10)，目標(biāo)點(diǎn)節(jié)為(90,90)。

仿真結(jié)果見表1(參數(shù)比值為改進(jìn)A*算法參數(shù)值/傳統(tǒng)A*算法參數(shù)值)。

表1 改進(jìn)A＊算法和標(biāo)準(zhǔn)A＊算法的部分參數(shù)比值Table 1 Partial parameter ratio between improved A＊algorithm and standard A＊ algorithm

表1 給出了傳統(tǒng)A*算法和改進(jìn)A*算法在不同尺寸柵格地圖中的搜索節(jié)點(diǎn)、路徑代價(jià)和路徑轉(zhuǎn)折角度的比值。地圖是以單位大小柵格組成，圖中的值表示改進(jìn)A*算法計(jì)算得到的結(jié)果與傳統(tǒng)A*算法得到的結(jié)果的比值。從表1 可以看出，改進(jìn)A*算法相比于傳統(tǒng)A*算法，搜索過程擴(kuò)展的節(jié)點(diǎn)數(shù)量減少20%左右，所得路徑代價(jià)減少8%～9%，路徑轉(zhuǎn)折角度減少75%～80%?！案倪M(jìn)A*/傳統(tǒng)A*”比值均小于1，表明改進(jìn)A*算法優(yōu)于傳統(tǒng)A*算法。

3.2 結(jié)果分析

經(jīng)過多次仿真實(shí)驗(yàn)得到的結(jié)果，分析可得實(shí)現(xiàn)路徑優(yōu)化的主要因素有以下3 點(diǎn)：

(1)擴(kuò)展鄰接點(diǎn)。通過擴(kuò)展救援機(jī)器人的鄰接點(diǎn)，增加了救援機(jī)器人允許的移動(dòng)方向，導(dǎo)致算法擴(kuò)展到某一節(jié)點(diǎn)時(shí)，加入開集O的鄰接點(diǎn)數(shù)量更多，從而使得開集中的節(jié)點(diǎn)數(shù)量增加得更快，擴(kuò)大了下一次比較f值的節(jié)點(diǎn)范圍。即若要在開集中加入相同數(shù)量的節(jié)點(diǎn)，改進(jìn)A*算法所需的迭代次數(shù)要少于傳統(tǒng)A*算法；因此規(guī)劃出相同路徑時(shí)，改進(jìn)A*算法所需的迭代次數(shù)更少，并且加入閉集C中的節(jié)點(diǎn)數(shù)量更少。因此改進(jìn)A*算法生成初始路徑的速度比傳統(tǒng)A*算法快。理論上可以進(jìn)一步擴(kuò)展鄰接點(diǎn)，即再增加鄰接點(diǎn)的層數(shù)，但是增加到一定數(shù)量時(shí)反而會(huì)大幅增加每次迭代消耗的時(shí)間，從而降低搜索速度，故本文僅將鄰接點(diǎn)數(shù)量擴(kuò)展到24 個(gè)。

(2)去除路徑冗余點(diǎn)。通過去除冗余點(diǎn)，減少了初始路徑的代價(jià)和轉(zhuǎn)折角度。改進(jìn)A*算法首先對(duì)初始路徑上的路徑點(diǎn)進(jìn)行去除；但由于初始路徑中兩路徑點(diǎn)的間距較大，因此路徑仍有優(yōu)化空間。故在對(duì)初始路徑去除冗余點(diǎn)后，再將新的路徑按照一定步長(zhǎng)進(jìn)行分割，獲得間距更小的路徑點(diǎn)集合，并對(duì)該路徑點(diǎn)集合去除冗余點(diǎn)。同樣地，理論上來說，可以多次重復(fù)進(jìn)行路徑分割和去除冗余點(diǎn)操作，可使最終路徑接近“最優(yōu)”；但多次使用路徑分割和去除冗余點(diǎn)反而會(huì)增加算法運(yùn)行時(shí)間，并且可能優(yōu)化效果不明顯；故本文中僅使用了一次，在保證算法搜索效率的同時(shí)，盡量減小路徑代價(jià)及轉(zhuǎn)折角度。

(3)路徑平滑。在去除冗余點(diǎn)后，獲得的路徑雖然轉(zhuǎn)折角度有所減少，但是路徑的轉(zhuǎn)角處并不是平滑的曲線。因此為實(shí)現(xiàn)救援機(jī)器人的平穩(wěn)運(yùn)動(dòng)，本文采用了5 次B 樣條曲線對(duì)路徑轉(zhuǎn)角進(jìn)行進(jìn)一步平滑。通過5 次B 樣條曲線擬合，可獲得更加平滑的最終路徑，保障救援機(jī)器人運(yùn)動(dòng)時(shí)速度和角度的平穩(wěn)性。

通過仿真實(shí)驗(yàn)可知，救援機(jī)器人采用改進(jìn)A*算法進(jìn)行路徑規(guī)劃可以獲得代價(jià)更小、轉(zhuǎn)折角度更小的路徑。故改進(jìn)A*算法滿足使用需求，且具有一定的應(yīng)用價(jià)值。

4 結(jié)論

a.本文采用的路徑優(yōu)化方法不僅可以用于傳統(tǒng)A*算法，也可用于其他全局路徑規(guī)劃算法，如Dijkstra 算法、RRT 算法等。以上算法均可以在獲得初始路徑后通過重連路徑點(diǎn)、曲線擬合來獲得代價(jià)更小、更平滑的路徑。

b.改進(jìn)A*算法在傳統(tǒng)A*算法的基礎(chǔ)上，擴(kuò)展機(jī)器人的鄰接點(diǎn)，最多允許機(jī)器人向周圍24 個(gè)柵格移動(dòng)，增加了機(jī)器人可能的移動(dòng)方向，可以快速獲得初始路徑；并對(duì)初始路徑重復(fù)去除冗余點(diǎn)，再利用5 次B 樣條曲線進(jìn)行路徑平滑，最終獲得路徑代價(jià)更小、累計(jì)轉(zhuǎn)折角度更小的優(yōu)化路徑。

c.通過仿真結(jié)果分析可知，改進(jìn)A*算法得到的最終路徑相比于傳統(tǒng)A*算法，路徑代價(jià)和路徑轉(zhuǎn)折角度都有明顯降低，并且搜索節(jié)點(diǎn)數(shù)量也有顯著減少(若隨機(jī)生成的障礙物柵格集中分布于最優(yōu)路徑附近，則搜索節(jié)點(diǎn)數(shù)量不會(huì)有顯著變化)。故該改進(jìn)A*算法能滿足救援機(jī)器人對(duì)“最優(yōu)”路徑的需求，可以進(jìn)行實(shí)際應(yīng)用。

d.改進(jìn)A*算法可以通過多次調(diào)用去除冗余點(diǎn)函數(shù)RemovePoint和路徑分割函數(shù)Spline，不斷處理路徑，獲得近似為“最優(yōu)”的優(yōu)化路徑，但是多次調(diào)用函數(shù)會(huì)占用大量計(jì)算機(jī)內(nèi)存，降低計(jì)算機(jī)處理速度。故后續(xù)研究應(yīng)針對(duì)該問題進(jìn)行進(jìn)一步優(yōu)化。同時(shí)，為進(jìn)一步減少生成初始路徑時(shí)的擴(kuò)展節(jié)點(diǎn)數(shù)量，可采用跳點(diǎn)搜索法(Jump Point Search，JPS)等進(jìn)行預(yù)處理。