手把手教学RRT*（RRTSTAR）三维算法MATLAB仿真（代码可直接运行，视频手把手教学）

最新推荐文章于 2024-05-19 23:27:04 发布

★不秃头★

最新推荐文章于 2024-05-19 23:27:04 发布

阅读量1.3k

点赞数 11

文章标签： matlab 开发语言

本文链接：https://blog.csdn.net/gentleman597/article/details/134617933

版权

在我以前的作品里有关于RRT算法的视频和代码，今天主要讲解一下RRT*算法的原理。RRT*算法主要是在RRT算法的基础上加上了重写父节点和随机重连的两个步骤。具体的实现方式我想以视频的方式向大家讲解，个人感觉讲解的十分详细。视频连接在这里，希望大家看完有任何不懂的地方直接指出。

视频中还讲述了RRT*算法的整套代码实现流程以及代码的手把手教学。代码内容清晰易懂，且低耦合。而且可以通过注释掉相应模块后直接变为传统RRT算法。下面是效果图。

其中左侧是传统RRT算法，右侧是RRT*算法，通过图片可以看出，RRT*算法相比于传统RRT算法节点收敛性好。下面是全部代码展示，还是挺多的。。。。。

下面是部分代码，代码获取方式我放在了评论区（不用复制了，指定是运行不成功，我就是单纯凑字数，混个播放量）。

首先是main函数，通过增加路径点可以实现多目标点路径规划。

%% 清空变量
clear;
clc;

%% 变量定义 
axisStart = [0 0 0];                                 %空间起始坐标
axisLWH = [1000 1000 1000];                          %空间长宽高
cubeInfo.exist = 0;                                  %长方体是否存在，若存在则置为1
cylinderInfo.exist = 0;                              %圆柱体是否存在，若存在则置为1
sphereInfo.exist = 0;                                %球是否存在，若存在则置为1
pathPoint = [0     0    0;
            600 200 300;
             600  600  200];                         %一系列的路径点
cubeInfo = CreateCubeObject(cubeInfo);               %创建长方体障碍物信息
cylinderInfo = CreateCylinderObject(cylinderInfo);   %创建圆柱体障碍物信息
sphereInfo = CreateSphereObject(sphereInfo);         %创建圆柱障碍物信息

%% 画图
DrawPicture(cubeInfo,cylinderInfo,sphereInfo,pathPoint,axisStart,axisLWH);

%% 寻找路径
totalPath = [];
for k1 = 1:size(pathPoint,1)-1
    startPoint = pathPoint(k1,:);
    goalPoint = pathPoint(k1+1,:);
    [path,T] = RRTStar(axisStart,axisLWH,startPoint,goalPoint,cubeInfo,cylinderInfo,sphereInfo);
    
    if ~isempty(path)
        for k2 = 1:size(path,1)-1
            line([path(k2,1),path(k2+1,1)],[path(k2,2),path(k2+1,2)],[path(k2,3),path(k2+1,3)],'LineWidth',1,'Color','red');
        end
        totalPath = [totalPath ;path];
    end
    
end

pathLength = 0;
for k1 = 1:size(totalPath,1)-1
    pathLength = pathLength+CalcuDistance([totalPath(k1,1) totalPath(k1,2) totalPath(k1,3)],[totalPath(k1+1,1) totalPath(k1+1,2) totalPath(k1+1,3)]);
end
disp(['路径长度为:',num2str(pathLength)]);

接下来是最核心代码，RRTStar函数，内部可修改一些参数实现不同效果。

function [path,T] = RRTStar(axisStart,axisLWH,startPoint,goalPoint,cubeInfo,cylinderInfo,sphereInfo)
%% 变量定义
iterMax = 5000;                         %最大迭代次数
iter = 0;                               %当前迭代次数
step = 10;                              %步长
count = 1;                              %计数器
Thr = 10;                               %阈值
randProbability = 0.8;                  %随机采样概率，范围0-1之间，越大随机性越大。越小导向性越大，收敛快,但是容易找不到路径
r = 5*step;                             %影响范围，若大一点路径规划效果好，但是迭代慢。若小一点，路径规划效果和rrt算法越贴近
flag = 0;                               %路径规划参数，当规划失败时返回0，规划成功返回1
%%%%%%%%%%%   配置树的信息    %%%%%%%%%%
T.x(1) = startPoint(1);
T.y(1) = startPoint(2);
T.z(1) = startPoint(3);
T.pre(1) = 0;
T.cost(1) = 0;
path = [];

while iter<=iterMax
    %% 迭代次数加1
    iter = iter+1;
    %% 空间中随机采样
    randCoor = RandSample(axisStart,axisLWH,goalPoint,randProbability);    
    %% 寻找树上最近的点
    [nearestCoor,parentIndex] = FindNearstPoint(T,randCoor);  
    %% 根据指定步长扩展新的点
    newCoor = ExpandPoint(nearestCoor,randCoor,step);    
    %% 重写
%     parentIndex = RewriteFunction(T,newCoor,r,parentIndex);        
    %% 碰撞检测
    A = [T.x(parentIndex),T.y(parentIndex),T.z(parentIndex)];
    B = newCoor;
    collisionFlag = CollisionDetection(cubeInfo,cylinderInfo,sphereInfo,A,B,step);    
    if collisionFlag
        continue;
    end   
    %% 将新点插入进来
    count = count + 1;
    T.x(count) = newCoor(1);
    T.y(count) = newCoor(2);
    T.z(count) = newCoor(3);
    T.pre(count) = parentIndex;
    T.cost(count) = CalcuDistance(A,B)+T.cost(parentIndex);    
    line([A(1),B(1)],[A(2),B(2)],[A(3),B(3)],'LineWidth',1);   % 要是想只画最终的路径图，不画扩展图就把这行注释掉
    pause(0.01)  %不想看动画，就把这行注释        
    %% 随机重连
%     T = RandRelink(T,newCoor,cubeInfo,cylinderInfo,sphereInfo,step,r);   
    if CalcuDistance(newCoor,goalPoint)<Thr
        flag = 1;
        break;
    end    
end
%% 路径规划失败直接返回
if ~flag 
    disp('路径规划失败');
    return;
else
    disp('路径规划成功');
end

%% 寻找路径
path = FindPath(T,startPoint,goalPoint);

接下来是RRT*算法核心部分，重写和随机重连部分

function parentIndex = RewriteFunction(T,newCoor,r,parentIndex)
%% 重写函数，将新点newCoor重新连接到代价最小

% 下面是寻找到潜在的父节点，在变量potentialParent里
potentialParent = -ones(1,size(T.x,2));
count = 1;
for k1 = 1:size(T.x,2)
    if CalcuDistance(newCoor,[T.x(k1),T.y(k1),T.z(k1)])<r
        potentialParent(count) = k1;
        count = count+1;
    end
end
potentialParent(potentialParent==-1)=[];

%迭代寻找最小代价，并重新选择新节点newCoor的父节点，并把parentIndex改为新父节点对应的索引
for k2 = 1:size(potentialParent,2)
    % potentialParent(k2)潜在父节点的序号
    pp = [T.x(potentialParent(k2)),T.y(potentialParent(k2)),T.z(potentialParent(k2))];
    p = [T.x(parentIndex),T.y(parentIndex),T.z(parentIndex)];
    if (CalcuDistance(pp,newCoor)+T.cost(potentialParent(k2)))<(CalcuDistance(p,newCoor)+T.cost(parentIndex))
        parentIndex = potentialParent(k2);
    end
end

function T = RandRelink(T,newCoor,cubeInfo,cylinderInfo,sphereInfo,step,r)
%% 随机重连，将新节点newCoor周围节点的父节点尝试改为新节点，若代价小于原来的代价值，则确认更改

% 寻找需要需要修改父节点的节点放入potentialParent里面。
potentialParent = -ones(1,size(T.x,2));
count = 1;
for k1 = 1:size(T.x,2)
    if CalcuDistance(newCoor,[T.x(k1),T.y(k1),T.z(k1)])<r
        potentialParent(count) = k1;
        count = count+1;
    end
end
potentialParent(potentialParent==-1)=[];

% 找到新节点的父节点集合，存在变量Index里
Index = -ones(1,size(T.x,2));
index = T.pre(end);
count = 1;
while T.pre(index)~=0
    Index(count) = index;
    index = T.pre(index);
    count = count+1;  
end
Index(Index==-1) = [];
potentialParent(ismember(potentialParent,Index)==1) = [];  %这行的意思是把需要修改父节点的节点集合，剔除新节点的父节点，以免出现环

% 根据代价决定是否修改父节点
for k2 = 1:size(potentialParent,2)
    pp = [T.x(potentialParent(k2)),T.y(potentialParent(k2)),T.z(potentialParent(k2))];
    if T.cost(potentialParent(k2))>( T.cost(end)+ CalcuDistance(pp,newCoor))
        if ~CollisionDetection(cubeInfo,cylinderInfo,sphereInfo,pp,newCoor,step)
            
            T.pre(potentialParent(k2)) = size(T.x,2)+1;
            T.cost(potentialParent(k2)) = CalcuDistance(newCoor,pp);
        end
    end
end

随机采样函数

function randCoor = RandSample(axisStart,axisLWH,goalPoint,randProbability)
%% 随机采样，当小于某一概率时，采用随机采样，其他情况直接将目标点作为采样点
if rand<randProbability 
    randX = rand*axisLWH(1)+axisStart(1);
    randY = rand*axisLWH(2)+axisStart(2);
    randZ = rand*axisLWH(3)+axisStart(3);
    randCoor = [randX,randY,randZ];
else
    randCoor = goalPoint;
end

找到最近节点函数

function [nearestCoor,parentIndex] = FindNearstPoint(T,randCoor)
%% 遍历整个树，寻找距离随机点randCoor最近的点，标记为nearestCoor
tempDis = inf;
parentIndex = -1;
for k1 = 1:size(T.x,2)
    dis = CalcuDistance(randCoor,[T.x(k1),T.y(k1),T.z(k1)]);
    if dis<tempDis
        tempDis = dis;
        parentIndex = k1;
    end
end
nearestCoor = [T.x(parentIndex),T.y(parentIndex),T.z(parentIndex)];

根据步长扩展点

function newCoor = ExpandPoint(nearCoor,randCoor,step)
%% 按照指定步长，随机点方向扩展新的点，命名为newCoor。这里按步长扩展点采用球坐标与直角坐标的转化方式
deltaX = randCoor(1)-nearCoor(1);
deltaY = randCoor(2)-nearCoor(2);
deltaZ = randCoor(3)-nearCoor(3);

r = sqrt(deltaX^2+deltaY^2+deltaZ^2);
fai = atan2(deltaY,deltaX);
theta = acos(deltaZ/r);

x = step*sin(theta)*cos(fai);
y = step*sin(theta)*sin(fai);
z = step*cos(theta);

newCoor = [x+nearCoor(1),y+nearCoor(2),z+nearCoor(3)];

这些是代码中最核心的部分，因为代码太多，不在这里展示过多，具体获取方式可以看我b站视频（up主名字叫“-不秃头-”）。代码实际上是不免费的，是因为有人拿着我开源代码去卖（很难理解）。但是如果你要是真想学代码，帮我公众号和b站点个关注，我也能免费给你，但你不能拿去赚钱，就当我谢谢你了。