基于BIM的建筑機器人自主路徑規(guī)劃及避障研究

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

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

1 引言

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

圖1 建筑機器人

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

2 地圖的建立

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

圖2 建筑模型

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

圖3 提取的幾何信息

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖6 搜索點選取示意圖

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

圖7 引入搜索點選取策略的路徑

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

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

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

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

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

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

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

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

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

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

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

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

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

圖10 算法流程圖

5 仿真與實驗

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

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

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

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

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

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

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

表1 原始A*算法與改進A*算法運行時間比較

表2 原始A*算法與改進A*算法路徑長度比較

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

設(shè)置第三組仿真實驗對最終融合算法路徑規(guī)劃及避障能力進行驗證，設(shè)定起點為（141，14），終點為（110，14），隨機障礙物坐標(biāo)為（134，14），其中，動態(tài)窗口法路徑規(guī)劃過程中，機器人各參數(shù)設(shè)置如下：機器人最大線速度為1 m/s，最大角速度為20（°）/s，加速度為0.2 m/s2，角加速度為50（°）/s2，速度分辨率為0.01 m/s，角速度分辨率為1（°）/s，機器人運動學(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。從圖中可以看出路徑能夠較好地避開障礙物。增加隨機障礙物個數(shù)，且將起點設(shè)在（141，10），終點設(shè)在（109，14），在全局路徑存在拐點且障礙物增加的情況下，仿真效果如圖15（b）所示，從圖中可以看出在全局路徑存在拐點且障礙物增加的情況下規(guī)劃出的路徑仍能夠較好地避開障礙物，整個算法所用時間為18.52 s，整個路徑長度24.6 m。

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

圖16 實驗平臺

圖17 融合算法實時避障實驗

圖18 融合算法實時避障實驗

6 結(jié)束語

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