机器人控制算法—TEB算法文献阅读Trajectory modification considering dynamic constraints of autonomous robots

摘要
经典的“弹性带”使全局规划器生成的路径相对于最短路径长度变形，而避免与障碍物接触。它没有直接考虑底层机器人的任何动态约束。
这一贡献引入了一种称为“定时弹性带”的新方法，该方法明确考虑了
根据诸如有限的机器人速度和加速度的动态约束的运动。“定时松紧带”
该问题是在一个加权多目标优化框架中提出的。大多数目标都是本地的，因为它们取决于在几个相邻的中间配置上。这导致了一个稀疏系统矩阵存在约束最小二乘优化方法。
实际机器人的仿真和实验结果表明，该方法具有鲁棒性和计算能力
有效地实时生成最佳机器人轨迹。“定时弹性带”转换由一系列路径指向一条明确依赖于时间的轨迹，从而能够实时控制机器人时间由于其模块化表述，该方法很容易扩展，以纳入额外的目标和限制。

有人翻译了，参考链接TEB论文翻译

主要写下问题部分的理解：


主要说明下公式7和公式8的惩罚函数是什么意思，如何去理解？
首先我们有如下结论：
对于路标点这种情况，距离目标点越近越好，即我们希望$d_{min,j}<r_{pmax}$,但如果$d_{min,j}>r_{pmax}$,就需要惩罚该约束条件了，结合公式(7)和（6）去思考，$x>x_r-E$就有惩罚值了，可以对得上，否则，不惩罚。
在障碍物的情况下，距离目标点越远越好，即我们希望$d_{min,j}>r_{omin}$,但如果$d_{min,j}<r_{omin}$,就有需要惩罚该约束条件了，将其变形后得$-d_{min,j}>-r_{omin}$,结合公式(8)和（6）去思考，$x>x_r-E$就有惩罚值了，否则不惩罚。

——————————————分割线——————————————————
笔者用了一周时间对TEB算法研究和代码分析，在此记录下我读此算法的理解：

上图是TEB算法的流程图，TEB算法根据全局规划轨迹，考虑障碍物以及各种性能约束，然后使用图优化，优化出局部姿态（序列点），然后再计算出控制指令v和w，但是，如果底层执行器对v和w执行不好的话，他是没有控制理论中的负反馈调节（属于开环控制），那么此算法对执行器的精度依赖很高，