RRT(快速搜索随机树)_无人机快速搜索随机树流程图

img
img

既有适合小白学习的零基础资料,也有适合3年以上经验的小伙伴深入学习提升的进阶课程,涵盖了95%以上物联网嵌入式知识点,真正体系化!

由于文件比较多,这里只是将部分目录截图出来,全套包含大厂面经、学习笔记、源码讲义、实战项目、大纲路线、电子书籍、讲解视频,并且后续会持续更新

需要这些体系化资料的朋友,可以加我V获取:vip1024c (备注嵌入式)

如果你需要这些资料,可以戳这里获取

delta = 5;

[vertices, edges, path] = rrt(map, q_start, q_goal, k, delta_q, p);

path_smooth = smooth(map, path, vertices, delta);

imshow(int32(1 - map), []);
title('RRT (Rapidly-Exploring Random Trees) - Smooth');
% imagesc(1 - map);
% colormap(gray);

hold on;

[edgesRowCount, ~] = size(edges);

for ii = 1 : edgesRowCount
    plot(vertices(ii, 1), vertices(ii, 2), 'cyan*', 'linewidth', 1);
    plot([vertices(edges(ii, 1), 1), vertices(edges(ii, 2), 1)], ...
    [vertices(edges(ii, 1), 2), vertices(edges(ii, 2), 2)], ...
     'b', 'LineWidth', 1);
end

plot(q_start(1), q_start(2), 'g*', 'linewidth', 1);
plot(q_goal(1), q_goal(2), 'r*', 'linewidth', 1);


[~, pathCount] = size(path);

for ii = 1 : pathCount - 1
    %plot(vertices(ii, 1), vertices(ii, 2), 'cyan*', 'linewidth', 1);
    plot([vertices(path(ii), 1), vertices(path(ii + 1), 1)], ...
    [vertices(path(ii), 2), vertices(path(ii + 1), 2)], ...
     'r', 'LineWidth', 1);
end

[~, pathCount] = size(path_smooth);

for ii = 1 : pathCount - 1
    %plot(vertices(ii, 1), vertices(ii, 2), 'cyan*', 'linewidth', 1);
    plot([vertices(path_smooth(ii), 1), vertices(path_smooth(ii + 1), 1)], ...
    [vertices(path_smooth(ii), 2), vertices(path_smooth(ii + 1), 2)], ...
     'black', 'LineWidth', 2);
end


//rrt.m
function [vertices, edges, path] = rrt(map, q_start, q_goal, k, delta_q, p)
%Algorithm to build a tree to solve map
% that goes from the start position till the goal position and to generate a path that connects
% both vertices
%
% map: matrix that you can obtain loading the mat files.
%
% q_start: coordinates x and y of the start position. You can find the coordinates below the figures
% of the environmentin the previous page.
%
% q_goal: coordinates x and y of the goal position. You can find the coordinates below the figures
% of the environment in the previous page.
%
% k: maximum number of samples that will be considered to generate the tree, if the goal is not
% found before.
%
% delta_q: distance between q_new and q_near.
%
% p: probability (between 0 and 1) of choosing q_goal as q_random.
%

img
img

既有适合小白学习的零基础资料,也有适合3年以上经验的小伙伴深入学习提升的进阶课程,涵盖了95%以上物联网嵌入式知识点,真正体系化!

由于文件比较多,这里只是将部分目录截图出来,全套包含大厂面经、学习笔记、源码讲义、实战项目、大纲路线、电子书籍、讲解视频,并且后续会持续更新

需要这些体系化资料的朋友,可以加我V获取:vip1024c (备注嵌入式)

如果你需要这些资料,可以戳这里获取

友,可以加我V获取:vip1024c (备注嵌入式)**

如果你需要这些资料,可以戳这里获取

  • 15
    点赞
  • 20
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
以下是一个简单的 MATLAB 程序,用于绘制快速搜索随机RRT): ```matlab clear all; close all; clc; % 随机数种子 rng(0); % 定义搜索空间 x_min = -10; x_max = 10; y_min = -10; y_max = 10; % 定义起点和终点 start_pos = [0, 0]; goal_pos = [9, 9]; % 定义的节点结构体 node = struct('pos', [], 'parent', []); % 初始化根节点 root.pos = start_pos; root.parent = []; % 定义和节点列表 tree = root; nodes(1) = root; % 定义步长和搜索次数 step_size = 1; num_iterations = 5000; % 定义绘图参数 figure; hold on; xlim([x_min, x_max]); ylim([y_min, y_max]); scatter(start_pos(1), start_pos(2), 'go', 'filled'); scatter(goal_pos(1), goal_pos(2), 'ro', 'filled'); xlabel('X'); ylabel('Y'); title('RRT'); % 开始搜索 for i = 1:num_iterations % 随机采样一个点 sample.pos = [randi([x_min, x_max]), randi([y_min, y_max])]; % 找到距离该点最近的节点 [nearest_node, nearest_index] = nearest_neighbor(nodes, sample); % 从最近的节点向采样点移动一步 new_pos = steer(nearest_node.pos, sample.pos, step_size); % 检查新节点是否在搜索空间内 if (new_pos(1) >= x_min && new_pos(1) <= x_max && ... new_pos(2) >= y_min && new_pos(2) <= y_max) % 创建新节点 new_node.pos = new_pos; new_node.parent = nearest_index; % 添加新节点到和节点列表 tree = [tree, new_node]; nodes = [nodes, new_node]; % 绘制新节点 line([nearest_node.pos(1), new_node.pos(1)], ... [nearest_node.pos(2), new_node.pos(2)], 'Color', 'k'); drawnow; % 检查是否到达终点 if (norm(new_node.pos - goal_pos) <= step_size) % 绘制路径 path = find_path(nodes, new_node); for j = 1:length(path)-1 line([path(j).pos(1), path(j+1).pos(1)], ... [path(j).pos(2), path(j+1).pos(2)], 'Color', 'b', 'LineWidth', 2); end break; end end end % 定义距离函数 function distance = euclidean_distance(pos1, pos2) distance = norm(pos1 - pos2); end % 定义寻找最近邻节点函数 function [nearest_node, nearest_index] = nearest_neighbor(nodes, sample) distances = arrayfun(@(node) euclidean_distance(node.pos, sample.pos), nodes); [~, nearest_index] = min(distances); nearest_node = nodes(nearest_index); end % 定义移动函数 function new_pos = steer(start_pos, end_pos, step_size) distance = euclidean_distance(start_pos, end_pos); if (distance <= step_size) new_pos = end_pos; else direction = (end_pos - start_pos) / distance; new_pos = start_pos + direction * step_size; end end % 定义寻找路径函数 function path = find_path(nodes, target) path = target; while (~isempty(target.parent)) target = nodes(target.parent); path = [target, path]; end end ``` 这个程序会在一个 20x20 的搜索空间中,使用 RRT 算法搜索从起点到终点的路径,并将和路径绘制在图形窗口中。你可以根据需要调整搜索空间的大小和起点/终点的位置。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值