一文解读无人驾驶全局路径规划 - RRT算法原理

发布者:太白山人最新更新时间:2023-09-11 来源: elecfans关键字:无人驾驶 手机看文章 扫描二维码
随时随地手机看文章

无人驾驶路径规划

众所周知,无人驾驶大致可以分为三个方面的工作:感知,决策及控制。 路径规划是感知和控制之间的决策阶段,主要目的是考虑到车辆动力学、机动能力以及相应规则和道路边界条件下,为车辆提供通往目的地的安全和无碰撞的路径。 路径规划问题可以分为两个方面: (一)全局路径规划:全局路径规划算法属于静态规划算法,根据已有的地图信息(SLAM)为基础进行路径规划,寻找一条从起点到目标点的最优路径。 通常全局路径规划的实现包括Dijikstra算法,A*算法,RRT算法等经典算法,也包括蚁群算法、遗传算法等智能算法; (二)局部路径规划:局部路径规划属于动态规划算法,是无人驾驶汽车根据自身传感器感知周围环境,规划处一条车辆安全行驶所需的路线,常应用于超车,避障等情景。通常局部路径规划的实现包括动态窗口算法(DWA),人工势场算法,贝塞尔曲线算法等,也有学者提出神经网络等智能算法。


全局路径规划 - 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数组中管理,同时定义地图的边界。

33b8ff94-a1d0-11ed-bfe3-dac502259ad0.png

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

 

33d52afc-a1d0-11ed-bfe3-dac502259ad0.png

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


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

33e35758-a1d0-11ed-bfe3-dac502259ad0.png

左图:合适的步长                                            右图:步长过大 判断标志位若为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);

程序运行最终效果如下:


[object Object]

红点都是生成点随机点,绿点是tree中节点,红色路径即为RRT算法规划的路径。 6、路径平滑(B样条曲线) 由于规划的路径都是线段连接,在节点处路径不平滑,这也是RRT算法的弊端之一。一般来说轨迹平滑的方法有很多种,类似于贝塞尔曲线,B样条曲线等。 我在这采用B样条曲线对规划的路径进行平滑处理,具体的方法和原理我后续有时间再进行说明,这里先给出结果:345b45ec-a1d0-11ed-bfe3-dac502259ad0.png 

黑色曲线即位平滑处理后的路径。 多组结果对比 ① 相邻两次仿真结果对比:3483794a-a1d0-11ed-bfe3-dac502259ad0.png

可以看出由于随机采样的原因,任意两次规划的路径都是不一样的。  ② 复杂环境下的路径规划。选取一个相对复杂的环境,仿真结果如下:34b1deca-a1d0-11ed-bfe3-dac502259ad0.png

可以看出RRT算法可以很好解决复杂环境下的路径规划问题。 ③ 狭窄通道下的路径规划。选取一个狭窄通道环境,仿真结果如下:34d243e0-a1d0-11ed-bfe3-dac502259ad0.png

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


结语

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


关键字:无人驾驶 引用地址:一文解读无人驾驶全局路径规划 - RRT算法原理

上一篇:现代电动汽车车载充电器的高效散热管理设计
下一篇:智能电车系列之车载雷达“激光雷达”

推荐阅读最新更新时间:2024-11-24 17:16

福特无人驾驶新专利曝光 方向盘踏板全消失
自动驾驶汽车发展速度迅猛,越来越多趋势表明或许在不久的将来,汽车方向盘的消失终于将成为可能,最近福特汽车公司的一项无人驾驶系统的新专利就给我们提供了足够多的信心。 在这套系统中,自动驾驶汽车的驾驶盘等操作装置可以被收缩放置进一个汽车内前方的封闭空间里并被安全锁定,从而将其隐藏掉,为汽车内部提供更加广阔充足的空间。为了驾驶员的安全,福特还准备了两个前置安全气囊,一个在驾驶盘上,一个放置在仪表盘上,一系列的传感设备将可以准确监测到车轮是否精准就位,并在意外事故中为乘客提供最大化的安全保障。 根据福特介绍,这种可拆卸驾驶盘还可以通过花键轴或者线控技术与路面建立物理连接,踏板也是一样,福特希望能够利用紧固件和弹簧卡接合器来安装
[汽车电子]
无人汽车/无人地铁来了,交通进入“无人”时代
很多人可能在科幻电影中看过,无人驾驶的汽车在天空中自由翱翔,如今,这个场景已经实现了一半,无人驾驶的时代即将来临。 在今年7月5日,百度AI开发者大会上,百度CEO李彦宏便乘坐着一辆百度与博世一起开发,基于Apollo技术的自动驾驶汽车,在来往会场的路上。这也是百度将人工智能、地图、大数据相结合的一项尝试,当然在行驶过程中还有些不完善的地方,但是这些都可以通过今后的技术革新来改进。   百度与奇瑞合作的无人驾驶汽车 除此之外,中国的无人驾驶地铁已经在13年底于香港特区启用,据悉,作为中车乃至全国首个运营在国际最高技术要求的香港市场的全无人驾驶项目,该产品的成功研制对于中车开拓国内、国际无人驾驶地铁车辆市场具有重要意义,也为今后中
[嵌入式]
丰田展示下一代无人驾驶汽车 两个方向盘是亮点
新浪科技讯 北京时间9月28日上午消息,多年来,有一家创业公司秘密运营,今年4月才揭开神秘面纱,为了快速研发自动驾驶汽车的速度,丰田向这家创业公司求助。 本周三,丰田研发分支机构展示下一代自动驾驶测试汽车,按照丰田的说法,新车侦测目标物和道路时更加精准,寻找安全驾驶路线时预测能力更强。 接下来,丰田准备在硅谷、密歇根安阿伯(Ann Arbor)、马萨诸塞剑桥的公路上测试Platform 2.1汽车,最开始会在封闭公路上测试,以后会在公共道路上测试。2017年3月,丰田研究院曾展示第一代无人驾驶汽车。现在丰田突然转向,由此可以证明汽车制造商、科技公司都在加快研发速度,它们希望能早一点推出无人驾驶汽车。 一直以来,汽车制造
[手机便携]
Waymo CTO:5G将成为下一代无人驾驶的基石
9日,Waymo首席技术官Dmitri Dolgov在加州山景城的会议上告诉外媒,5G技术和下一代蜂窝网络将成为公司无人驾驶车队的基石。 “我认为这些技术会在通讯延迟和带宽方面带来巨大的改善。我们的汽车还依赖车载电脑来确保安全,但5G将成为它的加速器。”他说道,并表示Waymo的无人驾驶Chrysler Pacifica都是通过双调制解调器来侦测危险和改变驾驶线路的。 5G技术将成为一大批高科技的核心,这包括新型的蜂窝天线、调制解调器、无线接入节点、载波聚合技术、大规模天线技术和正交振幅调制等。 除此之外,5G也不只有“一种”。毫米波指的是频率在24GHz到300GHz之间的超高频电磁波,它能在较短的距离内(约1000英尺)稳定地
[机器人]
无人驾驶与人相撞 技术不成熟遭质疑
  近日一辆 Uber 的自动驾驶汽车在亚利桑那州坦佩市的公共道路上与一名行人相撞,该行人在送往医院后不治身亡。这起意外事故将不仅影响Uber的自动驾驶的计划,还将影响到整个 无人驾驶 行业最终发布能在公共道路上行驶的无人汽车的计划。这则新闻将“ 无人驾驶 技术”推向热搜。下面就随汽车电子小编一起来了解一下相关内容吧。    现有 无人驾驶 技术路线优缺点   目前,国际上自动驾驶环境感知的技术路线主要有两种:一种是以特斯拉为代表的毫米波雷达主导的多传感器融合方案,另一种以高成本激光雷达为主导,典型代表如谷歌Waymo。我们来分析一下这两条线路对前方路况分析所使用的传感器:   特斯拉的无人驾驶方案以毫米波雷达+可见光摄像
[汽车电子]
无人驾驶车离现实还有多远
即使硅谷已经透过技术研发和并购方式发展无人驾驶技术,但是要取得实质进展之前,唯有等待万事俱备。 要将汽车钥匙交给自动化驾驶仍有一大段路要走 虽然机器学习已经渗透到人类日常生活的很多层面,但是自动驾驶车将是该技术最大的转折点。因为许多人根本搞不清楚人工智能的作用,所以无人驾驶车将成为体验人工智能的转折点。 其实,在2016年一份关于人工智能长期影响的研究报告中指出,自动化运输最有可能是人工智能大幅度深入人们生活的最大应用,所以其表现的好坏将极大地影响公众对人工智能的看法。 目前Delphi Technologies、英伟达和特斯拉等三家公司是华尔街认为在人工智能与无人驾驶车结合方面的领导厂商。现今也有数十家新创公
[网络通信]
从科技圈杀入汽车圈的一匹黑马
“文明像一场五千年的狂奔,不断的进步推动着更快的进步,无数的奇迹催生出更大的奇迹。” 在刘慈欣的小说《三体》中描绘的情形正发生在无人驾驶领域。在2016年11月份的洛杉矶车展上,英特尔首席执行官科再奇兴奋地宣布:“英特尔将在未来两年投入超过2.5亿美元的新投资,以期实现全面无人驾驶。”随即,2017年1月初的CES上,英特尔的一系列无人驾驶重磅消息再度令科技圈和汽车圈沸腾:宝马集团、英特尔和Mobileye将在2017年下半年开始无人驾驶汽车的路测;英特尔GO平台发布,Intel Go品牌诞生;英特尔收购HERE公司15%的股权。破年之际,科再奇对无人驾驶的承诺已经在大刀阔斧地实现。 早在英特尔无人驾驶事业部成立之前,英特尔的物联
[汽车电子]
要靠智能汽车赚钱?这些公司表示它们都很稳健
智能系统: 均胜电子 和亚太股份 均胜电子(600699)近年来通过全球并购,精准布局智能汽车和新能源汽车领域。近期并购的KSS公司和TS道恩公司信息板块业务进一步完善了公司的“智能汽车+BMS+高端功能件总成+机器人”四大战略。   均胜电子的优势硬件人车交互产品HMI将会和TS德累斯顿车载信息系统进行有效软硬件结合,同时将结合KSS公司在汽车主被动安全方面的历史积累技术,成为用户使用汽车的直接入口,巩固了均胜汽车电子产品在全球的地位。   亚太股份(002284)是国内研发生产整套汽车制动系统的一级汽车零部件供应商,公司成功研发并批量生产的ABS系统打破了国外品牌的垄断格局。近年来公司积极布局智能驾驶领域。
[嵌入式]

推荐帖子

用 FPGA 正确拿下 LVDS 的数据
用LVDS传送数据的优点,不多说了。用FPGA+LVDS传送IC的应用,这个没啥难的,不多说了。用FPGA直接进行LVDS传送,单说FPGA发送,这个也没什么悬念,不多说了。今天主要说如何用FPGA正确的接收LVDS数据。尤其是高速的数据。用FPGA正确拿下LVDS的数据对于一个时钟传送两个比特数据的,也就是传说的DDR,现在随处可见,调整好时序,做好约束,用iddr拿下数据即可 LVDS一个时钟可以传送几个bit数
5525 FPGA/CPLD
手把手教你自制神奇的果汁LED灯
果汁LED灯的制作方法:一需要的材料:纸一个发光二极管(LED)一个水果(苹果、梨等)长25cm的铜线(直径1-2mm)长25cm的铁线(直径1-2mm)二、需要的工具:电烙铁焊锡剪刀三、制作过程Timson,如果您要查看本帖隐藏内容请回复手把手教你自制神奇的果汁LED灯:P:P:P;P;P;P看看回复这个有创意看看是个啥回复楼主探路者的帖子看看怎么样看看是什么样子的
探路者 LED专区
关于RF24L01信号传输过程中的时延问题,求大神解答!
我想分别用两片ATMEGA16芯片控制两个RF24L01之间的信号及数据的传送,控制发送的芯片(简称芯片1)与上位机相连,而控制接收的芯片(简称芯片2)与传感器相连,这一套的目的是为了能够有上位机控制,远程截取传感器信号,然后传回上位机。我想询问的问题是:当上位机发出起始信号时,需要由芯片1控制24L01发送开始指令(一个字符串),然后由另一个24L01接收到指令传给芯片2,此时芯片2截取传感器信号。假设上位机发出起始信号的时刻为T1,芯片2截取传感器信号的时刻为T2,那么这两个时刻之间存在
longhui520 RF/无线
FPGA信号恢复
如果我想用fpga把图中下面两个信号恢复成上面的,大家有啥意见吗?这是视频流24为RGB中的信号FPGA信号恢复自己顶一下~~我是这样想:连续5个bit都是1,那么就认为是高电平到了,反之亦可对底下2个信号进行计数,高电平多于低电平就拉高,低电平多于高电平就拉低coyoo发表于2014-8-2109:24我是这样想:连续5个bit都是1,那么就认为是高电平到了,反之亦可 意思是把四个周期一下的变化省略?是这个意思吗以撒发表于2014-8-24
3008202060 FPGA/CPLD
急问:NMOS做开关,源极可否下拉电阻接地,导通时电压可以达到VDD=48V么
如题,我想要用NMOS做快速开关,源极下拉电阻接地,漏极接VDD=+48V,导通时将源极电压拉至+48V,不知道可不可以实现如果可以的话,下拉电阻大概应该是多少,多谢多谢急问:NMOS做开关,源极可否下拉电阻接地,导通时电压可以达到VDD=48V么那要看你G端的电压是多少,如果Vgs不足以让MOS导通,那S端就不会有电压。当然电阻接地时可以,当然电阻一定要大,不然会有电阻上会流比较大的电流从而造成浪费值得思考关注中如果用TPS2816是否可以驱动啊,vcc=12V一般的mos
JIAOYANGJITUAN 嵌入式系统
2812的AD转换问题
最近在学2812的片内AD转换,有几个问题不明白,请教大家:1、从AD转换被触发到一次AD转换完毕发生中断这个过程有多长的时间?2、AD转换的结果在结果寄存器中是怎么保存的?不管设置的是用哪个通道转换,结果都是从RESULT0寄存器开始保存的吗?一次保存多少个数据?3、我只设置了一个通道来进行转换,而且每次都是从RESULT0来把结果读到一个数组中,这个数组的长度设置有没有什么标准?我发现AD转换一次,这个存结果的数组中发生变化的个数很多而且每次都不一样,请问这正常吗?敬请高手解答!谢谢
saturday 微控制器 MCU
小广播
最新嵌入式文章
何立民专栏 单片机及嵌入式宝典

北京航空航天大学教授,20余年来致力于单片机与嵌入式系统推广工作。

换一换 更多 相关热搜器件

 
EEWorld订阅号

 
EEWorld服务号

 
汽车开发圈

电子工程世界版权所有 京B2-20211791 京ICP备10001474号-1 电信业务审批[2006]字第258号函 京公网安备 11010802033920号 Copyright © 2005-2024 EEWORLD.com.cn, Inc. All rights reserved