王 迪，黎 冠，李志偉，李明宇，謝家順

(1.華北科技學(xué)院 安全工程學(xué)院，北京 東燕郊 065201;2.華北科技學(xué)院 電子信息工程學(xué)院，北京 東燕郊 065201)

0 引言

森林、高層建筑及?；返然馂?zāi)事故的發(fā)生，往往具有較大的破壞性，其造成的后果也普遍較為嚴(yán)重，近年來(lái)，天津港爆炸事故、四川涼山深林火災(zāi)等事故的發(fā)生，均造成了消防救援人員的巨大傷亡和不同程度的經(jīng)濟(jì)損失，給消防救援工作帶來(lái)了巨大高效地開展何進(jìn)一步快速、高效的開展消防救援工作，也成為了消防救援工作面臨的主要問題。隨著智能機(jī)器人技術(shù)的發(fā)展，一些具備自主移動(dòng)、自主探測(cè)能力的消防機(jī)器人逐漸應(yīng)用到消防領(lǐng)域當(dāng)中[1]，并逐漸在消防救援中承擔(dān)重要的角色。當(dāng)出現(xiàn)火情時(shí)，消防機(jī)器人可快速深入到火源位置，及時(shí)撲滅火源，并配合消防救援人員開展現(xiàn)場(chǎng)火情勘察和人員搜救等工作，可進(jìn)一步提高消防救援效率和保障消防救援人員的人身安全。

在消防機(jī)器人領(lǐng)域，路徑規(guī)劃算法作為消防機(jī)器人研究的重要方面，合理的路徑規(guī)劃策略可確保消防機(jī)器人安全、快速地進(jìn)行救援和滅火工作，最大限度地減少人員傷亡和財(cái)產(chǎn)損失[2]。目前，常用的全局路徑規(guī)劃算法主要包括隨機(jī)采樣算法[3-4]、圖搜索算法[5-6]和一些智能算法[7-9]等。本文將傳統(tǒng)A*算法應(yīng)用于消防機(jī)器人的路徑規(guī)劃中，但隨著環(huán)境復(fù)雜度的增高，傳統(tǒng)A*算法規(guī)劃的路徑結(jié)果中轉(zhuǎn)折點(diǎn)較多，搜索效率明顯下降。并且傳統(tǒng)A*算法未能結(jié)合機(jī)器人本身的體積、物理學(xué)狀態(tài)等因素，在實(shí)際使用過程中，需對(duì)其路徑做進(jìn)一步改進(jìn)，以避免機(jī)器人與障礙物發(fā)生碰撞。在消防機(jī)器人路徑規(guī)劃算法的研究中，何光層[10]等利用改進(jìn)A*算法解決室內(nèi)智能消防機(jī)器人的全局路徑規(guī)劃問題。王宏北[11]等針對(duì)地形復(fù)雜，地貌特征易變的火場(chǎng)中消防機(jī)器人的路徑規(guī)劃問題，利用自適應(yīng)改進(jìn)蟻群算法來(lái)獲取火場(chǎng)中的最優(yōu)路徑。

綜上，本文針對(duì)消防機(jī)器人領(lǐng)域，利用傳統(tǒng)A*算法導(dǎo)致的路徑規(guī)劃結(jié)果中存在過多轉(zhuǎn)折點(diǎn)、搜索效率較低等問題，對(duì)傳統(tǒng)A*算法的搜索鄰域和軌跡方面進(jìn)行改進(jìn)，在鄰域搜索階段，將鄰域子節(jié)點(diǎn)進(jìn)行分級(jí)，依據(jù)優(yōu)先級(jí)進(jìn)行搜索，從而避免斜穿頂點(diǎn)問題的發(fā)生，使結(jié)果更符合實(shí)際環(huán)境要求。并利用改進(jìn)Floyd算法去除冗余的轉(zhuǎn)折點(diǎn)，增強(qiáng)路徑的平滑性，進(jìn)一步確保消防機(jī)器人路徑規(guī)劃結(jié)果的可行性。

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

傳統(tǒng)A*路徑規(guī)劃算法作為一種啟發(fā)式搜索算法，常用在全局地圖中搜索兩點(diǎn)之間的最短路徑。在A*算法的實(shí)現(xiàn)原理中，搜索區(qū)域被簡(jiǎn)化成一個(gè)二維數(shù)組，路徑被描述為從起始柵格至終點(diǎn)柵格經(jīng)過的所有柵格的集合，柵格的中心點(diǎn)則為節(jié)點(diǎn)n。A*算法的搜索方向是根據(jù)代價(jià)估算函數(shù)F(n)來(lái)確定的，如公式(1)所示，搜索過程中通過代價(jià)估算函數(shù)F(n) 來(lái)判斷區(qū)域內(nèi)當(dāng)前點(diǎn)至目標(biāo)點(diǎn)的距離，進(jìn)而決定搜索方向。具體實(shí)現(xiàn)過程中，需要建立開啟列表和關(guān)閉列表，開啟列表中存儲(chǔ)著所有可能出現(xiàn)在最終路徑中的節(jié)點(diǎn)，關(guān)閉列表中存儲(chǔ)著已檢測(cè)并且不需要再次檢測(cè)的節(jié)點(diǎn)。路徑搜索過程中，首先需選取當(dāng)前開啟列表中F(n)值最小的節(jié)點(diǎn)，然后將其從開啟列表中刪除，再添加到關(guān)閉列表中，最終路徑也將在關(guān)閉列表中產(chǎn)生。

F(n)=g(n)+h(n)

(1)

式中，F(xiàn)(n)為節(jié)點(diǎn)n的代價(jià)估算函數(shù)；g(n)為機(jī)器人從起始節(jié)點(diǎn)至當(dāng)前節(jié)點(diǎn)n所花費(fèi)的實(shí)際代價(jià)，一般情況下由當(dāng)前節(jié)點(diǎn)的父節(jié)點(diǎn)的實(shí)際代價(jià)值得到；h(n)為啟發(fā)函數(shù)，表示機(jī)器人從當(dāng)前節(jié)點(diǎn)n至目標(biāo)節(jié)點(diǎn)將要花費(fèi)的預(yù)估代價(jià)。在傳統(tǒng)A*算法中，分別采用曼哈頓距離與歐氏距離表示h(n)大小，兩種表示方式如式2、3所示。

(2)

h(n)Manhattan=|xn-xt|+|xn-xt|

(3)

式中，xn，yn分別為當(dāng)前節(jié)點(diǎn)n所處柵格的橫坐標(biāo)、縱坐標(biāo)；xt，yt分別為目標(biāo)節(jié)點(diǎn)所處柵格的橫坐標(biāo)、縱坐標(biāo)。

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

2.1 基于優(yōu)先級(jí)的鄰域搜索方法

在路徑搜索的過程中，傳統(tǒng)A*算法將機(jī)器人視為一個(gè)質(zhì)點(diǎn)，未考慮機(jī)器人的體積等因素，其中，圖1所示為一組傳統(tǒng)A*算法規(guī)劃的路徑結(jié)果，該路徑結(jié)果中，紅色圓點(diǎn)位置出現(xiàn)路徑斜穿障礙物頂點(diǎn)的問題，在實(shí)際使用A*算法的過程中，出現(xiàn)該問題時(shí)，機(jī)器人存在一定的安全風(fēng)險(xiǎn)。因此，為避免該問題的發(fā)生，本文對(duì)傳統(tǒng)A*算法的搜索方向進(jìn)行調(diào)整，將鄰域中的節(jié)點(diǎn)劃分優(yōu)先級(jí)，分為優(yōu)先組與普通組，如圖2所示，N、E、S、W四個(gè)方向?yàn)閮?yōu)先組，其余NE、NW、SE、SW四個(gè)方向?yàn)槠胀ńM，在子節(jié)點(diǎn)的搜索過程中，首先對(duì)優(yōu)先組中的子節(jié)點(diǎn)進(jìn)行搜索，然后按照表1中的規(guī)則對(duì)普通組的子節(jié)點(diǎn)進(jìn)行搜索。

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

圖2 鄰域表示圖

表1 普通組去除的子節(jié)點(diǎn)及路徑

2.2 基于改進(jìn)Floyd算法的軌跡優(yōu)化方法

在路徑的平滑性方面，隨著區(qū)域內(nèi)障礙物覆蓋率的逐漸增加，傳統(tǒng)A*算法規(guī)劃的路徑中存在較多的轉(zhuǎn)折點(diǎn)，路徑平滑性較差。因此針對(duì)此問題，本文將傳統(tǒng)A*算法規(guī)劃的路徑做進(jìn)一步的平滑處理，改善路徑的平滑性。目前，常用的路徑平滑處理算法有貝塞爾曲線以及Floyd算法等，F(xiàn)loyd算法作為一種基于動(dòng)態(tài)規(guī)劃方法的最短路徑算法，又被稱為插點(diǎn)法。在軌跡優(yōu)化方面，常規(guī)的Floyd算法只進(jìn)行從起始點(diǎn)至目標(biāo)點(diǎn)的軌跡優(yōu)化。本文采用一種基于改進(jìn)Floyd算法的軌跡優(yōu)化方法對(duì)路徑進(jìn)行平滑處理，該算法在常規(guī)Floyd算法的基礎(chǔ)上，增加了從目標(biāo)點(diǎn)至起始點(diǎn)的反向軌跡優(yōu)化，采取雙方向的軌跡優(yōu)化方式，進(jìn)一步增強(qiáng)路徑的平滑度。除此之外，改進(jìn)Floyd算法增加了碰撞距離檢測(cè)，通過計(jì)算障礙物中心點(diǎn)與路徑間的垂直距離，避免與障礙物發(fā)生碰撞。下面以圖3為例，對(duì)改進(jìn)Floyd算法的流程做具體闡述，其中S為起始點(diǎn)，T為目標(biāo)點(diǎn)。

首先去除路徑中冗余的節(jié)點(diǎn)，去除紅色路徑中除起始點(diǎn)S、目標(biāo)點(diǎn)T及拐點(diǎn)以外的其他節(jié)點(diǎn)，如圖3所示，去除后的路徑為：S→n1→n2→n3→T。

圖3 改進(jìn)Floyd算法示例圖

然后進(jìn)行正向的軌跡優(yōu)化，軌跡優(yōu)化階段，設(shè)置步長(zhǎng)為k，與障礙物安全距離為D。優(yōu)化過程中，從起始點(diǎn)S開始，在上一步保留的路徑中，相鄰兩個(gè)節(jié)點(diǎn)之間每隔k步長(zhǎng)選取一個(gè)節(jié)點(diǎn)，判斷選取節(jié)點(diǎn)與前端路徑節(jié)點(diǎn)之間的路徑是否有障礙物，并檢測(cè)障礙物與路徑的距離d，若d≥D，則該路徑滿足要求，否則不滿足要求。此時(shí)，經(jīng)前向軌跡優(yōu)化后的路徑可表示為：S→n4→T。

最后進(jìn)行反向的軌跡優(yōu)化，反向軌跡優(yōu)化采用與正向軌跡優(yōu)化相同的平滑處理方式，反向優(yōu)化后路徑進(jìn)一步表示為：S→n5→T。

在障礙物距離檢測(cè)方面，如圖4所示，A、B為路徑的起始點(diǎn)與終點(diǎn)，坐標(biāo)分別為(xA，yA)、(xB，yB)，AB與x軸之間的夾角為α，C為障礙物中心點(diǎn)，坐標(biāo)為(xC，yC)，點(diǎn)C至AB的垂直距離和縱向距離分別為d、e。因此，障礙物與路徑的距離d可由A、B、C三點(diǎn)坐標(biāo)表示為：

圖4 障礙物距離圖

d=e·cosα

(4)

其中，α、e可表示為：

(5)

(6)

根據(jù)柵格的大小，當(dāng)d≥D時(shí)，表示該路徑可選取，否則表示該路徑不可選。

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

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

基于以上對(duì)改進(jìn)A*算法的原理分析，本文對(duì)改進(jìn)A*算法進(jìn)行進(jìn)一步的測(cè)試與分析，測(cè)試中使用的系統(tǒng)環(huán)境為Intel(R)Core(TM)i7-5500U CPU@2.40GHz Windows10專業(yè)版，軟件環(huán)境為MATLAB 2019b。在障礙物覆蓋率為10%、20%、30%的情況下，分別對(duì)傳統(tǒng)A*算法、軌跡未平滑處理的改進(jìn)A*算法以及軌跡平滑處理后的改進(jìn)A*算法進(jìn)行了測(cè)試。機(jī)器人均從左上角的起始點(diǎn)搜索至右下角的目標(biāo)點(diǎn)，搜索結(jié)果分別如圖5～圖7所示。

圖5 仿真結(jié)果1(障礙物覆蓋率10%)

圖6 仿真結(jié)果2(障礙物覆蓋率20%)

圖7 仿真結(jié)果3(障礙物覆蓋率30%)

從圖中可以看出，三組測(cè)試均能規(guī)劃出從起始點(diǎn)至目標(biāo)點(diǎn)的全局路徑，并且隨著算法的逐步改進(jìn)，路徑規(guī)劃的結(jié)果逐步改善，測(cè)試后的具體實(shí)驗(yàn)參數(shù)如表2所示。由表2可知，相比于傳統(tǒng)A*算法，在轉(zhuǎn)折點(diǎn)方面，每條路徑中平均減少了62.49%的轉(zhuǎn)折點(diǎn)數(shù)。在規(guī)劃時(shí)間方面，平均提升了23.22%的搜索效率，在路徑長(zhǎng)度方面，平均減少了2.19%的路徑長(zhǎng)度。

表2 具體實(shí)驗(yàn)參數(shù)

3.2 基于GAZEBO的室內(nèi)消防救援實(shí)驗(yàn)結(jié)果與分析

為進(jìn)一步測(cè)試改進(jìn)A*算法在消防機(jī)器人中的應(yīng)用效果，如圖8所示。本文模擬實(shí)際的室內(nèi)消防救援場(chǎng)景，利用GAZEBO仿真軟件搭建了室內(nèi)仿真環(huán)境和消防機(jī)器人模型，仿真環(huán)境整體長(zhǎng)寬分別為30 m和20 m。結(jié)合ROS機(jī)器人操作系統(tǒng)，在構(gòu)建完成的場(chǎng)景內(nèi)平面柵格地圖的基礎(chǔ)上，利用Move_base導(dǎo)航框架，測(cè)試消防機(jī)器人消防滅火能力。

圖8 室內(nèi)仿真環(huán)境與消防機(jī)器人模型圖

實(shí)驗(yàn)過程中，設(shè)定當(dāng)室內(nèi)發(fā)生火災(zāi)時(shí)，消防機(jī)器人從起始點(diǎn)開始，在指定需到達(dá)的起火點(diǎn)位置后，機(jī)器人便自主規(guī)劃全局路徑，并沿著該路徑自主導(dǎo)航至起火點(diǎn)位置，及時(shí)撲滅火源，完成滅火任務(wù)。如圖9、圖10所示，分別為消防機(jī)器人利用傳統(tǒng)A*算法與改進(jìn)A*算法規(guī)劃的全局路徑。消防機(jī)器人導(dǎo)航過程中的相關(guān)數(shù)據(jù)見表3。

圖9 傳統(tǒng)A*算法路徑規(guī)劃效果圖

圖10 改進(jìn)A*算法路徑規(guī)劃效果圖

表3 消防機(jī)器人導(dǎo)航相關(guān)數(shù)據(jù)

通過圖表可以得出，在應(yīng)用方面，改進(jìn)A*算法可以較好的改善消防機(jī)器人的全局路徑規(guī)劃效果，并且在導(dǎo)航過程中，利用改進(jìn)A*算法，消防機(jī)器人的整體運(yùn)行速度有所提升，運(yùn)行較為平穩(wěn)。

4 結(jié)論

(1) 利用基于優(yōu)先級(jí)的鄰域搜索方式，可有效提高算法的鄰域搜索效率，同時(shí)也可避免路徑中出現(xiàn)斜穿障礙物頂點(diǎn)的現(xiàn)象，使路徑規(guī)劃結(jié)果更加適用于實(shí)際的消防救援場(chǎng)景。

(2) 利用基于改進(jìn)Floyd算法的軌跡優(yōu)化方法，在減少路徑中的轉(zhuǎn)折點(diǎn)數(shù)的同時(shí)，也進(jìn)一步減少了路徑長(zhǎng)度，有效提高消防機(jī)器人的路徑規(guī)劃效率。

(3) 通過兩組仿真實(shí)驗(yàn)，有效驗(yàn)證了改進(jìn)A*算法的可行性以及在室內(nèi)救援場(chǎng)景中的實(shí)用性，在此研究的基礎(chǔ)上，未來(lái)可進(jìn)一步搭建消防機(jī)器人平臺(tái)，結(jié)合相關(guān)局部路徑規(guī)劃算法，更為全面地測(cè)試消防機(jī)器人的實(shí)際消防能力。