⾃动驾驶路径规划算法学习-RRT算法及matlab实现
⾃动驾驶路径规划算法学习-RRT算法及matlab实现
参考
RRT快速随机数算法 Rapid Random Tree
是基于采样的规划⽅法的⼀种。
快速搜索随机树，就是在环境中随机撒⼀些点，这些点经过算法运算，最终可以连接起来，变成车辆可以运⾏的轨迹。
1.算法原理
RRT 适⽤于涉及⾮完整约束场合下的路径规划问题。
过程中，算法不断在搜索空间中随机⽣成状态点，如果该点位于⽆碰撞位置，则寻搜索树中离该节点最近的结点为基准结点，由基准结点出发以⼀定步长朝着该随机结点进⾏延伸，延伸线的终点所在的位置被当做有效结点加⼊搜索树中。这个搜索树的⽣长过程⼀直持续，直到⽬标结点与搜索树的距离在⼀定范围以内时终⽌。随后搜索算法在搜索树中寻⼀条连接起点到终点的最短路径。
计算实例参考 ，这篇博客讲的⾮常清楚，如下：
1.1计算实例
Step 1.初始化⼀个环境，包括地图，起点，终点。如下图所⽰,⿊⾊物体为障碍物，蓝⾊飞机位于起点位置，红⾊五⾓星为⽬标终点位置
Step2：从环境中随机采样状态点，如下图所⽰，采样点为 Xrand。
Step3: 从所构建的树中寻距离采样点 Xrand最近的结点 Xnear。现在树中只有起点⼀个结点，所有最近的结点就是起点
Step4：开始树的⽣长过程。⾸先连接 Xnear 和 Xrand连接起来，这个连接线的⽅向就是树⽣长的⽅向。设置⼀个步长 Stepsize作为树⼀次⽣长的步长，在树⽣长的这个⽅向上⽣长⼀个步长，然后就会在⽣长的末端会产⽣⼀个新的结点 Xnew。
Step5：判断从 Xnear 到 Xrand是否穿过障碍物，如果穿过，则放弃该新的结点，如果没有，则将 Xnew 结点加⼊到树中。
Step6：从步骤 2 开始再循环执⾏，从环境中随机采样状态点。
.........
重复上述树的⽣长过程，直到树新⽣成的结点到⽬标点的距离⼩于⼀个步长，则终⽌树的⽣长。直接将该新结点与⽬标点相连。整个步骤动图如下：
1.2算法伪代码
概述之：
输⼊地图，起点，终点→起点先加⼊树节点nodes表→在地图随机采样⼀个点xrand(同时保证有⼀定的概率会选择到⽬标点，保证有节点会向着⽬标点的⽅向扩展)→到树节点中离xrand最近的点xnearest→xnearest朝着xrand前进⼀个步长得到新的点xnew→判断xnearest到xnew连线进⾏碰撞检测，若有碰撞则放弃该节点重新选择，若⽆碰撞则将该节点加⼊树节点→重复xnew的扩展过程直到扩展的xnew在⽬标点附近。
2.算法Matlab实现
这⾥只介绍了关键代码的实现，⾮完整程序，完整代码链接附在⽂末。
2.1 ⼆维环境的搭建 CreateMap.m
CreateMap.m的主要作⽤输⼊起点终点障碍物等信息，障碍物是⼀⼀个个实⼼圆表⽰。绘制起点终点障碍物信息，代码如下：
%CreateMap.m
start = [0,0];
goal = [10,14];
%障碍物(x,y,radiu)
obstacle_list=[3,3,1.5;
12,2,3;
3,9,2;
9,11,2];
%画出地图框及障碍物
axis([-2,18,-2,15])
hold on
for i=1:length(obstacle_list(:,1))
%调⽤画障碍物函数
plot_obstacle(obstacle_list(i,1),obstacle_list(i,2),obstacle_list(i,3))
end
plot(start(1),start(2),'og')
hold on
plot(goal(1),goal(2),'or')
hold on
2.2 随机采样
matlab求导⾪属于RRT算法程序RRT_planning.m的⼀部分。
在状态空间中随机采样p_rand（这⾥采样的是⼀个坐标点）, 有⼀定的概率采样到⽬标点，确保路径能以⼀定的概率向着⽬标点前进。这⾥随机采样取得⽬标点的概率是0.3，这个参数越⼤，越快到路径，但障碍物较多时可能反⽽要耗费更多时间绕开。
%随机采样取得⽬标点的概率是0.3，这个参数越⼤，越快到路径，但障碍物较多时可能反⽽要耗费
更多时间绕开
p=0.3
%在环境中采样p_rand,p_int是start
p_randx = randi(16)-1; %x随机采样范围0-15
p_randy = randi(13)-1; %x随机采样范围0-12
p_rand=[p_randx,p_randy];
%⼀定概率采样点取得⽬标点
if rand(1)<p
p_rand=goal;
end
2.3 FindNearest.m
从节点树中到距随机采样点p_rand最近的节点p_nearest的程序FindNearest.m程序如下：
这⾥有⼀个要注意的细节，运⾏时出错，因为nodes节点树很可能存在好⼏个点同时到p_rand随机采样点距离最⼩，这⾥设置的返回值必须只有⼀个，如果有多个最近点，只取第⼀个返回。
function minID=FindNearest(p_rand,nodes)
%dist矩阵存放p_rand到nodes节点每个节点的距离
%nodes的节点数
nodes_num = length(nodes(:,1));
prand_matx=ones(nodes_num,1)*p_rand(1);
prand_maty=ones(nodes_num,1)*p_rand(2);
nodes_matx=nodes(:,1);
nodes_maty=nodes(:,2);
dist=((prand_matx-nodes_matx).^2+(prand_maty-nodes_maty).^2).^0.5;
minID=find(dist==min(dist));
minID=minID(1); %万⼀有多个同样⼩的，只取其中⼀个
end
2.4 扩展新节点
最近点朝着随机点⾛⼀个步长得到新节点。
先求出随机点p_rand和最近点p_nearest连线与x轴所成⾓theta，然后计算pnew新节点，代码如下：
%随机点和树中最近点连线与x轴夹⾓
theta = atan2(p_rand(2)-p_nearest(2),p_rand(1)-p_nearest(1));
%计算新节点
pnew(1)= p_nearest(1)+ RRT_stepsize*cos(theta);
pnew(2)= p_nearest(2)+ RRT_stepsize*sin(theta);
%看该随机点是否已在随机树nodes中，是的话重新选择，防⽌pnew在nodes⾥出现两次
father=FindFather(pnew, nodes);
if ~isempty(father) %如果father⾮空说明能到⽗节点说明在nodes⾥，重复了，避免出错
continue
end
特别注意，我在跑程序时开始跑很多次总有⼀两次会陷⼊死循环，怎么都不到错误所在，后来发现是在回溯路径时出现了两个节点互为⽗节点father的情况，原因是在扩展新节点pnew时，没有判断pnew是否已经在nodes树节点中了，如果已经在nodes树节点中，就不应再作为新的扩展点，本次随机采样放弃，进⼊下⼀次随机采样。下⾯⽤图说明：
发布评论