RRT系列算法基于随机采样的原理进行可行路径的搜索相较于混合A*算法等图搜索算法RRT算法不需要依赖栅格地图。RRT算法的原理和步骤如下1 在地图范围内随机采样得到采样点node_sample2 查找节点树上距离采样点最近的节点node_nearest初始状态下节点只有起点3 node_nearest朝着node_sample方向拓展一定步长stepsize得到新节点node_new4 判断node_new和node_nearest的连线是否发生碰撞如果发生碰撞则重新回到步骤1没有发生碰撞则将node_new放入节点树中并将node_nearest标记为node_new的父节点5 如果node_new距离终点小于一定阈值则尝试连接node_new和终点若连线无碰撞则成功找到路径并退出循环否则回到步骤16 查找到路径后基于终点所记录的父节点进行路径回溯代码中加入了终点偏向采样以加速查找到可行路径即以一定概率选取终点作为采样点如有需要可调节该终点偏向概率以查看路径搜索效率以学习为目的进行分享如有错误还请海涵指出后续将整理和分享RRT*的原理和代码路径查找动画如下代码如下% RRT 系列不需要地图栅格化 是基于采样的路径搜索算法 % 基础RRT算法 RRT搜索到的路径不一定甚至大概率不是最优的 %% RRT参数设置 clear; clc; close all; param.step_size0.6; %单次搜索的拓展步长 param.max_iter500; %最大迭代次数 param.goal_thr 0.4; %终点抵达阈值 到达终点 param.x_range [0,10]; %x方向上的搜索范围 param.y_range [0,10]; %y方向上的搜索范围 start [1.0,1.0]; %起点 goal [9.0,9.0]; %终点 %障碍物范围 用圆形区域表示 [x_center,y_center,radius] obstacles [ 3.0, 3.0, 1.0; 6.0, 4.0, 0.8; 4.0, 7.0, 0.9; ]; %% 初始化绘图窗口 fig figure(Color,w); hold on; axis equal; xlim(param.x_range); ylim(param.y_range); % 绘制圆形障碍物只画一次 for i 1:size(obstacles,1) xo obstacles(i,1); yo obstacles(i,2); ro obstacles(i,3); theta linspace(0,2*pi,100); plot(xoro*cos(theta), yoro*sin(theta),r-,LineWidth,2); end % 起点终点标记 h_start plot(start(1),start(2),go,MarkerSize,10,MarkerFaceColor,g); h_goal plot(goal(1),goal(2),mo,MarkerSize,10,MarkerFaceColor,m); title(RRT 生长动画); %% RRT算法 %初始化 % 树存储每一行 [x, y, parent_index] tree zeros(1,3); tree(1,:) [start, 0]; % 根节点父索引为0 find_path false; %查找成功标记 goal_idx -1; %表示终点回溯的父节点 if(isNodeInsideObstacles(start,obstacles)) disp(起点在障碍物内); return; end if(isNodeInsideObstacles(goal,obstacles)) disp(终点在障碍物内); return; end %主循环 for iter 1:param.max_iter %1 随机采样 node_sample 采样点处于障碍物内也没关系 node_samplezeros(1,2); node_sample(1)param.x_range(1) (param.x_range(2)-param.x_range(1))*rand();%采样点x坐标 node_sample(2)param.y_range(1) (param.y_range(2)-param.y_range(1))*rand();%采样点y坐标 %采样优化 以一定概率选择终点作为采样点 if(rand()0.02) node_samplegoal; end %2 寻找最近节点 node_near; [node_near,near_idx]findNearestNode(tree,node_sample); %near_idx用作新节点的父节点idx %3 最近节点沿采样方向拓展一步得到新节点 node_new node_newexpand_step(node_near,node_sample,param.step_size); %4 生长线段碰撞检测无碰撞则加入树中 if(is_seg_collision(node_new,node_near,obstacles)) continue; else %加入树中 new_idxsize(tree,1)1; tree(new_idx,:)[node_new,near_idx]; %绘图新增线段 % 动画只画新增的这一条边 plot([node_near(1), node_new(1)], [node_near(2), node_new(2)],b,LineWidth,0.4); pause(0.01); %10ms绘制一次搜索 内部会自动调用drawnow % drawnow; % 强制刷新画面 end %5 判断新节点是否达到终点邻域 是则进行连线尝试和碰撞检测成功则退出迭代否则进入下一步 if (norm(node_new-goal)param.goal_thr) %判断直接连是否有碰撞 if(is_seg_collision(goal,node_new,obstacles)) continue; else %直接连接表示达到终点 goal_idxnew_idx; find_pathtrue; break; end end end path[goal]; if find_path title([RRT成功迭代次数,num2str(iter)]) %路径回溯 parent_idxgoal_idx; while (parent_idx~0) path[tree(parent_idx,1:2);path]; parent_idxtree(parent_idx,3); end plot(path(:,1),path(:,2),k-,LineWidth,2.8); %黑色加粗表示最终轨迹 else title(RRT未找到可行路径); end %% 函数定义 % 点在障碍物内的判断 function isInsideisNodeInsideObstacles(node,obstacles) isInsidefalse; for i1:size(obstacles,1) disdistance(node,obstacles(i,1:2)); if(disobstacles(i,3)) isInsidetrue; break; end end end function [node_near,near_idx]findNearestNode(tree,node_sample) pts tree(:,1:2); dist vecnorm(pts - node_sample, 2, 2); %对每一行求向量模长 第一个2表示对每一行求 第二个2表示二范数 [~,near_idx] min(dist); node_near tree(near_idx,1:2); end % 两点之间距离 function disdistance(node1,node2) dissqrt((node1(1)-node2(1))^2(node1(2)-node2(2))^2); return; end %沿采样方向拓展新节点 function node_newexpand_step(node_near,node_sample,step) dir node_sample - node_near; %向量表示拓展方向 dir dir / norm(dir); %除本身范数获得单位向量 node_new node_near dir * step; end %单段拓展是否有碰撞 function isCollisionis_seg_collision(node_new,node_near,obstacles) isCollision false; for i 1:size(obstacles,1) center obstacles(i,1:2); radiu obstacles(i,3); d point_to_segment_dist(center, node_new, node_near); if d radiu isCollision true; return; end end end function distpoint_to_segment_dist(q, a, b) ab b - a; aq q - a; t dot(aq, ab) / dot(ab, ab); %得到q在ab上投影点p ap相对于ab的比例 t max(0, min(1, t)); proj a t * ab; %q在ab上投影点坐标 dist norm(q - proj); %pq线段长度 即点q到线段ab的最近距离 end