一文实现三维的RRT、RRT* 和informed-RRT*（提供matlab代码）
RRT
RRT（Rapidly-exploring Random Tree）算法是一种用于解决路径规划问题的算法，特别是在机器人和自动驾驶汽车领域中非常受欢迎。RRT算法能够快速地探索空间，找到从起点到终点的可行路径。下面是RRT算法的详细介绍：
算法原理
RRT算法的核心思想是随机地在搜索空间中生长一棵树，直到它到达目标区域。这个过程可以概括为以下几个步骤：
- 初始化：从起点开始，创建一个空的树。
- 随机采样：在搜索空间中随机选择一个点。
- 扩展：从树中最近的节点开始，尝试向随机采样的点扩展，如果这个扩展不会导致碰撞，就将新点加入树中。
- 连接：如果新点可以与树中的另一个节点连接而不发生碰撞，那么可以选择连接这两个点，以优化路径。
- 迭代：重复步骤2到4，直到达到某个停止条件，如找到目标点、达到最大迭代次数或运行时间限制。
算法特点
- 快速：RRT算法能够快速地探索空间，因为它不需要预先知道整个环境的地图。
- 简单：算法实现简单，容易理解和编程。
- 灵活性：适用于各种复杂的环境，包括动态和静态障碍物。
- 完备性：在理论上，如果给定无限的时间，RRT算法能够找到从起点到终点的路径（如果存在的话）。
- 概率完备性：实际上，RRT算法能够在有限时间内以很高的概率找到可行路径。
算法流程
- 选择：随机选择搜索空间中的一个点，作为探索的目标点。
- 扩展：从树中的节点集合中找到距离目标点最近的节点。
- 生长：从这个最近的节点出发，尝试向目标点生长。这一步通常涉及到线性插值或二次插值，以确保路径的平滑性。
- 安全检查：在生长过程中，需要检查路径是否与障碍物发生碰撞。如果发生碰撞，则停止生长。
- 终止条件：当目标点被包含在树中，或者达到预定的迭代次数或时间限制时，算法终止。
算法变体
RRT算法有多种变体，以适应不同的应用场景：
- RRT (RRT Star)*：优化了RRT算法，通过重新规划和修剪树来提高路径的质量。
- informed-RRT(和RRT 只有采样空间上的区别)**：随机采样的空间被限定在一个区域。
应用领域
RRT算法广泛应用于：
- 机器人路径规划：用于机器人在复杂环境中的导航。
- 自动驾驶汽车：用于车辆在城市环境中的动态路径规划。
- 空间探索：用于太空探测器在未知环境中的导航。
- 游戏AI：用于游戏角色的路径规划。
RRT算法因其简单性和有效性，在路径规划领域有着广泛的应用前景。
RRT初始代码参考博客
https://blog.csdn.net/gentleman597/article/details/124543703
该博客提供了RRT算法源码，这里不再赘述。
运行的结果图如下图所示：
代码地址：https://blog.csdn.net/BBeymax?type=download
RRT*算法
RRT*在RRT基础上做了改进，主要是进行了重新选择父节点和重布线的操作。
在采样点加入路径树以后，以其为圆心画了一个小圈，考虑是否有更好的父节点，连到那些节点上能使起点到该点的距离更短（尽管那些节点并不是离采样点最近的点）。如果选择了更加合适的父节点，那么就把它们连接起来，并去除原来的连线（重布线）。
代码方面，主要有三个内容需要修改：
1、搜索结束的判断条件为：迭代次数>最大迭代次数
2、搜索到新节点时，需要判断离最近的节点，作为其父节点
3、判断节点代价是否更小，是则将路径重新连接（图中绿色部分）
这里只改进了原算法的RRT函数模块：
RRT_improve.m
function [Path,totalcost] = RRT_improve(startPoint,axisStart,axisLWH,goalPoint,cubeInfo,cylinderInfo,sphereInfo)
%%RRT*算法寻找路径点
%%与RRT的区别在于加入了路径的代价，在扩展节点时，将树进行重组
%%变量定义
%% 变量定义
calcuDis = @(x,y) sqrt((x(1)-y(1))^2+(x(2)-y(2))^2+(x(3)-y(3))^2);
iterMax = 50000; %最大迭代次数
iter = 0; %当前迭代次数
step = 10; %步长
count = 1; %计数器
Thr = 20; %阈值
RadiusFornearp = 40; %rewrite的范围
findpath = 0;
%构建树
T.x(1) = startPoint(1);
T.y(1) = startPoint(2);
T.z(1) = startPoint(3);
T.pre(1) = 0;
T.cost(1) = 0; %从初始节点累计的cost,这里取欧氏距离
totalcost = 1800;
while iter < iterMax
iter = iter+1
%%在空间中随机采样
randCoor = samplePoint(axisStart,axisLWH,goalPoint);
tempDis = inf;
for k1 = 1:size(T.x,2)
dis = calcuDis([T.x(k1) T.y(k1) T.z(k1)],randCoor);
if tempDis>dis
tempDis = dis;
index = k1;
end
end
nearCoor = [T.x(index) T.y(index) T.z(index)];
%preIndex = index;
%临时父节点索引
temp_parent = index;
%计算临时的累计代价
temp_cost = step +T.cost(index);
%% 按照指定步长生成新的扩展点
newCoor = expandPoint(nearCoor,randCoor,step);
%% 碰撞检测
cubeFlag = isCubeCollision(cubeInfo,nearCoor,newCoor,step); %长方体碰撞检测函数
cylinderFlag = isCylinderCollision(cylinderInfo,nearCoor,newCoor,step); %圆柱体碰撞检测函数
sphereFlag = isSphereCollision(sphereInfo,nearCoor,newCoor,step); %球形障碍物碰撞检测函数
if cubeFlag || cylinderFlag || sphereFlag
continue;
end
%%在以新节点为中心，半径为40的圆内搜索节点
%每次循环都将队列清空
disToNewList = [];
nearIndexList = [];
for k4 = 1:size(T.x,2)
dis = calcuDis([T.x(k4) T.y(k4) T.z(k4)],newCoor);
if(dis < RadiusFornearp) %满足欧氏距离小于40
disToNewList = [disToNewList dis];%满足条件的所有节点到newCoor节点的距离
nearIndexList = [nearIndexList k4];%满足条件的所有节点基于树的索引
end
end
%%选择newCooor的父节点
for k2 =1:length(nearIndexList) %选取基于disToNewList的索引，而不是整个树的索引
costToNew = disToNewList(k2) +T.cost(nearIndexList(k2));
if(costToNew < temp_cost) %temp_cos为通过其临时父节点的路径的cost
wirenearCoor = [T.x(nearIndexList(k2)) T.y(nearIndexList(k2)) T.z(nearIndexList(k2))];%符合剪纸条件的坐标点
%preIndex = nearIndexList(k2);
%% 碰撞检测
cubeFlag = isCubeCollision(cubeInfo,wirenearCoor,newCoor,step); %长方体碰撞检测函数
cylinderFlag = isCylinderCollision(cylinderInfo,wirenearCoor,newCoor,step); %圆柱体碰撞检测函数
sphereFlag = isSphereCollision(sphereInfo,wirenearCoor,newCoor,step); %球形障碍物碰撞检测函数
if cubeFlag || cylinderFlag || sphereFlag
continue;
end
temp_cost = costToNew;
temp_parent = nearIndexList(k2);
end
end
%% 将新点插入树中
count = count+1; %更新节点索引
T.x(count) = newCoor(1);
T.y(count) = newCoor(2);
T.z(count) = newCoor(3);
T.cost(count) = temp_cost;
T.pre(count) = temp_parent;
line([nearCoor(1) newCoor(1)],[nearCoor(2) newCoor(2)],[nearCoor(3) newCoor(3)],'LineWidth',1); %绘制每一个新点
% pause(0.01);
%%剪枝操作
for k3 = 1:length(nearIndexList)
if(nearIndexList(k3) ~= temp_parent) %如果节点不是之前计算的最小cost的节点
newCost = temp_cost + disToNewList(k3); %计算新节点经过该节点再到起点的代价
if(newCost <T.cost(nearIndexList(k3))) %需要剪枝
nearCoor1 = [T.x(nearIndexList(k3)) T.y(nearIndexList(k3)) T.z(nearIndexList(k3))];
%% 碰撞检测
cubeFlag = isCubeCollision(cubeInfo,nearCoor1,newCoor,step); %长方体碰撞检测函数
cylinderFlag = isCylinderCollision(cylinderInfo,nearCoor1,newCoor,step); %圆柱体碰撞检测函数
sphereFlag = isSphereCollision(sphereInfo,nearCoor1,newCoor,step); %球形障碍物碰撞检测函数
if cubeFlag || cylinderFlag || sphereFlag
continue;
end
T.pre(nearIndexList(k3)) = count;
flag = 0;
line([nearCoor1(1) newCoor(1)],[nearCoor1(2) newCoor(2)],[nearCoor1(3) newCoor(3)],'Color','g','LineWidth',1); %绘制每一个新点
pause(0.01);
end
end
end
%%判断是否到达目标点附近,且该条件只运行一次
if (calcuDis(newCoor,goalPoint)<Thr && ~findpath)
findpath = 1;
count =count +1;%将目标点加入到树中
goal_index = count;
T.x(count)=goalPoint(1);
T.y(count)=goalPoint(2);
T.z(count)=goalPoint(3);
T.cost(count)=calcuDis(newCoor,goalPoint)+T.cost(count-1);
totalcost = T.cost(count);
T.pre(count)=count-1;
end
if findpath
index1 = goal_index;
costAll = 0;
while T.pre(index1) ~=0
C =[T.x(index1) T.y(index1) T.z(index1)];
F =[T.x(T.pre(index1)) T.y(T.pre(index1)) T.z(T.pre(index1))];
costAll = costAll + calcuDis(C,F);
% totalcost = T.cost(goal_index);
index1 = T.pre(index1);
end
totalcost = costAll
end
end
if(findpath == 1)
%% 寻找路径
% index = T.pre(end);
index = goal_index;
count = 1;
while T.pre(index)~=0
Path(count,1) = T.x(index);
Path(count,2) = T.y(index);
Path(count,3) = T.z(index);
index = T.pre(index);
count = count+1;
end
%将初始点添加到Path中
Path(count,1) = startPoint(1);
Path(count,2) = startPoint(2);
Path(count,3) = startPoint(3);
%将目标点添加到Path中
Path = flipud(Path);
end
end
50000次迭代
代码地址：https://blog.csdn.net/BBeymax?type=download
informed-RRT*
根据上图可以看出，RRT算法在搜索时，还是有很多采样点是无效的，当地图范围增大时，搜索效率还是非常低。informed-RRT算法就可以解决这种问题。
在二维平面中，informed*_RRT**算法将采样点约束在椭圆里，如果是三维的场景，则是将采样点约束在椭球里，这里如何在椭球里进行随机采样就是该算法实现的关键。
算法主要修改了RRT*算法的采样的空间
samplePoint_improve.m
function randCoor = samplePoint_improve(axisStart,axisLWH,goalPoint,startPoint,findpath,totalcost)
if findpath == 0
if rand<0.5
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
else
%赋初值，求x,y,z三轴的半轴长
x1 = startPoint(1);
y1 = startPoint(2);
z1 = startPoint(3);
x2 = goalPoint(1);
y2 = goalPoint(2);
z2 = goalPoint(3);
dis = sqrt((x1-x2)^2+(y1-y2)^2+(z1-z2)^2);
a = 0.5*totalcost;
b = 0.5*sqrt(totalcost^2-dis^2);
c = b;
%随机在一个以a为半径的球体内生成一个点
angle1=rand(1)*2*pi;
angle2=acos(rand(1)*2-1);
r=a*power(rand(1),1/3);
x=r.*cos(angle1).*sin(angle2);
y=r.*sin(angle1).*sin(angle2);
z=r.*cos(angle2);
%%将点映射到椭球体上
x = x;
y = b*y/a;
z = c*z/a;
%旋转和平移
an1 =atan2(x2-x1,y2-y1);
an2 =-atan2(z2-z1,sqrt((x1-x2)^2+(y1-y2)^2));
Rz=[cos(an1) -sin(an1) 0;
sin(an1) cos(an1) 0;
0 0 1];
Ry=[cos(an2) 0 sin(an2);
0 1 0;
-sin(an2) 0 cos(an2)];
% Rx=[1 0 0;
% 0 cos(an1) -sin(an1);
% 0 sin(an1) cos(an1)];
n=Ry*Rz*[x;y;z];
xn = n(1);
yn = n(2);
zn = n(3);
% k1 =(xn/a)^2+(yn/b)^2+(zn/c)^2;
x = xn +(x1+x2)/2;
y = yn +(y1+y2)/2;
z = zn +(z1+z2)/2;
if x < axisStart(1)
x =axisStart(1);
end
if y < axisStart(2)
y =axisStart(2);
end
if z < axisStart(3)
z =axisStart(3);
end
if x > axisLWH(1)
x = axisLWH(1);
end
if y > axisLWH(2)
x = axisLWH(2);
end
if z > axisLWH(3)
z = axisLWH(3);
end
randCoor =[x y z];
end
end
代码地址：https://blog.csdn.net/BBeymax?type=download
代码中都有详细注释，跑不通可以私聊。
1796
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
