【路径规划】基于拓展随机树算法RRT的路径规划问题附Matlab代码

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

个人主页：Matlab科研工作室

个人信条：格物致知。

更多Matlab仿真内容点击

智能优化算法  神经网络预测 雷达通信  无线传感器

信号处理 图像处理 路径规划 元胞自动机 无人机

⛄ 内容介绍

移动机器人运动规划技术是自主移动机器人导航的核心技术之一,而路径规划技术是导航技术研究的一个关键课题.路径规划的任务是:依据一定的评价准则(如距离最短,时间最短,工作代价最小等等),在一个存在障碍物的工作环境内,寻求一条从初始点开始到目标点结束的较优的无碰撞路径.本文旨在结合实际环境基于快速扩展随机树(Rapidly-Exploring Random Tree, RRT)算法实现自主移动机器人的路径规划。

⛄ 部分代码

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

%% 流程初始化

clear all; close all;

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

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

Thr=50;                 %设置目标点阈值

Delta= 20;              % 设置扩展步长

%% 建树初始化

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

    %Step 1: 在地图中随机采样一个点x_rand

    %提示：用（x_rand(1),x_rand(2)）表示环境中采样点的坐标

    x_rand=[xL, yL].*rand(1,2);

    

    %Step 2: 遍历树，从树中找到最近邻近点x_near 

    %提示：x_near已经在树T里

    x_near=[];

    min_dis_toT = inf;

    x_near_idx = -1;

    for i = 1:length(T.v)

        t_node = T.v(i);

        t_nodexy = [t_node.x, t_node.y];

        dis_toT = norm(t_nodexy - x_rand);

        if dis_toT < min_dis_toT

            min_dis_toT = dis_toT;

            x_near = t_nodexy;

            x_near_idx = i;

        end

    end

%     disp(['x_near is ', mat2str(x_near)]);

    %Step 3: 扩展得到x_new节点

    %提示：注意使用扩展步长Delta

    %检查节点是否是collision-free

    dxy = x_rand - x_near;

    dxynorm = dxy/norm(dxy);

    x_new = x_near + dxynorm*Delta;

%     disp(['x_new is ', mat2str(x_new)]);

    if ~collisionChecking(x_near,x_new,Imp) 

        continue;

    end

%     disp(['x_new is collision free, add to Tree']);

    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=norm(x_new - x_near);          

    T.v(count).indPrev = x_near_idx;     

    

    %Step 5:检查是否到达目标点附近 

    %提示：注意使用目标点阈值Thr，若当前节点和终点的欧式距离小于Thr，则跳出当前for循环

    if norm(x_new-[x_G,y_G]) < 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_new(1)],[x_near(2), x_new(2)], 'r');

   drawnow;

%    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

⛄ 运行结果

⛄ 参考文献

[1]王滨, 金明河, 谢宗武,等. 基于启发式的快速扩展随机树路径规划算法[J]. 机械制造, 2007, 45(12):4.

❤️ 关注我领取海量matlab电子书和数学建模资料

❤️部分理论引用网络文献，若有侵权联系博主删除