融合改進A?和時間彈性帶的移動機器人路徑規劃算法

沈斯杰，田昕，袁千賀

（1 上海理工大學 光電信息與計算機工程學院，上海 200093；2 上海理工大學 理學院，上海 200093）

0 引言

隨著智能制造2025 和工業4.0 的提出，機器人在化工、紡織、食品、電廠、農業等領域應用得越來越多。路徑規劃是機器人控制系統的基礎問題，是機器人順利完成各項作業任務的前提條件［1］。

按照對周圍環境信息的熟悉程度的不同，路徑規劃主要可分為2 類：

（1）全局路徑規劃。是指在已知機器人的全局環境信息的條件下，找到一條起點和終點之間距離最短的路徑，且該路徑不經過任何障礙物［2］。常見的全局路徑規劃算法有：D?算法［3-4］、A?算法［5-6］、Dijkstra 算法［7-8］等。

（2）局部路徑規劃。描述了如何讓機器人遵循全局路徑，同時避免未知的障礙物［9］。常見的局部路徑規劃算法有：DWA 算法［10-11］、TEB 算法［12-13］、人工勢場法［14-15］等。

在實際應用中，單一算法在解決路徑規劃問題時，具有局限性，并且無法處理機器人在移動過程中面對的突發情況。因此，將不同算法進行融合成為當下路徑規劃中的研究熱點，通過算法之間的優劣互補，從而得到較優的規劃結果［16］。王志中［17］通過融合改進A?算法和人工勢場法，在復雜環境下實現實時避障，但規劃的路徑在障礙物處不夠平滑，會增加機器人轉彎和變速的負擔。Dai 等人［18］通過將改良的蟻群算法和A?算法融合，提升算法的收斂速度和全局路徑的平滑度，但是沒能顧及機器人在移動過程中出現的未知障礙物。龐磊等人［19］提出將A?與TEB 算法融合，幫助機器人實現對目標行人的安全無碰撞跟隨，然而A?算法規劃出的路徑不夠平滑，轉折點多，不適合機器人長距離移動。

針對上述情況，為了提高A?算法的路徑平滑度，同時滿足機器人在復雜環境下實時避障的需求，提出一種融合改進A?和TEB 算法的路徑規劃方法。首先，通過A?算法進行全局路徑規劃，得到適合機器人平穩運行的全局路徑。其次，為了使TEB算法更遵循全局路徑，增加了路徑點與TEB 軌跡位姿點的約束條件。最后，如果機器人在移動時通過自身攜帶的傳感器感知到全局路徑上有障礙物出現，則TEB 算法能夠實時調整局部路徑，從而繞開障礙物。完成避障后，繼續沿著全局路徑前行至目標點。

本文的貢獻可以歸納為：

（1）引入梯度下降算法的思想，減少路徑轉折點，從而提高路徑平滑度。

（2）添加了路徑點與TEB 軌跡位姿點的約束條件，使TEB 算法規劃的局部路徑更好地跟隨全局路徑。

（3）在復雜環境下，通過將改進A?算法與TEB算法相融合，有效躲避未知的障礙物。

1 改進A?算法

1.1 A?算法

A?算法作為一種在全局環境信息已知的情況下尋找最優路徑的啟發式算法，從起始點開始，將起始點作為父節點，通過向周邊擴展，計算得到周圍8個子節點的評價函數值，選取最小值子節點作為下一輪擴展的父節點，以此往復，直至到達目標點，從而得到最優路徑。其評價函數為：

其中，f（n）是機器人從起始點經過節點n到目標點的總代價值；g（n）是機器人從起始點到節點n的實際代價值；h（n）是機器人從節點n到目標點的估計代價值，又被稱作啟發函數；n是機器人當前所在的節點。

1.2 平滑路徑

A?算法雖然能夠在靜態環境中得到最優的全局路徑，但規劃的路徑存在曲折點較多的現象，會增加機器人轉彎、變速的負擔。針對這一情況，利用梯度下降法對搜索出的路徑進行平滑處理，從而使A?算法規劃的路徑更適合機器人在實際的場景下運行。

梯度下降法常應用于求解最小二乘法問題，本文中將對搜索出的路徑做平滑處理，其優點為計算量較少，能夠最小化所有樣本損失函數值，并且使結果為全局最優解或者在最優解附近［20］。梯度下降法作為迭代法的一種，利用逐步迭代的方法得到評價函數的最小值，從而實現擬合曲線的功能。

首先，將A?算法規劃出的路徑近似為曲線，并采用函數的形式表示：

其中，xn為數據樣本中第n個特征值，θn為第n個特征值的權重。

接著，引入一個假設函數y′，將一組特征值代入上式中，因為各個權重值尚未確定，故y′不等于y。假設函數y′的表達式為：

然后，將預測值y′與真實值y做差，為了保證差值是正數，再對差值取平方，即可得到損失函數ΔT（θ），其表達式為：

損失函數的值越小意味著，預測值和真實值之間的誤差越小，因此損失函數越小越好。針對原始路徑的數據樣本存在樣本數量較大的情況，采用均方差的形式來表示損失值，其表達式為：

其中，J（θ）為均方差損失，涵蓋了所有樣本數據的表現。為了便于后續計算，在等式的右邊乘以二分之一，即得：

梯度下降法采用微分的思路進行求解，將權重θ的取值范圍分割成無限份，每一份的寬度為動態?，其實際寬度由微分步長α決定，通過不斷改變權重值，使得損失值J（θ）趨近于0。

由微分公式可以變形成權重θ的迭代公式，其表達式為：

關于原始路徑的樣本數據，僅需要改變權重值的大小和迭代的頻次，就能獲得目標函數的最小值，從而達成平滑處理路徑的目的，如圖1 所示。

圖1 路徑平滑處理Fig.1 Path smoothing

圖1中，藍色的點劃線為原始路徑，紅色的虛線為通過梯度下降法平滑處理后的路徑。從圖1 中可以明顯看出，原始路徑轉折點較多、不光滑，而利用梯度下降的方法能有效減少轉折點，路徑平滑度得到大幅提高。

2 改進TEB 算法

2.1 TEB 算法

全局路徑規劃是針對靜態環境進行點到點之間的路徑規劃。如果路徑中突然出現障礙物，則無法躲避。而局部路徑規劃可以通過傳感器實時采集周圍環境信息，從而知道機器人在靜態環境中所處的位置和突然出現的障礙物的分布情況。

TEB 算法利用圖優化的思想，將影響彈性帶形變的機器人位姿點與經過相鄰兩位姿點所需的運動時間定義為需要優化的節點，并且將路徑起始點、機器人的速度、加速度和周圍環境信息的約束定義為需要優化的邊，采用g2o 開源框架進行求解。TEB算法的設計步驟詳見如下。

首先，將機器人的位姿定義為：

其中，xi和yi是機器人所處空間的位置坐標，βi為機器人運動過程中的朝向信息。機器人位姿組成的序列定義為：

其次，將機器人經過相鄰位姿所花費的時間定義為ΔTi，由一個個時間間隔組成的序列定義為：

然后，將TEB 算法所需要優化的目標表示為：

最后，將加權求和得到的全局約束函數f（B）對多目標進行優化，表示形式為：

其中，fk（B）為局部約束函數；γk為約束函數所對應的權值；B?為TEB 算法優化后滿足約束函數的最優解。

機器人的位姿與時間間隔的關系，如圖2 所示。

圖2 機器人的位姿和時間間隔的關系Fig.2 Relationship between pose and time interval of robots

2.2 構建TEB 超圖

超圖作為一種特殊的圖，可以更加準確地描述多因素關聯的對象之間的關系。TEB 算法的局部性會導致稀疏性系數矩陣問題的產生。這一問題可以轉換成由位姿和時間間隔作為節點、目標函數和約束函數作為邊構成的超圖［21］，如圖3 所示。這是在路徑規劃情況下，由TEB 算法的約束轉變而來的超圖。超圖的約束主要包含機器人自身的幾何約束、速度、加速度、時間最優和相鄰障礙物，圖3 中起始位姿、終點位姿以及障礙物的節點是不能變動的，用雙圈表示。

圖3 TEB 超圖Fig.3 TEB hyper graph

2.3 增加路徑點與TEB 軌跡位姿點的約束

移動機器人在轉彎時，TEB 算法規劃出的局部路徑易偏離全局路徑，與墻面或者障礙物距離較近。這無疑增加了機器人過彎時的危險性。為了緩解這種情況，在超圖中加入路徑點與TEB 軌跡位姿點的約束關系。路徑點的集合是在規劃范圍內全局路徑點集合根據相同步長求得的子集合。當TEB 算法在進行優化時，將會受到路徑點的約束，需要求解出與各個路徑點距離最短的TEB 軌跡位姿點。因此TEB 算法規劃出的路徑更好地遵循全局路徑。路徑點與位姿點之間的歐式距離，記作drpmin。那么路徑點的目標函數的表達式為：

此時，將路徑點與TEB 軌跡位姿點加入超圖中，新的超圖如圖4 所示。

圖4 改進后的TEB 超圖Fig.4 Improved TEB hyper graph

3 融合算法

A?算法可以在靜態地圖下得到全局最優路徑，但是不能實時躲避未知障礙物。TEB 算法作為局部路徑規劃算法，由于缺少全局環境信息，規劃的路徑可能不是最優路徑，并且存在得到錯誤路徑的風險。所以，通過融合改進A?與TEB 算法，不僅能夠得到全局最優路徑，而且可以實時避障。融合算法的流程如圖5 所示。

由圖5 可知，首先，通過改進A?算法對靜態地圖進行全局路徑規劃，得到適合機器人運行的平滑路徑。其次，將規劃的路徑離散化為時間序列和位姿序列，并將其作為TEB 算法的軌跡序列點。然后，通過g2o 庫求解得到最優路徑，作為局部路徑。如果該路徑無效，則重新使用A?算法進行全局路徑規劃。如果有效，則將速度指令發送給運動控制單元。最后，機器人沿著全局路徑，朝著目標點移動，如果當前位置為全局目標位姿，則路徑規劃任務完成。反之，則繼續進行局部路徑規劃。

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

4 仿真實驗驗證

4.1 改進A?算法仿真實驗

為了檢驗改進A?算法的有效性和可行性，本文在Ubuntu 系統下對相關算法進行仿真實驗。實驗平臺為戴爾工作站，配備Ubuntu16.04 操作系統和ROS（kinetic）機器人操作系統。仿真實驗所使用的計算機配置為：處理器是英特爾Xeon E5-1630 v4，主頻是3.7 GHz。

A?算法和改進A?算法的路徑規劃對比圖，如圖6 所示。為了保證實驗的可靠性，進行了多組仿真實驗，將多組數據取均值作為最終的結果，性能對比結果見表1。圖6 和表1 中的結果表明，未經過梯度下降算法平滑處理的路徑，在拐彎處轉折點較多，而通過改進A?算法規劃出的路徑，轉折點顯著減少，路徑平滑度得到大幅提升。同時也縮短了路徑長度，更適合機器人運行。

圖6 A?算法改進前后對比Fig.6 Comparison of A?algorithm before and after improvement

表1 A?算法與改進A?算法的性能指標對比Tab.1 Performance comparison between traditional A?algorithm and improved A?algorithm

4.2 改進TEB 仿真實驗

將路徑點與TEB 軌跡位姿點約束加入后，TEB的軌跡優化如圖7 所示。圖7中，綠色線段為全局路徑，藍色線段為已行駛軌跡，紅色線段是TEB 算法規劃的局部路徑，藍色的小方塊是路徑點。各路徑點的間距為0.25 m，由這些路徑點構成的全局路徑的子集的長度不超過1.5 m。TEB 算法和改進TEB 算法的移動軌跡對比圖，如圖8 所示，算法改進前后與各拐彎處的最短距離，見表2。圖8 和表2中的結果表明，原始TEB 算法規劃的最優軌跡在過彎時，容易偏離全局路徑，與墻面距離較近；而改進后的TEB 算法與全局路徑能保持較高的吻合度，在轉彎時，也能較好地跟隨全局路徑，仍與墻面保持了足夠的距離。

表2 算法改進前后與轉角的距離Tab.2 Distance from the corner before and after algorithm improvement

圖7 改進后TEB 算法路徑規劃過程Fig.7 Path planning process of improved TEB algorithm

圖8 TEB 算法改進前后對比Fig.8 Comparison of TEB algorithm before and after improvement

4.3 融合算法仿真實驗

移動機器人的工作場景復雜多變，因此接下去將在仿真環境下，驗證融合算法面對未知障礙物時的避障性能。通過改進A?算法得到平滑的全局路徑后，將其離散化為時間序列和位姿序列，在此基礎上，改進TEB 算法進行局部路徑規劃。若攜帶的激光雷達傳感器感知到周圍環境中存在未知障礙物，TEB 算法規劃的局部路徑可修正全局路徑，從而達到實時避障的效果。為了檢驗融合算法的避障能力，在gazebo 仿真環境下進行移動機器人避障實驗，仿真環境如圖9 所示。

圖9 gazebo 仿真環境Fig.9 gazebo simulation environment

仿真實驗的過程為：首先確定機器人起始點與坐標點，然后通過A?算法規劃得到全局路徑，接著，在機器人沿著全局路徑前行的過程中，在全局路徑上加入2 個未知障礙物。機器人通過自身攜帶的激光雷達傳感器感知到障礙物，并通過改進TEB 算法規劃局部路徑，修正全局路徑，繞過障礙物。由于具有不同的路徑規劃空間，全局路徑規劃算法以1 Hz的頻率運行，局部路徑規劃算法以5 Hz的頻率運行。移動機器人避障過程如圖10 所示。

圖10 融合算法的避障過程Fig.10 Obstacle avoidance process of fusion algorithm

圖10中，藍線是機器人已行駛的軌跡，黑線是改進A?算法規劃的全局路徑，黃線是TEB 算法規劃的局部路徑，紅圈圈出的是未知的障礙物。可以看到，改進A?算法可以規劃出較為平滑的全局路徑，使機器人能沿著全局路徑移動。當機器人靠近障礙物時，TEB 算法規劃的局部路徑能夠修正全局路徑，從而繞過未知的障礙物。最終，機器人順利到達目標位置。從機器人行駛的軌跡可以看出，融合改進A?和TEB 的路徑規劃算法可以規劃出最優路徑，且能夠躲避機器人移動過程中出現的未知障礙物，在躲避障礙物時，與障礙物也能保持足夠的安全距離。

5 結束語

本文在A?算法的基礎上，提出了一種融合改進A?和TEB 的移動機器人路徑規劃算法，有效改善了A?算法環境適應性差及路徑規劃不平滑的問題。在A?算法中，通過梯度下降算法思想，減少大量轉折點，使規劃的路徑更平滑，更符合機器人實際運行的情況；在TEB 算法中，通過增加路徑點與TEB 軌跡位姿點的約束，使其規劃的路徑能更好地遵循全局路徑；與TEB 算法相融合，使其具備了實時避障的能力，面對周圍環境中出現的未知障礙物，能夠有效避障。仿真實驗結果表明，所提出的融合算法在移動機器人路徑規劃中具有一定的可行性和有效性。

在未來的研究工作中，會考慮減少TEB 算法中優化變量的數量，以此降低TEB 算法在局部路徑規劃中占用的計算資源，并會將融合算法應用于實際機器人中。