【路径规划】2D/3D RRT*算法（Matlab实现）

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

---

💥1 概述

RRT*（Rapidly-exploring Random Tree Star）算法是一种在机器人领域广泛应用的路径规划算法，适用于二维和三维空间。与传统的RRT算法相比，RRT*采用增量式搜索策略，通过逐步探索状态空间并不断改善路径质量，具有较高的效率和性能。这种算法具有渐近最优性和适应复杂环境的特点，可以在不断优化树结构和重新连接的过程中，随着时间的推移收敛到全局最优解。RRT*算法的独特之处在于能够在保持搜索效率的同时，提供更接近最优解的路径规划结果，使得机器人能够在复杂的环境中更有效地规划路径并完成任务。其在自主导航、无人车辆、机械臂控制等领域具有广泛的应用前景。

📚2 运行结果

主函数部分代码：

clearvars
close all
x_max = 640;
y_max = 480;
z_max = 400;

EPS = 20;
numNodes = 2000;        

q_start.coord = [0 0 0];
q_start.cost = 0;
q_start.parent = 0;
q_goal.coord = [640 400 180];
q_goal.cost = 0;

nodes(1) = q_start;
figure(1)

for i = 1:1:numNodes
    q_rand = [rand(1)*x_max rand(1)*y_max rand(1)*z_max];
    plot3(q_rand(1), q_rand(2), q_rand(3), 'x', 'Color',  [0 0.4470 0.7410])
    
    % Break if goal node is already reached
    for j = 1:1:length(nodes)
        if nodes(j).coord == q_goal.coord
            break
        end
    end
    
    % Pick the closest node from existing list to branch out from
    ndist = [];
    for j = 1:1:length(nodes)
        n = nodes(j);
        tmp = dist_3d(n.coord, q_rand);
        ndist = [ndist tmp];
    end
    [val, idx] = min(ndist);
    q_near = nodes(idx);
    
    q_new.coord = steer3d(q_rand, q_near.coord, val, EPS);
    line([q_near.coord(1), q_new.coord(1)], [q_near.coord(2), q_new.coord(2)], [q_near.coord(3), q_new.coord(3)], 'Color', 'k', 'LineWidth', 2);
    drawnow
    hold on
    q_new.cost = dist_3d(q_new.coord, q_near.coord) + q_near.cost;
    

🎉3 参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。

[[1]付留芳,周明,李文哲等.基于遗传算法的UUV应召搜潜路径规划[J/OL].电光与控制:1-9[2024-03-20].http://kns.cnki.net/kcms/detail/41.1227.tn.20240311.1942.014.html.

🌈4 Matlab代码实现