精华内容
下载资源
问答
  • 局部路径规划总的来说是在全局路径规划模块下,结合避障信息重新生成局部路径的模块,上层的全局路径规划确定了A到B的一个全局路径,不过轨迹跟踪模块(比如 pure persuit)实际进行跟踪的不能是这个直接生成的全局...

    d87308ef8abf33a2995dfa3b23de2ebc.png

    一、引言

    目前为系统添加了局部路径规划模块,结合之前做的视觉激光雷达信息融合模块,系统需要进一步对避障部分进行处理。

    局部路径规划总的来说是在全局路径规划模块下,结合避障信息重新生成局部路径的模块,上层的全局路径规划确定了A到B的一个全局路径,不过轨迹跟踪模块(比如 pure persuit)实际进行跟踪的不能是这个直接生成的全局路径,因为系统实际工作可能会有其他情况发生,轨迹跟踪模块实际跟踪的是结合障碍物信息的局部路径。

    局部路径规划算法有好多种,例如人工势场法,动态窗口法等,

    这里要介绍的是DARPA比赛中斯坦福大学Stanley自动驾驶系统所使用局部路径规划算法,是一种基于采样的局部路径规划算法,该算法在autoware的op_planner模块也有使用,本文主要是基于autoware的op_planner模块进行介绍。

    整体来看op_planner的local_planner主要分为两部分:Rollouts Generator 和 Rollouts Evaluator,前者根据全局中心路径生成一系列平滑的候选局部路径,后者结合障碍物信息和其他因素计算各个Rollout的代价Cost,从而选出最终平滑的,无障碍的局部路径。

    Rollouts 的含义就是根据中心全局路径生成的一些列候选局部路径。如下图所示:

    b6f36358dc8580d9c18586bb06d1f894.png

    中间的绿线是全局路径规划模块生成的全局路径,棕色线是生成的一系列候选局部路径。

    在有障碍物存在的情况下,经过计算每个rollout的代价Cost,选出最优的路径如下所示:

    b4eac2b3222d8f138b6afcf83b494240.png

    其中,白线就是评估后得到的最优的局部路径。

    下面针对local_planner的两个主要部分的算法Rollouts Generator 和 Rollouts Evaluator进行简要分析,相关代码已经同步到我的github,位于planning/local_planner

    https://github.com/sunmiaozju/smartcargithub.com

    参考论文: Open Source Integrated Planner for Autonomous Navigation in Highly Dynamic Environments

    完整的论文地址: github-smartcar/planning/local_planner/docs/Open Source Integrated Planner for Autonomous Navigation in Highly Dynamic Environments.pdf


    二、Rollouts Generator

    模块的输入信息是车辆当前位置,全局规划路径,rollouts数量,规划距离等等

    输出的是n条平滑的,起点位于车辆当前位置,终点位于最大规划距离的候选局部规划轨迹。

    根据论文中的描述,rollout被划分为了三个部分:Cartip,Rollin, Rollout,如下图所示:

    8cda36c7f5e94d95acb32e0acbf5af4a.png

    cartip部分从车辆中心点到水平采样的起点,这部分的长度决定了车辆切换不同轨迹的平滑程度。

    rollin部分从水平采样的起点到平行采样的起点,这部分的长度和车辆速度密切相关,车辆速度越快,rollin部分应越长,使得轨迹更加平滑。

    rollout部分从平行采样的起点到最大规划距离,这部分里每一条rollout都是平行的,相隔距离由rollout_density来确定。

    生成rollouts的算法主要包括三个部分:1、截取全局路径,长度为最大局部路径规划距离。2、针对截取的全局路径进行点采样。3、平滑得到的采样点,生成最终候选轨迹。具体如下图所示:

    a8c17ac73b316b24f3cc4b1b5e3fbfe4.png

    主循环函数主要的内容就是extractPartFromTrajectory()和generateRunoffTrajectory()。具体代码细节可以参考github.

    /**
     * @description: 主循环函数
     * @param {type}
     * @return:
     */
    void RolloutGenerator::run()
    {
        ros::Rate loop_rate(100);
        while (ros::ok())
        {
            ros::spinOnce();
            if (currentPose_flag && globalPaths.size() > 0)
            {
                globalPathSections.clear();
                for (size_t i = 0; i < globalPaths.size(); i++)
                {
                    centralTrajectorySmoothed.clear();
                    // 截取全局规划路径
                    extractPartFromTrajectory(globalPaths[i], current_pose, 50,
                                              PlanningParams.pathDensity, centralTrajectorySmoothed);
                    globalPathSections.push_back(centralTrajectorySmoothed);
                }
                std::vector<UtilityNS::WayPoint> sampled_points;
                // 生成候选rollouts
                generateRunoffTrajectory(globalPathSections,
                                         current_pose,
                                         speed,
                                         PlanningParams.microPlanDistance,
                                         PlanningParams.carTipMargin,
                                         PlanningParams.rollInMargin,
                                         PlanningParams.speedProfileFactor,
                                         PlanningParams.pathDensity,
                                         PlanningParams.rollOutDensity,
                                         PlanningParams.rollOutNumber,
                                         PlanningParams.smoothingDataWeight,
                                         PlanningParams.smoothingSmoothWeight,
                                         PlanningParams.smoothingToleranceError,
                                         rollOuts,
                                         sampled_points);
    }
    

    三、Rollouts Evaluator

    这部分的输入是之前生成的Rollouts以及感知模块得到的障碍物信息,输出的是最优的局部规划路径。

    这里使用三个代价函数来评估不同的局部路径:priority cost, collision cost 和 transition cost.

    priority cost 代表中间的局部轨迹优先级是最高的,在没有障碍物的情况下,优先选择中间的局部轨迹,如下所示:

    // cal priority cost
    for (int i = 0; i < rollouts.size(); i++)
    {
        tc.index = i;
        tc.relative_index = i - params.rollOutNumber / 2;
        tc.distance_from_center = params.rollOutDensity * tc.relative_index;
        tc.priority_cost = fabs(tc.distance_from_center);
    }
    

    transition cost 限制了车辆不会跳跃多个局部路径,确保了车辆前进路径的平滑性。根据车辆当前所处的局部路径位置,转换到临近车道代价较小,转换到较远车道代价较大。如下所示:

    // cal transition cost
    for (int ki = 0; ki < trajectoryCosts.size(); ki++)
        trajectoryCosts[ki].transition_cost = fabs(params.rollOutDensity * (currIndex - ki));
    

    collision cost 主要分为lateral_cost 和 longitudinal_cost,前者代表局部轨迹距离障碍物的水平距离,后者代表局部轨迹距离最近障碍物的垂直距离

    collision cost计算的相关代码细节位于函数 calLateralAndLongitudinalCostsStatic()

    for (int i = 0; i < rollOuts.size(); i++)
    {
        for (int k = 0; k < contourPoints.size(); k++)
        {
            UtilityNS::RelativeInfo contour_rela_info;
    
            UtilityNS::getRelativeInfo(centerPath, contourPoints[k], contour_rela_info);
    
            if (contour_rela_info.iFront == 0 && contour_rela_info.iBack == 0 && contour_rela_info.direct_distance > 3)
                continue;
    
            // 计算当前障碍物点到车辆位置的沿着中心轨迹的距离
            double longitudinalDist = getTwoPointsDistanceAlongTrajectory(centerPath, car_rela_info, contour_rela_info);
            if (contour_rela_info.iFront == 0 && longitudinalDist > 0)
                longitudinalDist = -longitudinalDist;
            double distance_from_center = trajectoryCosts[i].distance_from_center;
            lateralDist = fabs(contour_rela_info.perp_distance - distance_from_center) * 2;
    
            if (lateralDist < 2 && longitudinalDist < params.minFollowingDistance && longitudinalDist >= -critical_long_back_distance)
                trajectoryCosts[i].bBlocked = true;
    
            if (lateralDist != 0)
                trajectoryCosts[i].lateral_cost += 1.0 / lateralDist;
    
            if (longitudinalDist != 0)
                trajectoryCosts[i].longitudinal_cost += 1.0 / longitudinalDist;
    
            if (longitudinalDist >= -critical_long_back_distance && longitudinalDist < trajectoryCosts[i].closest_obj_distance)
                trajectoryCosts[i].closest_obj_distance = longitudinalDist;
        }
    }
    

    四、实际效果

    运行节点:

    roslaunch local_planner rollout_generator.launch
    roslaunch local_planner local_trajectory_generator.launch
    roslaunch waypoint_follower pure_persuit.launch

    启动rviz,订阅相关话题可以看到如下效果:

    5461af422a844c21b33be6365d522cac.png

    c0859492169db06d556d9ec3bfd0f46a.png

    3049a63e484a473105a9d7bbcf026b9e.png

    其中,白色线代表最优的局部路径规划轨迹


    五、总结

    本文简要介绍了autoware中的op_planner的局部路径规划算法,对关键的算法步骤进行简要说明,这部分模块可以实现基本的避障功能,相比于其他的局部路径规划算法,本文介绍的基于采样的局部路径规划实现简单,实时性较高,可以在实际部署运行。

    展开全文
  • 提出一种基于文化算法框架的萤火虫优化算法,结合动态避障和滑模控制求解足球机器人动态路径规划问题,并利用数学定理证明算法的收敛性.根据足球机器人在比赛中承担任务的分工不同,分别对进攻和防守两种角色进行分析...
  • 路径规划算法

    2021-05-10 16:29:08
    二、基于搜索的路径规划算法 1、Dijkstra算法 2、启发式搜索算法(A* ) 3、双向A *算法 4、任意时刻启发式搜索算法(ARA *) 5、实时学习A * 搜索算法(LRTA *) 6、实时适应性A *搜索算法(RTAA *) 7、动态...

    一、路径规划思维导图

    在这里插入图片描述

    二、基于搜索的路径规划算法

    1、Dijkstra算法
    在这里插入图片描述
    2、启发式搜索算法(A* )
    在这里插入图片描述
    3、双向A *算法
    在这里插入图片描述
    4、任意时刻启发式搜索算法(ARA *)
    在这里插入图片描述
    5、实时学习A * 搜索算法(LRTA *)
    在这里插入图片描述
    6、实时适应性A *搜索算法(RTAA *)
    在这里插入图片描述
    7、动态A *搜索算法(D *)
    在这里插入图片描述
    8、终身规划A *搜索算法
    在这里插入图片描述
    9、Anytime D * 搜索算法(小变动)图片描述
    10、Anytime D * 搜索算法(大变动)
    在这里插入图片描述
    11、最佳路径优先搜索算法
    在这里插入图片描述

    三、基于采样的运动规划算法

    1、快速扩展随机树(RRT)
    在这里插入图片描述
    2、目标偏好RRT算法
    在这里插入图片描述
    3、双向快速扩展随机树(RRT_CONNECT)
    在这里插入图片描述
    4、Extended_RRT算法
    在这里插入图片描述
    5、动态RRT算法
    在这里插入图片描述
    6、RRT *算法
    在这里插入图片描述
    7、快速行进树(FMT *)
    在这里插入图片描述
    8、启发式Informed RRT *算法
    在这里插入图片描述
    9、Batch Informed树算法(BIT *)
    在这里插入图片描述

    展开全文
  • 基于快速扩展随机树(RRT /rapidly exploring random tree)的路径规划算法,通过对状态空间中的采样点进行碰撞检测,避免了对空间的建模,能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速...

    基于快速扩展随机树(RRT / rapidly exploring random tree)的路径规划算法,通过对状态空间中的采样点进行碰撞检测,避免了对空间的建模,能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速有效地搜索高维空间,通过状态空间的随机采样点,把搜索导向空白区域,从而寻找到一条从起始点到目标点的规划路径,适合解决多自由度机器人在复杂环境下和动态环境中的路径规划。与PRM类似,该方法是概率完备且不最优的。

    RRT是一种多维空间中有效率的规划方法。它以一个初始点作为根节点,通过随机采样增加叶子节点的方式,生成一个随机扩展树,当随机树中的叶子节点包含了目标点或进入了目标区域,便可以在随机树中找到一条由从初始点到目标点的路径。基本RRT算法如下面伪代码所示:

    function feasible=collisionChecking(startPose,goalPose,map)%冲突检查:判断起始点到终点之间是否有障碍物

    feasible=true;%可行的,可执行的

    dir=atan2(goalPose(1)-startPose(1),goalPose(2)-startPose(2));%目标点和起始点之间的角度for r=0:0.5:sqrt(sum((startPose-goalPose).^2))%sqrt(sum((startPose-goalPose).^2)):两点间的距离

    posCheck= startPose + r.*[sin(dir) cos(dir)];%以0.5的间隔得到中间的点if ~(feasiblePoint(ceil(posCheck),map) && feasiblePoint(floor(posCheck),map) &&...

    feasiblePoint([ceil(posCheck(1)) floor(posCheck(2))],map) && feasiblePoint([floor(posCheck(1)) ceil(posCheck(2))],map))

    feasible=false;break;

    endif ~feasiblePoint([floor(goalPose(1)),ceil(goalPose(2))],map), feasible=false; end

    end

    function feasible=feasiblePoint(point,map)%判断点是否在地图内,且没有障碍物

    feasible=true;if ~(point(1)>=1 && point(1)<=size(map,1) && point(2)>=1 && point(2)<=size(map,2) && map(point(2),point(1))==255)%map(point(2),point(1))==255:没有障碍物

    feasible=false;

    end

    function distance=Distance(start_Pt,goal_Pt)

    distance=sqrt((start_Pt(1)-goal_Pt(1))^2+(start_Pt(2)-goal_Pt(2))^2);

    function [X_near,index]=Near(X_rand,T)

    min_distance=sqrt((X_rand(1)-T.v(1).x)^2+(X_rand(2)-T.v(1).y)^2);for T_iter=1:size(T.v,2)

    temp_distance=sqrt((X_rand(1)-T.v(T_iter).x)^2+(X_rand(2)-T.v(T_iter).y)^2);if temp_distance<=min_distance

    min_distance=temp_distance;

    X_near(1)=T.v(T_iter).x

    X_near(2)=T.v(T_iter).y

    index=T_iter;

    end

    end

    function X_rand=Sample(map,goal)% if rand<0.5

    % X_rand = rand(1,2) .* size(map); %random sample% else

    % X_rand=goal;%endif unifrnd(0,1)<0.5X_rand(1)= unifrnd(0,1)* size(map,1); %均匀采样

    X_rand(2)= unifrnd(0,1)* size(map,2); %random sampleelseX_rand=goal;

    end

    function X_new=Steer(X_rand,X_near,StepSize)

    theta= atan2(X_rand(1)-X_near(1),X_rand(2)-X_near(2)); %direction to extend sample to produce new node

    X_new= X_near(1:2) + StepSize *[sin(theta) cos(theta)];% dir_x = X_rand(1)- X_near(1);% dir_y = X_rand(2)- X_near(2);% dir = sqrt(dir_x^2 + dir_y^2);% X_new(1) = dir_x * StepSize/dir+X_near(1);% X_new(2) = dir_y * StepSize/dir+X_near(2);

    function X_rand=Sample(map,goal)% if rand<0.5

    % X_rand = rand(1,2) .* size(map); %random sample% else

    % X_rand=goal;%endif unifrnd(0,1)<0.5X_rand(1)= unifrnd(0,1)* size(map,1); %均匀采样

    X_rand(2)= unifrnd(0,1)* size(map,2); %random sampleelseX_rand=goal;

    end

    %***************************************

    %Author: Chenglong Qian%Date: 2019-11-5

    %***************************************

    %%流程初始化

    clear all; close all;

    x_I=1; y_I=1; %设置初始点

    x_G=700; y_G=700; %设置目标点

    goal(1)=x_G;

    goal(2)=y_G;

    Thr=50; %设置目标点阈值 当到这个范围内时则认为已到达目标点

    Delta= 30; %设置扩展步长,扩展结点允许的最大距离%%建树初始化

    T.v(1).x = x_I; %T是我们要做的树,v是节点,这里先把起始点加入到T里面来

    T.v(1).y =y_I;

    T.v(1).xPrev = x_I; %起始节点的父节点仍然是其本身

    T.v(1).yPrev =y_I;

    T.v(1).dist=0; %从父节点到该节点的距离,这里可取欧氏距离

    T.v(1).indPrev = 0; %父节点的索引%%开始构建树——作业部分

    figure(1);

    ImpRgb=imread('newmap.png');

    Imp=rgb2gray(ImpRgb);

    imshow(Imp)

    xL=size(Imp,1);%地图x轴长度

    yL=size(Imp,2);%地图y轴长度

    hold on

    plot(x_I, y_I,'ro', 'MarkerSize',10, 'MarkerFaceColor','r');

    plot(x_G, y_G,'go', 'MarkerSize',10, 'MarkerFaceColor','g');%绘制起点和目标点

    count=1;for iter = 1:3000

    % x_rand=[];%Step 1: 在地图中随机采样一个点x_rand%提示:用(x_rand(1),x_rand(2))表示环境中采样点的坐标

    x_rand=Sample(Imp,goal);% x_near=[];%Step 2: 遍历树,从树中找到最近邻近点x_near%提示:x_near已经在树T里

    [x_near,index]=Near(x_rand,T);

    plot(x_near(1), x_near(2), 'go', 'MarkerSize',2);% x_new=[];%Step 3: 扩展得到x_new节点%提示:注意使用扩展步长Delta

    x_new=Steer(x_rand,x_near,Delta);%检查节点是否是collision-freeif ~collisionChecking(x_near,x_new,Imp) %如果有障碍物则跳出continue;

    end

    count=count+1;%Step 4: 将x_new插入树T%提示:新节点x_new的父节点是x_near

    T.v(count).x= x_new(1);

    T.v(count).y= x_new(2);

    T.v(count).xPrev= x_near(1); %起始节点的父节点仍然是其本身

    T.v(count).yPrev= x_near(2);

    T.v(count).dist=Distance(x_new,x_near); %从父节点到该节点的距离,这里可取欧氏距离

    T.v(count).indPrev= index; %父节点的索引%Step 5:检查是否到达目标点附近%提示:注意使用目标点阈值Thr,若当前节点和终点的欧式距离小于Thr,则跳出当前for循环if Distance(x_new,goal)

    end%Step 6:将x_near和x_new之间的路径画出来%提示 1:使用plot绘制,因为要多次在同一张图上绘制线段,所以每次使用plot后需要接上hold on命令%提示 2:在判断终点条件弹出for循环前,记得把x_near和x_new之间的路径画出来% plot([x_near(1),x_near(2)],[x_new(1),x_new(2)]);%hold on

    line([x_near(1),x_new(1)],[x_near(2),x_new(2)]);

    pause(0.1); %暂停0.1s,使得RRT扩展过程容易观察

    end%%路径已经找到,反向查询if iter < 2000path.pos(1).x = x_G; path.pos(1).y =y_G;

    path.pos(2).x = T.v(end).x; path.pos(2).y =T.v(end).y;

    pathIndex= T.v(end).indPrev; %终点加入路径

    j=0;while 1path.pos(j+3).x =T.v(pathIndex).x;

    path.pos(j+3).y =T.v(pathIndex).y;

    pathIndex=T.v(pathIndex).indPrev;if pathIndex == 1

    breakend

    j=j+1;

    end%沿终点回溯到起点

    path.pos(end+1).x = x_I; path.pos(end).y = y_I; %起点加入路径for j = 2:length(path.pos)

    plot([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y], 'b', 'Linewidth', 3);

    endelsedisp('Error, no path found!');

    end

    参考链接:https://www.cnblogs.com/flyinggod/p/8727951.html

    展开全文
  • RRT路径规划算法(matlab实现)

    千次阅读 多人点赞 2019-11-05 01:07:00
    基于快速扩展随机树(RRT /rapidly exploring random tree)的路径规划算法,通过对状态空间中的采样点进行碰撞检测,避免了对空间的建模,能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速...

    基于快速扩展随机树(RRT / rapidly exploring random tree)的路径规划算法,通过对状态空间中的采样点进行碰撞检测,避免了对空间的建模,能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速有效地搜索高维空间,通过状态空间的随机采样点,把搜索导向空白区域,从而寻找到一条从起始点到目标点的规划路径,适合解决多自由度机器人在复杂环境下和动态环境中的路径规划。与PRM类似,该方法是概率完备且不最优的。

    RRT是一种多维空间中有效率的规划方法。它以一个初始点作为根节点,通过随机采样增加叶子节点的方式,生成一个随机扩展树,当随机树中的叶子节点包含了目标点或进入了目标区域,便可以在随机树中找到一条由从初始点到目标点的路径。基本RRT算法如下面伪代码所示:

     

     

     

     

     

    function feasible=collisionChecking(startPose,goalPose,map)%冲突检查:判断起始点到终点之间是否有障碍物
    
    feasible=true;%可行的,可执行的
    dir=atan2(goalPose(1)-startPose(1),goalPose(2)-startPose(2));%目标点和起始点之间的角度
    for r=0:0.5:sqrt(sum((startPose-goalPose).^2))%sqrt(sum((startPose-goalPose).^2)):两点间的距离
        posCheck = startPose + r.*[sin(dir) cos(dir)];%以0.5的间隔得到中间的点
        if ~(feasiblePoint(ceil(posCheck),map) && feasiblePoint(floor(posCheck),map) && ...
                feasiblePoint([ceil(posCheck(1)) floor(posCheck(2))],map) && feasiblePoint([floor(posCheck(1)) ceil(posCheck(2))],map))
            feasible=false;break;
        end
        if ~feasiblePoint([floor(goalPose(1)),ceil(goalPose(2))],map), feasible=false; end
    
    end
    
    function feasible=feasiblePoint(point,map)%判断点是否在地图内,且没有障碍物
    feasible=true;
    if ~(point(1)>=1 &&  point(1)<=size(map,1) && point(2)>=1 && point(2)<=size(map,2) && map(point(2),point(1))==255)%map(point(2),point(1))==255:没有障碍物
        feasible=false;
    end
    function distance=Distance(start_Pt,goal_Pt)
    distance=sqrt((start_Pt(1)-goal_Pt(1))^2+(start_Pt(2)-goal_Pt(2))^2);
    function [X_near,index]=Near(X_rand,T)
    min_distance=sqrt((X_rand(1)-T.v(1).x)^2+(X_rand(2)-T.v(1).y)^2);
    for T_iter=1:size(T.v,2)
        temp_distance=sqrt((X_rand(1)-T.v(T_iter).x)^2+(X_rand(2)-T.v(T_iter).y)^2);
        if temp_distance<=min_distance
            min_distance=temp_distance;
            X_near(1)=T.v(T_iter).x
            X_near(2)=T.v(T_iter).y
            index=T_iter;
        end
    end
    function X_rand=Sample(map,goal)
    % if rand<0.5
    %     X_rand = rand(1,2) .* size(map);   % random sample
    % else 
    %     X_rand=goal;
    % end
    
    if unifrnd(0,1)<0.5
       X_rand(1)= unifrnd(0,1)* size(map,1);   % 均匀采样
       X_rand(2)= unifrnd(0,1)* size(map,2);   % random sample
    else
       X_rand=goal;
    end
    function X_new=Steer(X_rand,X_near,StepSize)
    theta = atan2(X_rand(1)-X_near(1),X_rand(2)-X_near(2));  % direction to extend sample to produce new node
    X_new = X_near(1:2) + StepSize * [sin(theta)  cos(theta)];
    
    % dir_x = X_rand(1)- X_near(1);
    % dir_y = X_rand(2)- X_near(2);
    % dir = sqrt(dir_x^2 + dir_y^2);
    % X_new(1) = dir_x * StepSize/dir+X_near(1);
    % X_new(2) = dir_y * StepSize/dir+X_near(2);

     

    function X_rand=Sample(map,goal)
    % if rand<0.5
    %     X_rand = rand(1,2) .* size(map);   % random sample
    % else 
    %     X_rand=goal;
    % end
    
    if unifrnd(0,1)<0.5
       X_rand(1)= unifrnd(0,1)* size(map,1);   % 均匀采样
       X_rand(2)= unifrnd(0,1)* size(map,2);   % random sample
    else
       X_rand=goal;
    end

     

     

    %***************************************
    %Author: Chenglong Qian
    %Date: 2019-11-5
    %***************************************
    %% 流程初始化
    clear all; close all;
    x_I=1; y_I=1;           % 设置初始点
    x_G=700; y_G=700;       % 设置目标点
    goal(1)=x_G;
    goal(2)=y_G;
    Thr=50;                 %设置目标点阈值 当到这个范围内时则认为已到达目标点
    Delta= 30;              % 设置扩展步长,扩展结点允许的最大距离
    %% 建树初始化
    T.v(1).x = x_I;         % T是我们要做的树,v是节点,这里先把起始点加入到T里面来
    T.v(1).y = y_I; 
    T.v(1).xPrev = x_I;     % 起始节点的父节点仍然是其本身
    T.v(1).yPrev = y_I;
    T.v(1).dist=0;          %从父节点到该节点的距离,这里可取欧氏距离
    T.v(1).indPrev = 0;     %父节点的索引
    %% 开始构建树——作业部分
    figure(1);
    ImpRgb=imread('newmap.png');  
    Imp=rgb2gray(ImpRgb);
    imshow(Imp)
    xL=size(Imp,1);%地图x轴长度
    yL=size(Imp,2);%地图y轴长度
    hold on
    plot(x_I, y_I, 'ro', 'MarkerSize',10, 'MarkerFaceColor','r');
    plot(x_G, y_G, 'go', 'MarkerSize',10, 'MarkerFaceColor','g');% 绘制起点和目标点
    count=1;
    for iter = 1:3000
    %     x_rand=[];
        %Step 1: 在地图中随机采样一个点x_rand
        %提示:用(x_rand(1),x_rand(2))表示环境中采样点的坐标
        x_rand=Sample(Imp,goal);
    %     x_near=[];
        %Step 2: 遍历树,从树中找到最近邻近点x_near 
        %提示:x_near已经在树T里
        [x_near,index]= Near(x_rand,T);
        plot(x_near(1), x_near(2), 'go', 'MarkerSize',2);
    %     x_new=[];
        %Step 3: 扩展得到x_new节点
        %提示:注意使用扩展步长Delta
        x_new=Steer(x_rand,x_near,Delta);
        %检查节点是否是collision-free
        if ~collisionChecking(x_near,x_new,Imp) %如果有障碍物则跳出
           continue;
        end
        count=count+1;
        
        %Step 4: 将x_new插入树T 
        %提示:新节点x_new的父节点是x_near
        T.v(count).x = x_new(1);        
        T.v(count).y = x_new(2); 
        T.v(count).xPrev = x_near(1);     % 起始节点的父节点仍然是其本身
        T.v(count).yPrev = x_near(2);
        T.v(count).dist=Distance(x_new,x_near);          %从父节点到该节点的距离,这里可取欧氏距离
        T.v(count).indPrev = index;     %父节点的索引
        %Step 5:检查是否到达目标点附近 
        %提示:注意使用目标点阈值Thr,若当前节点和终点的欧式距离小于Thr,则跳出当前for循环
        if Distance(x_new,goal) < Thr
            break;
        end
       %Step 6:将x_near和x_new之间的路径画出来
       %提示 1:使用plot绘制,因为要多次在同一张图上绘制线段,所以每次使用plot后需要接上hold on命令
       %提示 2:在判断终点条件弹出for循环前,记得把x_near和x_new之间的路径画出来
    %    plot([x_near(1),x_near(2)],[x_new(1),x_new(2)]);
    %    hold on
      line([x_near(1),x_new(1)],[x_near(2),x_new(2)]);
       pause(0.1); %暂停0.1s,使得RRT扩展过程容易观察
    end
    %% 路径已经找到,反向查询
    if iter < 2000
        path.pos(1).x = x_G; path.pos(1).y = y_G;
        path.pos(2).x = T.v(end).x; path.pos(2).y = T.v(end).y;
        pathIndex = T.v(end).indPrev; % 终点加入路径
        j=0;
        while 1
            path.pos(j+3).x = T.v(pathIndex).x;
            path.pos(j+3).y = T.v(pathIndex).y;
            pathIndex = T.v(pathIndex).indPrev;
            if pathIndex == 1
                break
            end
            j=j+1;
        end  % 沿终点回溯到起点
        path.pos(end+1).x = x_I; path.pos(end).y = y_I; % 起点加入路径
        for j = 2:length(path.pos)
            plot([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y], 'b', 'Linewidth', 3);
        end
    else
        disp('Error, no path found!');
    end

     

     参考链接:https://www.cnblogs.com/flyinggod/p/8727951.html

     

    展开全文
  • 路径规划与避障算法(一)---DWA算法概述

    万次阅读 多人点赞 2018-09-18 22:01:12
    DWA(动态窗口算法) 算法概述 算法原理可见: https://blog.csdn.net/heyijia0327/article/details/44983551 此方法部分代码来自于ROS_Navigation包的源码,部分来自于项目中的改编: 通过线速度与角速度的交叉组合...
  • DWA算法全称为dynamic window approach,其原理主要是在速度空间(v,w)中采样多组速度,并模拟这些速度在一定时间内的运动轨迹,再通过一个评价函数对这些轨迹打分,最优的速度被选择出来发送给下位机。 1 原理分析...
  • DWA算法全称为dynamic window approach,其原理主要是在速度空间(v,w)中采样多组速度,并模拟出这些速度在一定时间内的运动轨迹,并通过评价函数对这些轨迹进行评价,选取最优轨迹对应的速度驱动机器人运动。...
  • DWA算法全称为dynamic window approach,其原理主要是在速度空间(v,w)中采样多组速度,并模拟这些速度在一定时间内的运动轨迹,再通过一个评价函数对这些轨迹打分,最优的速度被选择出来发送给下位机。 1 原理分析...
  • RRT路径搜索算法C++实现

    千次阅读 2019-05-28 11:45:22
    基于RRT的路径规划算法,通过对状态空间中的采样点进行碰撞检测,避免了对空间的建模,能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速有效地搜索高维空间,通过状态空间的随机采样点,把...
  • DWA算法全称为dynamic window approach,其原理主要是在速度空间(v,w)中采样多组速度,并模拟这些速度在一定时间内的运动轨迹,再通过一个评价函数对这些轨迹打分,最优的速度被选择出来发送给下位机。 1 原理分析...
  • 01 目录环境需求怎样使用本地化扩展卡尔曼滤波本地化无损卡尔曼滤波本地化...路径规划动态窗口方式基于网格的搜索迪杰斯特拉算法A*算法势场算法模型预测路径生成路径优化示例查找表生成示例状态晶格规划均匀极性采样...
  • 针对人工势场法易陷入局部极小值的缺陷,提出旋转速度矢量角以精确定位逃离点,并将TAS-RRT算法与人工势场算法结合进行动态路径规划。采用人工势场法进行避障规划,当陷入局部最小值时,使用基于速度矢量角度差引导...
  • 学习记录-基于采样路径规划算法内容来源RRT主要步骤动态效果展示优缺点:自己进行的改进尝试RRT*主要步骤NearCChooseParentrewire总结及动态效果图Informed RRT*其他优化RRT的方式总结 内容来源 记录学习深蓝路径...
  • 机器人局部避障的动态窗口法(dynamic window approach)

    万次阅读 多人点赞 2015-04-10 19:23:54
    机器人局部路径规划方法有很多,ROS中主要采用的是动态窗口法(但是和原论文中的dwa方法不一样,具体见最后)。动态窗口法主要是在速度(v,w)空间中采样多组速度,并模拟机器人在这些速度下一定时间(sim_period)内的...
  • 对于机器人局部路径规划算法有很多,DWA算法(中文叫动态窗口法)是其中之一,理解起来简单些。它能根据代价地图以及当前的位姿和速度,在当前点和目标点之间给一个合理的速度。 DWA算法的实现 主要是在速度...
  • 一、目录环境需求怎样使用本地化扩展卡尔曼滤波本地化无损卡尔曼滤波本地...路径规划动态窗口方式基于网格的搜索迪杰斯特拉算法A*算法势场算法模型预测路径生成路径优化示例查找表生成示例状态晶格规划均匀极性采样(.....
  • 其次,针对已识别车位类型,设计改进的 RRT 算法,进行路径规划研究。通过规 定情境构建采样地图,使路径起点与终点在安全范围内采样效率提高。采用采样点预处理方法,根据前一周期的环境搜索下一周期的初始环境,...
  • rrt自己写的.zip

    2020-04-06 11:46:06
    基于快速扩展随机树(RRT / rapidly exploring random tree)的路径规划算法,通过对状态空间中的采样点进行碰撞检测,避免了对空间的建模,能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够...
  • 蒙地卡罗算法相对动态规划,会有点不那么准。因为蒙地卡罗每一次的路径都是不一样的。 2. 如果环境的状态空间非常大,或者最终状态只有非常小的概率达到。那么蒙地卡罗算法将会很难处理。关于蒙地卡罗MC在马可洛夫...
  • 第一个控制器采用两层结构,由路径规划器和非线性模型预测控制器(NMPC)组成。第二控制器遵循轮廓控制的思想,将这两个任务结合在一个非线性优化问题中。利用线性化得到的线性齿形变化模型,在每个采样点建立凸二次...
  • OpenCV密集配准BM.cpp

    2020-01-02 16:55:50
    其中动态规划很重要两个参数P1,P2是这样设定的: P1 =8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize; P2 = 32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize; cn是图像的通道数, SADWindowSize是SAD窗口大小,...
  • 语音识别的MATLAB实现

    热门讨论 2009-03-03 21:39:18
    语音识别程序的核心部分即采用合适的算法来识别不同的语音信号,在特定人语音识别算法中,对于孤立词语语音识别而言,最为简单的方法是采用DTW(Dynamic Time Warping,动态时间弯折)算法,该算法基于动态规划)...
  • 1.2.2 路径规划的凸体和PVS计算 1.2.3 处理复杂的地形 1.2.4 BSP叶节点中的面 1.2.5 寻找叶凸体 1.2.6 凸体和伪人口 1.2.7 潜在可视集 1.3 光照贴图的构造 1.3.1 生成光照贴图的坐标 1.3.2 光照贴图的打包 1.3.3 对...
  • 1.2.2 路径规划的凸体和PVS计算 1.2.3 处理复杂的地形 1.2.4 BSP叶节点中的面 1.2.5 寻找叶凸体 1.2.6 凸体和伪人口 1.2.7 潜在可视集 1.3 光照贴图的构造 1.3.1 生成光照贴图的坐标 1.3.2 光照贴图的打包 1.3.3 对...
  • 语音识别技术文章.rar

    热门讨论 2011-05-12 10:31:08
    10.2.1 动态规划技术(DP) 10. 2.2 DTW算法的改进 10.3 隐马尔可夫模型(HMM) 10.3.1 隐马尔可夫模型的定义 10.3. 2 HMM中的3个基本问题及其解决方案 10. 3.3 隐马尔可夫模型的类型 10.3.4 HMM算法实现的问题 ...
  • 路径规划A*算法及SLAM自主地图创建导航算法 冯兵的blog slam imu和单目的数据融合开源代码(EKF) imu和单目的数据融合开源代码(非线性优化) 双目立体匹配 计算机视觉的一些库文件 人脸检测总结 行为识别总结 Free-...
  • 1 动态规划技术 DP  10. 2. 2 DTW算法的改进  10. 3 隐马尔可夫模型 HMM  10. 3. 1 隐马尔可夫模型的定义  10. 3. 2 HMM中的3个基本问题及其解决方案  10. 3. 3 隐马尔可夫模型的类型  10. 3. 4 ...
  • 实用语音识别基础

    热门讨论 2013-08-01 19:58:17
    1 动态规划技术 DP  10. 2. 2 DTW算法的改进  10. 3 隐马尔可夫模型 HMM  10. 3. 1 隐马尔可夫模型的定义  10. 3. 2 HMM中的3个基本问题及其解决方案  10. 3. 3 隐马尔可夫模型的类型  10. 3. 4 ...

空空如也

空空如也

1 2
收藏数 32
精华内容 12
关键字:

动态路径规划采样算法