【路径规划】快速探索随机树算法，用于自动驾驶汽车的路径规划，绕过静态障碍物（Matlab实现）

最新推荐文章于 2024-07-24 08:32:31 发布

yyds_2201

最新推荐文章于 2024-07-24 08:32:31 发布

阅读量264

点赞数 3

文章标签：算法自动驾驶汽车

本文链接：https://blog.csdn.net/2201_75535220/article/details/138061990

版权

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

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

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

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

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

💥1 概述

快速探索随机树（Rapidly-exploring Random Tree，RRT）算法是一种常用于路径规划的概率型算法，特别适用于自动驾驶汽车的路径规划，能够有效地绕过静态障碍物。 RRT算法通过随机采样和快速扩展树结构来生成路径。它以车辆当前位置为起点，随机采样可能的目标位置作为终点，并在树结构中搜索路径。每次迭代，根据一定的策略，将新的节点添加到树中，以扩展树结构。 RRT算法不断迭代，直到生成路径连接起始点和目标点，或达到最大迭代次数。RRT算法作为一种概率型路径规划算法，在自动驾驶汽车的路径规划中具有重要应用，能够有效地绕过静态障碍物并生成可行的行驶路径。

📚2 运行结果

主函数部分代码：

% RRT algorithm in 2D with disc obstacle avoidance.
% Anand Patel
% 
% nodes:    contains its coordinates, cost to reach, and its parent.
%           
% 
% How it works: 
% 1. Pick a random node q_rand.
% 2. Find the closest node q_near from nodes list to branch out from
% towards q_rand.
% 3. Move from q_near towards q_rand: interpolate if node is too far away,
% reach q_new. Check for collisions.
% 4. Update cost of reaching q_new from q_near, Cmin. q_near
% acts as the parent node of q_new.
% 5. Add q_new to node list.
% 6. Continue until maximum number of samples is reached or goal region is
% entered.

clearvars
close all

% make S = [0 100] X [0 100]
x_max = 100;
y_max = 100;

% read in obstacles
obstacle_array = csvread('H3_obstacles.txt');
% turn array into struct
for j=1:1:23
obstacle(j).coord = [obstacle_array(j,1) obstacle_array(j,2)];
obstacle(j).rad = obstacle_array(j,3);
end
nodes_id = 1;
EPS = 20;               % epsilon distance ASSIGNED
numNodes = 100000;        % max number of samples taken
del_t = 10;
delta = .5;

q_start.coord = [40 40];      % start node's (x,y) coordinate ASSIGNED
q_start.cost = 0;           % cost to reach start node set to 0
q_start.parent = 0;         % parent of start node set to 0
q_start.id = nodes_id;
q_start.time = 0;           % start node begins at t=0
q_start.theta = pi/4;         % start node theta ASSIGNED
q_start.v = 0;              % start node trans vel = 0