这篇论文将TEB算法做了一些改进，从而应用于 car-like robot 上。因此不熟悉TEB算法的读者可以先阅读IEEE Xplore Full-Text PDF:

目录

1 摘要

2 动力学模型

3 TEB 优化问题

3.1关于时间间隔

3.2 速度、加速度约束

3.3 避障

4 近似最小二乘优化

5 闭环预测控制

1 摘要

本文提出了一种新颖的通用公式TEB，用于类车机器人的高效在线运动规划。规划问题根据有限维和稀疏优化问题进行定义，该问题受制于机器人动力学约束和避障。控制动作隐式包含在优化的轨迹中。在动态环境中实现可靠的导航通过用状态反馈增强内部优化循环。这种预测控制方案具有实时性，并对机器人感知场内的障碍物具有响应能力。在大型和复杂的环境导航在纯跟踪算法中实现，通过向全局规划器请求中间点目标的方式。对初始全局路径的要求相当温和，不需要符合机器人运动学。 一个对比性的分析关于Reeds-shepp曲线与对原型汽车操作的调查可以发现这种方法的优点。

2 动力学模型

二自由度的自行车模型如下 

 汽车机器人的运动可以用一个运动序列来表示，同时每一个运动序列，输入的控制信号都是恒定的，也就是速度和前轮转向角恒定，这决定了机器人只能走一个曲率恒定的圆弧或者直线，既然曲线曲率恒定，我们可以用圆弧的性质

两个角相等我们可以用向量叉乘得到等式约束，同时这个等式也满足了原地转向，也就是dk等于0

我们也可以算出转弯半径，两个位置航向角之差就是运动圆弧的圆心角，因为两个位置很近，所以圆心角很小，再通过等价无穷小去近似替代就可以得到一个更简单的转弯半径计算公式。

3 TEB 优化问题

我们的目标是把一个汽车机器人从一个位置带到另一个位置，并在较短时间内，于是论文提出将每两个位置之间设置一个时间间隔，于是优化问题的决策变量就是n个位置和n-1个时间间隔，目标函数又被设计为时间间隔平方和，由下图可知，还需要很多约束，比如速度，加速度等

3.1关于时间间隔

时间间隔要有个最大值，使得时间能够完成比较合适的离散化，不会出现时间间隔忽大忽小的现象，影响后期跟踪，并且时间间隔最大值尽量小，保证转弯半径的估计可行。同时时间间隔的平方和目的让时间在n个运动序列之间均匀分布

3.2 速度、加速度约束

速度可以由圆心角乘转弯半径得到弧长，再通过转弯半径的近似得到公式

同时还需要注意这里有对速度正负大小，也就是方向的处理，论文令k=100，让qk与dk夹角不接近0时，能够趋近于1或-1，只有qk与dk正交的时候，这个公式才会出错，但根据动力学约束，这种情况不可能出现

同时角速度与角加速度是比较好表示的，里面用到了中心差分的思想，会比向前差分与向后差分更加精准，因此加速度与角加速度往往需要三个位置和两个时间间隔去表示

3.3 避障

 距离障碍物的距离会被实时计算，同时要保持安全距离，我认为这里会造成比较大的计算量

4 近似最小二乘优化

解决带有硬约束的非线性优化，是一个很难并且计算量很大的问题。但这个非线性优化问题是稀疏的，因此我们把这个问题转化成近似最小二乘问题，约束都被加入目标函数中作为惩罚项，从而等式约束与不等式约束都被转化成如下形式 ，等式约束就是越趋近于0越好，不等式约束就是在满足不等式的情况下没有惩罚

 转化成的无约束的非线性优化问题如下

 显然用罚函数的方法需要在惩罚权重在趋向于无穷的情况下才能达到硬约束的最优解，但是实际情况肯定不允许，因为这样没有办法在有限步数内快速收敛。因此为了保证高效性与稳定性，论文选择了一个权重不大得来较优的解。

5 闭环预测控制