无人驾驶路径规划
众所周知,无人驾驶大致可以分为三个方面的工作:感知,决策及控制。 路径规划是感知和控制之间的决策阶段,主要目的是考虑到车辆动力学、机动能力以及相应规则和道路边界条件下,为车辆提供通往目的地的安全和无碰撞的路径。 路径规划问题可以分为两个方面: (一)全局路径规划:全局路径规划算法属于静态规划算法,根据已有的地图信息(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-24 17:16
推荐帖子
- 用 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
- 紧凑型 PCI 热插拔控制器,具有总线充电、PCI 复位信号的片上拦截等功能
- AD5341 并行接口、单电压输出、12 位 DAC 的典型应用
- DC831A,用于具有电压 BB 接口的 LT5518EUH I/Q 调制器的演示板
- LTC2945HMS-1 在 -48V 系统中使用外部线性稳压器为光电耦合器和 SCL/SDA 电阻上拉提供电源监控的典型应用
- LTC4367CDD 用于限制浪涌电流的过压电源保护控制器的典型应用
- DC417B,使用 LT1806CS8 多尺寸单运算放大器原型的演示板
- LTC2945IMS 在 -48V 系统中使用低侧检测进行电源监控的典型应用
- 使用 ON Semiconductor 的 KA7805E 的参考设计
- 使用 ROHM Semiconductor 的 BD48E28G-TR 的参考设计
- SY8303_12V降压5V_3Amax