全局路径规划 - RRT算法原理及实现

2022-11-07 19:34

发布于：山西省

计算机视觉life”，选择“星标”

快速获得最新干货

文章转载古月居

无人驾驶路径规划

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

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

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

（一）全局路径规划：全局路径规划算法属于静态规划算法，根据已有的地图信息（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; whilei<=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。实际上可以将两个判断结合，即判断新节点和父节点的连线线段上的点是否在障碍物范围内。

fork= 1: 1:size(ob, 1) fori=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中，否则重复该过程直至找到目标点。

%% 显示 fori= 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 end

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]; fori=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);

程序运行最终效果如下：