基于TEB 算法的移動機器人運動軌跡規劃研究

高熙強

（200082 上海市 上海理工大學 機械工程學院）

0 引言

隨著現代科技的快速發展，移動機器人自主導航技術已逐漸應用在多個領域中，但要實現機器人更高效的自主性，還存在很多待解決的問題。當前，全局路徑和局部路徑規劃是實現機器人自主導航的關鍵，針對全局路徑規劃是給定起始點和終點，在全局地圖上規劃出一條最優路線，但由于受到地圖障礙物信息而造成運動速度和方向的改變，機器人往往會偏離這條路線，甚至會失去自主性，需要采用更高效、穩定的局部路徑算法規劃機器人更好地跟蹤全局路線[1]。本文引進TEB 局部路徑算法，將全局路線優化為基于時間間隔的位姿序列，設定約束目標后實現了機器人自主導航邊運動邊規劃，最終規劃出一條滿足約束條件的最優運動軌跡，通過與全局路線進行對比，驗證了該算法的有效性。

1 阿克曼運動學模型的建立

移動機器人采用阿克曼運動原理實現運動軌跡規劃研究。如圖1 所示建立了移動機器人在慣性坐標系下的轉向模型。

圖1 阿克曼運動學模型Fig.1 Ackermann kinematics model

在慣性坐標系XOY 下，O 表示機器人的瞬時旋轉中心，v 表示后軸中心速度，δ表示前輪虛擬轉向角，θ表示航向角，R 表示轉彎半徑，L 表示前后軸距離，l 表示輪距。其中阿克曼模型需滿足轉彎半徑的條件為

式中：ω——后軸橫擺角。

阿克曼模型運動學方程表示為

式中：[X，Y，θ]T——機器人位姿狀態，——位姿狀態的導數。

2 TEB 局部算法的描述

2.1 TEB 算法位姿定義

TEB 將全局路徑轉化為基于離散時間的軌跡點，形成位姿軌跡。TEB 位姿序列的描述如圖2 所示。xi表示機器人下的第i 位姿；ΔTi表示xi到xi+1的時間間隔，則機器人在世界坐標系下的位姿序列為

圖2 TEB 位姿序列Fig.2 Pose sequence of TEB

則兩位姿的時間序列為

將位姿序列和時間序列合并生成TEB 軌跡信息為

2.2 TEB 約束目標函數

機器人可以跟隨全局路徑和避障，跟隨則拉近全局路徑，避障則遠離障礙物。可定義：位姿序列或障礙物的最近距離為dmin，j，約束位姿點距全局路徑的最大距離為rpmax，約束位姿點距障礙物的最小距離為romin，ε，S，n 為取值常數，則跟隨路徑和避障約束的懲罰函數為

速度和加速度可用以上同樣的約束表示。vi，ωi分別為相鄰位姿和時間間隔計算得到的平均線速度和角速度，則懲罰函數為

考慮到非完整機器人的位姿狀態是做弧線運動，可得機器人在世界坐標系上與航向角θi，θi+1有關的代價函數為

目標函數依賴于最小化時間間隔的平方，使得機器人獲得最快路徑，約束函數可表示為

2.3 TEB 算法的實現

TEB 算法規劃流程如圖3 所示。根據給定的全局路徑，初始化成基于時間間隔的位姿序列，然后進行迭代求解循環。插入新的位姿，刪除已經初始化的位姿。更新獲取路徑點和障礙物信息。建立位姿序列、時間序列和約束目標函數的超圖形式，迭代求解出TEB 軌跡，并檢查軌跡是否可行。根據相鄰軌跡點計算出機器人的運動軌跡，使機器人到達目標點。

圖3 TEB 算法規劃流程圖Fig.3 TEB algorithm planning flow chart

3 基于TEB 局部路徑算法的運動規劃

本次規劃基于ROS 系統，實現各接口之間的通信。在圖4 運動功能框架中，move base 功能包通過訂閱動作、發布動作、訂閱話題、發布話題以及提供服務來實現運動規劃的主要運行、交互接口。

圖4 運動功能框架Fig.4 Motion function framework

為保證運動規劃的準確性，在地圖中精確定位，再由地圖服務器生成的全局代價地圖和目標位置提供給全局路徑規劃，維護靜態的全局代價地圖給全局路徑規劃，動態的本地代價地圖給局部路徑規劃，訂閱運動規劃目標位置，讓全局路徑規劃器規劃全局路徑，再將全局路徑發送給局部路徑規劃器，結合采集到的深度信息、里程計信息和TF 坐標變換進行最優軌跡規劃，輸出控制機器人線速度和角速度的指令，發布給機器人控制器，實現移動機器人的導航運動。

在move_base 節點中創建的配置文件應包含全局代價地圖配置文件、本地代價地圖配置文件、全局規劃配置文件、本地規劃配置文件和本地規劃器配置文件。考慮到采用TEB 算法來進行本地規劃器文件配置，可在rqt_reconfigure 參數調節器配置合理參數。在參數配置中，涉及機器人配置、軌跡配置、障礙配置、目標公差配置、優化配置和恢復配置，使移動機器人運動達到很好的效果。

4 實驗與結果分析

本次實驗仿真對象為阿克曼移動機器人。加載移動機器人到添加障礙物的gazebo 中，如圖5 所示，啟動后能查看移動機器人的運動仿真環境。同時啟動gazebo 和具有gmapping 節點的rviz 界面，控制移動機器人在gazebo 仿真環境中移動，建立運動場景柵格地圖，如圖6 所示。

圖5 Gazebo 仿真環境Fig.5 Gazebo emulation environment

圖6 運動場景Fig.6 Motion scene

之后，在場景地圖中做移動機器人的運動規劃，如圖7 所示，圖7（a）—圖7（d）是整個運動規劃的過程。在圖7（a）中確定了起點和終點，規劃了全局路線，且通過TEB 局部規劃生成初始的局部路徑；圖7（b）和圖7（c）是移動機器人在運動過程中實現了邊運動邊規劃局部路徑，且能跟隨全局路線；圖7（d）表示機器人能有效規劃臨近終點的路徑，使其能到達指定的終點，完成運動規劃。

圖7 運動過程Fig.7 Motion process

如圖8 所示，在整個運動過程中，由里程計記錄位姿，規劃出一條符合移動機器人運動學且與全局路線相似的運動軌跡，驗證了TEB 算法能跟隨全局路徑。

圖8 運動軌跡Fig.8 Motion trajectory

為了驗證TEB 算法跟隨全局路徑的效果，在數據中選取規劃的8 個位姿點，將移動機器人運動規劃的軌跡點與全局路徑點分別在x，y 方向上進行對比。如圖9、圖10 所示，運動軌跡點與全局路徑點在x 方向的波動較大，最大偏差為6 cm，是因為局部算法實時規劃跟隨全局路徑，軌跡需要反復調整；在y 方向的偏差相對平穩，低于5 cm，兩者都滿足該環境下的導航要求，可驗證TEB 算法能有效跟隨全局路徑，實現移動機器人的自主穩定運動。

圖9 X 方向位姿點Fig.9 Pose points in X direction

圖10 Y 方向位姿點Fig.10 Pose points in Y direction

5 結語

本文以移動機器人為模型，針對移動機器人在自主導航過程中會受到環境信息和物理因素的影響偏離全局路線，使得自主導航精準度降低問題，引進TEB 局部路徑算法規劃全局路線，生成基于時間間隔的位姿序列，規劃了一條滿足約束條件的最優運動軌跡；通過搭建運動功能框架，建立仿真環境進行驗證，結果表明TEB 算法規劃的運動軌跡能有效跟蹤全局路徑，提高了機器人自主導航的穩定性。

本文的不足之處在于基于仿真構建的場景地圖簡單，規劃的全局路徑和局部路徑較短。針對復雜地形下真實移動機器人的自主穩定運動，實際偏差會有一定的不同，后續將做進一步研究。