基于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算法原理示意图

三、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代码实现这三种算法，并在相同的环境下进行路径规划实验，得到以下对比结果：

算法路径长度收敛速度路径平滑度避障能力
基础RRT算法较长较慢较差一般
RRT*算法较短较快较好较好
改进RRT算法最短最快最好最强

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:30000x_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:Nx_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 > distmin_dist = dist;near_iter = j;endendx_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;endcount=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) < Thrbreak;end% pause(0.1);
end
%%elsedisp('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:30000x_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:Nx_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 > distmin_dist = dist;near_iter = j;endendx_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:Nif j == near_iter_tmpcontinue;endx_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%nearCif collisionDetec(x_near,x_new,radius,circleCenter,0)nearcount = nearcount + 1;nearptr(nearcount,1) = j;if neardist > distneardist = dist;near_iter = j;endendendendx_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:Mx_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;% drawnowendendplot3([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) < Thrif norm(x_new - goal) < Thrbreak;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 < 30000path.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 1path.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 == 1breakendj=j+1;endpath.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 = nodeArrayif (getDistanceBetween3D(randomNode, node) < getDistanceBetween3D(randomNode, nearestNode))nearestNode = node;  % 选择距离新生成节点最近的已有节点endend% === (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);endend% === (5) 选择合适步长 ===flag = pointCollisionCheck(nearestNode,radius,circleCenter,safe_dis2);if flag == 0step = step3;elseflag = pointCollisionCheck(nearestNode,radius,circleCenter,safe_dis2);if flag == 0step = step2;elsestep = step1;endend% === (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;endif newNode.xCoord <= 0 || newNode.xCoord >= col || newNode.yCoord <= 0 || newNode.yCoord >= row||...newNode.zCoord <= 0 || newNode.zCoord >= heightcontinue;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) <= goalRadiusgoalFound = 1;break;end
end
toc
time_all=toc;
%%
%判断最后点是不是终点
if nodeArray(nodeIndex).xCoord~=goalNode.xCoord||nodeArray(nodeIndex).yCoord~=goalNode.yCoord||nodeArray(nodeIndex).zCoord~=goalNode.zCoordnodeIndex=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~=0dis_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];
endbeacon_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个本人手写项目） ②辅导答疑（远程桌面一对一语音+文档指导，可以录屏反复观看）
③项目定制（根据您的现实问题，针对性建模求解，提供完整方案+代码实现） 长期在线，欢迎咨询，一般晚上看消息！！

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。
如若转载，请注明出处：http://www.pswp.cn/web/86036.shtml
繁体地址，请注明出处：http://hk.pswp.cn/web/86036.shtml
英文地址，请注明出处：http://en.pswp.cn/web/86036.shtml

如若内容造成侵权/违法违规/事实不符，请联系英文站点网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

PHP的命名空间与自动加载机制

PHP的命名空间与自动加载机制

在PHP 5.3版本之后&#xff0c;引入了命名空间的概念&#xff0c;这为解决全局命名冲突和促进代码的模块化提供了强有力的工具。命名空间允许开发者将类、函数和常量封装在不同的命名空间中&#xff0c;从而避免了全局范围内的名称冲突问题。 命名空间基础 命名空间在PHP中是…
阅读更多...
OpenSIPS 邂逅 Kafka：构建高效 VoIP 消息处理架构

OpenSIPS 邂逅 Kafka：构建高效 VoIP 消息处理架构

使用场景使用步骤 引入模块组装&发送数据消费数据故障转移 使用场景 异步日志处理&#xff1a;将 OpenSIPS 中的 SIP 信令日志、通话记录&#xff08;CDR&#xff09;等数据发送到 Kafka 队列中。 事件通知与监控&#xff1a;利用 OpenSIPS 的 event_interface 模块将 S…
阅读更多...
《AI大模型应用技术开发工程师》学习总结

《AI大模型应用技术开发工程师》学习总结

以下是对你提供的《AI大模型应用技术开发工程师》课程内容的系统梳理&#xff0c;已去除所有广告、价格、报名、个人信息等内容&#xff0c;并补全了技术要点&#xff0c;最后给出客观的学习建议和个人感想&#xff0c;适合公开分享或自我学习参考。 AI大模型应用技术开发工程师…
阅读更多...
Python爬虫实战：研究LOSO相关技术

Python爬虫实战：研究LOSO相关技术

1. 引言 1.1 研究背景与意义 随着互联网数据的爆炸式增长，个性化推荐系统成为提升用户体验的关键技术。准确捕捉用户兴趣需要大量多维度数据，但获取高质量标注数据面临隐私保护、数据分散等挑战。网络爬虫技术为自动采集用户行为数据提供了解决方案，而如何有效评估模型在个…
阅读更多...
stm32万年历仿真+keil5程序

stm32万年历仿真+keil5程序

stm32万年历 本设计是利用单片机实现一个简易万年历系统&#xff0c;能够准确显示时、分、秒信息。用户可通过特定按键对时间进行设置调整&#xff0c;具备基本的时间校准功能&#xff0c;可满足日常简易计时需求。运用了stm32单片机模块内部定时器 / 计数器功能来实现精确计时…
阅读更多...
操作系统--名称解释

操作系统--名称解释

第一章: 操作系统:位于硬件层之上,所有软件层之下的一个系统软件,是管理系统中各种软硬件资源,方便用户使用计算机系统的程序集合 并发:宏观上是同时发生,但是再微观是交替发生的(若干事件在同一时间间隔内发生,单CPU) 并行:微观上同时发生(要求多个CPU) 共享:系统的资源可以…
阅读更多...
2025.6.16-实习

2025.6.16-实习

2025.6.18--2025.6.23 1.使用Cocos&#xff0c;从0开发老虎棒子鸡2D游戏。实现&#xff1a;AI自动选择&#xff0c;倒计时&#xff0c;对战逻辑&#xff0c;播放动画&#xff0c;设置背景音乐等功能。 2.使用Cocos&#xff0c;开发2D手术游戏。实现&#xff1a;视频、音频控制播…
阅读更多...
构建你的 AI 模块宇宙：Spring AI MCP Server 深度定制指南

构建你的 AI 模块宇宙：Spring AI MCP Server 深度定制指南

引言&#xff1a;当模块化遇见 AI 在微服务架构的海洋中&#xff0c;MCP&#xff08;Module Communication Protocol&#xff09;就像一艘智能帆船&#xff0c;它让不同 AI 模块的通信变得优雅而高效。本文将带你构建一艘属于自己的 AI 智能帆船——自定义 Spring AI MCP Serv…
阅读更多...
从数据到洞察：UI前端如何利用大数据优化用户体验

从数据到洞察：UI前端如何利用大数据优化用户体验

hello宝子们...我们是艾斯视觉擅长ui设计、前端开发、数字孪生、大数据、三维建模、三维动画10年经验!希望我的分享能帮助到您!如需帮助可以评论关注私信我们一起探讨!致敬感谢感恩! 在当今数字化时代&#xff0c;数据如同蕴藏着无限价值的宝藏&#xff0c;源源不断地产生并积累…
阅读更多...
SQLite3 在嵌入式C环境中存储音频/视频文件的专业方案

SQLite3 在嵌入式C环境中存储音频/视频文件的专业方案

SQLite3 在嵌入式C环境中存储音频/视频文件的专业方案 在嵌入式系统中存储大型媒体文件需要平衡存储效率、访问速度和资源限制。以下是针对嵌入式C环境的优化方案&#xff1a; 一、存储策略选择 1. 直接存储 vs 文件路径存储 方法优点缺点适用场景BLOB直接存储数据一致性高…
阅读更多...
区块链技术概述：从比特币到Web3.0

区块链技术概述：从比特币到Web3.0

目录 区块链技术概述&#xff1a;从比特币到Web3.0引言&#xff1a;数字革命的下一篇章1. 区块链技术基础1.1 区块链定义与核心特征1.2 区块链数据结构可视化 2. 比特币&#xff1a;区块链的开端2.1 比特币的核心创新2.2 比特币交易生命周期 3. 以太坊与智能合约革命3.1 以太坊…
阅读更多...
Petrel导入well数据

Petrel导入well数据

加载井口位置数据&#xff1a;井头文件应包括name, X, Y, KB, TD这些基本信息&#xff0c;文件格式为txt或prn格式都可。具体步骤&#xff1a;① input面板下右键import file&#xff0c;进入import file界面&#xff0c;选择文件格式well heads&#xff08;*.*&#xff09…
阅读更多...
51c嵌入式~电路~合集8

51c嵌入式~电路~合集8

我自己的原文哦~ https://blog.51cto.com/whaosoft/12175265 一、高频电路布线的十大绝招 1 多层板布线 高频电路往往集成度较高&#xff0c;布线密度大&#xff0c;采用多层板既是布线所必须&#xff0c;也是降低干扰的有效手段。在PCB Layout阶段&#xff0c;合理的…
阅读更多...
【LLM学习笔记3】搭建基于chatgpt的问答系统（下）

【LLM学习笔记3】搭建基于chatgpt的问答系统（下）

目录 一、检查结果检查有害内容检查是否符合产品信息 二、搭建一个简单的问答系统三、评估输出1.当存在一个简单的正确答案2.当不存在一个简单的正确答案 一、检查结果 本章将引领你了解如何评估系统生成的输出。在任何场景中&#xff0c;无论是自动化流程还是其他环境&#x…
阅读更多...
多项目资料如何统一归档与权限管理

多项目资料如何统一归档与权限管理

在多项目管理环境中&#xff0c;统一资料归档与权限管控的关键在于&#xff1a;规范化文件结构、自动化归档流程、分级权限控制。其中&#xff0c;规范化文件结构是实现统一归档的第一步&#xff0c;它直接决定后续归类、检索和审计的效率。通过预设项目模板&#xff0c;明确文…
阅读更多...
【RTP】基于mediasoup的RtpPacket的H.264打包、解包和demo 1:不含扩展

【RTP】基于mediasoup的RtpPacket的H.264打包、解包和demo 1:不含扩展

目前打包、解包没有对扩展进行操作 测试结果 === H.264 RTP Packetization and Depacketization Test ===1. Generating simulated H.264 frames... Generated 6 H.264 frames2. Packetizing H.264 frames to RTP packets...Frame #0 (size: 1535 bytes, I-fra
阅读更多...
【AI论文】Sekai：面向世界探索的视频数据集

【AI论文】Sekai：面向世界探索的视频数据集

摘要&#xff1a;视频生成技术已经取得了显著进展&#xff0c;有望成为交互式世界探索的基础。然而&#xff0c;现有的视频生成数据集并不适合用于世界探索训练&#xff0c;因为它们存在一些局限性&#xff1a;地理位置有限、视频时长短、场景静态&#xff0c;以及缺乏关于探索…
阅读更多...
websocket服务端开发

websocket服务端开发

websocket技术在服务端实时消息的推送和im聊天系统中得到了广泛应用。作为一名后端研发人员，这其中又有哪些需要了解和注意的问题点呢？接下来，我一一进行阐明。 SpringBoot项目中引入依赖 引入依赖 <!--websocket支持包--> <dependency> <…
阅读更多...
学历信息查询API (IVYZ9A2B) 的对接实战 | 天远API

学历信息查询API (IVYZ9A2B) 的对接实战 | 天远API

摘要 本文是天远API学历信息查询API&#xff08;接口代码&#xff1a;IVYZ9A2B&#xff09;的深度技术解析文档。作为一名开发者&#xff0c;我将从实际应用场景出发&#xff0c;详细介绍该接口的调用方法、数据结构和最佳实践。无论您是在开发招聘系统、教育管理平台&#xf…
阅读更多...
2025年- H84-Lc192--75.颜色分类(技巧、三路指针排序）--Java版

2025年- H84-Lc192--75.颜色分类(技巧、三路指针排序）--Java版

1.题目描述 2.思路 3.代码实现 class Solution {public void sortColors(int[] nums) {int low 0; // 下一个 0 应该放的位置int mid 0; // 当前检查的位置int high nums.length - 1; // 下一个 2 应该放的位置while (mid < high) {if (nums[mid] …
阅读更多...
最新文章

Copyright @ 2022~2023