掌握TEB算法，才能让机器人走路更丝滑

　　导航模块，是实现机器人移动功能的重要能力，主要包括全局路径规划和局部路径规划。本文将带大家深度认识局部路径规划，以及TEB算法的规划原理和参数配置。01机器人局部路径规划
　　当我们旅游迷路了，想必大家都会毫不犹豫地——
　　打开手机，点开地图APP，开始「导航」。
　　其实，不止我们人类，机器人也会用到「导航」功能，只不过这个功能是它们自带的。
　　无论是在某底捞的送餐机器人，还是某朵酒店的配送机器人，又或者是智能工厂里的真·机器人搬运工……
　　都是机器人通过自己的导航模块，为自己规划出一条移动路径，然后控制底盘安全稳定地完成移动。
　　我们以机器人操作系统ROS为例，导航模块输出了Twist类型的速度指令后，将其下发到底盘，就成为了底盘运动的直接依据。
　　其中，最终向机器人底盘下发速度指令的部分，就是「局部路径规划」。
　　由于这个速度指令，不仅要遵循全局路径规划给出的路径，还需实时避开路径上的障碍物，导致算法需要频繁调用、高速响应，通常要将全局路径进行分割。
　　也就是说， 只考虑当前点附近的路径和障碍物进行规划。 「局部路径规划」因此而得名。 02 TEB算法规划原理
　　DWA、TEB、Trajectory Rollout是三种常用的局部路径规划算法。其中，TEB算法有很强的前瞻性，能够动态避障，表现较好，缺点是计算复杂——这也是其使用g2o算法优化计算量的原因。
　　TEB全称是Timed Elastic Band，译为时间橡皮筋。好比导航的起点和终点之间，拉了一条绷直的橡皮筋作为行走路线，如果路中间有障碍物，就会将橡皮筋撑开，影响行走时的路径。
　　就像这样：
　　当然，在实际情况中，机器人的行走路线会受到许多其他因素的影响，不会紧贴着障碍物表面。我们通常会将障碍物进行膨胀处理，让机器人的行走路线与真实的障碍物之间，保持一定的安全距离。
　　在TEB算法中，所有影响因素分为两类：约束和轨迹。
　　约束指的是速度和加速度的限制，这些往往由机器人本身决定；轨迹则指最短或最快的路径，或者与障碍物保持距离，又或者是与规划路径尽可能地跟随。
　　一般来说，TEB算法会通过参数的方式，调整各影响因素的权重，为每个影响因素设计奖励或者惩罚函数，然后对可行的路线进行打分，最后选择得分最高的局部路线下发速度指令。
　　这样规划出的路径，更符合小车的硬件结构以及运动学，能够让小车流畅地行驶和绕障，且行驶速度和到点时间都有着优秀表现。
　　但由于需考虑的参数较多，如何「合理地」配置参数，就成了最重要且最困难的一环。 03 如何为TEB算法配置参数？典型的「障碍物和中间点参数配置」
　　通常来说，我们会希望为机器人规划出一条远离障碍物，又尽可能经过中间点的路线，让机器人的行走路线更合理。基于此，我们可以在TEB算法中，对障碍物和中间点分别配置参数，并运用到惩罚函数中。
　　TEB与障碍物的最小距离演示，Xn为TEB路径点，Zj为障碍物
　　一方面，为远离障碍物，我们需要为机器人设置障碍物距离、障碍物膨胀距离，以及障碍物的权重等障碍物参数，对靠近障碍物的路线进行惩罚。
　　另一方面，为尽可能经过路线中间点，我们可以以相反的目的进行参数设置，如中间点权重、中间点间隔和中间点顺序等，对远离中间点的路线进行惩罚。
　　具体的惩罚公式如下：
　　（惩罚函数，d为TEB路径距离障碍物的实际最小距离，r为参数定义的最小距离）
　　由于障碍物和中间点相互独立，有时会出现障碍物阻挡在TEB路径，或中间点坐落在障碍物内部的情况。
　　这两种参数就像橡皮筋上的同一个点，受到相反方向两个力的牵制，一旦权衡控制不当，就会让机器人陷入局部最优困境，增大绕障时间，甚至让机器人撞向障碍物。
　　局部最优困境演示更复杂的「速度和加速度参数配置」
　　TEB算法还考虑到了机器人本身速度和加速度的限制，如对线速度、角速度、线加速度、角加速度各自的最大限制和到达最大值的权重进行参数配置。
　　首先，速度约束在TEB中能用固定时间间隔的特性解释。TEB路线中某一点的速度，可以由其前后点的距离推出：
　　（ ΔT 为TEB规定的两点间时间间隔）
　　同理，在得到速度之后，可以用同样的时间间隔推出加速度：
　　在差分驱动机器人上，车轮的速度可以表示为：
　　（ L 为机器人轴距的一半）
　　之后再进一步微分，就能得到车轮的加速度。最后，根据计算出的车轮速度与由最大速度制定的损失函数综合考虑，就能制定出固定时间下速度和位移合适的路线。 比某芙还丝滑的「运动学约束」
　　有些机器人底盘还会受到运动学约束，如差分驱动机器人，它在平面运动中只有两个自由度，只能在机器人朝向上执行运动。因此还需要进行运动学约束，规划出一条相对平滑的路线，让机器人走得更丝滑。
　　TEB路径规划就考虑到了这点，同样以差分驱动机器人为例：
　　（ βi 表示 i 点上机器人的绝对角度）
　　得出的方向向量为：
　　相应的目标函数为：
　　最后也是最根本的「最快路径轨迹约束」
　　最快路径轨迹约束，其实就是时间约束，需要对最优时间权重进行参数设置。即间接地对TEB路径点之间的时间间隔进行控制，从而影响最终到达时间。
　　如果TEB路径点之间的时间间隔是一致的，那么最终规划出的轨迹，就是约束后时间最短的路径轨迹。具体可以通过 最小化所有时间间隔之和的平方 来实现，这也是TEB算法最根本的特征和优势。
　　总的来说，我们在实践过程中，要根据TEB算法的规划原理，依次考虑障碍物和中间点、速度和加速度、运动学约束以及最快路径轨迹约束等参数配置，才能让机器人走出流畅又丝滑的步伐。
　　至于约束和轨迹，如何通过g2o——图优化算法来具体实施？我们之后再为大家拆解。