无人驾驶路径规划
众所周知,无人驾驶大致可以分为三个方面的工作:感知,决策及控制。 路径规划是感知和控制之间的决策阶段,主要目的是考虑到车辆动力学、机动能力以及相应规则和道路边界条件下,为车辆提供通往目的地的安全和无碰撞的路径。 路径规划问题可以分为两个方面: (一)全局路径规划:全局路径规划算法属于静态规划算法,根据已有的地图信息(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数组中管理,同时定义地图的边界。
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;
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即坐标变换的步长,步长越小判断的越精确,但同时会增加计算量;
步长越大计算速度快但是很可能出现误判,如下图所式。
左图:合适的步长 右图:步长过大 判断标志位若为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样条曲线对规划的路径进行平滑处理,具体的方法和原理我后续有时间再进行说明,这里先给出结果:
黑色曲线即位平滑处理后的路径。 多组结果对比 ① 相邻两次仿真结果对比:
可以看出由于随机采样的原因,任意两次规划的路径都是不一样的。 ② 复杂环境下的路径规划。选取一个相对复杂的环境,仿真结果如下:
可以看出RRT算法可以很好解决复杂环境下的路径规划问题。 ③ 狭窄通道下的路径规划。选取一个狭窄通道环境,仿真结果如下:
由于环境采样的随机性,在狭长通道内生成随机点的概率相对较低,导致可能无法规划出路径。
结语
由最终仿真结果可以看出,RRT算法通过对空间的随机采样可以规划出一条从起点到终点的路径,规划速度很快,同时不依赖于环境。但规划过程随机性很强,没有目的性,会产生很多冗余点,且每次规划的路径都不一样,对于狭窄通道可能无法规划出路径。
上一篇:现代电动汽车车载充电器的高效散热管理设计
下一篇:智能电车系列之车载雷达“激光雷达”
推荐阅读最新更新时间:2024-11-12 19:48
推荐帖子
- 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 下载中心专版