建筑機器人融合改進的A*與TEB算法的運動規劃研究

徐定明，李子信，張怡俊

（上海工程技術大學 機械與汽車工程學院，上海 201620）

0 引言

目前，建筑業正積極推行智能化、對象集約化等新型建造方式，智能建造環境下移動作業機器人就是該領域中較好的解決方案之一，涉及到避障、路徑規劃、定位、導航、傳感和通信等關鍵技術。其中，運動控制和路徑規劃問題被認為是復雜的任務［1］。

Zhu 等人［2］提出了基于深度學習的方法和導航框架。張賀等人［3］提出一種基于速度斥力場改進人工勢場方法的全向移動機器人自主動態避障方法，并驗證了自主動態避障算法的有效性和實用性。胡杰等人［4］改進了傳統人工勢場法在路徑規劃中存在的缺陷，將引力函數分段，避免引力過大而碰到障礙物的情況。Kashyap 等人［5］概述并實現一種新型的仿人機器人混合控制器來繪制最優路徑，采用人工勢場和飛蛾火焰優化混合方法對仿人機器人進行導航。劉子毅等人［6］提出了一種應用BIM（Building Information Model）信息與激光雷達獲取的現實環境信息進行導航并保證路徑最優的方法。Agarwal 等人［7］基于黏菌的改進群智能算法在Matlab 2020a 驗證出在生成最佳無碰撞路徑上花費的時間和精力更少。李承遠［8］對TEB 算法進行改進，提出了減少優化變量的數量，使用參數三次樣條插值的方法將路徑點插值成一條平滑的路徑曲線的方法，有效地降低了動態環境下TEB 算法的計算成本，解決了算法實時性能較低的問題。Mugarza 等人［9］提出了一種基于改進D*的交通控制器，用于動態確定AGV 的無碰撞軌跡。勞彩蓮等人［10］根據溫室環境下移動機器人作業的實時路徑規劃要求，提出一種基于改進A*算法與動態窗口法相結合的溫室機器人路徑規劃算法，跟蹤誤差保持在0.22 m以內、定位誤差不大于0.28 m，能夠滿足實際需求。柴紅杰等人［11］在障礙物邊緣放置虛擬障礙物形成緩沖區來改進A*算法，減少機器人碰撞幾率，提高路徑規劃效率和機器人穩定性。Qi 等人［12］提出的MOD-RRT*可以生成更高質量的初始路徑。Lyu 等人［13］提出了一種新的基于圖的機器人路徑優化算法，其中關鍵是動態分配權值的Floyd 算法，并證明該方法的有效性和魯棒性。

綜上，本文通過減少擴建節點方向改變和次數，以及根據障礙物占比率和復雜程度優化啟發式函數。改進的A*算法融合TEB 算法進行建筑機器人的運動控制，高效完成建筑機器人近距離停靠在平行于打孔位置的墻面的任務，驗證可行性。

1 傳統A*算法與TEB 算法

1.1 傳統A*算法

傳統A*算法流程如圖1 所示。圖1 中，柵格化環境地圖的節點信息全部加入到queue隊列中，初始化所有的節點信息。分別設置起始點到當前點的真實代價值G（n）和當前點到目標點的估算代價值H（n），隨后進行小頂堆排序，得到總代價值F（n）最小節點坐標的索引。判斷當前索引值是否等于目標坐標點的索引值，如果相等，到達終點，可以結束迭代求解。如果不等，繼續求解當前節點相鄰4 個方向節點的F（n）值，更新容器的節點代價值，排序找到最小值循環求解，直至到達目標點為止。在此基礎上根據已經擴展好的節點值，從目標點反向依次找到F（n）最小的值，也就找到了一條新路徑。

圖1 A*算法流程圖Fig. 1 Flowchart of A* algorithm

1.2 局部路徑規劃TEB 算法

A*算法生成全局路徑，TEB 路徑規劃期接收全局路徑，同時激光雷達負責定位以及獲得障礙物信息，以一定的頻率進行局部路徑規劃，計算控制指令并下發給颯智機器人下位機控制器。

1.2.1 機器人運動模型

圖2 為機器人運動學模型。由圖2 可知，6 個輪中2 個驅動輪分別位于兩側中心處，（Xw，Yw，Zw）為全局坐標系，（Xr，Yr，Zr）為機器人基坐標系，坐標中心點在兩驅動輪連線的中心軸線中點處，機器人前輪虛擬轉向角等于中心轉向角φ，為機器人姿態變化值，可得其求解方式如下所示：

圖2 機器人運動學模型Fig. 2 Kinematic model of the robot

從上式得出，給定機器人輸入u ＝（v（t），ω（t））T，即可得到機器人姿態變化以及求解出左右驅動輪編碼器的控制指令ωL（t）和ωR（t），從而驅動小車到達目標位置。

1.2.2 TEB 軌跡模型

TEB 模型示意如圖3 所示。由圖3 可知，世界坐標系下每個機器人的位姿可表示為Si ＝［xi，yi，φi］，一系列的位姿點組成了彈性帶C，并可由下式進行數學描述：

位姿Si到Si＋1的時間為ΔTi，其數學表述為：

機器人從位姿Si經過時間ΔTi運動到Si＋1便組成了時間彈性帶，詳見圖3。

圖3 TEB 模型示意圖Fig. 3 Schematic diagram of TEB model

為了優化運動軌跡，假設軌跡信息T：＝（C，Δτ），對軌跡加一定約束，由兩大部分組成。第一部分包括速度、角速度、線加速度、角加速度的約束以及自身運動學的限制；第二部分則是與障礙物的距離以及與全局路徑的偏離程度。Si處的速度約束求解方程分別見式（1）：

加速度相應的約束是由2 個連續速度的3 個位姿求解而來，可表示為：

非完整約束的差分驅動下相鄰的2 個機器人位姿可以看成弧線運動，如圖4 所示，Si與Si＋1在曲率恒定的圓弧上。

圖4 差分驅動求解圖Fig. 4 Differential drive solution diagram

圖4 中，［xi，yi，φi］、［xi＋1，yi＋1，φi＋1］ 是位姿；di，i＋1是圓的弦長；φi與φi＋1分別是全局坐標系下的轉角。由此推出運動學約束的代價函數為：

颯智機器人的轉彎半徑約束在最小半徑之上，此處需用到的公式為：

此外，還有障礙物代價函數、時間最優的目標函數等在機器人行走過程中起約束作用，不再贅述。

1.2.3 算法求解

算法求解流程如圖5 所示，對全局路徑｛Zj｝ 做離散化處理，成為帶有時間信息的位姿序列T（C，τ），每次迭代刪除求解過的位姿和插入新的狀態點。建立起T（C，τ）與目標函數以及約束函數之間的關系，求解最優軌跡T*，通過最小化一個非線性最小二乘代價函數求得：

圖5 TEB 求解流程圖Fig. 5 Flowchart of TEB solution

其中，fP表示速度、加速度等懲罰函數；fT表示障礙物、路徑等目標函數；σk表示配置的權重；T／｛S1，Sn｝ 表示起始和終止位姿S1與Sn不受優化。TEB 優化問題可以轉化成超圖問題。求解出來的結果T*，可以計算輸入的控制變量u ＝（v（t），ω（t））T，相應地求解出左右驅動輪編碼器的控制指令ωL（t）和ωR（t），從而驅動小車到達目標位置。

2 A*算法改進與TEB 算法融合

2.1 改進A*算法

2.1.1 A*算法改進策略

首先，從流程圖可以看出，在循環找到目標點的過程中，尋找擴展節點是找到F（n）最小的擴展方向，擴展方向的頻繁改變會導致生成的路徑曲折，機器人運行卡頓，對工業應用機器人效率影響十分嚴重。由于是搜索queue容器里所有節點最小值，搜索過程中可能有不同的路徑方向，所以只針對同一路徑條件下擴展方向進行研究。這里對每個轉向節點進行判斷，如果最小節點方向與原路徑方向下一節點代價值相差不大，則認定沒有必要進行轉向。

其次，為了擴大工業上移動機器人的運行效率，配合上文對啟發式函數H（n）的計算方法進行改進。3 種啟發式函數的計算方式，研究后可知，曼哈頓距離最大，歐幾里得距離次之，切比雪夫距離最小。H（n）值越大，則搜索路徑的目的性越強，但環境地圖障礙物太多會導致搜索路徑頻頻碰壁，效率反而不高，因此計算合適的啟發式函數值是必要的。

為了合理優化H（n），更好地描述障礙物狀態，引入障礙物占比系數K和離散率系數D，分別表示障礙物柵格數占環境地圖總柵格數比重和障礙物分散程度。2 個系數可以很好地表征環境地圖障礙物多少以及復雜程度等信息。相應數學公式具體如下：

其中，（xs，ys）是起始點；（xt，yt）是目標點；N表示障礙物柵格數。H（n）選用曼哈頓距離，評價函數F（n）相應發生變化。此處用到的數學公式可寫為：

由式（13）、式（14）可知，障礙物越多、越分散，則D的值越小，其柵格數N也越大，K值越大，1-lnK變小，將會擴展更多搜索方向，適應復雜環境。

2.1.2 仿真驗證對比分析

對機器人的路徑規劃算法進行仿真，驗證算法的有效性。根據國家標準中機器人移動能力算法測評要求建立仿真環境場景。仿真試驗場景如圖6 所示。地面為水平地面，包含4 面墻，墻內面積為2 500 m2，墻高度為2 500 mm，包含天花板，天花板高度為2 500 mm，顏色為白色，包含4 根圓柱和A、B、C、D、E、F、G、H共8 個物體，4 根圓柱的位置固定，A -H坐標非固定，這8 個物體相對均勻地分布于4 面墻內，且彼此之間并不接觸。圓柱及其它物體規格見表1。

圖6 仿真場景模擬圖Fig. 6 Simulation scene simulation diagram

表1 仿真場景物體尺寸表Tab.1 Objects size table of simulation scene

柵格化環境地圖如圖7 所示。在圖7 中，模擬實際導航，機器人長1 000 mm，且可將之看成機器人中心點的移動，障礙物需膨脹500 mm 長度，膨脹部分稱為代價地圖。仿真計算結果見表2。從仿真結果來看，改進A*算法相對于傳統A*算法在計算時間和拐點數上都有較大的改善。從表2 中的實驗仿真數據可以看出，改進A*算法有較好的實時性，路徑平滑性更好，便于機器人較好地沿著路徑軌跡移動到目標點。需通過實驗做進一步驗證。

圖7 仿真結果圖Fig. 7 Simulation results diagram

表2 仿真計算結果統計表Tab.2 Statistical table of simulation calculation results

2.2 TEB 參數優化

為了機器人導航避障效果更好，進行參數的一系列優化。研究可知，機器人加載了一系列的參數文件，其中teb_local_planner_params.yaml 等參數文件要著重考慮。并對機器人運行軌跡、機器人本體參數、打孔位置約束以及障礙物約束等參數進行修正。移動操作建筑機器人部分運動參數設置如下：最大線速度為0.5 m／s，最大角速度為0.6 rad／s，最大線加速度為0.5 m／s2，最大角加速度為0.5 rad／s2，轉角容許誤差為0.05 m，與障礙物間的最小距離為0.25 m，障礙物膨脹半徑為0.5 m。

2.3 融合策略

機器人采用精度較高的北陽UTS-10LX 激光雷達，使用cartographer 算法功能包進行環境柵格建圖，并通過雷達準確獲取周圍環境的深度信息，定位到自己的位置，結合時間彈性帶算法進行實時的局部路徑規劃，達到避障的目的，獲取機器人運動的最佳路徑。將改進的A*算法與TEB 相融合，以保證動態工作路徑的實時性和時效性。

根據已知的當前實驗室環境建立地圖，進行全局路徑規劃，從規劃的全局路徑中截取當前位置的一段，將之離散化為帶有時間信息的點，通過自身運動以及與障礙物代價函數的約束，求解到時間最優的位姿序列軌跡，并根據當前計算的速度以及角速度值，下發指令到AGV 左右驅動輪的編碼器，驅動小車高效到達目標位置。融合算法流程如圖8 所示。

圖8 融合算法流程圖Fig. 8 Flow chart of the fusion algorithm

3 機器人運動規劃實驗驗證與分析

3.1 實驗準備

在機器人上進行實驗驗證，使用平臺自帶的里程計以及北陽UTS-10LX 激光雷達感知周圍環境。且小車配有處理器為i7 八代的工控機作為主機，個人筆記本作為從機，從機配有i7 十代的虛擬機，主機和從機虛擬機都安裝有ubantu18.04 的操作系統和ROS（melodic）。在實驗室內網下，主機、從機以及從機虛擬機可通過IP 互相通信，從機和從機虛擬機可實現對主機的遠程控制。

在所搭建的實驗場景中，選用cartographer 算法功能包進行環境地圖的建模，運行程序節點如圖9所示。由圖9 可知，啟動teleop_twist_joy 為手柄控制節點以及cmdvel2can 為速度控制等節點，建立環境地圖。運行包內local.launch，通過對子圖的掃描匹配來實現定位，做好算法驗證前的建圖和定位準備。環境建圖如圖10 所示。

圖9 融合算法程序運行節點圖Fig. 9 Running nodes graph of the fusion algorithm program

圖10 cartographer 環境建圖Fig. 10 cartographer environment construction

3.2 實驗驗證

本文的融合算法適用于大范圍非結構化建筑環境，選擇實驗環境對算法可行性進行驗證。利用移動操作機器人分別對傳統A*算法和改進A*算法進行實驗。

3.2.1 傳統A*算法實驗驗證

現場實驗驗證圖如圖11 所示。圖11 中，白色點云是小車定位輪廓與環境地圖匹配點，1 指的黑色圓角矩形為移動機器人本體，2 指的黑色線條為改進A*算法生成的全局路徑，3 指的紅色線條為實時截取全局路徑生成的局部路徑。融合算法的實驗步驟描述如前文融合算法內容所述，加載使用調參cartographer 建好的柵格地圖，給定目標點位的坐標和方向，機器人生成全局路徑，TEB 算法實時截取全局路徑生成局部路徑，放置的桌子為4，可認為是建筑環境承重水泥柱，5 為打孔位置，實際驗證機器人繞過建筑環境水泥柱并近距離停靠在平行于墻面的打孔位置，觀察并記錄移動機器人到達后的位姿數據信息以及導航避障情況。

圖11 現場實驗驗證圖Fig. 11 Field experiment verification diagram

給定打孔位點，機器人運動規劃過程如圖12 所示。圖12 中，1 表示機器人本體，2 表示全局路徑，3表示局部路徑，4 表示水泥柱，5 表示墻面打孔位置。其中，采用柜子模擬建筑環境中的水泥承重柱。經過10 組實驗發現，實時生成的全局路徑局部有許多小齒狀拐點，影響機器人的運行效率。

圖12 建筑機器人運動規劃驗證實驗圖Fig. 12 Experimental diagram of construction robots motion planning verification

3.2.2 改進A*算法實驗驗證

同理，消除實驗數據的偶然性，共做10 組實驗。改進A*算法生成的全局路徑較傳統A*算法來說，減少了許多拐點，路徑更為平滑。到達的目標點和所需要的方向滿足建筑機器人位置和方向的要求，驗證該改進算法策略的可行性。

4 結束語

（1）提出改進的A*算法融合TEB 算法進行建筑機器人的運動控制，并進行了實驗驗證。高效完成建筑機器人近距離停靠在平行于打孔位置墻面的任務，驗證了建筑環境下該策略的有效性。其中，提出改進的A*算法，比傳統A*算法效率更高，路徑平滑性更好，便于建筑機器人生成較好的全局路徑軌跡。

（2）系統分析建筑機器人導航避障，提出進一步改進策略：可以通過優化建圖定位參數、比如環境子地圖匹配頻率等、減少小車定位變動次數，以減少小車導航避障過程中方向擺動，提高運動效率；也可通過研發算法繼續改進全局與局部路徑規劃算法，補償機器人運動誤差，以達到更好的導航效果。