自动驾驶路径规划算法学习-RRT算法及matlab实现

参考手把手教用matlab做无人驾驶（六）-路径规划RRT

RRT路径规划算法在二维仿真环境中的应用 -- Python代码实现

RRT 算法原理以及过程演示

RRT快速随机数算法 Rapid Random Tree

是基于采样的规划方法的一种。

快速搜索随机树，就是在环境中随机撒一些点，这些点经过算法运算，最终可以连接起来，变成车辆可以运行的轨迹。

1.算法原理

RRT 适用于涉及非完整约束场合下的路径规划问题。

过程中，算法不断在搜索空间中随机生成状态点，如果该点位于无碰撞位置，则寻找搜索树中离该节点最近的结点为基准结点，由基准结点出发以一定步长朝着该随机结点进行延伸，延伸线的终点所在的位置被当做有效结点加入搜索树中。这个搜索树的生长过程一直持续，直到目标结点与搜索树的距离在一定范围以内时终止。随后搜索算法在搜索树中寻找一条连接起点到终点的最短路径。
计算实例参考 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 随机采样

隶属于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)<pp_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树节点中，就不应再作为新的扩展点，本次随机采样放弃，进入下一次随机采样。下面用图说明：

假设

P2是由父节点P0扩展出；P1是由父节点P2扩展出；此时新一次的扩展P1扩展出的pnew正好是P2

我们程序里树节点存放在nodes中，是一层层往上堆的，后扩展的放在上面，但是在nodes中找父节点时又是从上往下，则会出现下图的情况

对扩展的新节点判断是否已经在nodes树节点中，若是则放弃本次采样，pnew也不加入树节点nodes中，就不会陷入死循环：

2.5 碰撞检测 collision_check.m

计算障碍物圆心o到线段p_nearest-pnew的最短距离是否小于半径，是则会发生碰撞。

障碍物的圆心o和线段p_nearest-pnew的距离计算三种情况（垂点在线段之间，垂点在线段下方，垂点在线段上方）：

点到线段最短距离d的计算方法为点到直线的距离

点到线段最短距离d的计算方法为圆心o到p_nearest的距离

点到线段最短距离d的计算方法为圆心o到p_new的距离

点到线段最短距离的实现封装为distance_squared_point_to_segment.m，其返回值为最短距离的平方，其代码如下：

程序中 x1--p_nearest, x2--pnew, x3--圆心

向量（x3-x1）乘向量（x2-x1）求到O-pnearest在pnearest-pnew上的投影，投影/l2求到垂足在线段长度中的百分比，可能超过1或为负数。超过1时，最短距离取x3x2,小于0时距离取x3x1

distance_squared_point_to_segment.m

function dd=distance_squared_point_to_segment(x1,x2,x3)
%""" 计算线段 vw 和 点 p 之间的最短距离""",x1 near v; x2 new w; x3 obstacle_圆心 p
%点 v 和 点 w 重合的情况
if isequal(x1,x2)dd=(x3(1)-x1(1))^2+(x3(2)-x1(2))^2;return
end
%线段 vw 长度的平方
l2=(x2(1)-x1(1))^2+(x2(2)-x1(2))^2;
t = max(0, min(1, (x3 - x1)*(x2 - x1)' / l2));   %向量（x3-x1）乘向量（x2-x1）求到O-pnearest在pnearest-pnew上的投影，投影/l2求导垂足在线段长度中的百分比，可能超过1或为负数。超过1时，最短距离取x3x2,小于0时距离取x3x1
projection = x1 + t * (x2 - x1);
dd = (x3 - projection)*(x3 - projection)';
end

整个碰撞检测函数collision_check.m代码如下：

function collisionflag=collision_check(pnew,p_nearest,obstacle_list)
collisionflag=0;
for i=1:length(obstacle_list(:,1))
x0=p_nearest;
x1=pnew;
x2=[obstacle_list(i,1),obstacle_list(i,2)];
dd = distance_squared_point_to_segment(x0,x1,x2);
if dd<(obstacle_list(i,3))^2   %%距离小于障碍物半径collisionflag=1;return
end
end

如果发生碰撞，就放弃本次采样，直接进入下一次

如果没有发生碰撞，计算出的新节点pnew加入节点树nodes，并在nodes存入pnew父节点为p_nearest

2.6 判断是否到达目标点is_near_goal.m

判断的原理就是计算通过碰撞检测的pnew新扩展节点到目标点距离是否小于一个步长RRT_stepsize,是的话，

则说明达到，并将目标点加入节点树nodes，记录其父节点为此时的pnew,实现代码如下：

function goalflag=is_near_goal(pnew,goal,RRT_stepsize)
goalflag=0;
dist=pdist([pnew;goal],'euclidean');
if dist<RRT_stepsizegoalflag=1;return
end
end

3. 运行结果

RRT算法的几个可调节参数步长RRT_stepsize，随机采样取得目标点概率p

RRT_stepsize越大，计算路径的速度越快，当步长过大可能来回震荡往复无法达到目标点附近；

p越大，计算路径的速度越快，节点更快向目标点生长，但p过大时，遇到障碍物时要花费更多的时间才能绕开，反而使搜索速度下降。

从运行结果来看，搜索到的并非最优路径，可以了解RRT相关的改进算法如RRT*等，考虑到路径代价的优化。

运行视频：https://www.bilibili.com/video/BV1wK4y1R7H7/

运行代码：https://download.csdn.net/download/weixin_39199083/18932919?spm=1001.2014.3001.5501