張偉民，張 月，張 輝

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

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

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

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

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

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

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

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

柵格法由于操作簡單、易于實現(xiàn)，被廣泛用來描述機(jī)器人環(huán)境地圖[14]。柵格法需要計算機(jī)利用傳感器收集環(huán)境信息，創(chuàng)建環(huán)境的二維平面圖M，救援機(jī)器人RR看作在M上移動的點狀物體；若M為不規(guī)則圖形，則將M填補為規(guī)則圖形，并將填補的區(qū)域設(shè)為障礙物區(qū)域。經(jīng)過柵格化處理的地圖如圖1 所示，設(shè)圖中左下角為坐標(biāo)原點，水平方向為x軸，豎直方向為y軸；白色柵格為“可通行柵格(點)”，允許機(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中的對應(yīng)坐標(biāo)為(x,y)。記左下角第一個柵格坐標(biāo)為[1,1]。在柵格地圖環(huán)境下進(jìn)行路徑規(guī)劃是指在M上使得救援機(jī)器人RR從指定起始節(jié)點node_S沿著一條安全、無碰撞的最優(yōu)路徑到達(dá)指定目標(biāo)節(jié)點node_G。

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

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

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

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

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

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

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

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

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

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

(2)將起始節(jié)點node_S加入O，并計算代價F(node_S)。

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

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

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

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

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

H(n)與H*(n)的關(guān)系會對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 算法。此時算法總能搜索到最優(yōu)路徑，但需擴(kuò)展大量節(jié)點，算法效率低。

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

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

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

常見的代價計算方式有曼哈頓距離、對角距離和歐幾里得距離[18]。采用歐幾里得距離時有：

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

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

對于可沿任意方向運動的救援機(jī)器人而言，由于A*算法最多允許機(jī)器人向周圍8 個柵格移動，故A*算法在柵格地圖環(huán)境下規(guī)劃的路徑是非最優(yōu)的。以無障礙地圖為例，A*算法規(guī)劃出的路徑如圖2 所示。若機(jī)器人最多只能朝周圍8 個柵格移動，則圖2 中的路徑可以稱為“最優(yōu)的”；但若機(jī)器人可沿任意方向移動，則其最優(yōu)路徑應(yīng)是起始節(jié)點node_S和目標(biāo)節(jié)點node_G的連線。故需要對A*算法進(jìn)行改進(jìn)，以獲得對可沿任意方向移動的救援機(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ù)移除路徑點集合中的冗余點；Spline函數(shù)將路徑點按照步長α進(jìn)行分割；SmoothPath函數(shù)采用5 次B 樣條曲線對路徑進(jìn)行平滑處理。

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

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

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

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

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

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

2.2 鄰接點擴(kuò)展

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

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

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

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

2.3 去除路徑冗余點

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

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

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

針對不同的運動方向，CheckPath函數(shù)需要檢查的節(jié)點不同：

(1)路徑方向為左上到右下時，需要檢查當(dāng)前節(jié)點所在柵格的左側(cè)NL和上側(cè)NT柵格。

(2)路徑方向為右上到左下時，需要檢查當(dāng)前節(jié)點所在柵格的右側(cè)NR和上側(cè)NT柵格。

(3)路徑方向為右下到左上時，需要檢查當(dāng)前節(jié)點所在柵格的右側(cè)NR和下側(cè)NU柵格。

(4)路徑方向為左下到右上時，需要檢查當(dāng)前節(jié)點所在柵格的左側(cè)NL和下側(cè)NU柵格。

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

路徑方向為左上到右下時CheckPath函數(shù)的偽代碼如圖5 所示。圖5 中dmin為與直線lsg斜率相同且經(jīng)過柵格NT左下角(NL右上角)的直線與柵格NT左上角(NL右下角)的最小距離。

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

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

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

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

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

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

圖7 去除路徑冗余點后的路徑Fig.7 The path after removing redundant points

2.4 路徑平滑

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

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

B 樣條曲線的總方程為：

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

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

3 仿真實驗及結(jié)果分析

3.1 仿真實驗

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

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

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

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

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

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

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

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

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

(5)地圖尺寸為100×100 時，起始節(jié)點為(10,10)，目標(biāo)點節(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é)點、路徑代價和路徑轉(zhuǎn)折角度的比值。地圖是以單位大小柵格組成，圖中的值表示改進(jìn)A*算法計算得到的結(jié)果與傳統(tǒng)A*算法得到的結(jié)果的比值。從表1 可以看出，改進(jìn)A*算法相比于傳統(tǒng)A*算法，搜索過程擴(kuò)展的節(jié)點數(shù)量減少20%左右，所得路徑代價減少8%～9%，路徑轉(zhuǎn)折角度減少75%～80%。“改進(jìn)A*/傳統(tǒng)A*”比值均小于1，表明改進(jìn)A*算法優(yōu)于傳統(tǒng)A*算法。

3.2 結(jié)果分析

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

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

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

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

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

4 結(jié)論

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

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

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

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