【路径规划】基于Matlab的改进RRT算法二维/三维路径规划

基于Matlab的改进RRT算法二维/三维路径规划

一、引言

在机器人学、自动驾驶等领域，路径规划是一个关键问题，它旨在为机器人或车辆找到一条从起始点到目标点的安全、高效的路径。RRT（Rapidly-exploring Random Trees）算法作为一种基于采样的路径规划算法，因其能够有效处理高维空间和复杂环境下的路径规划问题而受到广泛关注。然而，传统的RRT算法存在收敛速度慢、路径质量不高等问题。为了克服这些问题，人们提出了RRT算法以及各种改进的RRT算法。本文将详细介绍基础RRT算法、RRT算法和改进RRT算法的原理，并通过Matlab代码实现这三种算法，对比它们的路径规划结果。

二、基础RRT算法原理

2.1 算法概述

RRT算法是由Steven M. LaValle于1998年提出的一种基于采样的增量式路径规划算法。它的核心思想是通过在状态空间中随机采样，并以当前构建的随机树为基础，逐步向新的采样点进行扩展，从而快速探索未知区域。RRT算法的优点在于其无需显式地对整个状态空间进行离散化，能够有效地处理高维空间，并且具有概率完备性，即在无限时间内，如果存在解，RRT算法以概率1能够找到一条路径。然而，标准的RRT算法通常只能找到一条可行路径，而不能保证路径是最优的（例如，最短路径）。

2.2 算法步骤

RRT算法的主要步骤如下：

1. 初始化：在环境中随机生成一个起始节点，并将其作为树的根节点。
2. 扩展树：随机生成一个新的节点，并将其与树中最近的节点连接起来，形成新的分支。
3. 检查碰撞：检查新分支是否与障碍物发生碰撞。如果发生碰撞，则放弃该分支。
4. 更新目标节点：如果新分支与目标节点的距离小于阈值，则将目标节点与树连接起来，完成路径规划。
5. 重复步骤2 - 4：重复步骤2 - 4，直到找到一条连接起点和终点的路径。

2.3 算法示意图

三、RRT*算法原理

3.1 算法概述

RRT算法是RRT算法的改进版本，它在标准RRT算法的基础上引入了两个关键机制：选择父节点和重布线机制，以实现最优或近似最优的路径规划。通过这两个机制，RRT算法能够在构建随机树的同时，不断优化已存在的路径，使得整棵树趋向于包含最优路径。RRT*算法具有渐近最优性，即当迭代次数趋近于无穷时，算法以概率1能够找到最优路径。

3.2 算法步骤

RRT*算法的主要步骤如下：

1. 初始化：设定起始点start和目标点goal，并创建一个只包含start的RRT树T。
2. 重复步骤直到找到路径或达到最大迭代次数：
   a. 随机采样：在环境空间中随机采样一个点x_rand。
   b. 扩展树：从树T中找到最近的节点x_near，以x_near为起点，在方向上延伸一定的距离，得到新的节点x_new。
   c. 碰撞检测：检测路径x_near到x_new之间是否与障碍物发生碰撞，如果发生碰撞，则返回步骤2继续下一次迭代。
   d. 寻找最优连接：在树T中找到与x_new最近的节点x_min，并计算从x_min到x_new的代价cost(x_min, x_new)。
   e. 重新连接：对于树T中与x_new距离在一定范围内的节点x_near_neighbors，计算通过x_near连接到x_new的代价cost(x_near, x_new)，选择代价最小的连接方式。
   f. 更新树：将x_new加入树T，并更新节点间的连接关系和代价信息。
   g. 优化路径：对树T中的部分节点，以目标点goal为目标进行优化，通过调整连接关系和代价信息，改善路径的质量。
   h. 判断终止条件：如果新加入的节点x_new接近目标点goal，则找到了一条可行路径，算法结束。

四、改进RRT算法原理

4.1 算法概述

为了进一步提高RRT算法的性能，人们提出了多种改进策略。这些改进策略从多个方面对传统RRT算法进行优化，旨在提升算法的收敛速度、路径质量以及避障能力等。以下将详细介绍几种常见的改进策略。

4.2 多步长策略

4.2.1 原理

在障碍物稀疏区域使用大步长，在障碍物密集区域使用小步长，以提高搜索效率。通过动态调整步长，算法可以在不同环境下更灵活地扩展搜索树，加速搜索过程，减少无效扩展。

4.2.2 方法

• 计算当前节点附近障碍物的密度（但不直接依赖障碍物密度计算）。
• 根据路径连续碰撞情况动态调整步长：
  • 大步长 step3（自由区域）
  • 中步长 step2（一般区域）
  • 小步长 step1（障碍物密集区域）

4.2.3 优势

加速搜索过程，减少无效扩展，提高避障能力。

4.3 目标偏置概率P与权重系数W联合优化

4.3.1 原理

偏置概率P用于控制随机点采样，使算法有一定概率直接朝向目标点，提高收敛速度。权重系数W在生成新节点时，动态调整目标点与随机点的影响权重，使搜索更加平稳，减少不必要的波动。

4.3.2 计算方式

• 计算随机点Q1和目标点Q2方向的位移：
  • (Q_1=(1 - W)cdot(randNode - nearestNode)+nearestNode)
  • (Q_2=Wcdot(goal - nearestNode)+nearestNode)
• 最终采样点：
  • (sample = Q_1+Q_2)

4.4 路径与障碍物安全距离safe_dis约束

4.4.1 原理

路径点应始终与障碍物保持安全距离safe_dis以上，避免路径紧贴障碍物，提高鲁棒性。

4.4.2 方法

• 采用dis_fun函数，在路径点生成时检查是否满足安全距离。
• 若不满足，则重新采样，避免无效扩展。

4.5 拉伸与剪枝策略

4.5.1 拉伸

若路径点之间可以直接相连（无碰撞），则移除中间点，提高路径简洁性。例如，原路径点为(P_1→P_2→P_3→P_4→P_5)，若(P_1→P_4)直接可行，则移除(P_2)，(P_3)，得到优化路径(P_1→P_4→P_5)。

4.5.2 剪枝

从目标点向起点回溯，移除多余拐点，使路径更顺畅。

4.6 碰撞检测的改进

4.6.1 传统方法

采用固定分割点数进行路径检查，精度受限。

4.6.2 改进方案

• 动态调整采样点数：(n = round(min(max(baseN-(dist / maxDist), minN), maxN)))
• 逐点检测：
  • 生成插值点：(node_lin = linspace(node1, node2, n))
  • 遍历检测：若(dis_fun(temp_point, map, row, col, safe_dis)==1)，则认为碰撞

4.6.3 优势

碰撞检测更加精细，减少漏检情况，提高路径安全性。

4.7 终点判断的优化

4.7.1 方法

在新节点生成时，检查新节点是否能直接连通目标点（无障碍）。若可直连，则直接连接目标点，结束搜索。

4.7.2 优势

避免无意义的继续搜索，提高路径收敛速度。

4.8 生成新节点时优先选择“离目标最近的路径点”

4.8.1 原理

传统RRT随机选择父节点，可能导致路径较长。本方法确保新节点生成在“当前路径中最接近目标的点”附近，提高收敛效率。

4.8.2 实现

• 计算所有已生成路径点到目标点的距离：(nearestToGoal = arg min(dist(path.Nodes, goal)))
• 以该点作为新节点的父节点，提高搜索的导向性。

五、三种算法的路径规划结果对比

5.1 对比指标

为了对比基础RRT算法、RRT*算法和改进RRT算法的性能，我们可以从以下几个方面进行评估：

1. 路径长度：路径长度越短，说明算法找到的路径越优。
2. 收敛速度：收敛速度越快，说明算法能够在更短的时间内找到可行路径。
3. 路径平滑度：路径平滑度越高，说明机器人或车辆在行驶过程中越稳定。
4. 避障能力：避障能力越强，说明算法在复杂环境下的可靠性越高。

5.2 对比结果

通过Matlab代码实现这三种算法，并在相同的环境下进行路径规划实验，得到以下对比结果：

5.3 结果示意图

5.3.1 二维结果图

各个算法性能指标如下：

5.3.2 三维结果图

六、Matlab代码实现

6.1 基础RRT算法代码示例

% %***************************************
% %RRT
function [trace_coo,dis_yuan]=RRT3D(S_coo,E_coo,radius,circleCenter,mapsize)
tic
x_I=S_coo(1); y_I=S_coo(2);            z_I=S_coo(3);  
x_G=E_coo(1); y_G=E_coo(2);        z_G=E_coo(3);  
Thr=100;                 
Delta= 50;              

xL=mapsize(1)';
yL=mapsize(2)';
zL=mapsize(3)';
figure;
[x, y, z] = sphere;      %创建一个坐标在原点，半径为1的标准圆，用于绘制自定义的圆
for i = 1: length(radius)
    mesh(radius(i)*x + circleCenter(i,1), radius(i)*y + circleCenter(i,2), radius(i)*z + circleCenter(i,3));
    hold on;
end
axis equal;
hold on;
scatter3(S_coo(1),S_coo(2),S_coo(3),'filled','g');
scatter3(E_coo(1),E_coo(2),E_coo(3),'filled','b');
%% 图片标注
text(S_coo(1),S_coo(2),S_coo(3),'起点');
text(E_coo(1),E_coo(2),E_coo(3),'终点');
view(3);
grid on;
axis equal;
axis([0 xL 0 yL 0 zL]);
xlabel('x');
ylabel('y');
zlabel('z');



%% 
T.v(1).x = x_I;         
T.v(1).y = y_I; 
T.v(1).z = z_I; 


T.v(1).xPrev = x_I;   
T.v(1).yPrev = y_I;
T.v(1).zPrev = z_I;


T.v(1).dist=0;         
T.v(1).indPrev = 0;     
%% 
count=1;
goal = [x_G,y_G,z_G];
for iter = 1:30000
    x_rand=[];
    x_rand(1) = xL*rand; 
    x_rand(2) = yL*rand;
        x_rand(3) = zL*rand;

    x_near=[];
    min_dist = 1000000;
    near_iter = 1;
    %%=======寻找x_near===========%%
    [~,N]=size(T.v);
    for j = 1:N
       x_near(1) = T.v(j).x;
       x_near(2) = T.v(j).y;
              x_near(3) = T.v(j).z;

       dist = norm(x_rand - x_near);
       if min_dist > dist
           min_dist = dist;
           near_iter = j;
       end
    end

    x_near(1) = T.v(near_iter).x;
    x_near(2) = T.v(near_iter).y;
        x_near(3) = T.v(near_iter).z;

    %%========获取x_new============%%
    x_new=[];
    near_to_rand = [x_rand(1)-x_near(1),x_rand(2)-x_near(2),x_rand(3)-x_near(3)];
    normlized = near_to_rand / norm(near_to_rand) * Delta;
    x_new = x_near + normlized;
    %%=======障碍检测===============%%
    % if ~collisionChecking(x_near,x_new,Imp) 
    if ~collisionDetec(x_near,x_new,radius,circleCenter,0)
       continue;
    end
    count=count+1;
    %%========将X_NEW增加到树中========%%
    T.v(count).x = x_new(1);
    T.v(count).y = x_new(2); 
    T.v(count).z = x_new(3); 
    T.v(count).xPrev = x_near(1);     
	T.v(count).yPrev = x_near(2);
    	T.v(count).zPrev = x_near(3);

    T.v(count).dist= norm(x_new - x_near);          
    T.v(count).indPrev = near_iter;   
    plot3([x_near(1),x_new(1)],[x_near(2),x_new(2)],[x_near(3),x_new(3)],'-k');
    hold on;
    %========判断是否找到路径==========%%
    if norm(x_new - goal) < Thr
        break;
    end
    % pause(0.1);
end
%%else
    disp('Error, no path found!');
end

%%
trace_coo=[path.pos.x;path.pos.y;path.pos.z]';
toc
time_all=toc;

disp(['原始RRT算法采样点数：',num2str(count)])
disp(['原始RRT算法运行时间：',num2str(time_all)])
disp(['原始RRT算法路径总距离：',num2str(dis_yuan)])
disp('-----------------------------------------------------------------------' )

6.2 RRT*算法代码示例

function [trace_coo,dis_yuan]=RRTstar3D(S_coo,E_coo,radius,circleCenter,mapsize)
tic
x_I=S_coo(1); y_I=S_coo(2);            z_I=S_coo(3);  
x_G=E_coo(1); y_G=E_coo(2);        z_G=E_coo(3);  
Thr=100;                 
Delta= 100;              

xL=mapsize(1)';
yL=mapsize(2)';
zL=mapsize(3)';
figure;
[x, y, z] = sphere;      %创建一个坐标在原点，半径为1的标准圆，用于绘制自定义的圆
for i = 1: length(radius)
    mesh(radius(i)*x + circleCenter(i,1), radius(i)*y + circleCenter(i,2), radius(i)*z + circleCenter(i,3));
    hold on;
end
axis equal;
hold on;
scatter3(S_coo(1),S_coo(2),S_coo(3),'filled','g');
scatter3(E_coo(1),E_coo(2),E_coo(3),'filled','b');
%% 图片标注
text(S_coo(1),S_coo(2),S_coo(3),'起点');
text(E_coo(1),E_coo(2),E_coo(3),'终点');
view(3);
grid on;
axis equal;
axis([0 xL 0 yL 0 zL]);
xlabel('x');
ylabel('y');
zlabel('z');



%% 
T.v(1).x = x_I;         
T.v(1).y = y_I; 
T.v(1).z = z_I; 


T.v(1).xPrev = x_I;   
T.v(1).yPrev = y_I;
T.v(1).zPrev = z_I;


T.v(1).dist=0;         
T.v(1).indPrev = 0;     
%%
% figure(1);
% ImpRgb=imread('map6.bmp');
% if size(ImpRgb, 3) == 3
%     Imp = rgb2gray(ImpRgb);
% else
%     Imp = ImpRgb;
% end
% imshow(Imp)
% xL=size(Imp,1);
% yL=size(Imp,2);
% hold on
% plot(x_I, y_I, 'ro', 'MarkerSize',10, 'MarkerFaceColor','r');
% plot(x_G, y_G, 'go', 'MarkerSize',10, 'MarkerFaceColor','g');
count=1;
goal = [x_G,y_G,z_G];
start_goal_dist = 1000000;
% path.pos(1).x = 700;
% path.pos(1).y = 700;
% path.pos(1).z = 700;
dis_yuan=0;

for iter = 1:30000
    x_rand=[];
    x_rand(1) = xL*rand;
    x_rand(2) = yL*rand;
    x_rand(3) = zL*rand;

    %%=======寻找x_near===========%%
    x_near=[];
    min_dist = 1000000;
    near_iter = 1;
    near_iter_tmp = 1;
    [~,N]=size(T.v);
    for j = 1:N
        x_near(1) = T.v(j).x;
        x_near(2) = T.v(j).y;
                      x_near(3) = T.v(j).z;

        dist = norm(x_rand - x_near);
        if min_dist > dist
            min_dist = dist;
            near_iter = j;
        end
    end
    x_near(1) = T.v(near_iter).x;
    x_near(2) = T.v(near_iter).y;
            x_near(3) = T.v(near_iter).z;

    %%========获取x_new============%%
    x_new=[];
    near_to_rand = [x_rand(1)-x_near(1),x_rand(2)-x_near(2),x_rand(3)-x_near(3)];
    normlized = near_to_rand / norm(near_to_rand) * Delta;
    x_new = x_near + normlized;
    %%=======障碍检测===============%%
    if ~collisionDetec(x_near,x_new,radius,circleCenter,0)
       continue;
    end

    %%=======  nearC && chooseParent  =========%%
    nearptr = [];
    nearcount = 0;
    neardist =  norm(x_new - x_near) + T.v(near_iter_tmp).dist;
    for j = 1:N
        if j == near_iter_tmp
            continue;
        end
        x_neartmp(1) = T.v(j).x;
        x_neartmp(2) = T.v(j).y;
                x_neartmp(3) = T.v(j).z;

        dist = norm(x_new - x_neartmp) + T.v(j).dist;
        norm_dist = norm(x_new - x_neartmp);
        if norm_dist < 120
            %nearC
            if collisionDetec(x_near,x_new,radius,circleCenter,0)
                nearcount = nearcount + 1;
                nearptr(nearcount,1) = j;
                if neardist > dist
                    neardist = dist;
                    near_iter = j;
                end
            end
        end
    end

    x_near(1) = T.v(near_iter).x;
    x_near(2) = T.v(near_iter).y;
        x_near(3) = T.v(near_iter).z;

    count=count+1;
    %%========将X_NEW增加到树中========%%
    T.v(count).x = x_new(1);
    T.v(count).y = x_new(2); 
        T.v(count).z = x_new(3); 

    T.v(count).xPrev = x_near(1);     
	T.v(count).yPrev = x_near(2);
    	T.v(count).zPrev = x_near(3);
    T.v(count).dist= norm(x_new - x_near) + T.v(near_iter).dist;
    T.v(count).indPrev = near_iter;
    %%========  rewirte  =========%%
    [M,~] = size(nearptr);
    for k = 1:M
        x_1(1) = T.v(nearptr(k,1)).x;
        x_1(2) = T.v(nearptr(k,1)).y;
                x_1(3) = T.v(nearptr(k,1)).z;

        x1_prev(1) = T.v(nearptr(k,1)).xPrev;
        x1_prev(2) = T.v(nearptr(k,1)).yPrev;
                x1_prev(3) = T.v(nearptr(k,1)).zPrev;

        if T.v(nearptr(k,1)).dist >  (T.v(count).dist + norm(x_1-x_new))
            T.v(nearptr(k,1)).dist = T.v(count).dist + norm(x_1-x_new);
            T.v(nearptr(k,1)).xPrev = x_new(1);
            T.v(nearptr(k,1)).yPrev = x_new(2);
                        T.v(nearptr(k,1)).yPrev = x_new(3);

            T.v(nearptr(k,1)).indPrev = count;
            % plot([x_1(1),x1_prev(1)],[x_1(2),x1_prev(2)],'-w');
            % hold on;
            plot3([x_1(1),x_new(1)],[x_1(2),x_new(2)],[x_1(3),x_new(3)],'-k');
            hold on;
            % drawnow
        end
    end

    plot3([x_near(1),x_new(1)],[x_near(2),x_new(2)],[x_near(3),x_new(3)],'-k');
    hold on;
    % plot(x_new(1),x_new(2),'*r');
    % hold on;
    % if norm(x_new - goal) < Thr
    if norm(x_new - goal) < Thr
        break;
    end
end


% if (T.v(count).dist + norm(x_new - goal)) < start_goal_dist
%     start_goal_dist = (T.v(count).dist + norm(x_new - goal));
%     % if length(path.pos) > 2
%     %     for j = 2 : length(path.pos)
%     %         plot([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y], 'w', 'Linewidth', 3);
%     %     end
%     % end
%     path.pos = [];
if iter < 30000
    path.pos(1).x = x_G; path.pos(1).y = y_G;path.pos(1).z = z_G;
    path.pos(2).x = T.v(end).x; path.pos(2).y = T.v(end).y;path.pos(2).z = T.v(end).z;
    pathIndex = T.v(end).indPrev; % ??????·??
    j=0;
    while 1
        path.pos(j+3).x = T.v(pathIndex).x;
        path.pos(j+3).y = T.v(pathIndex).y;
                        path.pos(j+3).z = T.v(pathIndex).z;

        pathIndex = T.v(pathIndex).indPrev;
        if pathIndex == 1
            break
        end
        j=j+1;
    end
    path.pos(end+1).x = x_I; path.pos(end).y = y_I; path.pos(end).z = z_I; 
    for j = 2:length(path.pos)
        % plot([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y], 'b', 'Linewidth', 3);
        dis_yuan=dis_yuan+...
        norm([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]);
    end
    % else
    % disp('Error, no path found!');
    % end
end
% continue;
% end
% pause(0.01);
% end

%%
trace_coo=[path.pos.x;path.pos.y;path.pos.z]';
toc
time_all=toc;
% dis_yuan=0;
% trace_coo=[tracebackNode.xCoord,tracebackNode.yCoord];
% while tracebackNode.parentNode~=0
%     dis_yuan=dis_yuan+norm([tracebackNode.xCoord-nodeArray(tracebackNode.parentNode).xCoord,tracebackNode.yCoord-nodeArray(tracebackNode.parentNode).yCoord]);
%     tracebackNode =nodeArray(tracebackNode.parentNode);
%     trace_coo=[[tracebackNode.xCoord,tracebackNode.yCoord];trace_coo];
% end

% plot(trace_coo(:,1),trace_coo(:,2),'r','LineWidth',2);

disp(['RRT*算法采样点数：',num2str(count)])
disp(['RRT*算法运行时间：',num2str(time_all)])
disp(['RRT*算法路径总距离：',num2str(dis_yuan)])
disp('-----------------------------------------------------------------------' )

6.3 改进RRT算法代码示例

function [smoothedPath,dis_smooth]=imRRT3D(S_coo,E_coo,radius,circleCenter,mapsize)
row=mapsize(1);
col=mapsize(2);
height=mapsize(3);

figure;
[x, y, z] = sphere;      %创建一个坐标在原点，半径为1的标准圆，用于绘制自定义的圆
for i = 1: length(radius)
    mesh(radius(i)*x + circleCenter(i,1), radius(i)*y + circleCenter(i,2), radius(i)*z + circleCenter(i,3));
    hold on;
end
axis equal;
hold on;
scatter3(S_coo(1),S_coo(2),S_coo(3),'filled','g');
scatter3(E_coo(1),E_coo(2),E_coo(3),'filled','b');
%% 图片标注
text(S_coo(1),S_coo(2),S_coo(3),'起点');
text(E_coo(1),E_coo(2),E_coo(3),'终点');
view(3);
grid on;
axis equal;
axis([0 row 0 col 0 height]);
xlabel('x');
ylabel('y');
zlabel('z');
% title('本文改进RRT算法搜索树')

car_R = 3;  % 车的大小
safe_dis1 = 60;  % 与障碍物的一类安全距离
safe_dis2 = 110;  % 与障碍物的二类安全距离
step1 = 50;% 小步长
step2 = 100;  % 正常步长
step3 = 150;  % 大步长
P = 0.6;  % 概率参数
goalRadius = 100;

nodeIndex = 1;  % 树枝节点编号初始值
goalFound = 0;
% 起始节点初始化
initialNode.xCoord = S_coo(1);
initialNode.yCoord = S_coo(2);
initialNode.zCoord = S_coo(3);

initialNode.parentNode = 0;
goalNode.xCoord = E_coo(1);
goalNode.yCoord = E_coo(2);
goalNode.zCoord = E_coo(3);

nodeArray(nodeIndex).xCoord = initialNode.xCoord;
nodeArray(nodeIndex).yCoord = initialNode.yCoord;
nodeArray(nodeIndex).zCoord = initialNode.zCoord;

nodeArray(nodeIndex).parentNode = initialNode.parentNode;
nodeArray(nodeIndex).ind = nodeIndex;
nodeIndex = nodeIndex + 1;

%% 主要路径规划部分，可以改：随机生成几个点，引入代价线，从起点到终点的连线直线，查看离直线最短的点。
tic
while goalFound == 0
    % === (1) 在已有节点中选择最接近目标的节点 ===
    nearestToGoal = findNearestToGoal(nodeArray, goalNode);

    % === (2) 以该最近节点附近生成新节点 ===
    randomNode.xCoord = nearestToGoal.xCoord + (rand - 0.5) * 200; % 随机偏移，范围 [-50, 50]
    randomNode.yCoord = nearestToGoal.yCoord + (rand - 0.5) * 200;
    randomNode.zCoord = nearestToGoal.zCoord + (rand - 0.5) * 200;

    % if rand < 0.1  % 目标偏置
    %     randomNode.xCoord = E_coo(1);
    %     randomNode.yCoord = E_coo(2);
    % else
    %     randomNode.xCoord = randi([1, col]);
    %     randomNode.yCoord = randi([1, row]);
    % end
    % === (3) 找到最近邻节点 ===
    nearestNode = nodeArray(1);
    for node = nodeArray
        if (getDistanceBetween3D(randomNode, node) < getDistanceBetween3D(randomNode, nearestNode))
            nearestNode = node;  % 选择距离新生成节点最近的已有节点
        end
    end

    % === (4) 目标点偏置 + 权重调整 ===
    % flag = dis_fun3D(nearestNode, map, row, col, safe_dis1);
    flag =pointCollisionCheck(nearestNode,radius,circleCenter,safe_dis1);
    if flag == 0  % 障碍少
        distToGoal = getDistanceBetween3D(nearestNode, goalNode);
        w = min(1, distToGoal / (row + col));  % 根据目标距离动态调整权重

        if rand < P  % 目标点方向
            randomNode.xCoord = goalNode.xCoord;
            randomNode.yCoord = goalNode.yCoord;
            randomNode.yCoord = goalNode.zCoord;

        else  % 目标点与随机点的平衡
            d1 = getDistanceBetween3D(randomNode, nearestNode);
            d2 = getDistanceBetween3D(goalNode, nearestNode);

            Q1(1) = (randomNode.xCoord - nearestNode.xCoord) * (1 - w) / d1 + nearestNode.xCoord;
            Q1(2) = (randomNode.yCoord - nearestNode.yCoord) * (1 - w) / d1 + nearestNode.yCoord;
            Q1(3) = (randomNode.zCoord - nearestNode.zCoord) * (1 - w) / d1 + nearestNode.zCoord;

            Q2(1) = (goalNode.xCoord - nearestNode.xCoord) * w / d2 + nearestNode.xCoord;
            Q2(2) = (goalNode.yCoord - nearestNode.yCoord) * w / d2 + nearestNode.yCoord;
            Q2(3) = (goalNode.zCoord - nearestNode.zCoord) * w / d2 + nearestNode.zCoord;

            Q = Q1 + Q2;
            randomNode.xCoord = Q(1);
            randomNode.yCoord = Q(2);
            randomNode.zCoord = Q(3);

        end
    end

    % === (5) 选择合适步长 ===
    flag = pointCollisionCheck(nearestNode,radius,circleCenter,safe_dis2);
    if flag == 0
        step = step3;
    else
        flag = pointCollisionCheck(nearestNode,radius,circleCenter,safe_dis2);
        if flag == 0
            step = step2;
        else
            step = step1;
        end
    end

    % === (6) 生成新节点 ===
    angleAlpha = atan2(randomNode.yCoord - nearestNode.yCoord, randomNode.xCoord - nearestNode.xCoord);
    newNode.xCoord = nearestNode.xCoord + step * cos(angleAlpha);
    newNode.yCoord = nearestNode.yCoord + step * sin(angleAlpha);
    newNode.zCoord = nearestNode.zCoord + step * sin(angleAlpha);


    growVec = [randomNode.xCoord - nearestNode.xCoord,randomNode.yCoord - nearestNode.yCoord,randomNode.zCoord - nearestNode.zCoord];
    growVec = growVec/sqrt(sum(growVec.^2));
    newNode.xCoord = nearestNode.xCoord + step * growVec(1);
    newNode.yCoord = nearestNode.yCoord + step * growVec(2);
    newNode.zCoord = nearestNode.zCoord + step * growVec(3);


    flag1 = ~pointCollisionCheck(newNode,radius,circleCenter,car_R*1.1);

    % flag2 = checkConstraint(newNode, nearestNode, map, row, col, car_R * 1.1);
    flag2= ~collisionDetec([newNode.xCoord,newNode.yCoord,newNode.zCoord],...
        [nearestNode.xCoord,nearestNode.yCoord,nearestNode.zCoord],radius,circleCenter,car_R * 1.1);

    if flag1||flag2  % 遇到障碍则跳过
        continue;
    end


    if newNode.xCoord <= 0 || newNode.xCoord >= col || newNode.yCoord <= 0 || newNode.yCoord >= row||...
            newNode.zCoord <= 0 || newNode.zCoord >= height
        continue;
    end

    % === (7) 记录并绘制路径 ===
    plot3([newNode.xCoord nearestNode.xCoord], [newNode.yCoord nearestNode.yCoord], [newNode.zCoord nearestNode.zCoord], 'k-');
    % drawnow;

    nodeIndex = nodeIndex + 1;
    nodeArray(nodeIndex).xCoord = newNode.xCoord;  % 记录新节点
    nodeArray(nodeIndex).yCoord = newNode.yCoord;
    nodeArray(nodeIndex).zCoord = newNode.zCoord;

    nodeArray(nodeIndex).parentNode = nearestNode.ind;
    nodeArray(nodeIndex).ind = nodeIndex;

    % === (8) 目标点直连检测 ===
    if collisionDetec([newNode.xCoord,newNode.yCoord,newNode.zCoord],...
            [goalNode.xCoord,goalNode.yCoord,goalNode.zCoord],radius,circleCenter,car_R * 1.1)
        plot3([newNode.xCoord goalNode.xCoord], [newNode.yCoord goalNode.yCoord], [newNode.zCoord goalNode.zCoord], 'k-');
        goalFound = 1;
        break;
    end

    % === (9) 判断是否到达目标点 ===
    if getDistanceBetween3D(newNode, goalNode) <= goalRadius
        goalFound = 1;
        break;
    end
end
toc
time_all=toc;
%%
%判断最后点是不是终点
if nodeArray(nodeIndex).xCoord~=goalNode.xCoord||nodeArray(nodeIndex).yCoord~=goalNode.yCoord||nodeArray(nodeIndex).zCoord~=goalNode.zCoord
    nodeIndex=nodeIndex+1;
    nodeArray(nodeIndex).xCoord=goalNode.xCoord;
    nodeArray(nodeIndex).yCoord=goalNode.yCoord;
    nodeArray(nodeIndex).zCoord=goalNode.zCoord;

    nodeArray(nodeIndex).parentNode=nodeIndex-1;
    nodeArray(nodeIndex).ind=nodeIndex;
    plot3([nodeArray(nodeIndex-1).xCoord nodeArray(nodeIndex).xCoord],[nodeArray(nodeIndex-1).yCoord nodeArray(nodeIndex).yCoord],...
        [nodeArray(nodeIndex-1).zCoord nodeArray(nodeIndex).zCoord],'-k');
end
tracebackNode = nodeArray(nodeIndex);
dis_yuan=0;
trace_coo=[tracebackNode.xCoord,tracebackNode.yCoord,tracebackNode.zCoord];
while tracebackNode.parentNode~=0
    dis_yuan=dis_yuan+norm([tracebackNode.xCoord-nodeArray(tracebackNode.parentNode).xCoord,...
        tracebackNode.yCoord-nodeArray(tracebackNode.parentNode).yCoord,...
        tracebackNode.zCoord-nodeArray(tracebackNode.parentNode).zCoord]);
    tracebackNode =nodeArray(tracebackNode.parentNode);
    trace_coo=[[tracebackNode.xCoord,tracebackNode.yCoord,tracebackNode.zCoord];trace_coo];
end

beacon_coo= prunePath3D(trace_coo,radius,circleCenter,1.1*car_R);
% plot3(beacon_coo(:,1),beacon_coo(:,2),beacon_coo(:,3),'b--','LineWidth',1);

k = 0.5; % 控制圆弧大小的参数
smoothedPath = smooth_path3D(beacon_coo, k);
dis_smooth=0;
for i=2:size(smoothedPath,1)
    dis_smooth=dis_smooth+norm(smoothedPath(i,:)-smoothedPath(i-1,:));
end
% plot3(smoothedPath(:,1),smoothedPath(:,2),smoothedPath(:,3),'r-','LineWidth',2);

disp(['本文改进RRT算法采样点数：',num2str(nodeIndex)])
disp(['本文改进RRT算法运行时间：',num2str(time_all)])
disp(['本文改进RRT算法路径总距离：',num2str(dis_smooth)])
title('本文改进RRT算法路径图')
% disp('-----------------------------------------------------------------------' )

七、结论

本文详细介绍了基础RRT算法、RRT算法和改进RRT算法的原理，并通过Matlab代码实现了这三种算法，对比了它们的路径规划结果。实验结果表明，基础RRT算法虽然能够快速找到一条可行路径，但路径质量不高；RRT算法通过引入选择父节点和重布线机制，能够找到渐近最优的路径，但收敛速度较慢；改进RRT算法通过引入多步长策略、目标偏置概率与权重系数联合优化等多种改进策略，能够在提高收敛速度的同时，显著提升路径质量和避障能力。在实际应用中，我们可以根据具体的需求选择合适的算法。

本人擅长各类优化模型的建模和求解，具有近400个优化项目的建模仿真经验，擅长模型构建，算法设计，算法实现和算法改进。累计指导各类建模/算法比赛和SCI写作超过100人次。
本人长期提供： ①源码分享（近1000个本人手写项目） ②辅导答疑（远程桌面一对一语音+文档指导，可以录屏反复观看）
③项目定制（根据您的现实问题，针对性建模求解，提供完整方案+代码实现） 长期在线，欢迎咨询，一般晚上看消息！！