基于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 算法规划的运动轨迹能有效跟踪全局路径，提高了机器人自主导航的稳定性。

本文的不足之处在于基于仿真构建的场景地图简单，规划的全局路径和局部路径较短。针对复杂地形下真实移动机器人的自主稳定运动，实际偏差会有一定的不同，后续将做进一步研究。