前言 由于后续可能要做一些无人驾驶相关的项目和实验 所以这段时间学习一些路径规划算法并自己编写了matlab程序进行仿真。开启这个系列是对自己学习内容的一个总结 也希望能够和优秀的前辈们多学习经验。

一、无人驾驶路径规划

众所周知 无人驾驶大致可以分为三个方面的工作 感知 决策及控制。

路径规划是感知和控制之间的决策阶段 主要目的是考虑到车辆动力学、机动能力以及相应规则和道路边界条件下 为车辆提供通往目的地的安全和无碰撞的路径。

路径规划问题可以分为两个方面

一 全局路径规划 全局路径规划算法属于静态规划算法 根据已有的地图信息 SLAM 为基础进行路径规划 寻找一条从起点到目标点的最优路径。通常全局路径规划的实现包括Dijikstra算法 A*算法 RRT算法等经典算法 也包括蚁群算法、遗传算法等智能算法

二 局部路径规划 局部路径规划属于动态规划算法 是无人驾驶汽车根据自身传感器感知周围环境 规划处一条车辆安全行驶所需的路线 常应用于超车 避障等情景。通常局部路径规划的实现包括动态窗口算法 DWA 人工势场算法 贝塞尔曲线算法等 也有学者提出神经网络等智能算法。

本系列就从无人驾驶路径规划的这两方面进行展开 对一些经典的算法原理进行介绍 并根据个人的一些理解和想法提出了一些改进的意见 通过Matlab2019对算法进行了仿真和验证。过程中如果有错误的地方 欢迎在评论区留言讨论 如有侵权请及时联系。

那么废话不多说 直接进入第一部分的介绍 全局路径规划算法-RRT算法。

二、全局路径规划 - RRT算法原理

RRT算法 即快速随机树算法 Rapid Random Tree 是LaValle在1998年首次提出的一种高效的路径规划算法。RRT算法以初始的一个根节点 通过随机采样的方法在空间搜索 然后添加一个又一个的叶节点来不断扩展随机树。当目标点进入随机树里面后 随机树扩展立即停止 此时能找到一条从起始点到目标点的路径。算法的计算过程如下

step1 初始化随机树Tree。将环境中起点作为随机树搜索的起点 此时树中只包含一个节点即根节点qq_{start}  

stpe2 在环境中随机采样。在环境中随机产生一个点x_{random} 若该点不在障碍物范围内则计算随机树Tree中所有节点到x_{random}的欧式距离 并找到距离最近的节点x_{near} 若在障碍物范围内则重新生成x_{random}并重复该过程直至找到x_{near};  

stpe3 生成新节点。在x_{near}和x_{random}连线方向 由x_{near}指向x_{random}固定生长距离growdistance生成一个新的节点x_{new} 并判断该节点是否在障碍物范围内 若不在障碍物范围内则将x_{new}添加到随机树 Tree中 否则的话返回step2重新对环境进行随机采样

step4  停止搜索。当x_{near}和目标点x_{goal}之间的距离小于设定的阈值时 则代表随机树已经到达了目标点 将x_{goal}作为最后一个路径节点加入到随机树中 算法结束并得到所规划的路径 。

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 % 分辨率robot_radius 0.2; % 机器人半径goal [-10, -10]; % 目标点x_start [13, 10]; % 起点grow_distance % 生长距离goal_radius 1.5; % 在目标点为圆心 1.5m内就停止搜索
3、初始化随机树

初始化随机树 定义树结构体tree以保存新节点及其父节点 便于后续从目标点回推规划的路径。

%% 初始化随机树tree.child []; % 定义树结构体 保存新节点及其父节点tree.parent [];tree.child x_start; % 起点作为第一个节点flag % 标志位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 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 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) i i 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 break end endend

在这我采用的方法是写出新节点和父节点连线的直线方程 然后将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 % 标志位归位

最后就是把障碍物、起点终点等显示在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 endhold onplot(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 ontitle( 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! breakend
5、绘制最优路径

从目标点开始 依次根据节点及父节点回推规划的路径直至起点 要注意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 endendplot(trajectory(:,1), trajectory(:,2), -r , LineWidth ,2);pause(2);

程序运行最终效果如下

 红点都是生成点随机点 绿点是tree中节点 红色路径即为RRT算法规划的路径。

6、路径平滑 B样条曲线

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

 黑色曲线即位平滑处理后的路径。

四、多组结果对比

① 相邻两次仿真结果对比

可以看出由于随机采样的原因 任意两次规划的路径都是不一样的。 

② 复杂环境下的路径规划。选取一个相对复杂的环境 仿真结果如下

可以看出RRT算法可以很好解决复杂环境下的路径规划问题。

③ 狭窄通道下的路径规划。选取一个狭窄通道环境 仿真结果如下

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

五、结语

由最终仿真结果可以看出 RRT算法通过对空间的随机采样可以规划出一条从起点到终点的路径 规划速度很快 同时不依赖于环境。但规划过程随机性很强 没有目的性 会产生很多冗余点 且每次规划的路径都不一样 对于狭窄通道可能无法规划出路径。

下篇文章我将对RRT算法的优化提出一些自己的想法 并在现有的程序上进行修改 最终对比改进前后的RRT算法效果。

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

PS 需要matlab源码的朋友可以在评论区留下邮箱。

优惠劵
前言：由于后续可能要做一些无人驾驶相关的项目和实验，所以这段时间学习一些路径规划算法并自己编写了matlab程序进行仿真。开启这个系列是对自己学习内容的一个总结，也希望能够和优秀的前辈们多学习经验。一、无人驾驶路径规划众所周知，无人驾驶大致可以分为三个方面的工作：感知，决策及控制。路径规划是感知和控制之间的决策阶段，主要目的是考虑到车辆动力学、机动能力以及相应规则和道路边界条件下，为车辆提供通往目的地的安全和无碰撞的路径。路径规划问题可以分为两个方面：（一）全局路径规划：全局路径规划算法 复制链接
无人驾驶路径规划（三）局部路径规划-Frenet坐标系下的动态轨迹规划 21435
ROS学习记录（二）阿克曼转向车运动学模型及在gazebo中搭建仿真环境 11802

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

更多文章请关注《万象专栏》