沈斯杰，田昕，袁千賀

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

0 引言

隨著智能制造2025 和工業(yè)4.0 的提出，機器人在化工、紡織、食品、電廠、農(nóng)業(yè)等領(lǐng)域應(yīng)用得越來越多。路徑規(guī)劃是機器人控制系統(tǒng)的基礎(chǔ)問題，是機器人順利完成各項作業(yè)任務(wù)的前提條件［1］。

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

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

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

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

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

本文的貢獻可以歸納為：

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

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

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

1 改進A?算法

1.1 A?算法

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

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

1.2 平滑路徑

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

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

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

其中，xn為數(shù)據(jù)樣本中第n個特征值，θn為第n個特征值的權(quán)重。

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

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

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

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

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

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

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

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

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

2 改進TEB 算法

2.1 TEB 算法

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

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

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

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

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

然后，將TEB 算法所需要優(yōu)化的目標表示為：

最后，將加權(quán)求和得到的全局約束函數(shù)f（B）對多目標進行優(yōu)化，表示形式為：

其中，fk（B）為局部約束函數(shù)；γk為約束函數(shù)所對應(yīng)的權(quán)值；B?為TEB 算法優(yōu)化后滿足約束函數(shù)的最優(yōu)解。

機器人的位姿與時間間隔的關(guān)系，如圖2 所示。

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

2.2 構(gòu)建TEB 超圖

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

圖3 TEB 超圖Fig.3 TEB hyper graph

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

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

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

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

3 融合算法

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

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

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

4 仿真實驗驗證

4.1 改進A?算法仿真實驗

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

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

圖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的軌跡優(yōu)化如圖7 所示。圖7中，綠色線段為全局路徑，藍色線段為已行駛軌跡，紅色線段是TEB 算法規(guī)劃的局部路徑，藍色的小方塊是路徑點。各路徑點的間距為0.25 m，由這些路徑點構(gòu)成的全局路徑的子集的長度不超過1.5 m。TEB 算法和改進TEB 算法的移動軌跡對比圖，如圖8 所示，算法改進前后與各拐彎處的最短距離，見表2。圖8 和表2中的結(jié)果表明，原始TEB 算法規(guī)劃的最優(yōu)軌跡在過彎時，容易偏離全局路徑，與墻面距離較近；而改進后的TEB 算法與全局路徑能保持較高的吻合度，在轉(zhuǎn)彎時，也能較好地跟隨全局路徑，仍與墻面保持了足夠的距離。

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

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

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

4.3 融合算法仿真實驗

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

圖9 gazebo 仿真環(huán)境Fig.9 gazebo simulation environment

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

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

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

5 結(jié)束語

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

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