无人驾驶汽车路径规划技术研究（无人驾驶路径规划（一）全局路径规划 – 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中模拟栅格地图环境 ，自定义障碍物位置 。
%% 生成障碍物 ob1 = [0,-10,10,5]; % 三个矩形障碍物 ob2 = [-5,5,5,10]; ob3 = [-5,-2,5,4]; ob_limit_1 = [-15,-15,0,31]; % 边界障碍物 ob_limit_2 = [-15,-15,30,0]; ob_limit_3 = [15,-15,0,31]; ob_limit_4 = [-15,16,30,0]; ob = [ob1;ob2;ob3;ob_limit_1;ob_limit_2;ob_limit_3;ob_limit_4]; % 放到一个数组中统一管理 x_left_limit = -16; % 地图的边界 x_right_limit = 15; y_left_limit = -16; y_right_limit = 16;我在这随便选择生成三个矩形的障碍物 ，并统一放在ob数组中管理 ，同时定义地图的边界 。
2 、初始化参数设置
初始化障碍物膨胀范围 、地图分辨率 ，机器人半径 、起始点 、目标点 、生长距离和目标点搜索阈值 。
%% 初始化参数设置 extend_area = 0.2; % 膨胀范围 resolution = 1; % 分辨率 robot_radius = 0.2; % 机器人半径 goal = [-10, -10]; % 目标点 x_start = [13, 10]; % 起点 grow_distance = 1; % 生长距离 goal_radius = 1.5; % 在目标点为圆心 ，1.5m内就停止搜索3 、初始化随机树
初始化随机树 ，定义树结构体tree以保存新节点及其父节点 ，便于后续从目标点回推规划的路径 。
%% 初始化随机树 tree.child = []; % 定义树结构体 ，保存新节点及其父节点 tree.parent = []; tree.child = x_start; % 起点作为第一个节点 flag = 1; % 标志位 new_node_x = x_start(1,1); % 将起点作为第一个生成点 new_node_y = x_start(1,2); new_node = [new_node_x, new_node_y];4 、主函数部分
主函数中首先生成随机点 ，并判断是否在地图范围内 ，若超出范围则将标志位置为0 。
rd_x = 30 * rand() - 15; % 生成随机点 rd_y = 30 * rand() - 15; if (rd_x >= x_right_limit || rd_x <= x_left_limit ||... % 判断随机点是否在地图边界范围内 rd_y >= y_right_limit || rd_y <= y_left_limit) flag = 0; end调用函数cal_distance计算tree中距离随机点最近的节点的索引 ，并计算该节点与随机点连线和x正向的夹角 。
[angle, min_idx] = cal_distance(rd_x, rd_y, tree); % 返回tree中最短距离节点索引及对应的和x正向夹角cal_distance函数定义如下：
function [angle, min_idx] = cal_distance(rd_x, rd_y, tree) distance = []; i = 1; while i<=size(tree.child,1) dx = rd_x - tree.child(i,1); dy = rd_y - tree.child(i,2); d = sqrt(dx^2 + dy^2); distance(i) = d; i = i+1; end [~, min_idx] = min(distance); angle = atan2(rd_y - tree.child(min_idx,2),rd_x - tree.child(min_idx,1)); end随后生成新节点 。
new_node_x = tree.child(min_idx,1)+grow_distance*cos(angle);% 生成新的节点 new_node_y = tree.child(min_idx,2)+grow_distance*sin(angle); new_node = [new_node_x, new_node_y];接下来需要对该节点进行判断：
① 新节点是否在障碍物范围内；
② 新节点和父节点的连线线段是否和障碍物有重合部分 。
若任意一点不满足 ，则将标志位置为0 。实际上可以将两个判断结合 ，即判断新节点和父节点的连线线段上的点是否在障碍物范围内 。
for k=1:1:size(ob,1) for i=min(tree.child(min_idx,1),new_node_x):0.01:max(tree.child(min_idx,1),new_node_x) % 判断生长之后路径与障碍物有无交叉部分 j = (tree.child(min_idx,2) - new_node_y)/(tree.child(min_idx,1) - new_node_x) *(i - new_node_x) + new_node_y; 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)) flag = 0; break end end 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中 ，之后重置标志位 。
if (flag == true) % 若标志位为1 ，则可以将该新节点加入tree中 tree.child(end+1,:) = new_node; tree.parent(end+1,:) = [tree.child(min_idx,1), tree.child(min_idx,2)]; plot(rd_x, rd_y, .r);hold on plot(new_node_x, new_node_y,.g);hold on plot([tree.child(min_idx,1),new_node_x], [tree.child(min_idx,2),new_node_y],-b); end flag = 1; % 标志位归位最后就是把障碍物 、起点终点等显示在figure中 ，并判断新节点到目标点距离 。若小于阈值则停止搜索 ，并将目标点加入到node中 ，否则重复该过程直至找到目标点 。
%% 显示 for i=1:1:size(ob,1) % 绘制障碍物 fill([ob(i,1)-resolution, ob(i,1)+ob(i,3),ob(i,1)+ob(i,3),ob(i,1)-resolution],... [ob(i,2)-resolution,ob(i,2)-resolution,ob(i,2)+ob(i,4),ob(i,2)+ob(i,4)],k); end hold on plot(x_start(1,1)-0.5*resolution, x_start(1,2)-0.5*resolution,b^,MarkerFaceColor,b,MarkerSize,4*resolution); % 起点 plot(goal(1,1)-0.5*resolution, goal(1,2)-0.5*resolution,m^,MarkerFaceColor,m,MarkerSize,4*resolution); % 终点 set(gca,XLim,[x_left_limit x_right_limit]); % X轴的数据显示范围 set(gca,XTick,[x_left_limit:resolution:x_right_limit]); % 设置要显示坐标刻度 set(gca,YLim,[y_left_limit y_right_limit]); % Y轴的数据显示范围 set(gca,YTick,[y_left_limit:resolution:y_right_limit]); % 设置要显示坐标刻度 grid on title(D-RRT); xlabel(横坐标 x); ylabel(纵坐标 y); pause(0.05); if (sqrt((new_node_x - goal(1,1))^2 + (new_node_y- goal(1,2))^2) <= goal_radius) % 若新节点到目标点距离小于阈值 ，则停止搜索 ，并将目标点加入到node中 tree.child(end+1,:) = goal; % 把终点加入到树中 tree.parent(end+1,:) = new_node; disp(find goal!); break end5 、绘制最优路径
从目标点开始 ，依次根据节点及父节点回推规划的路径直至起点 ，要注意tree结构体中parent的长度比child要小1 。最后将规划的路径显示在figure中 。
%% 绘制最优路径 temp = tree.parent(end,:); trajectory = [tree.child(end,1)-0.5*resolution, tree.child(end,2)-0.5*resolution]; for i=size(tree.child,1):-1:2 if(size(tree.child(i,:),2) ~= 0 & tree.child(i,:) == temp) temp = tree.parent(i-1,:); trajectory(end+1,:) = tree.child(i,:); if(temp == x_start) trajectory(end+1,:) = [temp(1,1) - 0.5*resolution, temp(1,2) - 0.5*resolution]; end end end plot(trajectory(:,1), trajectory(:,2), -r,LineWidth,2); pause(2);程序运行最终效果如下：
红点都是生成点随机点 ，绿点是tree中节点 ，红色路径即为RRT算法规划的路径 。
6 、路径平滑（B样条曲线）
由于规划的路径都是线段连接 ，在节点处路径不平滑 ，这也是RRT算法的弊端之一 。一般来说轨迹平滑的方法有很多种 ，类似于贝塞尔曲线 ，B样条曲线等 。我在这采用B样条曲线对规划的路径进行平滑处理 ，具体的方法和原理我后续有时间再进行说明 ，这里先给出结果：
黑色曲线即位平滑处理后的路径 。
四 、多组结果对比
① 相邻两次仿真结果对比：
可以看出由于随机采样的原因 ，任意两次规划的路径都是不一样的 。
② 复杂环境下的路径规划 。选取一个相对复杂的环境 ，仿真结果如下：
可以看出RRT算法可以很好解决复杂环境下的路径规划问题 。
③ 狭窄通道下的路径规划 。选取一个狭窄通道环境 ，仿真结果如下：
由于环境采样的随机性 ，在狭长通道内生成随机点的概率相对较低 ，导致可能无法规划出路径 。
五 、结语
由最终仿真结果可以看出 ，RRT算法通过对空间的随机采样可以规划出一条从起点到终点的路径 ，规划速度很快 ，同时不依赖于环境 。但规划过程随机性很强 ，没有目的性 ，会产生很多冗余点 ，且每次规划的路径都不一样 ，对于狭窄通道可能无法规划出路径 。
下篇文章我将对RRT算法的优化提出一些自己的想法 ，并在现有的程序上进行修改 ，最终对比改进前后的RRT算法效果 。
文中如有错误或侵权的地方还欢迎各位指出 ，我会及时回复并进行修改 。
PS：需要matlab源码的朋友可以在评论区留下邮箱 。
创心域SEO版权声明：以上内容作者已申请原创保护，未经允许不得转载，侵权必究！授权事宜、对本内容有异议或投诉，敬请联系网站管理员，我们将尽快回复您，谢谢合作！