rls算法 matlab



参考

机器人路径规划、轨迹优化课程-第六讲-RRT*算法原理和代码讲解

路径规划 | 随机采样算法：PRM、RRT、RRT-Connect、RRT*

基于采样的运动规划算法-RRT(Rapidly-exploring Random Trees)

《改进RRT算法在移动机器人路径规划中的应用研究》

理论基础

RRT*（Rapidly-exploring Random Tree Star）算法是RRT算法的改进版本，它通过引入和步骤，提高了路径规划的质量和效率。下面是对RRT*算法的详细描述：

1. 初始化：设定起始点start和目标点goal，并创建一个只包含start的RRT树T
2. 重复步骤直到找到路径或达到最大迭代次数：
   a. 随机采样：在环境空间中随机采样一个点x_rand
   b. 扩展树：从树T中找到最近的节点x_near，以x_near为起点，在方向上延伸一定的距离，得到新的节点x_new
   c. 碰撞检测：检测路径x_near到x_new之间是否与障碍物发生碰撞，如果发生碰撞，则返回步骤2继续下一次迭代
   d. 寻找最优连接：在树T中找到与x_new最近的节点x_min，并计算从x_min到x_new的代价cost(x_min, x_new)
   e. 重新连接：对于树T中与x_new距离在一定范围内的节点x_near_neighbors，计算通过x_near连接到x_new的代价cost(x_near, x_new)，选择代价最小的连接方式
   f. 更新树：将x_new加入树T，并更新节点间的连接关系和代价信息
   g. 优化路径：对树T中的部分节点，以目标点goal为目标进行优化，通过调整连接关系和代价信息，改善路径的质量
   h. 判断终止条件：如果新加入的节点x_new接近目标点goal，则找到了一条可行路径，算法结束
   
   
   
   
   
   
   
   

RRT*算法通过重新连接和优化步骤，不断改善路径的质量和长度。重新连接步骤尝试改变已有的连接关系，以找到更优的路径。优化步骤则通过调整节点间的连接关系和代价信息，进一步优化路径

RRT*算法具有高效、快速搜索、易于实现等优点，可以用于路径规划问题，特别适用于复杂环境和高维空间。它通过随机采样和树的扩展来探索环境，并通过重新连接和优化来提高路径的质量。通过合理的调整算法参数，可以平衡探索和利用，获得高效的路径规划结果

RRT* 算法伪代码如下：

重新连接（Rewrite）过程

RRT* 在找到距离 Node_rand 最近的节点 Node_near 并通过碰撞检测后，并不立即将 加入扩展树中

而是以 Node_rand 为圆心，R 为半径(不一定是步长) 的圆内，寻找所有潜在的父节点，并与通过当前父节点 Node_near 到达该节点 Node_rand 的代价对比，看是否存在代价更小的父节点

如下图所示，当路径 Node_init→Node_parent→Node_child 的代价大于 Node_init→Node_potential_parent→Node_child 的代价时，RRT* 算法会将 Edge{Node_parent→Node_child}剔除，并新增

至此完成了一次重新连接的过程，新的随机数如下图所示

优化过程

优化就是在以新添加节点 为圆心，R 为半径(不一定是步长)的圆，圆内会包含一些节点，这些节点尝试通过新增节点到达的代价是否小于原始代价，小于则更新该节点父节点为新增的节点，使得到达该节点的代价更小

如上图所示，X_new 为新生成节点，4、6、8 是 X_new 的近邻节点，0、4、5 为近邻节点的父节点

• 路径{0->4}的Cost为: 10
• 路径{0->4->6}的Cost为： 10 + 5 = 15
• 路径{0->1->5->8}的Cost为: 3 + 5 + 1 = 9
• 先尝试将节点4的父节点改为 X_new，到达节点4的路径变为{0->1->5->9->4}，新路径的Cost=3+5+3+4=15，新路径的Cost大于原路径Cost，所以不改变节点4的父节点
• 再尝试改变节点8的父节点为 X_new，到达节点8的路径变为{0->1->5->9->8},新路径的Cost=3+5+3+3=14，新路径的Cost大于原路径Cost，随意不改变节点8的父节点
• 再尝试改变节点6的父节点为 X_new，到达路径6的路径变为{0->1->5->9->6},新的Cost=3+5+3+1=12,新路径的Cost小于原路径Cost，因此将节点6的父节点更新为节点9

MATLAB 实现

https://github.com/olzhas/rrt_toolbox

运行输出

Python 实现

https://github.com/zhm-real/PathPlanning

运行输出

主要的逻辑还是在 函数中，代码对于功能函数做了很好的封装，很值得学习

一共进行最大 10000 次迭代（），每次迭代中进行如下操作

1、生成随机点，选取最近点，生成新点

2、如果新节点通过了碰撞检测，则找出搜索圆内的所有相邻节点，并将该新节点添加到路径中

3、若搜索圆内存在节点，则进行重连接和优化操作，分别对应 和 函数

4、回溯确定搜索路径

总结

鉴于RRT算法不是最优的缺点，RRT算法却能够实现了，但与此同时，在RRT中又出现了一些新的问题：

1. 在不规则障碍物环境中，产生了的问题：由于RRT算法的随机扩展树在产生采样点时，是对周围的环境采用均匀的采样方法来进行采样，同时RRT算法的随机扩展树在生成采样点时会将周围不是渐近最优的冗余节点直接删除，虽然路径可以实现渐近最优，但是生成的路径不够光滑，从而使得机器人按照这种路径运动时会发生问题
2. RRT算法能够实现递进优化，但实现过程缓慢，：RRT算法的随机扩展树在生成采样点时，依次计算起始点到父节点、父节点到采样点、采样点到最近的节点的距离，并将这些距离依次进行比较，最终删除原先那些距离较长的连线，然后再对那些线进行重新连接。此时虽然会找到一条渐近最优的路径，但是由于迭代次数过多导致路径规划的时间较长，效率也会降低

RRT 算法与 RRT* 算法对比

版权声明：

本文来源网络，所有图片文章版权属于原作者，如有侵权，联系删除。

本文网址：https://www.mushiming.com/mjsbk/9725.html