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

  欢迎来到本博客❤️❤️

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

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

本文目录如下：

目录

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

3 参考文献

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

[1]金煦,莫愿斌.多策略混合山地瞪羚优化器在机器人路径规划问题中的应用[J/OL].系统仿真学报:1-18[2024-04-21].https://doi.org/10.16182/j.issn1004731x.joss.23-1392.

[2]牛旭,张志安.改进RRT的复杂障碍物环境路径规划算法研究[J].电子设计工程,2024,32(08):162-167+172.DOI:10.14022/j.issn1674-6236.2024.08.035.

4 Matlab代码实现