自动驾驶路径规划算法学习-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 开始再循环执⾏，从环境中随机采样状态点。

.........

重复上述树的⽣长过程，直到树新⽣成的结点到⽬标点的距离⼩于⼀个步长，则终⽌树的⽣长。直接将该新结点与⽬标点相连。整个步骤动图如下：

dc-dc变换器

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)<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树节点中，就不应再作为新的扩展点，本次随机采样放弃，进⼊下⼀次随机采样。下⾯⽤图说明：

本文发布于:2024-09-22 22:38:32，感谢您对本站的认可！

本文链接：https://www.17tex.com/tex/3/311429.html

上一篇：基于粒子算法的二维路径规划算法设计与仿真

下一篇：【机器人智能控制】-运动规划、路径规划和轨迹规划概念

标签：节点算法障碍物采样结点路径标点起点

留言与评论（共有 0 条评论）