基于TEB 算法的移動(dòng)機(jī)器人運(yùn)動(dòng)軌跡規(guī)劃研究

高熙強(qiáng)

（200082 上海市 上海理工大學(xué) 機(jī)械工程學(xué)院）

0 引言

隨著現(xiàn)代科技的快速發(fā)展，移動(dòng)機(jī)器人自主導(dǎo)航技術(shù)已逐漸應(yīng)用在多個(gè)領(lǐng)域中，但要實(shí)現(xiàn)機(jī)器人更高效的自主性，還存在很多待解決的問(wèn)題。當(dāng)前，全局路徑和局部路徑規(guī)劃是實(shí)現(xiàn)機(jī)器人自主導(dǎo)航的關(guān)鍵，針對(duì)全局路徑規(guī)劃是給定起始點(diǎn)和終點(diǎn)，在全局地圖上規(guī)劃出一條最優(yōu)路線，但由于受到地圖障礙物信息而造成運(yùn)動(dòng)速度和方向的改變，機(jī)器人往往會(huì)偏離這條路線，甚至?xí)プ灾餍?，需要采用更高效、穩(wěn)定的局部路徑算法規(guī)劃?rùn)C(jī)器人更好地跟蹤全局路線[1]。本文引進(jìn)TEB 局部路徑算法，將全局路線優(yōu)化為基于時(shí)間間隔的位姿序列，設(shè)定約束目標(biāo)后實(shí)現(xiàn)了機(jī)器人自主導(dǎo)航邊運(yùn)動(dòng)邊規(guī)劃，最終規(guī)劃出一條滿足約束條件的最優(yōu)運(yùn)動(dòng)軌跡，通過(guò)與全局路線進(jìn)行對(duì)比，驗(yàn)證了該算法的有效性。

1 阿克曼運(yùn)動(dòng)學(xué)模型的建立

移動(dòng)機(jī)器人采用阿克曼運(yùn)動(dòng)原理實(shí)現(xiàn)運(yùn)動(dòng)軌跡規(guī)劃研究。如圖1 所示建立了移動(dòng)機(jī)器人在慣性坐標(biāo)系下的轉(zhuǎn)向模型。

圖1 阿克曼運(yùn)動(dòng)學(xué)模型Fig.1 Ackermann kinematics model

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

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

阿克曼模型運(yùn)動(dòng)學(xué)方程表示為

式中：[X，Y，θ]T——機(jī)器人位姿狀態(tài)，——位姿狀態(tài)的導(dǎo)數(shù)。

2 TEB 局部算法的描述

2.1 TEB 算法位姿定義

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

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

則兩位姿的時(shí)間序列為

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

2.2 TEB 約束目標(biāo)函數(shù)

機(jī)器人可以跟隨全局路徑和避障，跟隨則拉近全局路徑，避障則遠(yuǎn)離障礙物?？啥x：位姿序列或障礙物的最近距離為dmin，j，約束位姿點(diǎn)距全局路徑的最大距離為rpmax，約束位姿點(diǎn)距障礙物的最小距離為romin，ε，S，n 為取值常數(shù)，則跟隨路徑和避障約束的懲罰函數(shù)為

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

考慮到非完整機(jī)器人的位姿狀態(tài)是做弧線運(yùn)動(dòng)，可得機(jī)器人在世界坐標(biāo)系上與航向角θi，θi+1有關(guān)的代價(jià)函數(shù)為

目標(biāo)函數(shù)依賴于最小化時(shí)間間隔的平方，使得機(jī)器人獲得最快路徑，約束函數(shù)可表示為

2.3 TEB 算法的實(shí)現(xiàn)

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

圖3 TEB 算法規(guī)劃流程圖Fig.3 TEB algorithm planning flow chart

3 基于TEB 局部路徑算法的運(yùn)動(dòng)規(guī)劃

本次規(guī)劃基于ROS 系統(tǒng)，實(shí)現(xiàn)各接口之間的通信。在圖4 運(yùn)動(dòng)功能框架中，move base 功能包通過(guò)訂閱動(dòng)作、發(fā)布動(dòng)作、訂閱話題、發(fā)布話題以及提供服務(wù)來(lái)實(shí)現(xiàn)運(yùn)動(dòng)規(guī)劃的主要運(yùn)行、交互接口。

圖4 運(yùn)動(dòng)功能框架Fig.4 Motion function framework

為保證運(yùn)動(dòng)規(guī)劃的準(zhǔn)確性，在地圖中精確定位，再由地圖服務(wù)器生成的全局代價(jià)地圖和目標(biāo)位置提供給全局路徑規(guī)劃，維護(hù)靜態(tài)的全局代價(jià)地圖給全局路徑規(guī)劃，動(dòng)態(tài)的本地代價(jià)地圖給局部路徑規(guī)劃，訂閱運(yùn)動(dòng)規(guī)劃目標(biāo)位置，讓全局路徑規(guī)劃器規(guī)劃全局路徑，再將全局路徑發(fā)送給局部路徑規(guī)劃器，結(jié)合采集到的深度信息、里程計(jì)信息和TF 坐標(biāo)變換進(jìn)行最優(yōu)軌跡規(guī)劃，輸出控制機(jī)器人線速度和角速度的指令，發(fā)布給機(jī)器人控制器，實(shí)現(xiàn)移動(dòng)機(jī)器人的導(dǎo)航運(yùn)動(dòng)。

在move_base 節(jié)點(diǎn)中創(chuàng)建的配置文件應(yīng)包含全局代價(jià)地圖配置文件、本地代價(jià)地圖配置文件、全局規(guī)劃配置文件、本地規(guī)劃配置文件和本地規(guī)劃器配置文件?？紤]到采用TEB 算法來(lái)進(jìn)行本地規(guī)劃器文件配置，可在rqt_reconfigure 參數(shù)調(diào)節(jié)器配置合理參數(shù)。在參數(shù)配置中，涉及機(jī)器人配置、軌跡配置、障礙配置、目標(biāo)公差配置、優(yōu)化配置和恢復(fù)配置，使移動(dòng)機(jī)器人運(yùn)動(dòng)達(dá)到很好的效果。

4 實(shí)驗(yàn)與結(jié)果分析

本次實(shí)驗(yàn)仿真對(duì)象為阿克曼移動(dòng)機(jī)器人。加載移動(dòng)機(jī)器人到添加障礙物的gazebo 中，如圖5 所示，啟動(dòng)后能查看移動(dòng)機(jī)器人的運(yùn)動(dòng)仿真環(huán)境。同時(shí)啟動(dòng)gazebo 和具有g(shù)mapping 節(jié)點(diǎn)的rviz 界面，控制移動(dòng)機(jī)器人在gazebo 仿真環(huán)境中移動(dòng)，建立運(yùn)動(dòng)場(chǎng)景柵格地圖，如圖6 所示。

圖5 Gazebo 仿真環(huán)境Fig.5 Gazebo emulation environment

圖6 運(yùn)動(dòng)場(chǎng)景Fig.6 Motion scene

之后，在場(chǎng)景地圖中做移動(dòng)機(jī)器人的運(yùn)動(dòng)規(guī)劃，如圖7 所示，圖7（a）—圖7（d）是整個(gè)運(yùn)動(dòng)規(guī)劃的過(guò)程。在圖7（a）中確定了起點(diǎn)和終點(diǎn)，規(guī)劃了全局路線，且通過(guò)TEB 局部規(guī)劃生成初始的局部路徑；圖7（b）和圖7（c）是移動(dòng)機(jī)器人在運(yùn)動(dòng)過(guò)程中實(shí)現(xiàn)了邊運(yùn)動(dòng)邊規(guī)劃局部路徑，且能跟隨全局路線；圖7（d）表示機(jī)器人能有效規(guī)劃臨近終點(diǎn)的路徑，使其能到達(dá)指定的終點(diǎn)，完成運(yùn)動(dòng)規(guī)劃。

圖7 運(yùn)動(dòng)過(guò)程Fig.7 Motion process

如圖8 所示，在整個(gè)運(yùn)動(dòng)過(guò)程中，由里程計(jì)記錄位姿，規(guī)劃出一條符合移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)且與全局路線相似的運(yùn)動(dòng)軌跡，驗(yàn)證了TEB 算法能跟隨全局路徑。

圖8 運(yùn)動(dòng)軌跡Fig.8 Motion trajectory

為了驗(yàn)證TEB 算法跟隨全局路徑的效果，在數(shù)據(jù)中選取規(guī)劃的8 個(gè)位姿點(diǎn)，將移動(dòng)機(jī)器人運(yùn)動(dòng)規(guī)劃的軌跡點(diǎn)與全局路徑點(diǎn)分別在x，y 方向上進(jìn)行對(duì)比。如圖9、圖10 所示，運(yùn)動(dòng)軌跡點(diǎn)與全局路徑點(diǎn)在x 方向的波動(dòng)較大，最大偏差為6 cm，是因?yàn)榫植克惴▽?shí)時(shí)規(guī)劃跟隨全局路徑，軌跡需要反復(fù)調(diào)整；在y 方向的偏差相對(duì)平穩(wěn)，低于5 cm，兩者都滿足該環(huán)境下的導(dǎo)航要求，可驗(yàn)證TEB 算法能有效跟隨全局路徑，實(shí)現(xiàn)移動(dòng)機(jī)器人的自主穩(wěn)定運(yùn)動(dòng)。

圖9 X 方向位姿點(diǎn)Fig.9 Pose points in X direction

圖10 Y 方向位姿點(diǎn)Fig.10 Pose points in Y direction

5 結(jié)語(yǔ)

本文以移動(dòng)機(jī)器人為模型，針對(duì)移動(dòng)機(jī)器人在自主導(dǎo)航過(guò)程中會(huì)受到環(huán)境信息和物理因素的影響偏離全局路線，使得自主導(dǎo)航精準(zhǔn)度降低問(wèn)題，引進(jìn)TEB 局部路徑算法規(guī)劃全局路線，生成基于時(shí)間間隔的位姿序列，規(guī)劃了一條滿足約束條件的最優(yōu)運(yùn)動(dòng)軌跡；通過(guò)搭建運(yùn)動(dòng)功能框架，建立仿真環(huán)境進(jìn)行驗(yàn)證，結(jié)果表明TEB 算法規(guī)劃的運(yùn)動(dòng)軌跡能有效跟蹤全局路徑，提高了機(jī)器人自主導(dǎo)航的穩(wěn)定性。

本文的不足之處在于基于仿真構(gòu)建的場(chǎng)景地圖簡(jiǎn)單，規(guī)劃的全局路徑和局部路徑較短。針對(duì)復(fù)雜地形下真實(shí)移動(dòng)機(jī)器人的自主穩(wěn)定運(yùn)動(dòng)，實(shí)際偏差會(huì)有一定的不同，后續(xù)將做進(jìn)一步研究。