在我以前的作品里有关于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) 接下来是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)]) 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)])( 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 找到最近节点函数
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 根据步长扩展点
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站点个关注，我也能免费给你，但你不能拿去赚钱，就当我谢谢你了。