运动规划RRT*算法图解

RRT*算法：

具体过程：

1. 产生一个随机点x_rand。

2. 在树上找到与x_rand最近的节点x_nearest。

3. 连接x_rand与x_nearest。

4. 以x_rand为中心，r_i为半径，在树上搜索节点。

5. 找出潜在的父节点集合X_{potential_parent}，其目的是要更新x_rand，看看有没有比它更好的父节点。

6. 从某一个潜在的父节点x_{potential_parent}开始考虑。

7. 计算出x_parent作为父节点时的代价。

8. 先不进行碰撞检测，而是将x_{potential_parent}与x_child（也就是x_rand）连接起来。

9. 计算出这条路径的代价。

10. 将新的这条路径的代价与原路径的代价作比较，如果新的这条路径的代价更小则进行碰撞检测，如果新的这条路径代价更大则换为下一个潜在的父节点。

11. 碰撞检测失败，该潜在父节点不作为新的父节点。

12. 开始考虑下一个潜在父节点。

13. 将潜在父节点和x_child连接起来

14. 计算出这条路径的代价。

15. 将新的这条路径的代价与原路径的代价作比较，如果新的这条路径的代价更小则进行碰撞检测，如果新的这条路径代价更大则换为下一个潜在的父节点。

16. 碰撞检测通过。

17. 在树中将之前的边删掉。

18. 在树中将新的边添加进去，将x_{potential_parent}作为x_parent。

19. 遍历所有的潜在父节点，得到更新后的树。