机器人控制算法——局部规划器TEB算法原理及C++可视化仿真

1.背景介绍

最近一段时间，由于项目需要，一直在做TEB算法的工程化的工作，于是就考虑写下一篇系统些的文章，作为笔记，后续自己看也方便，TEB的英文名Time elastic band”，是一种局部规划器,它的核心思想是将路径规划问题转化为一个带有时间弹性的优化问题,通过对时间弹性的控制，来实现路径的优化。
TEB算法网上有很多的相关资料，本文主要侧重于如何在ubuntu系统下对C++开源的TEB算法进行可视化仿真，这件事是很有意义的，如果做纯理论算法的，可以通过此代码验证自己对TEB算法改进后的效果，如果要做工程化的产品，可用此代码研究TEB算法的不同的参数和算法的效果之间的关系，然后使其在真正的产品上调试出一组好的参数。那现在开始正文吧！

2.TEB算法原理

正如开头引言所介绍，TEB算法是一种局部规划器，它的核心思想是将路径规划问题转化为一个带有时间弹性的优化问题,通过对时间弹性的控制，来实现路径的优化。但是我不想这么去解释，如此解释，只会让人很疑惑。

问题一：那我们该如何理解这个TEB算法？

如下图公式（2），是TEB算法核心思想，从公式中，我们注意到，我们的目标函数V(b)设置成了起点到终点的所耗费的时间，约束条件考虑初始姿态信息、运动学约束、障碍物约束等因素，你可以理解为，任意给定一个起点和终点以及障碍物信息，我们希望找到一连串姿态点，这个序列姿态点要时间最短、同时要满足机器人运动学约束、障碍物约束、加速度和速度限制约束等（保证时间最快、满足机器人约束