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

描述

无人驾驶路径规划

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

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

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

(一)全局路径规划:全局路径规划算法属于静态规划算法,根据已有的地图信息(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数组中管理,同时定义地图的边界。

RRT

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内就停止搜索

 

RRT

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即坐标变换的步长,步长越小判断的越精确,但同时会增加计算量;

步长越大计算速度快但是很可能出现误判,如下图所式。

RRT

左图:合适的步长        右图:步长过大

判断标志位若为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_limitx_right_limit]); % 设置要显示坐标刻度
set(gca,'YLim',[y_left_limit y_right_limit]); % Y轴的数据显示范围
set(gca,'YTick',[y_left_limity_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];
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);

 

程序运行最终效果如下:

RRT

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

6、路径平滑(B样条曲线)

由于规划的路径都是线段连接,在节点处路径不平滑,这也是RRT算法的弊端之一。一般来说轨迹平滑的方法有很多种,类似于贝塞尔曲线,B样条曲线等。

我在这采用B样条曲线对规划的路径进行平滑处理,具体的方法和原理我后续有时间再进行说明,这里先给出结果:

RRT

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

多组结果对比

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

RRT

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

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

RRT

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

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

RRT

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

结语

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

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

 审核编辑:汤梓红
 
打开APP阅读更多精彩内容
声明:本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人,不代表电子发烧友网立场。文章及其配图仅供工程师学习之用,如有内容侵权或者其他违规问题,请联系本站处理。 举报投诉

全部0条评论

快来发表一下你的评论吧 !

×
20
完善资料,
赚取积分