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

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

1 引言

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

圖1 建筑機器人

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

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

2 地圖的建立

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

圖2 建筑模型

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

圖3 提取的幾何信息

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

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

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

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

圖4 機器人導航地圖

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

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

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

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

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

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

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

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

對于建筑機器人而言，除了其機身及末端執(zhí)行機構尺寸之外，其操作對象的結構尺寸在路徑規(guī)劃時亦需要充分考慮，以避免與障礙物發(fā)生碰撞。這種基于原始A*算法規(guī)劃出的路徑顯然并不適用，不利于安全操作，因此，需要在選取搜索點時設定一定的優(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方向無障礙物。此策略保證機器人行走過程中兩側空間充足，轉向時不會與障礙物發(fā)生碰撞。

圖6 搜索點選取示意圖

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

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

3.2 刪除冗余轉折點

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

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

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

圖8 刪除冗余轉折點后的路徑

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

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

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

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

（2）若當前終點是最終目標點，則進行剎車距離判定，在到達終點時停止運動。

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

式中，H(v,w)用來評價機器人以當前的采樣速度，達到模擬軌跡末端時的朝向與機器人坐標系原點與下一個最優(yōu)節(jié)點連線之間的角度差θ，如圖9所示。D(v,w)為機器人在當前軌跡上與最近障礙物之間的距離，若此軌跡上沒有障礙物，則將其設為常數(shù)。V(v,w)用于評價當前軌跡速度。α、β、γ為加權系數(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ī)劃

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

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

圖12 機械工程學院樓三樓BIM模型

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

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

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

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

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

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

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

圖16 實驗平臺

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

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

6 結束語

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