rrt算法

@wangqi

于 2024-04-15 14:33:55 发布

阅读量647

点赞数 10

分类专栏：规划算法文章标签：算法 matlab

本文链接：https://blog.csdn.net/qq_54644501/article/details/137759893

版权

规划算法专栏收录该内容

5 篇文章 0 订阅

订阅专栏

1、算法原理

1）将起点 $q_{ini}$ 加入树，随机扩展一个节点 $q_{rand}$ ，并在已有节点中寻找距离其最近的节点 $q_{near}$ ；

2）在 $q_{near}$ ---> $q_{rand}$ 方向上，以采样步长γ扩展，并进行collisionchecking，无碰撞，得到新节点 $q_{new}$ ；有碰撞，重新采样 $q_{rand}$ ；

3）计算 $q_{new}$ 和 $q_{goal}$ 之间距离，当小于某一给定值时，终止采样；

4）从 $q_{goal}$ 回溯，找到起点 $q_{ini}$ ，从而确定从起点到终点的路径。

rrt算法具有概率的完备性，只要采样次数足够多，总可以找到连接起点终点的一条路径；

但在路径搜索过程中，完全没有考虑cost,这会导致找到的路径比较差。

2、matlab代码

clear;
close all;
clc;

%% 载入地图
figure(1);
pic=imread('map.png');
fig=rgb2gray(pic);
imshow(fig);
hold on;
title('RRT* algorithm');
mLength=size(fig,2);
mWidth=size(fig,1);
Point=ginput(2);  %获取起点坐标、终点坐标
startPoint=Point(1,:);
goalPoint=Point(2,:);
disp(['起点坐标为：(',num2str(startPoint),')']);
disp(['终点坐标为：(',num2str(goalPoint),')']);
plot(startPoint(1),startPoint(2),'mo','MarkerSize',8,'MarkerFaceColor','m');
plot(goalPoint(1),goalPoint(2),'rp','MarkerSize',8,'MarkerFaceColor','r');
%% 参数初始化
sampleNums=30000;  %采样次数
stepLength=20;     %采样步长
%将起点加入树
nodes_list(1).position=startPoint;
nodes_list(1).parentind=0;
nodes_list(1).cost=0;
count=0;
for j=1:sampleNums
    count=count+1;
%随机采样，rand-->new
node_rand=[mLength*rand,mWidth*rand];
N=size(nodes_list,2); %获取树中当前节点数
minDis=Inf; %初始化树中节点距离newnode的最小距离
for i=1:N
    d=norm(node_rand-nodes_list(i).position);
    if minDis > d
        minDis=d;
        ind=i;
    end
end
near2rand=(node_rand-nodes_list(ind).position)/norm(node_rand-nodes_list(ind).position)*stepLength;
newnode=nodes_list(ind).position+near2rand;
% collisionchecking
flag=collisionCheck(newnode,nodes_list(ind).position,fig);
if flag
    plot(newnode(1),newnode(2),'ro','MarkerFaceColor','r','MarkerSize',2);
    plot([nodes_list(ind).position(1),newnode(1)],[nodes_list(ind).position(2),newnode(2)],'b-','LineWidth',1);
    nodes_list(N+1).position=newnode;
    nodes_list(N+1).parentind=ind;
    nodes_list(N+1).cost=nodes_list(ind).cost+norm(near2rand);
    if norm(newnode-goalPoint)<=stepLength
        nodes_list(N+2).position=goalPoint;
        nodes_list(N+2).parentind=N+1;
        nodes_list(N+2).cost=nodes_list(N+1).cost+norm(newnode-goalPoint);
        plot([newnode(1),goalPoint(1)],[newnode(2),goalPoint(2)],'b-');
  break;
    end
end
pause(0.0);
end
% 从终点回溯，寻找路径
index=N+2;
while nodes_list(index).parentind~=0
    plot([nodes_list(index).position(1),nodes_list(nodes_list(index).parentind).position(1)], ...
        [nodes_list(index).position(2),nodes_list(nodes_list(index).parentind).position(2)],'r-',...
        'LineWidth',3);
    index=nodes_list(index).parentind;
end
if count==sampleNums
    disp('超出采样次数');
else
    disp(['SUCCESS!采样次数为：',num2str(count)]);
    disp(['cost:',num2str(nodes_list(N+2).cost)]);
end

碰撞检测函数

function flag = collisionCheck(newnode,nearnode,map)
%COLLISIONCHECK 
%   flag==0 有障碍物，拓展失败 flag==1 无障碍物
    near2new=newnode-nearnode;
    flag=1;
    if newnode(1)>=size(map,2)||newnode(1)<=1||newnode(2)>=size(map,1)||newnode(2)<=1
        flag=0;
    end
for i=0:0.1:1
    if flag==0
        break;
    end
    checkingPoint=nearnode+i*near2new;
    if checkingPoint(1)>=size(map,2)||checkingPoint(1)<=1||checkingPoint(2)>=size(map,1)||checkingPoint(2)<=1
        flag=0;
        break;
    end
    if map(ceil(checkingPoint(2)),ceil(checkingPoint(1)))==0||map(ceil(checkingPoint(2)),floor(checkingPoint(1)))==0 ...
            ||map(floor(checkingPoint(2)),ceil(checkingPoint(1)))==0||map(floor(checkingPoint(2)),floor(checkingPoint(1)))==0
        flag=0;
        break;
    end
end

欢迎批评指正！