Question about the initial value of optimization

[JiangxinkeTao]

I wrote a similar code on a trajectory planner, and it failed to iterate, or iterated poorly, if the chosen initial values were not close to the true value.
我编写了一个和您论文中相似的算法，但是我经过测试，如果初值不是一个比较好的数值，那么迭代进行不下去，甚至会出错，如下图所示，我模仿TEB中的例子，一个圆形的可拖动的障碍物，如果初始路径点选取经过障碍物，尽管添加了障碍物约束，但是它不能有效的弹开，不能达到teb算法中的范例中的效果；

我编写代码实现的效果图，效果比较差；

这是我的代码运行效果的视频，请大佬给一点建议，诚盼您的回复，真心感激不尽！
https://github.com/ZJU-FAST-Lab/Dftpav/assets/42513042/5e7503de-1673-4e6b-ac29-e49de5bf4044

[Han-Sin]

因为我们的避障是围绕走廊设计的，如果您的安全约束是通过ESDF施加的，那么确实可能陷入局部最优中。
所以我们建议您还是首先对环境凸分解，将避障约束构造为关于位置的线性约束，便可有效避障
当然，TEB其实也会遇到该本质问题，之所以在你的这个测试场景似乎成功避开的原因可能是因为比起我们的算法，TEB的轨迹更粗糙，他的优化变量只是涉及了一些点，而不是轨迹的全状态。这种化简可能让他的算法更容易跳出这些局部最优点。
如果环境更加复杂，且障碍物也高度非凸，TEB这种情况应该也不能work.

[Han-Sin]

如果需要细节的技术上帮助，可以联系我 zhichaohan@zju.edu.cn， 邮件里也可以附上微信，我们一起深度交流