rrt star smart 全局规划器

简介

原理参考我另外两篇文章：

K-D 树邻近算法

RRT*-Smart 论文解读

效果

红色圆点是根据采样产生的新节点，箭头由子节点指向父节点，黄色箭头是 rrt 初始产生的方向，紫色箭头是 rrt star 重新布局修改后的方向。绿色路径是找到的初始路径，青色路径是 rrt star smart 优化后的路径。可以看到优化过程中，信标节点附近的采样会更多一点。

test 0

test 1

小结

• 有时候出现 visualization_msgs 没有显示的问题，只发现有时候发布太快，会有两个 mark 时间戳相同的情况，此时第一个 mark 会显示，第二个 mark 不会显示。但 mark 不是通过 id 来区分的吗？这个问题暂时没弄明白。
• new point 的产生需要根据系统而定，本文只是简单地用线性移动产生 new point。
• 信标采样频率不能太高，否则采样都集中在初始路径附近，降低了更短路径被搜索到的可能。