随机树算法 自动驾驶汽车的路径规划 静态障碍物（Matlab）

随着自动驾驶技术的蓬勃发展，安全、高效的路径规划成为核心挑战之一。快速探索随机树（RRT）算法作为一种强大的路径搜索策略，为自动驾驶汽车在复杂环境下绕过静态障碍物规划合理路径提供了有效解决方案。

RRT 算法基于随机采样思想构建树形结构。从初始状态点出发，在车辆的状态空间内反复随机采样，将新采样点与已有树中的节点依据距离、可达性等规则进行连接拓展，逐步生长形成一棵能够覆盖状态空间大部分区域的树，向着目标区域快速探索。

在构建树的过程中，算法实时检测采样点与障碍物的碰撞情况。一旦发现新采样点或连接路径与静态障碍物相交，立即舍弃该点或重新规划连接方式，确保生成的路径始终位于无碰撞空间内，巧妙地引导车辆绕过障碍物。

首先，确定自动驾驶汽车的初始位置作为树的根节点，设定目标区域。接着，不断重复随机采样、节点连接、碰撞检测与规避操作，持续拓展树结构。当树的分支成功延伸至目标区域附近，通过回溯从目标点到起始点的连接节点，即可提取出一条从起点绕过障碍物抵达终点的可行路径。

尽管存在挑战，但 RRT 算法在自动驾驶路径规划领域已展现出巨大潜力。随着算法改进、硬件算力提升，未来有望更精准、高效地处理各类复杂静态障碍场景，助力自动驾驶汽车畅行无忧，推动智能交通迈向新高度。

% 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;

% readin 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