【路径规划】RRT-路径规划与避障（Matlab实现）

荧光Matlab

已于 2024-09-18 19:20:44 修改

阅读量199

点赞数 2

文章标签： matlab 开发语言

于 2024-09-18 19:05:28 首次发布

本文链接：https://blog.csdn.net/Matlab88888/article/details/142341042

版权

“在代码的海洋里，有无尽的知识等待你去发现。我就是那艘领航的船，带你乘风破浪，驶向代码的彼岸。

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

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

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

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

💥1 概述

RRT即快速搜索随机树，是一种在机器人运动规划、路径规划等领域广泛应用的算法。在路径规划方面，RRT通过在状态空间中随机采样并逐步构建一棵随机树来探索可行路径。它具有以下显著特点： - 随机性：随机采样的方式使得算法能够在复杂的环境中快速探索不同的区域，增加找到可行路径的可能性。高效性：能够在高维空间中进行有效的路径搜索，对于复杂的环境和大规模的问题具有较好的适应性。灵活性：可以根据不同的约束条件和目标进行调整，适用于多种场景。在避障方面，RRT能够实时感知周围环境中的障碍物，并在路径规划过程中自动避开这些障碍物。它通过不断地检测和更新障碍物信息，调整随机树的生长方向，以确保生成的路径是无碰撞的。具体而言：障碍物检测：利用传感器等设备获取周围环境的信息，识别出障碍物的位置和形状。路径调整：当发现障碍物时，算法会调整随机树的生长方向，尝试从不同的方向绕过障碍物，确保生成的路径不会与障碍物发生碰撞。实时性：能够在动态环境中快速响应障碍物的变化，实时调整路径规划，保证机器人的安全运行。总之，RRT在路径规划与避障方面具有重要的应用价值，为机器人在复杂环境中的自主导航提供了有效的解决方案。

📚2 运行结果

主函数部分代码：

rng('shuffle')
ITER = 1500; 
close all; 

start_point = [10 -8];

goal_point = [-10 5]; 

map = [start_point, 1]; 
epsilon = 0.3; 

%obstacles: 
obstacles = [2 3 2; 
             7 8 2;
             -1 -1 2;
             -8 -5 2]; %centers and radii
         
NoOfobstacles = size(obstacles,1);
         
viscircles(obstacles(:, 1:end-1) ,obstacles(:, end), 'Color', 'g')


 hold on
 plot(start_point(1),start_point(2),'r*','MarkerSize',10)
 text(start_point(1),start_point(2), 'S')
 plot(goal_point(1),goal_point(2),'r*','MarkerSize',10)
 text(goal_point(1),goal_point(2), 'G')
 X_new = start_point; 
while norm(X_new-goal_point) > 0.2
    i = i+1;
    if randn>0

        X_rand = 10*randn([1, 2]); %take a random point in the environment
    else
        X_rand = goal_point; 
    end
    
    %calculate which point in the existing map has the smallest distance to
    %X_rand
    
    [smallest_idx] = closest_point(map,X_rand); 
    X_near = map(smallest_idx, 1:end-1); 
    X_new = X_near + (X_rand - X_near)/norm(X_rand - X_near)*epsilon ; %new point is proportional to the distance from the nearest point
    
    dis_from_obs = sqrt((X_new(1)-obstacles(:, 1)).^2 + (X_new(2)-obstacles(:, 2)).^2); 
    if sum(dis_from_obs > obstacles(:, end))== NoOfobstacles
        map = [map; [X_new, smallest_idx]]; 
        con = [X_near; X_new];
        line(con(:,1), con(:,2))
        drawnow
    end

end


[smallest_idx] = closest_point(map,goal_point); 
X_near = map(smallest_idx, 1:end-1); % nearest point to the goal 
con = [X_near; goal_point];
line(con(:,1), con(:,2),'Color', 'r')

while X_near~=start_point 
    
    parent_idx = map(smallest_idx,end);
    
    X_parent = map(parent_idx, 1:end-1);
    
    con = [X_near; X_parent];
    
    line(con(:,1), con(:,2),'Color', 'r', 'linewidth',2)
    
    smallest_idx = parent_idx;
    X_near = X_parent;
    
end