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

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

1 引言

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

圖1 建筑機(jī)器人

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

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

2 地圖的建立

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

圖2 建筑模型

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

圖3 提取的幾何信息

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

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

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

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

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

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

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

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

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

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

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

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

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

根據(jù)上述搜索點(diǎn)選取策略后，對圖5 相同起點(diǎn)、終點(diǎn)下重新進(jìn)行路徑規(guī)劃，結(jié)果如圖7所示。可以看出搜索區(qū)域明顯減少，因此，運(yùn)算時間顯著縮短，并且機(jī)器人在通過門時更好地避開了墻體。

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

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

如圖7所示，改進(jìn)的路徑雖然減少了運(yùn)算量且可以更好地避開障礙，但是路徑上增加了許多不必要的轉(zhuǎn)折點(diǎn)，這對于機(jī)器人的路徑跟隨是不利的，會降低機(jī)器人行進(jìn)速度，延長行程時間，因此，需要將冗余轉(zhuǎn)折點(diǎn)刪除，只保留必要的轉(zhuǎn)折點(diǎn)。本文設(shè)定如下冗余點(diǎn)刪除規(guī)則（從第二節(jié)點(diǎn)開始依次判斷，第一節(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)的距離均為一個柵格則視為冗余轉(zhuǎn)折點(diǎn)，進(jìn)行刪除；若距離大于一個柵格則視為有效必要轉(zhuǎn)折點(diǎn)保留，結(jié)果如圖8所示。

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

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

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

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

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

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

令動態(tài)窗口法評價函數(shù)為：

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

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

圖10 算法流程圖

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

6 結(jié)束語

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