加速长路径求解器

[maniema4real]

首先，这项工作非常棒，我在其他地方没有找到比这更好的东西......我在 ROS 之外使用它，并根据我的需要对其进行了调整。 为了加速最终的求解操作，我将 search_ds 和 delta_s 更改为稍高的值。 我还将最终状态的权重提高到了 1000000。看起来效果很好。 但似乎改变 DS 会稍微改变性能，并且车辆似乎超出限制或在某些区域稍微接触。 您有什么建议可以加快长路径求解器的速度？

[LiJiangnanBit]

感谢关注~

目前代码实现里面，增大search_ds是可能会导致边界约束不精确的，原因是在FreeSpace::get_l_bound_for_circle这个函数里，计算边界是使用的离散边界点，而不是点的连线。因此当点之间间隔较大时，容易有误差。可以考虑将这个函数优化一下。

这个代码没有为了提高求解速度做很多调试，可以改进的地方比如：尝试不同的barrier函数（目前使用的是q1 exp(q2 h(x))）；调试barrier函数的参数；尝试调整收敛的判定条件；另外可以对path的后半段简化约束，或者增大后半段的采样间隔，毕竟后半段对当前车辆的影响较小。