RRT(matlab）

Ruo_su.

已于 2022-11-26 11:22:42 修改

阅读量623

点赞数

文章标签： matlab 算法

于 2022-04-14 21:09:41 首次发布

本文链接：https://blog.csdn.net/qq_44752711/article/details/124181795

版权

该博客详细介绍了如何使用A*算法实现机器人寻路的路径规划。首先，通过参数初始化设定起点和终点，然后利用随机采样点生成树结构并不断扩展节点，同时进行碰撞检测以确保路径的可行性。当达到目标点附近时，通过回溯路径找出最优路径。整个过程在地图上动态显示，便于理解和调试。最终，博客展示了路径规划的实现细节和效果。

摘要由CSDN通过智能技术生成

%主函数
clc;
clear all;
close all;
%% 参数初始化
x_s = 1; y_s = 1;
x_g = 750; y_g = 750;
Thr=30;
step=40;
%% Tree初始化
T.v(1).x = x_s;  %列坐标
T.v(1).y = y_s;  %行坐标
T.v(1).xPre = x_s;
T.v(1).yPre = y_s;
T.v(1).indPre = 0; %父节点在T中的索引
%% plot初始化
figure(1);
ImpRgb = imread('newmap.png');
Imp = rgb2gray(ImpRgb);
imshow(Imp);
xL = size(Imp,1); %地图row
yL = size(Imp,2); %地图col
hold on;
% 画起始点和目标点
plot(x_s, y_s, 'mp', 'MarkerSize', 10, 'MarkerFaceColor', 'm');
plot(x_g, y_g, 'gp', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
%% 开始循环生长节点，搜索路径
count=1;
findPath=false;
tic;
for iter=1:3000
    %step1.在地图上生成随机采样点
    x_rand = [unifrnd(0,800),unifrnd(0,800)];
    %step2.遍历树节点，找到最近点
    minDis = sqrt((x_rand(1) - T.v(1).x)^2 + (x_rand(2) - T.v(1).y)^2);
    minIndex=1;
    for i = 2:size(T.v,2)
        dist