从零开始实现TEB局部轨迹规划算法（优化器基于ceres-solver）

TEB学习总结

TEB introduction

时间弹性：TEB算法考虑了机器人在路径规划过程中的运动时间，通过对路径特征进行建模，可以根据不同的速度限制和运动约束生成时间弹性的轨迹。这意味着算法可以根据需要自动调整运动速度和规划时间，以适应不同的应用场景和运动要求。

局部优化：TEB算法是一种局部路径规划算法，主要用于解决机器人在具体环境中的实时路径规划问题。它将机器人当前状态和目标状态作为输入，考虑机器人的动力学约束、障碍物避障和平滑性等因素，生成包含时间信息的平滑路径。

多目标支持：TEB算法能够有效处理机器人面临的多个目标点或轨迹约束。通过灵活的轨迹生成和优化策略，可以在考虑多个目标的情况下生成最优的运动轨迹。这对于需要在复杂环境中进行导航或任务执行的机器人特别有用。

高效性：TEB算法通过合理的优化策略和启发式方法，在保证路径质量的同时，能够实现较高的计算效率。这使得算法适合于实时机器人路径规划应用，并能够处理较复杂的环境和运动要求。

TEB parameters

$${\mathbf{X}}_{\mathbf{i}}=[x_i,y_i,{\theta }_i,\delta T_i]$$

TEB constraints

safety constraint

$$f_{safety}=\{\begin{array}{ll}(\Vert {\mathbf{x}}_{\mathbf{i}}-{\mathbf{o}}_{\mathbf{i}}\Vert -d_{min}),& \text{if }\Vert {\mathbf{x}}_{\mathbf{i}}-{\mathbf{o}}_{\mathbf{i}}\Vert <d_{min}\\ 0,& \text{else}\end{array}\text{\hspace{1em}(1)}$$
公式中${\mathbf{x}}_{\mathbf{i}}=[x_i,y_i]$路径点， ${\mathbf{o}}_{\mathbf{i}}=[o_x,o_y]$ 是距离当前点最近的一个障碍物点， $d_{min}$ 是允许的障碍物和机器人的最小距离， 所以这个代价函数的目的就是计算每一个点和障碍物的距离，如果距离小于 $d_{min}$ 这个代价函数就会对整体代价引入新误差，这样计算梯度的时候就会把当前点往远离障碍物的方向踢，这里会有一个问题，就是当路径点被障碍物包围的时候， 优化就会很大概率会失败，因为障碍物附近的梯度如下图, 中间的白色区域是距离障碍物最远的，那么优化的就会把在障碍物里面的点往中间推， 把外面的关键点往远离障碍物的方向推， 优化的结果可能会变成右图（左图是优化前）， 为了解决这个问题可以参考一下ego planner的论文

via-point constraint

$$f_{via}=||{\mathbf{x}}_{\mathbf{i}}-\mathbf{p}||\text{\hspace{1em}(2)}$$
公式中的$\mathbf{p}$ 是希望轨迹穿过的点，比如有三个点${\mathbf{p}}_1,{\mathbf{p}}_2,{\mathbf{p}}_3$，希望机器人在运动的时候经过，优化开始前首先需要找到距离这三个点最近的轨迹点，分别是${\mathbf{x}}_1,{\mathbf{x}}_2,{\mathbf{x}}_3$, 优化过程中代价函数只对这三个点起作用${\mathbf{x}}_1,{\mathbf{x}}_2,{\mathbf{x}}_3$，这种需要经过的点，如果是距离初始轨迹比较远的话，往往单次优化的效果都会很不好甚至可能收敛的结果很不好，因此最好是分多次优化，每次优化之前都需要根据需要调整轨迹点

velocity constraint

$$\begin{array}{c}{v}_i=\frac{||{\mathbf{x}}_{\mathbf{i}+1}-{\mathbf{x}}_{\mathbf{i}}||}{\delta T_i}\end{array}\text{\hspace{1em}(3)}$$
$$\begin{array}{c}{w}_i=\frac{Normalize({\theta }_{i+1}-{\theta }_i)}{\delta T_i}\end{array}\text{\hspace{1em}(4)}$$
函数$Normalize()$是把角度插值限定在[-pi,pi]之间，不然角速度就计算出错了， 线速度的计算不考虑正负的话直接求两个点的距离除时间就完事了，后续求加速度的时候才需要考虑正负线速度, 速度计算公式有了，对应的代价函数就好处理了
$$f_{linear\_vel}=\{\begin{array}{ll}{v}_i-v_{max}& \text{if }{v}_i>v_{max}\\ 0,& \text{else}\end{array}\text{\hspace{1em}(5)}$$

$$f_{angular\_vel}=\{\begin{array}{ll}|w_i|-w_{max}& \text{if }|w_i|>w_{max}\\ 0,& \text{else}\end{array}\text{\hspace{1em}(6)}$$

accleration constraint

$$\begin{array}{c}vac{c}_i=\frac{2(v_{i+1}-v_i)}{\delta T_{i+1}+\delta T_i}\end{array}\text{\hspace{1em}(7)}$$
这里求线性加速度的时候就需要考虑线速度的正负了， 这里可以考虑使用点乘的方法来确定线速度的方向， 点乘$a\cdot b=\Vert a\Vert \Vert b\Vert cos(\theta )$, 几何含义是向量a在b上的投影乘b的模， 根据点乘的结果的正负来判断速度的方向， 点乘为正，则速度为正，反之为负数， 其中$\theta$ 是$x_i$上的航向

$$\begin{array}{c}wac{c}_i=\frac{2(w_{i+1}-w_i)}{\delta T_{i+1}+\delta T_i}\end{array}\text{\hspace{1em}(8)}$$
同样加速度公式有了，代价函数也就可以写了
$$f_{linear\_acc}=\{\begin{array}{ll}|vac{c}_i|-vac{c}_{max}& \text{if }|vac{c}_i|>vac{c}_{max}\\ 0,& \text{else}\end{array}\text{\hspace{1em}(9)}$$

$$f_{angular\_acc}=\{\begin{array}{ll}|wac{c}_i|-wac{c}_{max}& \text{if }|wac{c}_i|>wac{c}_{max}\\ 0,& \text{else}\end{array}\text{\hspace{1em}(10)}$$

non-holonomic kinematics

$$\begin{array}{c}{f}_{kinematic}=\left|(\cos ({\theta }_{i+1})+\cos ({\theta }_i))\cdot \mathbf{dx}[1]-(\sin ({\theta }_{i+1})+\sin ({\theta }_i))\cdot \mathbf{dx}[0]\right|\end{array}\text{\hspace{1em}(11)}$$
公式中的$\mathbf{dx}={\mathbf{x}}_{\mathbf{i}+1}-{\mathbf{x}}_{\mathbf{i}}$， 这个代价函数的功能就是让参数$\theta$要符合运动学约束，也就是把机器人的角度和位置给耦合在一起了，如果没有这个约束，$\theta ,{\mathbf{x}}_{\mathbf{i}}$这两个参数就不会有联系

fastest constraint

这个约束的目的就是让优化出来的轨迹在符合运动学约束的情况下， 轨迹的速度要尽可能大， 也就是$\delta T_i$尽可能小

$$\begin{array}{c}{f}_{fastest}=\delta T_i\end{array}\text{\hspace{1em}(12)}$$

shortest constraint

仅仅是有速度最快是不够的， 速度快不代表路径短，轨迹符合运动学的前提下，我们还希望轨迹越短越好

$$\begin{array}{c}{f}_{fastest}=||{\mathbf{x}}_{\mathbf{i}}-{\mathbf{x}}_{\mathbf{i}+1}||\end{array}\text{\hspace{1em}(13)}$$

jerk constraint

teb论文中是没有对机器人的jerk做约束的，但是实际使用上有的场合可能希望机器人运动的要平稳一点，速度不要突变， 所以我觉得jerk也是挺重要的

$$\begin{array}{c}{f}_{jerk}=\frac{3(ac{c}_{i+1}-ac{c}_i)}{\delta T_i+\delta T_{i+1}+\delta T_{i+2}}\end{array}\text{\hspace{1em}(14)}$$

least square problem

所有代价函数都写好啦，现在剩下的就是如何把这些代价函数给建模成最小二乘问题，下面公式15就是我自己建模后的结果，其中n是路径点的个数， m是需要经过的点的个数， 公式中的$\mathbf{x}$需要根据代价函数来确定， 比如安全约束中$\mathbf{x}={\mathbf{x}}_{\mathbf{i}}$，线速度约束中$\mathbf{x}=[{\mathbf{x}}_{\mathbf{i}},{\mathbf{x}}_{\mathbf{i}+1}]$， 运动学约束中$\mathbf{x}=[{\mathbf{x}}_{\mathbf{i}},{\mathbf{x}}_{\mathbf{i}+1},{\theta }_{\mathbf{i}},{\theta }_{\mathbf{i}+1}]$
$$\begin{array}{rl}\underset{\mathbf{x}}{\min }\sum _{i=0}^n\Vert f_i(\mathbf{x}){|}_2^2=& \sum _{i=0}^n{f}_{safety}^i(\mathbf{x}{)}^2+\sum _{i=0}^m{f}_{via}^i(\mathbf{x}{)}^2+\sum _{i=0}^{n-1}{f}_{linear\_vel}^i(\mathbf{x}{)}^2+\sum _{i=0}^{n-1}{f}_{angular\_vel}^i(\mathbf{x}{)}^2+\\ & \sum _{i=0}^{n-2}{f}_{linear\_acc}^i(\mathbf{x}{)}^2+\sum _{i=0}^{n-2}{f}_{angular\_acc}^i(\mathbf{x}{)}^2+\sum _{i=0}^{n-3}{f}_{jerk}^i(\mathbf{x}{)}^2+\sum _{i=0}^{n-1}{f}_{kinematic}^i(\mathbf{x}{)}^2+\\ & \sum _{i=0}^{n-1}{f}_{fastest}^i(\mathbf{x}{)}^2+\sum _{i=0}^{n-1}{f}_{shortest}^i(\mathbf{x}{)}^2+\end{array}\text{\hspace{1em}(15)}$$
teb开源算法使用的优化器是g2o，我觉得使用g2o太麻烦了，这里我采用的是ceres-solver的最小二乘求解器

experiment

本测试参数，最大角速度为0.35， 最大线速度为0.45， 最大角加速度和线加速度都是0.6
为了测试优化效果，我把优化时间，路径长度，线速度，角速度，加速度，加加速度的数据都画出来了

从上述图片很容易看得出来， 哪一个是轨迹是比较平滑的，我跑过teb的源码， 从rviz上看他的结果，路径还是挺光滑的，但是没有速度加速度方面的数据，也不知道速度会不会存在突变的情况，我自己实现的如果代价函数不包括jerk，轨迹速度变化会比较剧烈，不知道是否是参数没有调好， 加了jerk后参数轨迹就很平滑了， teb这个算法还是挺简单的，主要是建模部分，优化部分也不需要自己动手，调包就完事儿了。这个算法的速度对硬件要求比较高，并且优化时长随着路径点数量的增多（dt越小），优化需要的时间会相应增加。如果路径总长度大约在3m左右，优化的平均时间可以维持在50ms，这是在X86笔记本上，如果在一些比较垃圾的板子上，时间估计就的100ms+了

为什么叫弹性带算法呢？ 请看视频

vokoscreen-2023-11-03_15-52-32

reference

C. Rösmann, W. Feiten, T. Wösch, F. Hoffmann and T. Bertram: Trajectory modification considering dynamic constraints of autonomous robots. Proc. 7th German Conference on Robotics, Germany, Munich, May 2012, pp 74–79.

C. Rösmann, W. Feiten, T. Wösch, F. Hoffmann and T. Bertram: Efficient trajectory optimization using a sparse model. Proc. IEEE European Conference on Mobile Robots, Spain, Barcelona, Sept. 2013, pp. 138–143.

http://ceres-solver.org/tutorial.html