100字范文 > 自动驾驶路径规划算法学习-RRT算法及matlab实现

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

时间：2019-02-18 06:50:13

参考手把手教用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.mstart = [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 onfor i=1:length(obstacle_list(:,1))%调用画障碍物函数plot_obstacle(obstacle_list(i,1),obstacle_list(i,2),obstacle_list(i,3))endplot(start(1),start(2),'og')hold onplot(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是startp_randx = randi(16)-1; %x随机采样范围0-15p_randy = randi(13)-1; %x随机采样范围0-12p_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里，重复了，避免出错continueend

特别注意，我在跑程序时开始跑很多次总有一两次会陷入死循环，怎么都找不到错误所在，后来发现是在回溯路径时出现了两个节点互为父节点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;returnend%线段 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时距离取x3x1projection = 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;returnendend

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

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