无人驾驶路径规划（一）全局路径规划 - RRT算法原理及实现

前言：由于后续大概要做一些无人驾驶相关的项目和实验，以是这段时间学习一些路径规划算法并自己编写了matlab步调举行仿真。开启这个系列是对自己学习内容的一个总结，也希望可以或许和良好的前辈们多学习履历。
一、无人驾驶路径规划

众所周知，无人驾驶大抵可以分为三个方面的工作：感知，决策及控制。
路径规划是感知和控制之间的决策阶段，重要目的是思量到车辆动力学、机动本事以及相应规则和道路边界条件下，为车辆提供通往目的地的安全和无碰撞的路径。
路径规划题目可以分为两个方面：
（一）全局路径规划：全局路径规划算法属于静态规划算法，根据已有的舆图信息（SLAM）为基础举行路径规划，寻找一条从出发点到目的点的最优路径。通常全局路径规划的实现包括Dijikstra算法，A*算法，RRT算法等经典算法，也包括蚁群算法、遗传算法等智能算法；
（二）局部路径规划：局部路径规划属于动态规划算法，是无人驾驶汽车根据自身传感器感知四周环境，规划处一条车辆安全行驶所需的门路，常应用于超车，避障等情景。通常局部路径规划的实现包括动态窗口算法（DWA），人工势场算法，贝塞尔曲线算法等，也有学者提出神经网络等智能算法。
本系列就从无人驾驶路径规划的这两方面举行展开，对一些经典的算法原理举行先容，并根据个人的一些理解和想法提出了一些改进的意见，通过Matlab2019对算法举行了仿真和验证。过程中如果有错误的地方，欢迎在批评区留言讨论，如有侵权请及时接洽。
那么废话不多说，直接进入第一部分的先容，全局路径规划算法-RRT算法。
二、全局路径规划 - RRT算法原理

RRT算法，即快速随机树算法（Rapid Random Tree），是LaValle在1998年初次提出的一种高效的路径规划算法。RRT算法以初始的一个根节点，通过随机采样的方法在空间搜刮，然后添加一个又一个的叶节点来不停扩展随机树。当目的点进入随机树里面后，随机树扩展立刻克制，此时能找到一条从起始点到目的点的路径。算法的计算过程如下：
step1：初始化随机树。将环境中出发点作为随机树搜刮的出发点，此时树中只包罗一个节点即根节点； 
stpe2：在环境中随机采样。在环境中随机产生一个点，若该点不在障碍物范围内则计算随机树中所有节点到的欧式距离，并找到距离最近的节点，若在障碍物范围内则重新生成并重复该过程直至找到;  
stpe3：生成新节点。在和连线方向，由指向固定生长距离生成一个新的节点，并判断该节点是否在障碍物范围内，若不在障碍物范围内则将添加到随机树 中，否则的话返回step2重新对环境举行随机采样；
step4： 克制搜刮。当和目的点之间的距离小于设定的阈值时，则代表随机树已经到达了目的点，将作为末了一个路径节点参加到随机树中，算法竣事并得到所规划的路径 。
RRT算法由于其随机采样及概率完备性的特点，使得其具有如下上风：
（1）不须要对环境具体建模，有很强空间搜刮本事；
（2）路径规划速率快；
（3）可以很好解决复杂环境下的路径规划题目。
但同样是因为随机性，RRT算法也存在很多不敷的方面：
（1）随机性强，搜刮没有目的性，冗余点多，且每次规划产生的路径都不一样，均不一是最优路径；
（2）大概出现计算复杂、所需的时间过长、易于陷入死区的题目；
（3）由于树的扩展是节点之间相连，使得终极生成的路径不平滑；
（4）不适当动态环境，当环境中出现动态障碍物时，RRT算法无法举行有效的检测；
（5）对于狭长地形，大概无法规划出路径。
三、RRT算法Matlab实现

利用matlab2019来编写RRT算法，下面将贴出部分代码举行解释。
1、生成障碍物

在matlab中模仿栅格舆图环境，自界说障碍物位置。

1. %% 生成障碍物
2. ob1 = [0,-10,10,5];             % 三个矩形障碍物
3. ob2 = [-5,5,5,10];
4. ob3 = [-5,-2,5,4];
6. ob_limit_1 = [-15,-15,0,31];    % 边界障碍物
7. ob_limit_2 = [-15,-15,30,0];
8. ob_limit_3 = [15,-15,0,31];
9. ob_limit_4 = [-15,16,30,0];
11. ob = [ob1;ob2;ob3;ob_limit_1;ob_limit_2;ob_limit_3;ob_limit_4];  % 放到一个数组中统一管理
13. x_left_limit = -16;             % 地图的边界
14. x_right_limit = 15;
15. y_left_limit = -16;
16. y_right_limit = 16;

复制代码

我在这任意选择生成三个矩形的障碍物，并统一放在ob数组中管理，同时界说舆图的边界。

2、初始化参数设置

初始化障碍物膨胀范围、舆图分辨率，机器人半径、起始点、目的点、生长距离和目的点搜刮阈值。

1. %% 初始化参数设置
2. extend_area = 0.2;        % 膨胀范围
3. resolution = 1;           % 分辨率
4. robot_radius = 0.2;       % 机器人半径
6. goal = [-10, -10];        % 目标点
7. x_start = [13, 10];       % 起点
9. grow_distance = 1;        % 生长距离
10. goal_radius = 1.5;        % 在目标点为圆心，1.5m内就停止搜索

复制代码

3、初始化随机树

初始化随机树，界说树布局体tree以保存新节点及其父节点，便于后续从目的点回推规划的路径。

1. %% 初始化随机树
2. tree.child = [];               % 定义树结构体，保存新节点及其父节点
3. tree.parent = [];
4. tree.child = x_start;          % 起点作为第一个节点
6. flag = 1;                      % 标志位
9. new_node_x = x_start(1,1);     % 将起点作为第一个生成点
10. new_node_y = x_start(1,2);
11. new_node = [new_node_x, new_node_y];

复制代码

4、主函数部分

主函数中首老师成随机点，并判断是否在舆图范围内，若超出范围则将标记位置为0。

1. rd_x = 30 * rand() - 15;    % 生成随机点
2. rd_y = 30 * rand() - 15;   
3. if (rd_x >= x_right_limit || rd_x <= x_left_limit ||... % 判断随机点是否在地图边界范围内
4.     rd_y >= y_right_limit || rd_y <= y_left_limit)
5.     flag = 0;
6. end

复制代码

调用函数cal_distance计算tree中距离随机点最近的节点的索引，并计算该节点与随机点连线和x正向的夹角。

1. [angle, min_idx] = cal_distance(rd_x, rd_y, tree);    % 返回tree中最短距离节点索引及对应的和x正向夹角

复制代码

cal_distance函数界说如下：

1. function [angle, min_idx] = cal_distance(rd_x, rd_y, tree)
2.     distance = [];
3.     i = 1;
4.     while i<=size(tree.child,1)
5.         dx = rd_x - tree.child(i,1);
6.         dy = rd_y - tree.child(i,2);
7.         d = sqrt(dx^2 + dy^2);
8.         distance(i) = d;
9.         i = i+1;
10.     end
11.     [~, min_idx] = min(distance);
12.     angle = atan2(rd_y - tree.child(min_idx,2),rd_x - tree.child(min_idx,1));
13. end

复制代码

随后生成新节点。

1. new_node_x = tree.child(min_idx,1)+grow_distance*cos(angle);% 生成新的节点
2. new_node_y = tree.child(min_idx,2)+grow_distance*sin(angle);
3. new_node = [new_node_x, new_node_y];

复制代码

接下来须要对该节点举行判断：
① 新节点是否在障碍物范围内；
②  新节点和父节点的连线线段是否和障碍物有重合部分。
若任意一点不满足，则将标记位置为0。现实上可以将两个判断团结，即判断新节点和父节点的连线线段上的点是否在障碍物范围内。

1. for k=1:1:size(ob,1)
2.     for i=min(tree.child(min_idx,1),new_node_x):0.01:max(tree.child(min_idx,1),new_node_x)    % 判断生长之后路径与障碍物有无交叉部分
3.         j = (tree.child(min_idx,2) - new_node_y)/(tree.child(min_idx,1) - new_node_x) *(i - new_node_x) + new_node_y;
4.         if(i >=ob(k,1)-resolution && i <= ob(k,1)+ob(k,3) && j >= ob(k,2)-resolution && j <= ob(k,2)+ob(k,4))
5.             flag = 0;
6.             break
7.         end
8.     end
9. end

复制代码

在这我采用的方法是写出新节点和父节点连线的直线方程，然后将x厘革范围限定在min(tree.child(min_idx,1),new_node_x)到max(tree.child(min_idx,1),new_node_x)内，0.01即坐标变换的步长，步长越小判断的越精确，但同时会增长计算量；步长越大计算速率快但是很大概出现误判，如下图所式。

            左图：符合的步长                                                          右图：步长过大

判断标记位若为1，则可以将该新节点参加到tree中，留意保存新节点和它的父节点，同时表现在figure中，之后重置标记位。

1. if (flag == true)           % 若标志位为1，则可以将该新节点加入tree中
2.     tree.child(end+1,:) = new_node;
3.     tree.parent(end+1,:) = [tree.child(min_idx,1), tree.child(min_idx,2)];
4.     plot(rd_x, rd_y, '.r');hold on
5.     plot(new_node_x, new_node_y,'.g');hold on
6.     plot([tree.child(min_idx,1),new_node_x], [tree.child(min_idx,2),new_node_y],'-b');
7. end
8.    
9. flag = 1;                   % 标志位归位

复制代码

末了就是把障碍物、出发点尽头等表现在figure中，并判断新节点到目的点距离。若小于阈值则克制搜刮，并将目的点参加到node中，否则重复该过程直至找到目的点。

1. %% 显示
2. for i=1:1:size(ob,1)        % 绘制障碍物
3.     fill([ob(i,1)-resolution, ob(i,1)+ob(i,3),ob(i,1)+ob(i,3),ob(i,1)-resolution],...
4.          [ob(i,2)-resolution,ob(i,2)-resolution,ob(i,2)+ob(i,4),ob(i,2)+ob(i,4)],'k');
5. end
6. hold on
8. plot(x_start(1,1)-0.5*resolution, x_start(1,2)-0.5*resolution,'b^','MarkerFaceColor','b','MarkerSize',4*resolution); % 起点
9. plot(goal(1,1)-0.5*resolution, goal(1,2)-0.5*resolution,'m^','MarkerFaceColor','m','MarkerSize',4*resolution); % 终点
10. set(gca,'XLim',[x_left_limit x_right_limit]); % X轴的数据显示范围
11. set(gca,'XTick',[x_left_limit:resolution:x_right_limit]); % 设置要显示坐标刻度
12. set(gca,'YLim',[y_left_limit y_right_limit]); % Y轴的数据显示范围
13. set(gca,'YTick',[y_left_limit:resolution:y_right_limit]); % 设置要显示坐标刻度
14. grid on
15. title('D-RRT');
16. xlabel('横坐标 x');
17. ylabel('纵坐标 y');
18. pause(0.05);
19. if (sqrt((new_node_x - goal(1,1))^2 + (new_node_y- goal(1,2))^2) <= goal_radius) % 若新节点到目标点距离小于阈值，则停止搜索，并将目标点加入到node中
20.     tree.child(end+1,:) = goal;         % 把终点加入到树中
21.     tree.parent(end+1,:) = new_node;
22.     disp('find goal!');
23.     break
24. end

复制代码

5、绘制最优路径

从目的点开始，依次根据节点及父节点回推规划的路径直至出发点，要留意tree布局体中parent的长度比child要小1。末了将规划的路径表现在figure中。

1. %% 绘制最优路径
2. temp = tree.parent(end,:);
3. trajectory = [tree.child(end,1)-0.5*resolution, tree.child(end,2)-0.5*resolution];
4. for i=size(tree.child,1):-1:2
5.     if(size(tree.child(i,:),2) ~= 0 & tree.child(i,:) == temp)
6.         temp = tree.parent(i-1,:);
7.         trajectory(end+1,:) = tree.child(i,:);
8.     if(temp == x_start)
9.         trajectory(end+1,:) = [temp(1,1) - 0.5*resolution, temp(1,2) - 0.5*resolution];
10.     end
11.     end
12. end
13. plot(trajectory(:,1), trajectory(:,2), '-r','LineWidth',2);
14. pause(2);

复制代码

步调运行终极结果如下：

 红点都是生成点随机点，绿点是tree中节点，红色路径即为RRT算法规划的路径。
6、路径平滑（B样条曲线）

由于规划的路径都是线段毗连，在节点处路径不平滑，这也是RRT算法的毛病之一。一般来说轨迹平滑的方法有很多种，类似于贝塞尔曲线，B样条曲线等。我在这采用B样条曲线对规划的路径举行平滑处理惩罚，具体的方法和原理我后续偶然间再举行说明，这里先给出结果：

 玄色曲线即位平滑处理惩罚后的路径。
四、多组结果对比

① 相邻两次仿真结果对比：

可以看出由于随机采样的缘故原由，任意两次规划的路径都是不一样的。 
② 复杂环境下的路径规划。选取一个相对复杂的环境，仿真结果如下：

可以看出RRT算法可以很好解决复杂环境下的路径规划题目。
③ 局促通道下的路径规划。选取一个局促通道环境，仿真结果如下：

 由于环境采样的随机性，在狭长通道内生成随机点的概率相对较低，导致大概无法规划出路径。
五、结语

由终极仿真结果可以看出，RRT算法通过对空间的随机采样可以规划出一条从出发点到尽头的路径，规划速率很快，同时不依靠于环境。但规划过程随机性很强，没有目的性，会产生很多冗余点，且每次规划的路径都不一样，对于局促通道大概无法规划出路径。
下篇文章我将对RRT算法的优化提出一些自己的想法，并在现有的步调上举行修改，终极对比改进前后的RRT算法结果。

文中如有错误或侵权的地方还欢迎各位指出，我会及时复兴并举行修改。

PS：须要matlab源码的朋侪可以在批评区留下邮箱。

来源：https://blog.csdn.net/m0_55205668/article/details/123922046
免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！