一文解读无人驾驶全局路径规划 - 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-12 19:48

传福特携谷歌研发无人驾驶汽车 最快CES公布
     上周Google刚表示将把自动驾驶汽车部门从Google脱离出来,从而成立一个母公司Alphabet旗下的独立公司。据外媒报道,知情人士透 露,福特公司将与Google建立一个独立的自动驾驶汽车公司,共同研发使用谷歌技术的无人驾驶汽车,并且成立新公司的消息将在CES2016展会上正式宣布。   消息人士称,通过与谷歌合作,福特在无人驾驶软件开发方面将取得重大进展。福特已经测试其自主研发的无人驾驶系统多年,直到12月份才透露其开始在加州街头进行公开测试。谷歌目前在加州和得克萨斯州拥有53辆测试车辆,已经自动行驶了近200万公里。   而通过与福特结盟,谷歌可以避免花费大量资金和时间研发自己的汽车制造技术。今年年初,谷歌联
[手机便携]
机器人比赛的意义,国外居然玩出这么多花样
2004年,第一届DARPA Grand Challenge 无人驾驶 汽车挑战赛的结果并不如预期。虽然奖金高达100万美元,但没有任何一个团队完成比赛。表现最好的队伍是CMU的Red Team,但也只行驶了11.78KM(共240KM),就一头栽进了石头中。   结果是失败了,但在某种意义上整个比赛极为成功。   2005年,一年后的第二次挑战赛,完成比赛的团队数一下就升到了5个,其中Sebastian Thrun带领的斯坦福队,以Stanley汽车获得了冠军。不久后Thrun加入Google,创立了神秘的X部门,而无人驾驶汽车也在Google孵化了十年,并逐步走入公众视野。     除了DARPA、
[嵌入式]
沃尔沃首个矿山无人驾驶卡车运输服务落地
近日沃尔沃宣布,他们的首个商用无人驾驶卡车项目落地挪威的Kalk矿井,这些无人驾驶卡车将会在港口与矿井间往返运输矿石。 沃尔沃近年来在无人驾驶卡车上可谓是下足了功夫。在巴西,沃尔沃测试了自动转向卡车,以帮助甘蔗农扩大种植范围,同时,还有无人驾驶垃圾转运车协助城市卫生工作。 而早在2016年,沃尔沃就在瑞典的地下矿井里展开无人驾驶卡车的测试了,这项技术既可以提高运输效率,又能在极端的工作环境下保证工人的安全。 对于自动驾驶进程的推动来说,这绝对是个好消息,根据目前自动驾驶技术的发展来看,完全落地民用还遥不可及,仅能在一些小范围区域展开测试,但是封闭场地里的稳定运行对于特定场景的应用却可大大节省成本和劳动力。 沃尔沃卡车的自动驾驶部门
[机器人]
图森未来登陆美国,无人驾驶服务获美国邮政认可
图森未来宣布,与美国邮政(USPS)达成合作,为其提供无人驾驶运输服务,并在亚利桑那州凤凰城邮政服务中心和德克萨斯州达拉斯配送中心之间超过1600公里的运输线路上往返运输货物。 这条新运输路线的开通是图森未来又一个重要的里程碑,也是图森未来首次将其在美无人驾驶运输业务扩展到亚利桑那州以外。在为美国邮政运输期间,图森未来的无人驾驶卡车将每天运行22小时,跨越白天和黑夜,经过I-10、I-20和I-30公路,穿越亚利桑那州、新墨西哥州和德克萨斯州。在运输服务期间,图森未来无人驾驶卡车将在车上始终配备一名工程师和一名安全员,以监控车辆性能并确保公共运输的安全。 除了美国邮政以外,其他与图森未来合作的客户也对往返亚利桑那州和德克萨
[嵌入式]
韩国SK电信成功测试无人驾驶汽车
英国运营商已经用汽车展示过5G网络,韩国SK电信更进一步,它在快速公路上公开测试无人驾驶汽车。 汽车是在京釜快速公路上测试的,那可是韩国最繁忙的公路,汽车时速约26公里,最高时速可以达到80公里,从首尔服务区一直开到水原-Shingal交叉路口。根据韩国监管机构的要求,无人驾驶汽车时速不能超过80公里。 自动驾驶汽车实时分析速度和相邻车辆的距离。汽车在Pangyo IC检查了环境数据,比如道路标志、驾驶车道,与附近车辆保持安全距离后开始朝着水原市行驶。 SK电信网络研发中心主管、高级副总裁Park Jin-hyo在声明中表示:“今天我们在快速公路上成功测试自动驾驶汽车,接下来我们会在市区公路、国道、高速公路测试。不只如此,我们还
[嵌入式]
气死苹果和谷歌:本田宣布2020年销售无人驾驶汽车
日本汽车制造商本田表示,计划将在2020年开始销售无人驾驶汽车。作为一家传统汽车厂商,本田将在该领域和谷歌等科技公司展开竞争。 日本另外两家汽车制造商丰田和日产也表示在2020年推出无人驾驶汽车。届时日本将会举办东京奥运会。   本田公司发言人拒绝在下周的东京汽车展前透露相关细节。在这次车展上,其他厂商预计将向公众展示“机器人汽车”原型机。 由于编写代码在汽车开发过程中变得越来重要,传统汽车制造商担心未来将被谷歌、苹果等科技公司超越。谷歌目前已经在美国测试无人驾驶汽车,而也有传言指出苹果未来将涉足汽车领域。
[嵌入式]
什么时候能实现无人驾驶啊?
智能汽车未来将对整个汽车产业产生何种影响?无人驾驶“砸钱”!“联姻”!进入9月,车企、互联网企业、资本争相涌入智能汽车市场,跨界合作频频进入公众视野。 有研究机构预计,2020年我国智能汽车市场规模接近600亿元。智能汽车未来将对整个汽车产业产生何种影响?无人驾驶何时从科幻变成现实?   车企、互联网企业、资本争相入场 9月20日,百度宣布升级Apollo智能汽车开放平台,分三年投入100亿元,完成超过100家项目的投资,建设Apollo系统的生态圈。 9月27日,阿里巴巴集团宣布升级操作系统战略,发布全新的AliOS品牌,并提出“携手斑马网络和更多汽车全产业链的合作伙伴,共同推动汽车行业的智能化转型”。 与此同时,车企与互联网
[嵌入式]
比亚迪突围汽车电子:Android车型+无人驾驶
    比亚迪汽车电子已经有多年积累,2014年将有内置Android操作系统的新车上市。“车内互联在技术上是可行的,3G视频等开放式在技术实现上没有问题,主要是考虑到流量和费用的方面问题。”比亚迪相关负责人表示,以后在车内一样可以玩微博、微信。关于汽车操作系统,目前比亚迪的汽车主要还是采用WIN CE操作系统,采用Android操作系统的车型正在研发,预计2014年会上市。“长期来看Android肯定是的大趋势,但是Android面临的一个很重要问题是可靠性问题,如果发生死机、病毒感染等问题会对汽车的驾驶安全造成影响。”这位负责人说,未来比亚迪可能将推出一个名为QNX,即能有Win CE系统的稳定,又能保持Android系统的高
[汽车电子]

推荐帖子

C2000 解析器至数字转换套件
C2000解析器至数字转换套件,是一款主板样式分解器数字转换套件,此套件使用片上ADC对各种C2000微控制器进行基于软件的分解器数字转换的实验。此分解器套件还允许连接到分解器和逆变器控制处理器。特性 -controlCARDDIMM100样式接口 -XDS100v2USB转JTAG仿真(采用额外UART) -通过四个PWMDAC引脚可在示波器上轻松观察系统 -正弦波载波接口和滤波器 -连接至MCU
Aguilera 微控制器 MCU
单片机重启死机问题求助
现在在STM32F429的使用过程中有偶发性的出现这样的问题,就是在重启的时候会死机,无论是按RESET按键,还是程序软重启或者是断电重启都会出现这个问题,不是每次必现,偶尔会出现,出现之后必须重新烧写程序才会回复正常,不太清楚死机是因为程序没启动还是因为程序跑飞了。我的BOOT0和BOOT1都是0.广大网友有没有遇到过这种问题,能有什么解决方法呢?单片机重启死机问题求助什么单片机,程序里有没有擦除
zhangliyuan stm32/stm8
2015年全国大学生电子竞赛指定芯片最强资料
2015年全国大学生电子竞赛指定芯片最强资料2015年全国大学生电子竞赛指定芯片最强资料你那里有没关于PLL的原理图啊,亲能侧用的,不晓得今年会怎么用100mhz的振荡器,放大会出什么题目PLL
xiayuandong 电子竞赛
直接面向AI场景的开发板:phyBOARD-i.MX 8M Plus 开发板免费申请进行时!
德国PHYTEC是一家为工业设备开发商提供高稳定,成本优化解决方案的公司,本次论坛给大家提供的板卡就是他家的。本次测评开发板是直接面向AI场景的开发板,包含了搭载NXPi.MX8MPlusSoC,适合边缘计算的核心板、底板、24V电源适配器、USB-AtoUSB-BMicroCable(WK345)、RS-232Cable(WF072)、网线以及预装系统镜像的U盘。核心板上的i.MX8MPlus处理器具有四个Cortex-A53核和
EEWORLD社区 测评中心专版
STM32的VDD和VSS脚短路的问题(不是焊接造成的)急
STM32的VDD和VSS脚短路的问题(不是焊接造成的)急我一共焊了12块STM32F103C8T6的板子,其中有11块,不能用,经过割线和测试空板子和用放大镜看,我发现不是板子和焊接的问题,而是STM32芯片的VDD和VSS脚短路。板子上公有4对VDD和VSS,其中的VDDA(9脚)和VSSA(8脚)没事。其余3对VDD和VSS全部短路。(我把板子上的VDD脚从板子上锨起来测)。我的板子上没有负压。难道是ESD的问题?如果是把IO口打坏了,能说的过去。可是把VDD和VSS短路,还
chenmaoxiong stm32/stm8
入门必备!STM32物联网实战教程
《STM32物联网实战教程》集成了单片机教学、计算机网络以及物联网实战这三部分。配合着风媒电子出品的青柚ZERO物联网开发板以及配套的丰富的例程和资料,使得该教程非常适合各大高校信息专业学生以及电子爱好者入门单片机和物联网,并快速开发出自己的物联网项目。本套教程在截稿之时累计字数已经达到12万,400余页,共计38章教程,这38章教程按照内容将其分为三大部分:第一部分是STM32的学习,其内容为各个外设及其驱动程序的讲解,在第三十一章结束,该部分就是一套完整的STM32教程,如果
arui1999 下载中心专版
小广播
最新嵌入式文章
何立民专栏 单片机及嵌入式宝典

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

换一换 更多 相关热搜器件

 
EEWorld订阅号

 
EEWorld服务号

 
汽车开发圈

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