CN115451983B - 一种复杂场景下的动态环境建图与路径规划方法及装置 - Google Patents
一种复杂场景下的动态环境建图与路径规划方法及装置Info
- Publication number
- CN115451983B CN115451983B CN202210952678.4A CN202210952678A CN115451983B CN 115451983 B CN115451983 B CN 115451983B CN 202210952678 A CN202210952678 A CN 202210952678A CN 115451983 B CN115451983 B CN 115451983B
- Authority
- CN
- China
- Prior art keywords
- point cloud
- vehicle
- ground point
- path
- dimensional
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 42
- 238000013507 mapping Methods 0.000 title claims abstract description 15
- 230000006870 function Effects 0.000 claims abstract description 31
- 238000012216 screening Methods 0.000 claims abstract description 12
- 230000001133 acceleration Effects 0.000 claims description 16
- 238000012545 processing Methods 0.000 claims description 16
- 238000010586 diagram Methods 0.000 claims description 15
- 230000011218 segmentation Effects 0.000 claims description 14
- 238000004422 calculation algorithm Methods 0.000 claims description 10
- 238000005070 sampling Methods 0.000 claims description 10
- 238000003860 storage Methods 0.000 claims description 10
- 230000008859 change Effects 0.000 claims description 5
- 238000010276 construction Methods 0.000 claims description 2
- 230000008569 process Effects 0.000 abstract description 5
- 230000008447 perception Effects 0.000 abstract description 4
- 238000004590 computer program Methods 0.000 description 11
- 238000005516 engineering process Methods 0.000 description 7
- 235000013312 flour Nutrition 0.000 description 5
- 238000012986 modification Methods 0.000 description 4
- 230000004048 modification Effects 0.000 description 4
- 238000013461 design Methods 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 3
- 238000003672 processing method Methods 0.000 description 3
- 238000000638 solvent extraction Methods 0.000 description 3
- 230000002776 aggregation Effects 0.000 description 2
- 238000004220 aggregation Methods 0.000 description 2
- 230000004075 alteration Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 238000009826 distribution Methods 0.000 description 2
- 238000000605 extraction Methods 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 244000025254 Cannabis sativa Species 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000005094 computer simulation Methods 0.000 description 1
- 230000036461 convulsion Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000007667 floating Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000011002 quantification Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 239000004576 sand Substances 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Abstract
本发明涉及一种复杂场景下的动态环境建图与路径规划方法及装置，获取车辆周围雷达扫描空间范围内的物体点云数据；对激光雷达扫描的点云数据进行分割获取地面点云和非地面点云，对非地面点云，进行障碍物识别；对非地面点云进行栅格化及二值化处理，生成二维栅格化环境地图；根据车辆当前行驶状态，实时生成多条局部路径；根据二维栅格化环境地图以及安全驾驶需求设置约束条件以及损失函数；根据约束条件对各条局部路径进行筛选，得到可安全通行的局部路径；利用损失函数计算可安全通行的局部路径的损失大小，其中损失最小即为最优路径。本发明可为驾驶员在驾驶过程中，提高对复杂环境的感知能力，保障行车安全。
Description
技术领域
本发明涉及辅助驾驶技术领域，具体涉及一种复杂场景下的动态环境建图与路径规划方法及装置。
背景技术
目前，城市车辆在结构化道路场景下的辅助驾驶技术已经取得了显著的发展与应用。然而当车辆在崎岖不平的山地、草地、戈壁等复杂场景下执行任务时，由于环境复杂性、障碍的突变性、路面状态的不确定性以及先验信息不足等因素使得车辆安全驾驶面临着严峻挑战。为了增强车辆驾驶的安全性与舒适性，提高驾驶员对环境信息的感知和动态路径规划属于车辆辅助驾驶系统的核心，也是保障车辆安全行驶的关键。
在车辆辅助驾驶技术的领域，现有文献已有关于车辆动态环境建图问题的研究，以及车辆局部路径规划方法的设计。例如：专利CN201811592793.5提出一种基于视觉的无人驾驶汽车同时定位于建图创建方法；专利CN202111142908.2提出了一种基于网联的无人驾驶测试环境地图搭建方法及系统；但这些方法都为面向城市结构化道路场景下的车辆建图方法。《复杂动态环境下智能汽车局部路径规划与跟踪》提出了基于最小代价函数的局部路径规划方法，但仅仅是考虑了车辆自身的运动参数，忽略了环境信息。《野外无人车路径规划与轨迹规划技术》提出了基于RBF神经网络对环境建模，然后基于优化约束函数实现无人车局部路径规划，此方法为对环境建立模型，没有获得真实的环境地图。因此对于复杂场景下的辅助驾驶技术仍需进一步研究。相比与公路、城市道路等结构化道路场景下，车辆面临的是更为动态多变的复杂环境。仅基于现有的车辆辅助驾驶技术，难以处理动态复杂场景下的实时建图以及动态局部路径规划问题。
发明内容
本发明针对现有技术中存在的技术问题，提供一种复杂场景下的动态环境建图与路径规划方法及装置。首先基于激光雷达传感器获取复杂场景下的三维环境信息；由于复杂场景下的地面常为崎岖不平，区别于传统地面平面拟合法的提取技术，本发明提出分区域地面提取算法实现将地面分割，同时，采用自适应参数聚类算法识别动静态障碍物；通过阈值过滤器对点云过滤并同时进行栅格化处理，然后基于图像处理技术生成二维动态环境地图；最后在此基础上，基于改进Frenet坐标系实时生成有限条局部路径，根据复杂场景分类，采用权重聚合法设计行驶损失函数，通过损失最小选出最优路径，从而实现车辆的动态局部路径规划。
本发明解决上述技术问题的技术方案如下：
第一方面，本发明提供一种复杂场景下的动态环境建图与路径规划方法，包括以下步骤：
基于激光雷达实时扫描获取车辆周围三维环境信息，得到雷达扫描空间范围内的物体点云数据；
对激光雷达扫描的点云数据进行分割获取地面点云和非地面点云，对非地面点云，进行障碍物识别；
对非地面点云进行栅格化及二值化处理，生成二维栅格化环境地图，所述二维栅格化环境地图包含障碍物区域和非障碍物区域；
根据车辆当前行驶状态，实时生成多条局部路径；
根据所述二维栅格化环境地图以及安全驾驶需求设置约束条件以及损失函数；
根据所述约束条件对各条局部路径进行筛选，得到可安全通行的局部路径；
利用所述损失函数计算可安全通行的局部路径的损失大小，其中损失最小即为最优路径。
进一步的，所述的对激光雷达扫描的点云数据进行分割获取地面点云和非地面点云，包括：
基于RANSAC算法，对激光雷达扫描的点云数据进行粗分割，得到粗平面点云；
根据激光雷达线束间距d，对所述粗平面点云划分为多个区域；
确定每块区域的斜率阈值ks，并计算各个区域点云相对于原点的斜率k，若k≤ks，则该激光点云属于地面点云，否则为非地面点云。
进一步的，对非地面点云进行障碍物识别，包括：
根据激光雷达线束间距d，将非地面点云划分为多个区域；
设置各个区域的聚类系数，所述聚类系数包括最小搜索半径以及最小聚类数量；
采用DBSCAN算法对各区域点云进行逐行逐列扫描，按照聚类系数对各区域点云进行聚类，生成聚类轮廓，并计算各区域的中心点位置。
进一步的，对非地面点云进行栅格化及二值化处理，生成二维栅格化环境地图，包括：
建立矩形栅格图，并对非地面点云坐标进行转换，将点云坐标中的(x,y)变换为对应的像素坐标；
根据z值将栅格图中每个像素点进行二值化处理，对图像进行闭运算去噪点，得到二维栅格化环境地图。
进一步的，根据车辆当前行驶状态，实时生成多条局部路径，包括：
以车辆初始的行驶位置为中心，建立Frenet坐标系，车辆的航向为s轴，基于车辆中心的横向偏移为d轴；
根据车辆宽度设置路径采样宽度Δd，并根据道路宽度以及路径采样宽度Δd确定局部路径的数量；
根据车速以及规划时间需求确定采样时间Δt，计算得到路径长度；
在保证车辆始末航向相同的前提下，使用三次样条曲线拟合生成多条局部路径。
进一步的，所述约束条件包括：
其中vmax为最大限速、amax为最大限加速度、ρmax为最大限曲率、rmax为最大碰撞半径vt为任意时刻车辆纵向速度，at为任意时刻车辆加速度，ρt为任意时刻车辆曲率，d为车辆与绝对不能通行障碍物之间的距离。
进一步的，所述损失函数如下所示：
f损失＝ηsfs+ηcfc
其中，fs为车辆行驶的安全性指标，用于描述车辆通过障碍物以及不同地形的能力，fc为车辆行驶的驾驶舒适性指标，描述车辆行驶时加速度变化的快慢，ηs和ηc分别为安全性指标和驾驶舒适性指标的权重。
第二方面，本发明提供一种复杂场景下的动态环境建图与路径规划装置，包括：
点云获取模块，基于激光雷达实时扫描获取车辆周围三维环境信息，得到雷达扫描空间范围内的物体点云数据；
点云分割以及障碍物识别模块，对激光雷达扫描的点云数据进行分割获取地面点云和非地面点云，对非地面点云，进行障碍物识别；
环境地图生成模块，对非地面点云进行栅格化及二值化处理，生成二维栅格化环境地图，所述二维栅格化环境地图包含障碍物区域和非障碍物区域；
初始路径规划模块，根据车辆当前行驶状态，实时生成多条局部路径；
路径筛选优化模块，根据所述二维栅格化环境地图以及安全驾驶需求设置约束条件以及损失函数；根据所述约束条件对各条局部路径进行筛选，得到可安全通行的局部路径；利用所述损失函数计算可安全通行的局部路径的损失大小，其中损失最小即为最优路径。
第三方面，本发明提供一种电子设备，包括：
存储器，用于存储计算机软件程序；
处理器，用于读取并执行所述计算机软件程序，进而实现本发明第一方面所述的一种复杂场景下的动态环境建图与路径规划方法。
第四方面，本发明提供一种非暂态计算机可读存储介质，所述存储介质中存储有用于实现本发明第一方面所述的一种复杂场景下的动态环境建图与路径规划方法的计算机软件程序。
本发明的有益效果是：
1、本发明针对车辆在复杂多变场景中的辅助驾驶问题，提供了一种复杂场景下的动态环境建图与路径规划方法，可为驾驶员在驾驶过程中，提高对复杂环境的感知能力，保障行车安全。
2、本发明提出的分区域地面点云分割以及障碍物聚类的方法，能够准确的提取障碍物的位置。可以适应野外复杂的非结构化道路，保证对复杂场景下的动态环境建图的准确性。
3、本发明提出的基于Frenet坐标系，使用三次样条曲线拟合生成可行驶的有限条路径，然后针对复杂场景，基于权重聚合法设计路径规划的损失函数。将路径规划问题化为寻找损失最小的问题。具有适应性好，准确高效等优势。
附图说明
图1是本发明实施例提供的复杂场景下的动态环境建图与路径规划方法流程示意图；
图2是本发明实施例提供的以越野车为例动态环境建图与路径规划总体流程图。
图3是本发明实施例提供的分区域地面点云分割与障碍物识别算法流程图；
图4是本发明实施例提供的复杂场景下的动态环境建图与路径规划装置结构示意图；
图5是本发明实施例提供的电子设备的实施例示意图
图6是本发明实施例提供的一种计算机可读存储介质的实施例示意图。
具体实施方式
以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。
如图1所示，本发明实施例提供一种复杂场景下的动态环境建图与路径规划方法，包括以下步骤：
S100，基于激光雷达实时扫描获取车辆周围三维环境信息，得到雷达扫描空间范围内的物体点云数据。
在该步骤中，物体点云数据以坐标形式存储。物体点云坐标(x,y,z)，指的是在车辆行驶任意时刻下，基于激光雷达坐标系下，雷达扫描一圈得到的单帧点云信息，然后通过格式转换得到的(x,y,z)坐标信息。
S200，对激光雷达扫描的点云数据进行分割获取地面点云和非地面点云，对非地面点云，进行障碍物识别。
首先对激光雷达扫描的点云数据进行分割获取地面点云和非地面点云。
具体包括以下两个步骤：
a)基于RANSAC算法，对激光雷达扫描的点云数据进行粗分割，得到粗平面点云。
设置随机种子，然后在单帧点云中随机选取3个点云，进行平面拟合。平面拟合的方程为：Ax+By+Cz+D＝0。
然后计算其他各点云到平面法向量之间的距离d点-面，计算公式为：
判断距离d点-面是否小于初定的分割阈值，如果小于则属于同一平面，然后进行迭代，不断更新分割阈值，直到有一半以上的点云属于同一平面，此时退出循环，完成点云数据粗分割。得到的这一平面点云即为粗平面点云。
b)分区域地面点云精分割
根据激光雷达线束间距d线束，将所述粗平面点云划分为多个区域。激光雷达扫描到的点云数据并非均匀分布，随着到激光雷达原点的欧氏距离增加，相邻线束间的距离越来越大，故划分每个区域的距离间距不应相等，应考虑激光雷达的线束分布特征。
式中(xn,yn)、(xn-1,yn-1)为相邻线束且处于同一方向的两点云坐标。
确定每块区域的斜率阈值ks＝λk0；k0为根据实际情况设定的初始斜率阈值。
计算各个区域点云相对于激光雷达原点的斜率k，
式中x,y,z为单个点云的坐标。若k≤ks，则该激光点云属于地面点云，否则为非地面点云。
在分割完成后，对非地面点云进行障碍物识别。
根据激光雷达线束间距d，将非地面点云划分为多个区域；
设置各个区域的聚类系数，所述聚类系数包括最小搜索半径以及最小聚类数量；
采用DBSCAN算法对各区域点云进行逐行逐列扫描，按照聚类系数对各区域点云进行聚类，生成聚类轮廓，并计算各区域的中心点位置。
S300，对非地面点云进行栅格化及二值化处理，生成二维栅格化环境地图，所述二维栅格化环境地图包含障碍物区域和非障碍物区域。
首先，建立矩形栅格图，并对非地面点云坐标进行转换，将点云坐标中的(x,y)变换为对应的像素坐标；
根据z值将栅格图中每个像素点进行二值化(0，255)处理，然后基于图像处理的方法对图像进行闭运算去噪点，得到二维栅格化环境地图。
S400，根据车辆当前行驶状态，实时生成多条局部路径。
以车辆初始的行驶位置为中心，建立Frenet坐标系，车辆的航向为s轴，基于车辆中心的横向偏移为d轴；
根据车辆宽度设置路径采样宽度Δd，并根据道路宽度以及路径采样宽度Δd确定局部路径的数量；
根据车速以及规划时间需求确定采样时间Δt，计算得到路径长度；
在保证车辆始末航向相同的前提下，使用三次样条曲线拟合生成多条局部路径，如下式所示：
d(s)＝λ1(s-s0)+λ2(s-s0)2+λ3(s-s0)3+d0
式中λ1,λ2,λ3为样条曲线的系数，(s0,d0)为初始车辆位置。
生成多条局部路径后，路径规划问题即简化为从这些路径中选取出最优路径。详见步骤S500～S700。
S500，根据所述二维栅格化环境地图以及安全驾驶需求设置约束条件以及损失函数。
所述约束条件包括：
其中vmax为最大限速、amax为最大限加速度、ρmax为最大限曲率、rmax为最大碰撞半径vt为任意时刻车辆纵向速度，at为任意时刻车辆加速度，ρt为任意时刻车辆曲率，d为车辆与绝对不能通行障碍物之间的距离。
对于车辆行驶损失函数的设计，需要设立相应的性能指标，实现对车辆安全性、驾驶舒适性进行数学量化。
所述损失函数如下所示：
f损失＝ηsfs+ηcfc
其中，fs为车辆行驶的安全性指标，用于描述车辆通过障碍物以及不同地形的能力，fc为车辆行驶的驾驶舒适性指标，描述车辆行驶时加速度变化的快慢，ηs和ηc分别为安全性指标和驾驶舒适性指标的权重。
S600，根据所述约束条件对各条局部路径进行筛选，得到可安全通行的局部路径。
S700，利用所述损失函数计算可安全通行的局部路径的损失大小，其中损失最小即为最优路径。
越野车作为一种在非结构化道路，以及复杂多变场景下行驶的特殊车辆。经常面临着路面崎岖不平，动态障碍物，行驶场景复杂多变。进而易导致车辆发生倾覆、碰撞等驾驶风险。越野车的辅助驾驶技术能够很大程度的提高驾驶员对动态环境的感知以及预知能力，从而可以减少驾驶风险。然而高性能的辅助驾驶技术迫切需要对越野车动态环境实时建图，以及对越野车进行路径规划选择出最优通行路径。因此下面以越野车为例，对本发明方法做进一步说明。
如图2所示，主要流程如下：
a)首先启动车辆，打开安装在车顶端的32线激光雷达；利用激光雷达获取环境信息并生成点云坐标；
b)对获取到的点云进行处理，将地面点云提取出来，对非地面点云进行障碍物的聚类识别。
c)将点云坐标转换为像素坐标，然后基于视觉生成动态地图。
d)以小车为中心建立Frenet坐标系，拟合生成有限条路径，计算每条路径损失大小，选择最优路径。
具体实施步骤如下：
S1：雷达正常工作时会在ROS平台建立节点，并发布Rsliadr话题，同时发送Pointcloud2点云数据。通过订阅Rslidar话题，并使用rosnumpy库便可将其转换为清晰坐标(x,y,z)的形式。
S2：如图3所示，首先设置随机种子，然后在帧点云中随机取出3个点云，进行平面的拟合。然后计算其他各点云到平面法向量之间的距离d点-面。最后判断距离d点-面是否小于初定的分割阈值e＝0.4，如果小于则属于同一平面。通过阈值e确定迭代次数K：
式中：p表示置信度常取0.99。不断迭代直到属于同一平面的数量达到了总点云的1-e倍时。此时退出循环。得到的这一平面点云，即属于粗略的拟合平面点云。
然后考虑激光雷达的线束分布特征，对点云进行分区域处理。以车辆行驶X轴正方向，确定所要划分区域的数量。首先32线激光雷达各相邻线束之间的距离d线束：
当线束距离|d线束n-d线束(n-1)|<0.1时，划分为同一区域。然后求出该区域达到激光雷达原点间的平均距离D：
然后给每个区域确定斜率阈值，因为当D趋近于0时，此时该区域与原点间的斜率为无穷大，同理当D趋近于无穷大时，此时该区域与原点间的斜率为0。故用越野车为例，能够通过的最大坡度阈值为θ＝20°。初始斜率阈值k0＝θ，故可以通过插值法得到各分区域的斜率：
式中初始斜率阈值k0＝θ，Ds为各区域到激光雷达原点间的距离，Dmax为点云到激光雷达原点间距离的最大值。最后计算各区域点云相对于激光雷达原点的斜率k。
分割地面点云后，便可以对非地面点云进行障碍物的识别。首先需要设置点云的分区域聚类系数(εs,minpointss)，此区域的划分与地面分割时相同。传统的聚类系数ε0＝0.2m，minpoints0＝10。同样采用插值法可以求出各区域的最小搜索半径εs，以及最小聚类数量minpointss：
这样便能获取到各区域的自适应聚类参数，然后对各区域获得中心点P，采用DBSCAN算法逐行逐列扫描，按照区域聚类系数聚类系数(εs,minpointss)对非地面点云进行聚类。并同时生成聚类轮廓，计算出中心点位置坐标(x0,y0,z0)。
S3：点云进行障碍物识别处理之后，便可以基于栅格法处理，然后基于图像处理的方法建立动态环境地图。本发明首先基于coppeliasim软件建立越野车行驶仿真环境图。其次以车辆为地图中心，建立500×500的栅格地图。然后将聚类后的非地面点云坐标转换为像素坐标。具体的数据处理如下：
a)将点云坐标(x,y,z)转为1行3列的矩阵，然后乘以矩阵[10,10,1]，将各点云坐标的x与y扩大10倍，使得生成的动态地图的扩大10倍，增强观感。
b)将点云坐标16位浮点数，转换为16位整数。
c)将点云坐标(x,y)同时各加上250，目的时为了使得生成的动态地图中心化。
d)将(x,y)中有大于500的统一赋值499。因为栅格图的像素坐标最大即为(499,499)。
然后以z值对栅格图中每个像素点进行二值化(0,255)处理，有障碍物的位置赋予255，无障碍物则为0。这样便能得到初步二值化栅格动态地图。然后基于图像处理对地图进行闭运算去噪点，实时生成越野车动态仿真环境地图。
S4：以越野车为中心，首先设置越野车路径规划满足的基本参数如表格所示：
| 道路宽度d | 道路间隔Δd | 采样时间Δt | vmax | amax | ρmax | rmax |
| 20m | 2.5m | 0.1s | 100km/h | 5m/s2 | 5m | 2m |
以车辆行驶的航向为s轴，基于车辆中心的横向偏移为d轴建立Frenet坐标系，然后根据当前车速v，以及采样时间Δt，计算出规划的局部路径长度l＝vgΔt，即假设车辆初始位置为(0，0)，当前车速为60km/h，则局部路径的末位置坐标为(1.66,10)，(1.66,7.5)，(1.66,5)，(1.66,2.5)，(1.66，0)，(1.66，-2.5)，(1.66，-5)，(1.66，-7.5)。并且保证始末航向相同，可通过三次样条曲线即可以拟合出8条待选路径。每条路径间隔Δd＝2.5m。以常见军用越野车为例，其能够通行的障碍物为沙地、草坪，壕沟阈值为0.5m，涉水深阈值为0.5m。则当越野车行驶在可通行的场景下，ηs＝1。对于权重ηc的确定，通过加速度传感器求出车辆每一时刻的加速度a(t)，然后对加速度求一阶导数，得到每一时刻的加加速度J＝ag(t)，来描述车辆加速度的变化大小，从而根据线性调整ηc的大小。通过实时计算出每条路径的f损失大小，损失最小的即为越野车行驶的最佳路径。
基于上述实施例，如图4所示，本发明实施例还提供一种复杂场景下的动态环境建图与路径规划装置，包括：
点云获取模块，基于激光雷达实时扫描获取车辆周围三维环境信息，得到雷达扫描空间范围内的物体点云数据；
点云分割以及障碍物识别模块，对激光雷达扫描的点云数据进行分割获取地面点云和非地面点云，对非地面点云，进行障碍物识别；
环境地图生成模块，对非地面点云进行栅格化及二值化处理，生成二维栅格化环境地图，所述二维栅格化环境地图包含障碍物区域和非障碍物区域；
初始路径规划模块，根据车辆当前行驶状态，实时生成多条局部路径；
路径筛选优化模块，根据所述二维栅格化环境地图以及安全驾驶需求设置约束条件以及损失函数；根据所述约束条件对各条局部路径进行筛选，得到可安全通行的局部路径；利用所述损失函数计算可安全通行的局部路径的损失大小，其中损失最小即为最优路径。
请参阅图5，图5为本发明实施例提供的电子设备的实施例示意图。如图5所示，本发明实施例提了一种电子设备500，包括存储器510、处理器520及存储在存储器520上并可在处理器520上运行的计算机程序511，处理器520执行计算机程序511时实现以下步骤：
S100，基于激光雷达实时扫描获取车辆周围三维环境信息，得到雷达扫描空间范围内的物体点云数据；
S200，对激光雷达扫描的点云数据进行分割获取地面点云和非地面点云，对非地面点云，进行障碍物识别；
S300，对非地面点云进行栅格化及二值化处理，生成二维栅格化环境地图，所述二维栅格化环境地图包含障碍物区域和非障碍物区域；
S400，根据车辆当前行驶状态，实时生成多条局部路径；
S500，根据所述二维栅格化环境地图以及安全驾驶需求设置约束条件以及损失函数；
S600，根据所述约束条件对各条局部路径进行筛选，得到可安全通行的局部路径；
S700，利用所述损失函数计算可安全通行的局部路径的损失大小，其中损失最小即为最优路径。
请参阅图6，图6为本发明实施例提供的一种计算机可读存储介质的实施例示意图。如图6所示，本实施例提供了一种计算机可读存储介质600，其上存储有计算机程序611，该计算机程序611被处理器执行时实现如下步骤：
S100，基于激光雷达实时扫描获取车辆周围三维环境信息，得到雷达扫描空间范围内的物体点云数据；
S200，对激光雷达扫描的点云数据进行分割获取地面点云和非地面点云，对非地面点云，进行障碍物识别；
S300，对非地面点云进行栅格化及二值化处理，生成二维栅格化环境地图，所述二维栅格化环境地图包含障碍物区域和非障碍物区域；
S400，根据车辆当前行驶状态，实时生成多条局部路径；
S500，根据所述二维栅格化环境地图以及安全驾驶需求设置约束条件以及损失函数；
S600，根据所述约束条件对各条局部路径进行筛选，得到可安全通行的局部路径；
S700，利用所述损失函数计算可安全通行的局部路径的损失大小，其中损失最小即为最优路径。
需要说明的是，在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详细描述的部分，可以参见其它实施例的相关描述。
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式计算机或者其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。
显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包括这些改动和变型在内。
Claims (8)
1.一种复杂场景下的动态环境建图与路径规划方法，其特征在于，包括以下步骤：
基于激光雷达实时扫描获取车辆周围三维环境信息，得到雷达扫描空间范围内的物体点云数据；
对激光雷达扫描的点云数据进行分割获取地面点云和非地面点云，对非地面点云，进行障碍物识别；
对非地面点云进行栅格化及二值化处理，生成二维栅格化环境地图，所述二维栅格化环境地图包含障碍物区域和非障碍物区域；
根据车辆当前行驶状态，实时生成多条局部路径；
根据所述二维栅格化环境地图以及安全驾驶需求设置约束条件以及损失函数；
根据所述约束条件对各条局部路径进行筛选，得到可安全通行的局部路径；
利用所述损失函数计算可安全通行的局部路径的损失大小，其中损失最小即为最优路径；
所述约束条件包括：
；
其中为最大限速、为最大限加速度、为最大限曲率、为最大碰撞半径为任意时刻车辆纵向速度，为任意时刻车辆加速度，为任意时刻车辆曲率，为车辆与绝对不能通行障碍物之间的距离；
所述损失函数如下所示：
；
其中，为车辆行驶的安全性指标，用于描述车辆通过障碍物以及不同地形的能力，为车辆行驶的驾驶舒适性指标，描述车辆行驶时加速度变化的快慢，和分别为安全性指标和驾驶舒适性指标的权重。
2.根据权利要求1所述的方法，其特征在于，所述的对激光雷达扫描的点云数据进行分割获取地面点云和非地面点云，包括：
基于RANSAC算法，对激光雷达扫描的点云数据进行粗分割，得到粗平面点云；
根据激光雷达线束间距d，对所述粗平面点云划分为多个区域；
确定每块区域的斜率阈值ks，并计算各个区域点云相对于原点的斜率k，若k≤ks，则该激光点云属于地面点云，否则为非地面点云。
3.根据权利要求1或2所述的方法，其特征在于，对非地面点云，进行障碍物识别，包括：
根据激光雷达线束间距d，将非地面点云划分为多个区域；
设置各个区域的聚类系数，所述聚类系数包括最小搜索半径以及最小聚类数量；
采用DBSCAN算法对各区域点云进行逐行逐列扫描，按照聚类系数对各区域点云进行聚类，生成聚类轮廓，并计算各区域的中心点位置。
4.根据权利要求1所述的方法，其特征在于，对非地面点云进行栅格化及二值化处理，生成二维栅格化环境地图，包括：
建立矩形栅格图，并对非地面点云坐标进行转换，将点云坐标中的（x,y）变换为对应的像素坐标；
根据z值将栅格图中每个像素点进行二值化处理，对图像进行闭运算去噪点，得到二维栅格化环境地图。
5.根据权利要求1所述的方法，其特征在于，根据车辆当前行驶状态，实时生成多条局部路径，包括：
以车辆初始的行驶位置为中心，建立Frenet坐标系，车辆的航向为s轴，基于车辆中心的横向偏移为d轴；
根据车辆宽度设置路径采样宽度，并根据道路宽度以及路径采样宽度确定局部路径的数量；
根据车速以及规划时间需求确定采样时间，计算得到路径长度；
在保证车辆始末航向相同的前提下，使用三次样条曲线拟合生成多条局部路径。
6.一种复杂场景下的动态环境建图与路径规划装置，其特征在于，包括：
点云获取模块，基于激光雷达实时扫描获取车辆周围三维环境信息，得到雷达扫描空间范围内的物体点云数据；
点云分割以及障碍物识别模块，对激光雷达扫描的点云数据进行分割获取地面点云和非地面点云，对非地面点云，进行障碍物识别；
环境地图生成模块，对非地面点云进行栅格化及二值化处理，生成二维栅格化环境地图，所述二维栅格化环境地图包含障碍物区域和非障碍物区域；
初始路径规划模块，根据车辆当前行驶状态，实时生成多条局部路径；
路径筛选优化模块，根据所述二维栅格化环境地图以及安全驾驶需求设置约束条件以及损失函数；根据所述约束条件对各条局部路径进行筛选，得到可安全通行的局部路径；利用所述损失函数计算可安全通行的局部路径的损失大小，其中损失最小即为最优路径；
所述约束条件包括：
；
其中为最大限速、为最大限加速度、为最大限曲率、为最大碰撞半径为任意时刻车辆纵向速度，为任意时刻车辆加速度，为任意时刻车辆曲率，为车辆与绝对不能通行障碍物之间的距离；
所述损失函数如下所示：
；
其中，为车辆行驶的安全性指标，用于描述车辆通过障碍物以及不同地形的能力，为车辆行驶的驾驶舒适性指标，描述车辆行驶时加速度变化的快慢，和分别为安全性指标和驾驶舒适性指标的权重。
7.一种电子设备，其特征在于，包括：
存储器，用于存储计算机软件程序；
处理器，用于读取并执行所述计算机软件程序，进而实现权利要求1-5任一项所述的一种复杂场景下的动态环境建图与路径规划方法。
8.一种非暂态计算机可读存储介质，其特征在于，所述存储介质中存储有用于实现权利要求1-5任一项所述的一种复杂场景下的动态环境建图与路径规划方法的计算机软件程序。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210952678.4A CN115451983B (zh) | 2022-08-09 | 一种复杂场景下的动态环境建图与路径规划方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210952678.4A CN115451983B (zh) | 2022-08-09 | 一种复杂场景下的动态环境建图与路径规划方法及装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115451983A CN115451983A (zh) | 2022-12-09 |
| CN115451983B true CN115451983B (zh) | 2024-07-09 |
Family
ID=
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114234993A (zh) * | 2021-12-15 | 2022-03-25 | 北京福田戴姆勒汽车有限公司 | 车辆局部路径规划方法、自动驾驶系统和自动驾驶车辆 |
| CN114509079A (zh) * | 2020-11-16 | 2022-05-17 | 通用汽车环球科技运作有限责任公司 | 用于自主驾驶的地面投影的方法和系统 |
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114509079A (zh) * | 2020-11-16 | 2022-05-17 | 通用汽车环球科技运作有限责任公司 | 用于自主驾驶的地面投影的方法和系统 |
| CN114234993A (zh) * | 2021-12-15 | 2022-03-25 | 北京福田戴姆勒汽车有限公司 | 车辆局部路径规划方法、自动驾驶系统和自动驾驶车辆 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110531753B (zh) | 自主车辆的控制系统、控制方法和控制器 | |
| CN110588653B (zh) | 自主车辆的控制系统、控制方法以及控制器 | |
| CN107229973B (zh) | 一种用于车辆自动驾驶的策略网络模型的生成方法及装置 | |
| CN111192284B (zh) | 一种车载激光点云分割方法及系统 | |
| CN108345822B (zh) | 一种点云数据处理方法及装置 | |
| CN107341819B (zh) | 目标跟踪方法及存储介质 | |
| CN102248947B (zh) | 使用3-d激光测距仪的目标和车辆检测及跟踪 | |
| CN112184736B (zh) | 一种基于欧式聚类的多平面提取方法 | |
| CN112200171B (zh) | 一种基于扫描线的道路点云的提取方法 | |
| CN113252027B (zh) | 井下无人驾驶车辆局部路径规划方法、装置、设备及存储介质 | |
| CN114359876B (zh) | 一种车辆目标识别方法及存储介质 | |
| CN116783620A (zh) | 根据点云的高效三维对象检测 | |
| CN115620263B (zh) | 基于摄像机和激光雷达图像融合的智能车障碍物检测方法 | |
| Saleem et al. | Steering angle prediction techniques for autonomous ground vehicles: a review | |
| CN113536232A (zh) | 用于无人驾驶中激光点云定位的正态分布变换方法 | |
| CN107220632B (zh) | 一种基于法向特征的路面图像分割方法 | |
| CN113936215A (zh) | 一种矿区路面凹坑的识别方法、系统及无人驾驶货车 | |
| CN112435336B (zh) | 一种弯道类型识别方法、装置、电子设备及存储介质 | |
| Zhao et al. | Improving autonomous vehicle visual perception by fusing human gaze and machine vision | |
| CN116523970B (zh) | 基于二次隐式匹配的动态三维目标跟踪方法及装置 | |
| CN113077473B (zh) | 三维激光点云路面分割方法、系统、计算机设备及介质 | |
| CN115451983B (zh) | 一种复杂场景下的动态环境建图与路径规划方法及装置 | |
| CN112070787A (zh) | 基于对立推理理论的航空三维点云平面分割方法 | |
| Chun-Zhao et al. | Drivable road boundary detection for intelligent vehicles based on stereovision with plane-induced homography | |
| CN115861481A (zh) | 一种基于激光惯性实时动态对象去除的slam系统 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant |