Rapidly exploring random tree (RRT/快速拓展随机树)
优势
通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效解决高维空间和复杂约束条件下的路径求解问题。
核心思想
通过随机采样，在自由空间中快速构建一个树形结构，以探索可能的路径。在每次迭代中，算法随机生成一个节点，并试图将该节点连接到最接近的树节点，形成一条边。通过反复迭代这一过程，不断扩展树的范围，直至达到目标状态或达到最大迭代次数为止。
伪代码
- 将 \(X_{start}\) 作为起点，并放入随机树中
- 随机取一个采样点 \(X_{rand}\) ，并找出距离这个采样点最近的随机数中的点 \(X_{nearest}\)
- 沿着\(X_{nearest}\)->\(X_{rand}\)的方向移动一个步长 \(\delta\) 作为新的\(X_{new}\) , 如果\(X_{rand}\)到\(X_{nearest}\)的距离小于步长 \(\delta\) ，则将\(X_{rand}\)设为新的\(X_{new}\)
- 对新的\(X_{new}\)进行碰撞检测，如果通过碰撞检测，则将\(X_{new}\)加入random Tree中，若没通过则重新迭代
- 判断新节点\(X_{new}\)是否满足\(||X_{new}-X_{goal}||<\epsilon\)，若满足则表示到达终点，反之则继续迭代。
matlab代码仿真
rrt_plan.m
function rrt()
% Parameters
map_size = [100, 100];
start_pos = [1, 1];
end_pos = [100, 100];
num_obstacles = 10;
max_iter = 1000;
step_size = 5;
% Generate map with random rectangular obstacles
obstacles = generate_random_obstacles(map_size, num_obstacles);
% RRT Algorithm
tree = start_pos;
parent = 0; % Root has no parent
for i = 1:max_iter
rand_point = [randi(map_size(1)), randi(map_size(2))];
nearest_idx = find_nearest(tree, rand_point);
new_point = steer(tree(nearest_idx,:), rand_point, step_size);
if ~check_collision(new_point, obstacles)
tree = [tree; new_point]; %#ok<AGROW>
parent = [parent; nearest_idx]; %#ok<AGROW>
plot_tree(tree, parent, obstacles, map_size, start_pos, end_pos);
if norm(new_point - end_pos) < step_size
disp('Path found!');
plot_path(tree, parent, end_pos);
return;
end
end
end
disp('Path not found within the maximum iterations.');
function obstacles = generate_random_obstacles(map_size, num_obstacles)
obstacles = [];
for i = 1:num_obstacles
x = randi([1, map_size(1) - 10]);
y = randi([1, map_size(2) - 10]);
w = randi([5, 20]);
h = randi([5, 20]);
obstacles = [obstacles; x, y, w, h]; %#ok<AGROW>
end
end
function idx = find_nearest(tree, point)
distances = vecnorm(tree - point, 2, 2);
[~, idx] = min(distances);
end
function new_point = steer(from, to, step_size)
direction = (to - from) / norm(to - from);
new_point = from + step_size * direction;
end
function collision = check_collision(point, obstacles)
collision = false;
for i = 1:size(obstacles, 1)
x = obstacles(i, 1);
y = obstacles(i, 2);
w = obstacles(i, 3);
h = obstacles(i, 4);
if point(1) >= x && point(1) <= x + w && point(2) >= y && point(2) <= y + h
collision = true;
return;
end
end
end
function plot_tree(tree, parent, obstacles, map_size, start_pos, end_pos)
clf;
hold on;
axis([0 map_size(1) 0 map_size(2)]);
for i = 2:size(tree, 1)
plot([tree(i,1), tree(parent(i),1)], [tree(i,2), tree(parent(i),2)], 'b-');
end
for i = 1:size(obstacles, 1)
rectangle('Position', obstacles(i,:), 'FaceColor', [0.5 0.5 0.5]);
end
plot(start_pos(1), start_pos(2), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
plot(end_pos(1), end_pos(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
drawnow;
end
function plot_path(tree, parent, end_pos)
path = [end_pos];
nearest_idx = find_nearest(tree, end_pos);
while nearest_idx > 1
path = [tree(nearest_idx,:); path]; %#ok<AGROW>
nearest_idx = parent(nearest_idx);
end
path = [tree(1,:); path];
plot(path(:,1), path(:,2), 'r-', 'LineWidth', 2);
end
end
仿真结果
