王 凡，李鐵軍，劉今越，趙海文

河北工業(yè)大學(xué) 機(jī)械工程學(xué)院，天津 300131

1 引言

傳統(tǒng)的移動(dòng)機(jī)器人自主導(dǎo)航地圖的建立主要運(yùn)用SLAM[1-2]技術(shù)，即在機(jī)器人對(duì)周圍環(huán)境部分已知甚至完全未知的狀態(tài)下，根據(jù)自身傳感器獲取的相關(guān)信息，完成對(duì)當(dāng)前環(huán)境地圖的構(gòu)建與自身定位。該過(guò)程需要事先操作機(jī)器人在未知環(huán)境下進(jìn)行完整的巡視檢測(cè)，由外部傳感器充分采集環(huán)境信息來(lái)構(gòu)建地圖，數(shù)據(jù)計(jì)算量大。對(duì)于建筑機(jī)器人（圖1）而言，其本體尺寸、施工環(huán)境空間均很大，而移動(dòng)速度則較為緩慢，故該方法用于建筑機(jī)器人將耗時(shí)、耗能。

圖1 建筑機(jī)器人

路徑規(guī)劃旨在已知障礙物環(huán)境的前提下，為機(jī)器人規(guī)劃出一條從起點(diǎn)到終點(diǎn)的最優(yōu)路徑。目前，運(yùn)用于該類問(wèn)題的算法主要分為全局路徑規(guī)劃和局部路徑規(guī)劃兩類，其中，全局路徑規(guī)劃算法主要包括遺傳算法[3]、蟻群算法[4]、Dijkstra算法、A*算法[5]等。遺傳算法等智能算法充分發(fā)揮自身迭代優(yōu)勢(shì)的情況下，可以很好地與其他算法融合使用，擁有優(yōu)秀的自組織性和自學(xué)習(xí)性、在路徑規(guī)劃中對(duì)最優(yōu)路徑有優(yōu)秀的搜索能力，但存在編碼長(zhǎng)度變化范圍大，求解效率低，求解規(guī)模小，易陷于局部最優(yōu)的問(wèn)題。Dijkstra 算法[6]直接搜索全局空間而不考慮目標(biāo)信息，路徑求解時(shí)間長(zhǎng)，難以滿足快速規(guī)劃路徑的需求。局部路徑規(guī)劃算法主要包括人工勢(shì)場(chǎng)法[7]、動(dòng)態(tài)窗口法[8-9]等。人工勢(shì)場(chǎng)法結(jié)構(gòu)簡(jiǎn)單、計(jì)算量較小但容易產(chǎn)生局部最小值。

A*算法是一種適用于地圖環(huán)境已知的全局路徑規(guī)劃算法，它結(jié)合了啟發(fā)函數(shù)與常規(guī)的Dijkstra 算法的優(yōu)點(diǎn)，可以保證在提高搜索效率的同時(shí)找到一條最優(yōu)路徑。文獻(xiàn)[10]中提出一種A*算法的改進(jìn)策略，簡(jiǎn)化了路徑且能計(jì)算出拐點(diǎn)處機(jī)器人的旋轉(zhuǎn)方向和角度，但不具備實(shí)時(shí)避障的能力。文獻(xiàn)[11]中提出在A*算法最優(yōu)節(jié)點(diǎn)之間使用動(dòng)態(tài)窗口法進(jìn)行局部避障，但未考慮其輸出的控制參數(shù)不連續(xù)的問(wèn)題，在實(shí)際控制機(jī)器人運(yùn)動(dòng)過(guò)程中會(huì)導(dǎo)致機(jī)器人運(yùn)動(dòng)不連續(xù)。針對(duì)以上問(wèn)題，本文基于建筑物BIM模型提出了一種新型的導(dǎo)航地圖建立方法；基于傳統(tǒng)A*算法，優(yōu)化其搜索點(diǎn)選取策略；在A*算法最優(yōu)節(jié)點(diǎn)之間使用動(dòng)態(tài)窗口法進(jìn)行局部避障，且設(shè)定了新的剎車判定條件使得機(jī)器人在未到達(dá)最終目標(biāo)點(diǎn)時(shí)不會(huì)停止運(yùn)動(dòng)，具有局部避障能力。

2 地圖的建立

BIM[12-14]模型具有信息參數(shù)化、信息關(guān)聯(lián)性、信息完備性、信息一致性的特點(diǎn)。在建立地圖的過(guò)程中引入BIM模型。使用Autodesk Revit軟件進(jìn)行BIM建模，現(xiàn)以圖2所示的建筑BIM模型為例，介紹該導(dǎo)航地圖的建立及全局路徑規(guī)劃。

圖2 建筑模型

BIM 模型里包含了建筑物的一切幾何信息及語(yǔ)義信息，在BIM 模型完成之后，將其轉(zhuǎn)換為IFC（Industry Foundation Classes）文件[15-16]，在IFC文件中提取建筑物的幾何信息及語(yǔ)義信息，提取后的部分幾何信息如圖3所示，實(shí)線框中為柱1的橫截面尺寸及底面中心點(diǎn)坐標(biāo)。

圖3 提取的幾何信息

將提取的有效幾何信息映射到二維柵格圖中完成地圖的建立?？紤]到映射時(shí)，存在某一幾何特征（一面墻或一根柱）無(wú)法完全占滿整個(gè)柵格的可能，故制定如下規(guī)則：

（1）對(duì)于任意障礙物Oi(i=1,2,…)映射到二維柵格圖時(shí)，將Oi占據(jù)但未完全占滿的柵格擴(kuò)張補(bǔ)齊為長(zhǎng)方形，將補(bǔ)齊的區(qū)域亦視為障礙物區(qū)域不允許機(jī)器人通過(guò)。

（2）行、列柵格數(shù)遵循以下公式：

其中，Nr為行柵格數(shù)；Nc為列柵格數(shù)；xmax、ymax為BIM模型在x軸、y軸上的最大值；s為步長(zhǎng)，即每個(gè)柵格代表的實(shí)際長(zhǎng)度。依據(jù)該規(guī)則建立機(jī)器人在圖2 所示建筑物里的導(dǎo)航地圖，如圖4所示。

圖4 機(jī)器人導(dǎo)航地圖

3 改進(jìn)A*算法的全局路徑規(guī)劃

A*算法以建立良好的導(dǎo)航地圖為基礎(chǔ)，通過(guò)定義全局路徑搜索代價(jià)函數(shù)獲取最優(yōu)路徑節(jié)點(diǎn)。其代價(jià)函數(shù)定義為：

式中，n為當(dāng)前節(jié)點(diǎn)；f(n)為當(dāng)前節(jié)點(diǎn)的綜合優(yōu)先級(jí)；g(n)為當(dāng)前節(jié)點(diǎn)距離起點(diǎn)的代價(jià)；h(n)為當(dāng)前節(jié)點(diǎn)距離終點(diǎn)的預(yù)計(jì)代價(jià)，即A*算法的啟發(fā)函數(shù)。

運(yùn)用A*算法進(jìn)行路徑規(guī)劃過(guò)程中，每次搜索，選擇當(dāng)前節(jié)點(diǎn)的4 領(lǐng)域柵格還是8 領(lǐng)域柵格，取決于算法中定義的搜索方向。由于建筑機(jī)器人體積較大，為了避免在墻角處轉(zhuǎn)向時(shí)與墻體發(fā)生碰撞，本文采用搜索4領(lǐng)域柵格方式，即機(jī)器人不會(huì)沿柵格對(duì)角線方向前行，同時(shí)，啟發(fā)函數(shù)h(n)設(shè)定為Manhattan距離形式，即：

式中，nx、ny、gx、gy為當(dāng)前節(jié)點(diǎn)與終點(diǎn)坐標(biāo)。

3.1 優(yōu)化搜索點(diǎn)選取策略

原始的A*算法未考慮機(jī)器人寬度信息，規(guī)劃的路徑往往緊貼障礙物，如圖5所示。

圖5 原始A*算法規(guī)劃出的路徑

對(duì)于建筑機(jī)器人而言，除了其機(jī)身及末端執(zhí)行機(jī)構(gòu)尺寸之外，其操作對(duì)象的結(jié)構(gòu)尺寸在路徑規(guī)劃時(shí)亦需要充分考慮，以避免與障礙物發(fā)生碰撞。這種基于原始A*算法規(guī)劃出的路徑顯然并不適用，不利于安全操作，因此，需要在選取搜索點(diǎn)時(shí)設(shè)定一定的優(yōu)先級(jí)?？紤]建筑機(jī)器人本體特性，制定搜索點(diǎn)優(yōu)先選取策略為：

（1）如圖6 所示，機(jī)器人R在某一時(shí)刻選取下一搜索點(diǎn)時(shí)，其周圍四個(gè)點(diǎn)A、B、C、D均為潛在備選點(diǎn)，若隨機(jī)選定沿A方向，先判斷A′點(diǎn)是否為障礙物，若A′不是障礙物，則可選A；若A′為障礙物，則放棄A方向，重新選擇其他三個(gè)方向之一。而后，其他三個(gè)方向按照同樣的規(guī)律依次判斷。

（2）若機(jī)器人沿A或C方向行走，則需限制B和D方向無(wú)障礙物；若機(jī)器人沿B或D方向行走，則需限制A和C方向無(wú)障礙物。此策略保證機(jī)器人行走過(guò)程中兩側(cè)空間充足，轉(zhuǎn)向時(shí)不會(huì)與障礙物發(fā)生碰撞。

圖6 搜索點(diǎn)選取示意圖

根據(jù)上述搜索點(diǎn)選取策略后，對(duì)圖5 相同起點(diǎn)、終點(diǎn)下重新進(jìn)行路徑規(guī)劃，結(jié)果如圖7所示?？梢钥闯鏊阉鲄^(qū)域明顯減少，因此，運(yùn)算時(shí)間顯著縮短，并且機(jī)器人在通過(guò)門時(shí)更好地避開(kāi)了墻體。

圖7 引入搜索點(diǎn)選取策略的路徑

3.2 刪除冗余轉(zhuǎn)折點(diǎn)

如圖7所示，改進(jìn)的路徑雖然減少了運(yùn)算量且可以更好地避開(kāi)障礙，但是路徑上增加了許多不必要的轉(zhuǎn)折點(diǎn)，這對(duì)于機(jī)器人的路徑跟隨是不利的，會(huì)降低機(jī)器人行進(jìn)速度，延長(zhǎng)行程時(shí)間，因此，需要將冗余轉(zhuǎn)折點(diǎn)刪除，只保留必要的轉(zhuǎn)折點(diǎn)。本文設(shè)定如下冗余點(diǎn)刪除規(guī)則（從第二節(jié)點(diǎn)開(kāi)始依次判斷，第一節(jié)點(diǎn)為起點(diǎn)）：

（1）若當(dāng)前節(jié)點(diǎn)與前一節(jié)點(diǎn)及后一節(jié)點(diǎn)在同一條直線上，則刪除。

（2）執(zhí)行完步驟（1）后繼續(xù)判斷，若當(dāng)前節(jié)點(diǎn)與其前、后節(jié)點(diǎn)的距離均為一個(gè)柵格則視為冗余轉(zhuǎn)折點(diǎn)，進(jìn)行刪除；若距離大于一個(gè)柵格則視為有效必要轉(zhuǎn)折點(diǎn)保留，結(jié)果如圖8所示。

圖8 刪除冗余轉(zhuǎn)折點(diǎn)后的路徑

4 融合動(dòng)態(tài)窗口法

動(dòng)態(tài)窗口法[9]具有良好的局部避障能力，但其規(guī)劃出的路徑并不符合全局最優(yōu)的要求，本文將其與A*算法結(jié)合，使其滿足全局路徑最優(yōu)的同時(shí)兼具實(shí)時(shí)避障能力。給定起點(diǎn)和終點(diǎn)后，利用動(dòng)態(tài)窗口法進(jìn)行計(jì)算，生成一個(gè)速度窗口，在該速度窗口內(nèi)采樣多組速度（線速度和角速度），依據(jù)機(jī)器人運(yùn)動(dòng)學(xué)模型模擬機(jī)器人在該速度下一定時(shí)間間隔內(nèi)的軌跡，獲取多組軌跡后，根據(jù)評(píng)價(jià)函數(shù)選取最優(yōu)軌跡對(duì)應(yīng)的速度驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)。

在上述刪除了冗余轉(zhuǎn)折點(diǎn)之后，A*算法規(guī)劃出的路徑只剩下起點(diǎn)、終點(diǎn)及必要轉(zhuǎn)折點(diǎn)，這些點(diǎn)可視為全局路徑上的最優(yōu)節(jié)點(diǎn)。在最優(yōu)節(jié)點(diǎn)之間使用動(dòng)態(tài)窗口法，可使得局部路徑滿足全局最優(yōu)要求，但若在每?jī)蓚€(gè)節(jié)點(diǎn)之間都使用一次動(dòng)態(tài)窗口法，當(dāng)算法判定機(jī)器人到達(dá)該兩個(gè)節(jié)點(diǎn)的終點(diǎn)時(shí)會(huì)輸出控制參數(shù)：v=0，w=0。這會(huì)使得機(jī)器人沒(méi)有到達(dá)最終的終點(diǎn)提前停止運(yùn)動(dòng)，在后兩個(gè)節(jié)點(diǎn)之間再一次使用動(dòng)態(tài)窗口法時(shí)機(jī)器人又從停止開(kāi)始加速。這樣一個(gè)過(guò)程使機(jī)器人在整個(gè)運(yùn)動(dòng)過(guò)程中雖然路徑符合全局最優(yōu)，但其前行過(guò)程是不連續(xù)的，在機(jī)器人到達(dá)終點(diǎn)之前一直處于加速—減速—停止—加速的狀態(tài)。這對(duì)于質(zhì)量、尺寸較大的建筑機(jī)器人而言是非常不利的。故需在使用動(dòng)態(tài)窗口法時(shí)加入如下判定條件：

（1）若當(dāng)前終點(diǎn)不是最終目標(biāo)點(diǎn)，則不進(jìn)行剎車距離判斷，繼續(xù)以當(dāng)前速度運(yùn)動(dòng)。

（2）若當(dāng)前終點(diǎn)是最終目標(biāo)點(diǎn)，則進(jìn)行剎車距離判定，在到達(dá)終點(diǎn)時(shí)停止運(yùn)動(dòng)。

令動(dòng)態(tài)窗口法評(píng)價(jià)函數(shù)為：

式中，H(v,w)用來(lái)評(píng)價(jià)機(jī)器人以當(dāng)前的采樣速度，達(dá)到模擬軌跡末端時(shí)的朝向與機(jī)器人坐標(biāo)系原點(diǎn)與下一個(gè)最優(yōu)節(jié)點(diǎn)連線之間的角度差θ，如圖9所示。D(v,w)為機(jī)器人在當(dāng)前軌跡上與最近障礙物之間的距離，若此軌跡上沒(méi)有障礙物，則將其設(shè)為常數(shù)。V(v,w)用于評(píng)價(jià)當(dāng)前軌跡速度。α、β、γ為加權(quán)系數(shù)，σ為平滑函數(shù)。本文算法流程圖如圖10所示。

圖9 最優(yōu)節(jié)點(diǎn)之間使用動(dòng)態(tài)窗口法示意圖

圖10 算法流程圖

5 仿真與實(shí)驗(yàn)

在相同起始點(diǎn)情況下分別使用遺傳算法、蟻群算法及本文改進(jìn)的A*算法進(jìn)行全局路徑規(guī)劃，其效果如圖11 所示。圖11（a）、（b）、（c）分別為使用遺傳算法、蟻群算法及本文改進(jìn)后A*算法規(guī)劃出的全局路徑，用時(shí)分別為 6.85 s、34.48 s、2.32 s；路徑拐點(diǎn)個(gè)數(shù)分別為 5、9、3。遺傳算法及蟻群算法規(guī)劃出的路徑拐點(diǎn)多，路徑緊靠障礙物且算法運(yùn)行時(shí)間長(zhǎng)；本文改進(jìn)后的A*算法拐點(diǎn)少，運(yùn)行時(shí)間快，對(duì)于建筑機(jī)器人更加有利。

圖11 不同算法進(jìn)行全局路徑規(guī)劃

為驗(yàn)證算法的有效性，對(duì)我校機(jī)械學(xué)院樓三樓進(jìn)行BIM 建模（圖12），并獲取該樓層導(dǎo)航地圖，其中s=600 mm，Nr=32，Nc=154。分別對(duì)原始A*算法路徑規(guī)劃和改進(jìn)的A*算法路徑規(guī)劃設(shè)置兩組仿真實(shí)驗(yàn)，第一組對(duì)起點(diǎn)（142，20），終點(diǎn)（109，7）之間的空間進(jìn)行路徑規(guī)劃，結(jié)果如圖13 所示；第二組對(duì)起點(diǎn)（142，20），終點(diǎn)（21，4）之間的空間進(jìn)行路徑規(guī)劃，結(jié)果如圖14 所示。同時(shí)，利用MATLAB 軟件分別對(duì)兩種算法的運(yùn)行時(shí)間和路徑長(zhǎng)度進(jìn)行仿真計(jì)算，結(jié)果如表1、表2所示。

對(duì)比圖13、圖14以及表1、表2中的結(jié)果可知：改進(jìn)的A*算法運(yùn)行時(shí)間相較于原始A*算法減少50%以上，規(guī)劃出的路徑長(zhǎng)度較原始A*算法減少0.4 m（路徑只有一個(gè)冗余轉(zhuǎn)折點(diǎn)），原始算法規(guī)劃出的路徑距離障礙物0.3 m，改進(jìn)之后路徑距離障礙物0.9 m。規(guī)劃出的路徑不再緊貼障礙物，使得機(jī)器人跟蹤該路徑前進(jìn)時(shí)更加安全。路徑與障礙物的距離取決于步長(zhǎng)s，建筑機(jī)器人的尺寸越大，在建立地圖時(shí)取得s就越大，規(guī)劃出的路徑與障礙物之間的距離也就越大，將該距離用d表示，算法經(jīng)過(guò)改進(jìn)后d≥1.5s。

圖12 機(jī)械工程學(xué)院樓三樓BIM模型

圖13 第一組路徑規(guī)劃

圖14 第二組路徑規(guī)劃

表1 原始A*算法與改進(jìn)A*算法運(yùn)行時(shí)間比較

表2 原始A*算法與改進(jìn)A*算法路徑長(zhǎng)度比較

圖15 融合算法路徑規(guī)劃及避障能力仿真

設(shè)置第三組仿真實(shí)驗(yàn)對(duì)最終融合算法路徑規(guī)劃及避障能力進(jìn)行驗(yàn)證，設(shè)定起點(diǎn)為（141，14），終點(diǎn)為（110，14），隨機(jī)障礙物坐標(biāo)為（134，14），其中，動(dòng)態(tài)窗口法路徑規(guī)劃過(guò)程中，機(jī)器人各參數(shù)設(shè)置如下：機(jī)器人最大線速度為1 m/s，最大角速度為20（°）/s，加速度為0.2 m/s2，角加速度為50（°）/s2，速度分辨率為0.01 m/s，角速度分辨率為1（°）/s，機(jī)器人運(yùn)動(dòng)學(xué)模型時(shí)間間隔為0.1 s。障礙物判定半徑為0.6 m。評(píng)價(jià)函數(shù)各參數(shù)為：α=0.05，β=0.2，γ=0.1，預(yù)測(cè)周期為3.0 s。仿真效果如圖15（a）所示，路徑發(fā)生偏轉(zhuǎn)的位置距離障礙物0.9 m，偏轉(zhuǎn)結(jié)束的位置距離障礙物0.9 m，整個(gè)算法所用時(shí)間為11.26 s，整個(gè)路徑長(zhǎng)度21.6 m。從圖中可以看出路徑能夠較好地避開(kāi)障礙物。增加隨機(jī)障礙物個(gè)數(shù)，且將起點(diǎn)設(shè)在（141，10），終點(diǎn)設(shè)在（109，14），在全局路徑存在拐點(diǎn)且障礙物增加的情況下，仿真效果如圖15（b）所示，從圖中可以看出在全局路徑存在拐點(diǎn)且障礙物增加的情況下規(guī)劃出的路徑仍能夠較好地避開(kāi)障礙物，整個(gè)算法所用時(shí)間為18.52 s，整個(gè)路徑長(zhǎng)度24.6 m。

為驗(yàn)證融合算法的實(shí)際效果，進(jìn)行實(shí)驗(yàn)驗(yàn)證，如圖16所示，實(shí)驗(yàn)平臺(tái)采用Discover Q2四輪全向機(jī)器人平臺(tái)，YDLIDAR G4 雷達(dá)，上位機(jī)采用聯(lián)想P50s 工作站。實(shí)驗(yàn)起點(diǎn)、終點(diǎn)以及隨機(jī)障礙物設(shè)置與第三組仿真實(shí)驗(yàn)一致，機(jī)器人最大線速度0.5 m/s，最大角速度90（°）/s，其他參數(shù)與第三組仿真一致。實(shí)驗(yàn)效果如圖17 所示，圖17（a）機(jī)器人開(kāi)始移動(dòng)，在圖17（b）開(kāi)始偏轉(zhuǎn)，此時(shí)距離障礙物約0.5 m，圖17（c）、（d）為繞開(kāi)障礙物過(guò)程，圖17（e）為偏轉(zhuǎn)結(jié)束位置，此時(shí)距離障礙物約1 m，繞開(kāi)障礙物耗時(shí)約6 s，圖17（f）為機(jī)器人到達(dá)終點(diǎn)位置，此時(shí)距離障礙物距離約14.9 m，整個(gè)路徑長(zhǎng)度約22.8 m，整個(gè)過(guò)程耗時(shí)約80 s。圖18 所示為設(shè)置兩個(gè)隨機(jī)障礙物，機(jī)器人從起點(diǎn)啟動(dòng)左拐經(jīng)過(guò)拐角后所處位置并沒(méi)有在中央，向右發(fā)生偏移如圖18（b）所示，但仍能避開(kāi)障礙物如圖18（d）、（e）所示，整個(gè)路徑長(zhǎng)度約25.7 m，整個(gè)過(guò)程耗時(shí)約92 s。兩次實(shí)驗(yàn)結(jié)果與仿真結(jié)果比較發(fā)現(xiàn)機(jī)器人開(kāi)始偏轉(zhuǎn)與結(jié)束偏轉(zhuǎn)的位置都有滯后，到達(dá)終點(diǎn)的位置也有滯后，第一組實(shí)驗(yàn)整個(gè)路徑長(zhǎng)度比仿真結(jié)果多出約1.2 m，第二組實(shí)驗(yàn)多出約1.1 m，這種誤差在于使用Matlab 給機(jī)器人發(fā)送指令與機(jī)器人接收指令之間存在一定的延時(shí)，造成機(jī)器人會(huì)多執(zhí)行上一個(gè)指令一段時(shí)間，使其行駛距離增加。兩組實(shí)驗(yàn)的整體效果驗(yàn)證了算法的可行性，規(guī)劃出的路徑可以繞開(kāi)隨機(jī)障礙物，且由于增加了剎車判定條件，使機(jī)器人前進(jìn)過(guò)程連續(xù)，中途沒(méi)有斷點(diǎn)，路徑保持了全局最優(yōu)。

圖16 實(shí)驗(yàn)平臺(tái)

圖17 融合算法實(shí)時(shí)避障實(shí)驗(yàn)

圖18 融合算法實(shí)時(shí)避障實(shí)驗(yàn)

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

本文引入BIM 方法建立機(jī)器人導(dǎo)航地圖，優(yōu)化了A*算法搜索點(diǎn)選取策略，刪除了規(guī)劃路徑中的冗余轉(zhuǎn)折點(diǎn)，使得路徑規(guī)劃時(shí)間減少50%以上。對(duì)比原始A*算法，改進(jìn)后算法規(guī)劃出的路徑更加安全且每刪除一個(gè)冗余轉(zhuǎn)折點(diǎn)，路徑長(zhǎng)度減少0.4 m。在最優(yōu)路徑節(jié)點(diǎn)之間采用動(dòng)態(tài)窗口法進(jìn)行局部區(qū)域內(nèi)的路徑規(guī)劃，且加入了新的剎車判定條件，使得輸出的控制參數(shù)連續(xù)化，機(jī)器人兼具局部避障與路徑全局最優(yōu)的特點(diǎn)。本文主要研究了建筑機(jī)器人在單一樓層的導(dǎo)航，尚未涉及多樓層間的導(dǎo)航，并且地圖的建立主要依賴于BIM模型的詳細(xì)程度，因此，未來(lái)可針對(duì)BIM信息提取、多樓層間導(dǎo)航等問(wèn)題開(kāi)展更深入的研究。