基于采样的RRT/RRT*/RRT_connect算法笔记及C++实现

最新推荐文章于 2024-07-18 16:30:00 发布
最新推荐文章于 2024-07-18 16:30:00 发布
阅读量7k 收藏 151
点赞数 18
分类专栏： 运动规划 文章标签： 算法 c++
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_37705221/article/details/120751931
版权

本文记录常见的基于采样的RRT算法及相关改进算法（RRT*，RRT_connect）的原理和代码实现效果。与上一章介绍A*算法的文章不同，本文会先给出几种算法之间的效果对比，先有个直观的感受，再介绍其内在算法差异。

1. RRT/RRT*/RRT_connect算法效果对比

表1.1 简单地图路径搜索对比

算法采样次数实现效果
RRT3198在这里插入图片描述
RRT*3570在这里插入图片描述
RRT_connect214在这里插入图片描述
RRT*_connect195在这里插入图片描述

表1.2 复杂地图路径搜索对比（最大采样次数设置为10000）

算法采样次数实现效果
RRT10000在这里插入图片描述
RRT*10000在这里插入图片描述
RRT_connect1650在这里插入图片描述
RRT*_connect3960在这里插入图片描述

通过对比上述结果可以得出以下结论：
1）若路径存在，则通过RRT采样方法终究能找到1条路径，RRT算法是概率完备的
2）随机采样的缘故，RRT算法找到的路径通常情况下都不是最优的
3）相比于RRT算法，RRT*算法能对搜索树进行“剪枝”操作，优化路径使之更接近于真实的最优路径
4）传统的RRT算法是从起始点向终点进行搜索寻找路径，而RRT_connect是从起点和终点同时开始向中间搜索路径，能够显著提高路径搜索效率，减少采样次数
5）对于狭小通道（narrow passage）工况，传统的RRT可能需要很长时间才能通过随机采样通过狭小通道，而RRT_connect由于是从两头同时往中间搜索，在遇到狭小通道时，比RRT更能快速高效地寻找到路径

RRT算法就是在空间中随机撒点，通过点之间的连线确定1条起点到终点的路径。在直观上感受到RRT系列算法的路径搜索过程及效果对比之后，接下来介绍其内在的算法原理。

2. RRT/RRT*/RRT_connect算法流程

便于理解，先上基础RRT算法伪代码流程，针对流程逐一介绍
在这里插入图片描述
输入：带障碍物的地图，起点，终点位置
输入：从起点到终点的位置
维护1个节点集T用于存放已经搜索过且可到达的节点，初始化T放入起始节点
进入循环，进行路径搜索：
（1）在地图M中随机采集节点Xrand；
（2）从节点集T中找到离Xrand最近的节点Xnear；
（3）从Xnear出发，沿着指向Xrand的方向走指定步长StepSize，得到节点Xnew；
（4）连接Xnear和Xnew点，得到边Ei；
（5）对Ei进行碰撞检测，若是collision free的，则将Xnew放入T中，并设置Xnew的父节点为Xnear；若与障碍物有碰撞，进入下1轮循环，继续随机采样节点；
（6）如果Xnew等于Xgoal或者Xnew距离Xgoal小于某一设定阈值，则路径寻找成功，退出循环。

通过算法流程可以看出，RRT算法旨在找到起点到终点的1条路径，从起点出发，向终点不断搜索。但是也存在一些缺点：搜索出的路径不是最优的；在整个空间采样，效率低下。特别是对于文中示例的复杂路况，存在“狭窄通道”情况，一般的RRT算法可能需要经历很多次采样才能穿过通道，比如文中复杂工况，RRT算法经过10000次采样，仍然未找到1条路径，尽管从直观上看可行的路径是肯定存在的。

基本的RRT每次搜索都只有从初始状态点生长的快速扩展随机树来搜索整个状态空间，如果从初始状态点和目标状态点同时生长两棵快速扩展随机树来搜索状态空间，效率会更高。为此，基于双向扩展平衡的连结型双树RRT算法,即RRT_connect算法
在这里插入图片描述
而RRT*算法是在RRT算法的基础上加了"剪枝”操作，使路径更趋向于最优路径。算法流程如下
在这里插入图片描述
可以看出，相对于RRT算法，主要区别在于，在循环中多了ChooseParent和rewire两步。其中ChooseParent是指得到Xnew点之后，不直接将Xnear和Xnew连接，而是将Xnew周围给定半径（要比stepsize大）区域内的点加入一个节点集中，依次比较以这个节点集中节点作为Xnew的父节点的cost，选取cost最小的节点作为Xnew的父节点。而rewire是指依次比较节点集中的节点当前cost1和若以Xnew节点作为父节点的cost2，若cost2<cost1，则将当前节点的父节点更新为Xnew。

3. C++代码实现

在上一篇A*算法代码框架基础上，重新写了二维平面内的RRT/RRT*/RRT_connect算法，并通过调用opencv库展示路径搜索效果。具体代码见：
https://github.com/immune1993/path_planning_algorithm/tree/master/Sample_based
下面仅贴出核心的搜索部分代码

//RRT/RRT*搜索算法
bool RRTPathFinder::RRTGraphSearch(Eigen::Vector2d start_pt, Eigen::Vector2d end_pt) {
    //初始化起始和目标点
    GridNodePtr startNode=new GridNode(start_pt);startNode->r=5;
    GridNodePtr endNode= new GridNode(end_pt);endNode->r=5;

    //起始和目标点绘制
    startNode->drawNode(picture,Eigen::Vector3i(0,0,255));
    endNode->drawNode(picture,Eigen::Vector3i(255,0,0));

    GridNodeSet.clear();

    //将开始点放入搜索树中
    GridNodeSet.push_back(startNode);
    int count=0;
    srand((unsigned)time(NULL));

    //for(int i=0;i<5000;i++){
    while (1){
        //生成随机点
        count++;
        int x=(rand()%100+1)*5;
        int y=(rand()%100+1)*5;
//        int x=goal_dis(goal_gen)*5;
//        int y=goal_dis(goal_gen)*5;
        Eigen::Vector2d randcoo((double)x,(double)y);
        GridNodePtr randNode =new GridNode(randcoo);

        //获取最近节点
        GridNodePtr nearNode= getNearestNode(randNode);
        //获取next节点
        GridNodePtr nextNode= getNextNode(nearNode,randNode,10);
        //碰撞检测
        if(!collisioncheck(nextNode,nearNode)){
            continue;
        }
        nextNode->father=nearNode;

        //RRT*
        nextNode->cost=nearNode->cost+10;
        //获取next节点周围半径R内的节点集合
        std::vector<GridNodePtr> neighborsNode= getNeighbors(nextNode,25);
        //为next确定父节点
        for(auto n:neighborsNode){
            if(n->cost+getCost(n,nextNode)<nextNode->cost&& collisioncheck(nextNode,n)){
                nextNode->father=n;
                nextNode->cost=n->cost+ getCost(n,nextNode);
            }
        }

        //为周围节点更新父节点是否为nextnode
        for(auto n:neighborsNode){
            if(nextNode->cost+ getCost(nextNode,n)<n->cost&& collisioncheck(n,nextNode)){
                //去除原来连线
                cv::line(picture,cv::Point(n->father->coord(0),n->father->coord(1)),cv::Point(n->coord(0),n->coord(1)),cv::Scalar(255,255,255));
                n->father=nextNode;
                n->cost=nextNode->cost+ getCost(nextNode,n);
                //重新连线
                cv::line(picture,cv::Point(n->father->coord(0),n->father->coord(1)),cv::Point(n->coord(0),n->coord(1)),cv::Scalar(0,0,0));
            }
        }
        //

        nextNode->drawNode(picture,Eigen::Vector3i(0,255,0));
        cv::line(picture,cv::Point(nextNode->father->coord(0),nextNode->father->coord(1)),cv::Point(nextNode->coord(0),nextNode->coord(1)),cv::Scalar(0,0,0));
        GridNodeSet.push_back(nextNode);
        cv::imshow("RRT",picture);
        if(count%15==0) {write.write(picture);std::cout<<count<<std::endl;}
        cv::waitKey(1);
        if(getCost(endNode,nextNode)<5){
            goalNode=nextNode;
            std::cout<<"Path has been found after " + std::to_string(count) +" times search"<<std::endl;
            return true;
        }
    }
    std::cout<<"Path has been not found!"<<std::endl;
    return false;
}

//RRT_connect搜索函数
bool RRTPathFinder::RRTGraphSearch(Eigen::Vector2d start_pt, Eigen::Vector2d end_pt) {
    //初始化起始和目标点
    GridNodePtr startNode=new GridNode(start_pt);startNode->r=5;
    GridNodePtr endNode= new GridNode(end_pt);endNode->r=5;

    //起始和目标点绘制
    startNode->drawNode(picture,Eigen::Vector3i(0,0,255));
    endNode->drawNode(picture,Eigen::Vector3i(255,0,0));

    FromStartNodeSet.clear();
    FromEndNodeSet.clear();

    //将开始点放入搜索树中
    FromStartNodeSet.push_back(startNode);
    FromEndNodeSet.push_back(endNode);
    int count=0;
    srand((unsigned)time(NULL));

    //for(int i=0;i<5000;i++){
    while (1){
        //生成随机点
        count++;
        int x=(rand()%100+1)*5;
        int y=(rand()%100+1)*5;
//        int x=goal_dis(goal_gen)*5;
//        int y=goal_dis(goal_gen)*5;
        Eigen::Vector2d randcoo((double)x,(double)y);
        GridNodePtr randNode =new GridNode(randcoo);

        //获取最近节点
        GridNodePtr nearNode_s= getNearestNode(randNode,FromStartNodeSet);
        GridNodePtr nearNode_e= getNearestNode(randNode,FromEndNodeSet);
        //获取next节点
        GridNodePtr nextNode_s= getNextNode(nearNode_s,randNode,10);
        GridNodePtr nextNode_e= getNextNode(nearNode_e,randNode,10);
        //碰撞检测
        bool collcheck_s=collisioncheck(nextNode_s,nearNode_s);
        bool collcheck_e=collisioncheck(nextNode_e,nearNode_e);
        if(collcheck_s){
            nextNode_s->father=nearNode_s;

            //RRT*
            nextNode_s->cost=nearNode_s->cost+10;
            //获取next节点周围半径R内的节点集合
            std::vector<GridNodePtr> neighborsNode= getNeighbors(nextNode_s,FromStartNodeSet,25);
            //为next确定父节点
            for(auto n:neighborsNode){
                if(n->cost+getCost(n,nextNode_s)<nextNode_s->cost&& collisioncheck(nextNode_s,n)){
                    nextNode_s->father=n;
                    nextNode_s->cost=n->cost+ getCost(n,nextNode_s);
                }
            }

            //为周围节点更新父节点是否为nextnode
            for(auto n:neighborsNode){
                if(nextNode_s->cost+ getCost(nextNode_s,n)<n->cost&& collisioncheck(n,nextNode_s)){
                    //去除原来连线
                    cv::line(picture,cv::Point(n->father->coord(0),n->father->coord(1)),cv::Point(n->coord(0),n->coord(1)),cv::Scalar(255,255,255));
                    n->father=nextNode_s;
                    n->cost=nextNode_s->cost+ getCost(nextNode_s,n);
                    //重新连线
                    cv::line(picture,cv::Point(n->father->coord(0),n->father->coord(1)),cv::Point(n->coord(0),n->coord(1)),cv::Scalar(0,0,0));
                }
            }
            //

            nextNode_s->drawNode(picture,Eigen::Vector3i(0,255,0));
            cv::line(picture,cv::Point(nextNode_s->father->coord(0),nextNode_s->father->coord(1)),cv::Point(nextNode_s->coord(0),nextNode_s->coord(1)),cv::Scalar(0,0,0));
            FromStartNodeSet.push_back(nextNode_s);
        }
        if(collcheck_e){
            nextNode_e->father=nearNode_e;

            //RRT*
            nextNode_e->cost=nearNode_e->cost+10;
            //获取next节点周围半径R内的节点集合
            std::vector<GridNodePtr> neighborsNode= getNeighbors(nextNode_e,FromEndNodeSet,25);
            //为next确定父节点
            for(auto n:neighborsNode){
                if(n->cost+getCost(n,nextNode_e)<nextNode_e->cost&& collisioncheck(nextNode_e,n)){
                    nextNode_e->father=n;
                    nextNode_e->cost=n->cost+ getCost(n,nextNode_e);
                }
            }

            //为周围节点更新父节点是否为nextnode
            for(auto n:neighborsNode){
                if(nextNode_e->cost+ getCost(nextNode_e,n)<n->cost&& collisioncheck(n,nextNode_e)){
                    //去除原来连线
                    cv::line(picture,cv::Point(n->father->coord(0),n->father->coord(1)),cv::Point(n->coord(0),n->coord(1)),cv::Scalar(255,255,255));
                    n->father=nextNode_e;
                    n->cost=nextNode_e->cost+ getCost(nextNode_e,n);
                    //重新连线
                    cv::line(picture,cv::Point(n->father->coord(0),n->father->coord(1)),cv::Point(n->coord(0),n->coord(1)),cv::Scalar(0,0,0));
                }
            }
            //

            nextNode_e->drawNode(picture,Eigen::Vector3i(0,255,0));
            cv::line(picture,cv::Point(nextNode_e->father->coord(0),nextNode_e->father->coord(1)),cv::Point(nextNode_e->coord(0),nextNode_e->coord(1)),cv::Scalar(0,0,0));
            FromEndNodeSet.push_back(nextNode_e);
        }
        cv::imshow("RRT_Connect",picture);
        if(count%15==0) {write.write(picture);std::cout<<count<<std::endl;}
        cv::waitKey(1);
        if(collcheck_s&&collcheck_e&&getCost(nextNode_s,nextNode_e)<10&& collisioncheck(nextNode_s,nextNode_e)){
            Mid_s_Node=nextNode_s;
            Mid_e_Node=nextNode_e;
            std::cout<<"Path has been found after " + std::to_string(count) +" times search"<<std::endl;
            return true;
        }
    }
    std::cout<<"Path has been not found!"<<std::endl;
    return false;
}

4. 参考

[1] 深蓝学院课程：移动机器人路径规划
[2] https://blog.csdn.net/qq_14977553/article/details/107494695

安安的胖胖
关注
  • 18
    点赞
  • 踩
  • 151
    收藏
    觉得还不错? 一键收藏
  • 5
    评论
专栏目录
路径规划 | 图解RRT-Connect算法(附ROS C++/Python/Matlab仿真)
FRIGIDWINTER的博客
06-05 5426
考虑到终点附近的区域信息不能得到有效利用，因此可以分别以起点和终点为根节点进行双搜索树双向扩展，称为RRT-Connect算法。本文图解RRT-Connect的核心原理，并提供ROS C++/Python/Matlab三种实现加深理解
基于采样的规划算法之RRT家族（二）：RRT-Connect
昔风不起，唯有努力生存！
11-23 2311
原始的RRT算法以起点作为根结点拓展一棵路径树，双向RRT算法则同时以起点与终点作为根结点拓展两棵树，RRT-Connect算法是在双向RRT算法的基础上，增加了贪婪拓展策略，也即：某一子树上最新节点往另一子树最近的节点不断延伸拓展节点，直到遇到障碍物。
rrt_connect
qq_54644501的博客
04-22 612
rrt_connect的matlab实现
（7-2-03）RRT算法：RRT算法的定义与实现(3)RRT_Connect 算法
最新发布
码农三叔
07-18 1126
同时第二棵树扩展的方式略有不同，首先它会扩展第一步得到𝑞′𝑛𝑒𝑤，如果没有碰撞，继续往相同的方向扩展第二步，直到扩展失败或者𝑞′𝑛𝑒𝑤= 𝑞𝑛𝑒𝑤表示与第一棵树相连了，即 connect了，整个算法结束。当然在每次迭代中必须考虑两棵树的平衡性，即两棵树的节点数的多少（也可以考虑两棵树总共花费的路径长度），交换次序选择“小”的那棵树进行扩展。如果找到路径，则输出路径上的节点；与标准的 RRT 算法不同，RRT-Connect 算法旨在通过两个相互连接的 RRT 树来更快地找到起始点和目标点之间的路径。
rrt：C ++ RRT（快速探索随机树）实现
02-05
rrt：C ++ RRT（快速探索随机树）实现
RRT路径搜索算法C++实现
xiezhaokun55的博客
05-28 4940
基于RRT的路径规划算法，通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径。适合解决多自由度机器人在复杂环境下和动态环境中的路径规划。 对图片进行灰度图处理，并对像素点进行修改为0和255两个值，...
C++实现双向RRT算法
polaris的博客
09-01 1131
RRT（Rapidly-exploring Random Trees）是Steven M. LaValle和James J. Kuffner Jr.提出的一种通过所及构建空间搜索树实现对非凸高维空间快速搜索算法。该算法可以很容易的处理包含障碍和差分运动约束的场景，因此被广泛应用在各种机器人、无人车的运动规划场景中。
【自动驾驶】基于采样的路径规划算法——RRT-Connect（含python实现 | c++实现）
weixin_42301220的博客
06-13 5472
前文回顾基于采样的路径规划算法——RRT（含python实现）基于随机采样的路径规划算法适用于高维度空间，它们以概率完备性(当时间接近无限时一定有解)来代替完备性，从而提高搜索效率。基于随机采样的路径规划算法又分为单查询算法(single-query path planning)以及渐近最优算法(asymptotically optimal path planning)，前者只要找到可行路径即可，侧重快速性，后者还会对找到的路径进行逐步优化，慢慢达到最优，侧重最优性。单查询方法包括概率路图算法(Probab
RRT、RRT-Connect、LazyRRT、RRTextend、RRT*的2D和3D算法(Matlab)
11-23
基于RRT，通过对RRT-Connect、LazyRRT、RRTextend以及RRT*的2D和3D算法，在Matlab进行仿真，更有利于对算法的理解甚至后续的改进。
路径规划 | 图解RRT*算法(附ROS C++/Python/Matlab仿真)
FRIGIDWINTER的博客
05-29 6735
RRT*算法针对传统RRT算法进行了渐进最优改进，在添加节点到搜索树的过程中进行重连选择。本文图解RRT*算法原理，并采用ROS C++、Python、Matlab三种语言实现算法，加深理解
RRT* CONNECT
08-18
- *1* *3* [基于采样的RRT/RRT*/RRT_connect算法笔记及C++实现](https://blog.csdn.net/weixin_37705221/article/details/120751931)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{...
RRT_Connect
07-11
RRT_Connect算法的matlab代码，二维平面路径规划。参考别人代码实现，代码有点小乱，不过能跑，欢迎多指教！matlab下之间运行RRT_Connect.m文件。
RRT 算法的实现RRT 算法的实现
07-23
RRT 算法的实现RRT 算法的实现RRT 算法的实现RRT 算法的实现RRT 算法的实现
快速扩展随机树（RRT）算法的实现
07-19
快速扩展随机树（RRT）算法的实现 visual c++开发 rar格式，无需解压密码
随机快速扩展树（RRT）路径规划算法代码实现
07-22
随机快速扩展树路径规划算法代码实现，好几个例子，非常值得学习。
C++代码：双向RRTstar.zip
07-09
C++代码：双向RRTstar。附原文标题：[1] Ahmed Hussain Qureshi and Yasar Ayaz, "Intelligent Bidirectional Rapidly-Exploring Random Trees for Optimal Motion Planning in Complex Cluttered Environments",International Journal of Robotics and Autonomous Systems, Elsevier, Vol. 68, pp. 1-11, 2015. Paper:https://arxiv.org/abs/1703.08944
全局路径规划之5.1：Rapidly exploring Random Trees(RRT)类算法（附C++代码）
qq_35635374的博客
04-28 743
认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对Rapidly exploring Random Trees(RRT)类算法做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容（1）优点快速随机树算法是一种较为迅捷搜索的方式，增量式的构建树其优点明显，主要体现在针对性的找一条从起点到终点的路径其较强较快的搜索能力，【防盗标记–盒子君hzj】并且运用快速拓展随机树的路径规划对路径运行环境没有限制要求。（2）缺点。
RRT、RRT_CONNECT、ARRT_CONNECT
HDHX_DHU的博客
02-28 1911
1.RRT 算法说明：先在空间中随机撒点，之后挑选树中距离qrand最近的点记为qnear之后，让qnear沿着qrand的方向移动一定距离，如果一定之后的距离直接就是qrand，直接把qrand加入树，如果一定距离之后没有达到qrand，就记为qnew加入树。 算法流程： 主函数： 初始化qinit节点（起点） 执行循环 { random_config函数，用来在空间中随机撒点，得到qrand EXTEND（T,qrand） } EXTEND(T...
路径规划学习笔记1——经典算法（RRT、RRT*、RRT-CONNECT）
weixin_49944426的博客
03-14 1009
在RRT的基础上进行改进，碰撞检测之后，从GOAL出发，在Xnew和GOAL的连线上通过步长选取节点，直到发生碰撞（贪婪思想），每一回合的STAR和GAOL都被替换为上一回合的末端节点。在RRT的基础上进行改进，碰撞检测之后，Xnew不再直接连接到上一个节点上，而是连接到距离Xnear最近的节点上。
评论 5
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值