RRT*算法的三维实现

主函数：

%% 参数初始化
goal=[10 10 10];              %目标点
source=[150 150 150];         %起点
GoalThreshold = 30;         % 设置目标点阈值
Delta =10;                 % 设置扩展步长 default:30
RadiusForNeib = 40;         % rewire的范围,半径r
MaxIterations = 2500;       % 最大迭代次数
UpdateTime = 50;            % 更新路径的时间间隔
DelayTime = 0.0;            % 绘图延迟时间
%% 建树初始化:T是树,v是节点
T.v(1).x = source(1);             % 把起始节点加入到T中
T.v(1).y =source(2); 
T.v(1).z =source(3);
T.v(1).xPrev = source(1);         % 节点的父节点坐标:起点的父节点是其本身
T.v(1).yPrev = source(2);
T.v(1).zPrev = source(3);
T.v(1).totalCost = 0;         % 从起始节点开始的累计cost，这里取欧氏距离
T.v(1).indPrev = 0;           % 父节点的索引
% 绘制障碍物(以球为例，主要是方便计算)
x0=100; y0=100; z0=100;%球心
circleCenter = [100,100,100;50,50,50;100,40,60;150,100,100;60,130,50];
r=[15;15;15;15;15];%半径
%下面开始画
figure(1);
[x,y,z]=sphere;
for i = 1:length(circleCenter(:,1))
    mesh(r(i)*x+circleCenter(i,1),r(i)*y+circleCenter(i,2),r(i)*z+circleCenter(i,3));hold on;
end
axis equal
searchSize = [250 250 250];      %探索空间六面体
hold on
%% 绘制起点和终点
hold on;
scatter3(source(1),source(2),source(3),"filled","g");
scatter3(goal(1),goal(2),goal(3),"filled","b");
tic;  % tic-toc: Functions for Elapsed Time
count = 1;
pHandleList = [];
lHandleList = [];
resHandleList = [];
findPath = 0;
update_count = 0;
path.pos = [];
for iter = 1:MaxIterations
    %Step 1: 在地图中随机采样一个点x_rand (Sample)
    if rand < 0.5
        x_rand = rand(1,3) .* searchSize;   % random sample
    else
        x_rand = goal; % sample taken as goal to bias tree generation to goal
    end
    %Step 2: 遍历树，从树中找到最近邻近点x_near (Near)
    minDis = sqrt((x_rand(1) - T.v(1).x)^2 + (x_rand(2) - T.v(1).y)^2+(x_rand(3) - T.v(1).z)^2);
    minIndex = 1;
    for i = 2:size(T.v,2)	% T.v按行向量存储，size(T.v,2)获得节点总数
    	distance = sqrt((x_rand(1) - T.v(i).x)^2 + (x_rand(2) - T.v(i).y)^2+(x_rand(3) - T.v(i).z)^2);   %两节点间距离
        if(distance < minDis)
            minDis = distance;
            minIndex = i;   
        end     
    end
    x_near(1) = T.v(minIndex).x;    % 找到当前树中离x_rand最近的节点
    x_near(2) = T.v(minIndex).y;
    x_near(3) = T.v(minIndex).z;
    temp_parent = minIndex;         % 临时父节点的索引
    temp_cost = Delta + T.v(minIndex).totalCost;   % 临时累计代价

    %Step 3: 扩展得到x_new节点 (Steer)
    movingVec = [x_rand(1)-x_near(1),x_rand(2)-x_near(2),x_rand(3)-x_near(3)];
    movingVec = movingVec/sqrt(sum(movingVec.^2));  %单位化
    newPoint = x_near + Delta * movingVec;
    %plot3(x_rand(1), x_rand(2), x_rand(3),'ro', 'MarkerSize',10, 'MarkerFaceColor','r');
    %plot3(newPoint(1), newPoint(2), newPoint(3), 'bo', 'MarkerSize',10, 'MarkerFaceColor','b');
    
    %theta = atan2((x_rand(2) - x_near(2)),(x_rand(1) - x_near(1)));
    %x_new(1) = x_near(1) + cos(theta) * Delta;
    %x_new(2) = x_near(2) + sin(theta) * Delta;  
    %plot(x_rand(1), x_rand(2), 'ro', 'MarkerSize',10, 'MarkerFaceColor','r');
    %plot(x_new(1), x_new(2), 'bo', 'MarkerSize',10, 'MarkerFaceColor','b');
    
    % 检查节点是否是collision-free
    if ~checkPath3(x_near, newPoint, circleCenter,r) % if extension of closest node in tree to the new point is feasible
        continue;
    end

    %Step 4: 在以x_new为圆心,半径为R的圆内搜索节点 (NearC)
    disToNewList = [];    % 每次循环要把队列清空
    nearIndexList = [];
    for index_near = 1:count
        disTonew = sqrt((newPoint(1) - T.v(index_near).x)^2 + (newPoint(2) - T.v(index_near).y)^2+(newPoint(3) - T.v(index_near).z)^2);
        if(disTonew < RadiusForNeib)    % 满足条件:欧氏距离小于R
            disToNewList = [disToNewList disTonew];     % 满足条件的所有节点到x_new的cost
            nearIndexList = [nearIndexList index_near];     % 满足条件的所有节点基于树T的索引
        end
    end
    
    %Step 5: 选择x_new的父节点,使x_new的累计cost最小 (ChooseParent)
    for cost_index = 1:length(nearIndexList)    % cost_index是基于disToNewList的索引,不是整棵树的索引
        costToNew = disToNewList(cost_index) + T.v(nearIndexList(cost_index)).totalCost;
        if(costToNew < temp_cost)    % temp_cost为通过minDist节点的路径的cost
            x_mincost(1) = T.v(nearIndexList(cost_index)).x;     % 符合剪枝条件节点的坐标
            x_mincost(2) = T.v(nearIndexList(cost_index)).y;
            x_mincost(3) = T.v(nearIndexList(cost_index)).z;
            if ~checkPath3(x_mincost, newPoint, circleCenter,r) % if extension of closest node in tree to the new point is feasible
        continue;
            end
        	temp_cost = costToNew;
        	temp_parent = nearIndexList(cost_index);
        end
    end
    
    %Step 6: 将x_new插入树T (AddNodeEdge)
    count = count+1;    %最新节点的索引
    
    T.v(count).x = newPoint(1);          
    T.v(count).y = newPoint(2); 
    T.v(count).z = newPoint(3); 
    T.v(count).xPrev = T.v(temp_parent).x;     
    T.v(count).yPrev = T.v(temp_parent).y;
    T.v(count).zPrev = T.v(temp_parent).z;
    T.v(count).totalCost = temp_cost; 
    T.v(count).indPrev = temp_parent;     %其父节点x_near的index
    
   l_handle = plot3([T.v(count).xPrev; newPoint(1)], [T.v(count).yPrev; newPoint(2)],[T.v(count).zPrev; newPoint(3)], 'b', 'Linewidth', 2);
   p_handle = plot3(newPoint(1), newPoint(2),newPoint(3), 'ko', 'MarkerSize', 4, 'MarkerFaceColor','k');
   
   pHandleList = [pHandleList p_handle];    %绘图的句柄索引即为count
   lHandleList = [lHandleList l_handle];
   pause(DelayTime);
    %Step 7: 剪枝 (rewire)
    for rewire_index = 1:length(nearIndexList)
        if(nearIndexList(rewire_index) ~= temp_parent)    % 若不是之前计算的最小cost的节点
            newCost = temp_cost + disToNewList(rewire_index);    % 计算neib经过x_new再到起点的代价          
            if(newCost < T.v(nearIndexList(rewire_index)).totalCost)    % 需要剪枝
                x_neib(1) = T.v(nearIndexList(rewire_index)).x;     % 符合剪枝条件节点的坐标
                x_neib(2) = T.v(nearIndexList(rewire_index)).y;
                x_neib(3) = T.v(nearIndexList(rewire_index)).z;
                if ~checkPath3(x_neib, newPoint, circleCenter,r) % if extension of closest node in tree to the new point is feasible
                    continue;
                end
                T.v(nearIndexList(rewire_index)).xPrev = newPoint(1);      % 对该neighbor信息进行更新
                T.v(nearIndexList(rewire_index)).yPrev = newPoint(2);
                T.v(nearIndexList(rewire_index)).zPrev = newPoint(3);
                T.v(nearIndexList(rewire_index)).totalCost = newCost;
                T.v(nearIndexList(rewire_index)).indPrev = count;       % x_new的索引
                
                %delete(pHandleList());
                %delete(lHandleList(nearIndexList(rewire_index)));
                lHandleList(nearIndexList(rewire_index)) = plot3([T.v(nearIndexList(rewire_index)).x, newPoint(1)], [T.v(nearIndexList(rewire_index)).y, newPoint(2)],[T.v(nearIndexList(rewire_index)).z, newPoint(3)], 'r', 'Linewidth', 2);
                %pHandleList = [pHandleList p_handle];    %绘图的句柄索引即为count
                %lHandleList = [lHandleList l_handle];
            end
        end
    end
    
    %Step 8:检查是否到达目标点附近 
    disToGoal = sqrt((newPoint(1) - goal(1))^2 + (newPoint(2)- goal(2))^2+(newPoint(3)- goal(3))^2);
    if(disToGoal < GoalThreshold && ~findPath)    % 找到目标点，此条件只进入一次
        findPath = 1;
        count = count+1;    %手动将Goal加入到树中
        Goal_index = count;
        T.v(count).x = goal(1);          
        T.v(count).y = goal(2);
        T.v(count).z = goal(3); 
        T.v(count).xPrev = newPoint(1);     
        T.v(count).yPrev = newPoint(2);
        T.v(count).yPrev = newPoint(3);
        T.v(count).totalCost = T.v(count - 1).totalCost + disToGoal;
        T.v(count).indPrev = count - 1;     %其父节点x_near的index
    end
    
    if(findPath == 1)
        update_count = update_count + 1;
        if(update_count == UpdateTime)
            update_count = 0;
            j = 2;
            path.pos(1).x = goal(1); 
            path.pos(1).y = goal(2);
            path.pos(1).z = goal(3);
            pathIndex = T.v(Goal_index).indPrev;
            while 1     
                path.pos(j).x = T.v(pathIndex).x;
                path.pos(j).y = T.v(pathIndex).y;
                path.pos(j).z = T.v(pathIndex).z;
                pathIndex = T.v(pathIndex).indPrev;    % 沿终点回溯到起点
                if pathIndex == 0
                    break
                end
                j=j+1;
            end  
            
            for delete_index = 1:length(resHandleList)
            	delete(resHandleList(delete_index));
            end
            for j = 2:length(path.pos)
                res_handle = plot3([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y],[path.pos(j).z; path.pos(j-1).z;], 'g', 'Linewidth', 4);
                resHandleList = [resHandleList res_handle];
            end
        end
    end  
	pause(DelayTime); %暂停DelayTime s,使得RRT*扩展过程容易观察
end

for delete_index = 1:length(resHandleList)
	delete(resHandleList(delete_index));
end
for j = 2:length(path.pos)
	res_handle = plot3([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y],[path.pos(j).z; path.pos(j-1).z;], 'g', 'Linewidth', 4);
	resHandleList = [resHandleList res_handle];
end
            
disp('The path is found!');

 检测函数：

%% checkPath3.m	
function feasible=checkPath3(n,newPos,circleCenter,r)%n是cloestnode
feasible=true;
movingVec = [newPos(1)-n(1),newPos(2)-n(2),newPos(3)-n(3)];
movingVec = movingVec/sqrt(sum(movingVec.^2)); %单位化
for R=0:0.5:sqrt(sum((n-newPos).^2))
    posCheck=n + R .* movingVec;
    if ~(feasiblePoint3(ceil(posCheck),circleCenter,r) && feasiblePoint3(floor(posCheck),circleCenter,r))
        feasible=false;break;
    end
end
if ~feasiblePoint3(newPos,circleCenter,r), feasible=false; end
end