CN117553820B - 一种越野环境中路径规划方法、系统及设备 - Google Patents
一种越野环境中路径规划方法、系统及设备 Download PDFInfo
- Publication number
- CN117553820B CN117553820B CN202410043887.6A CN202410043887A CN117553820B CN 117553820 B CN117553820 B CN 117553820B CN 202410043887 A CN202410043887 A CN 202410043887A CN 117553820 B CN117553820 B CN 117553820B
- Authority
- CN
- China
- Prior art keywords
- path
- vehicle
- sampling
- road
- environment
- 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 34
- 238000005070 sampling Methods 0.000 claims abstract description 163
- 238000010586 diagram Methods 0.000 claims abstract description 28
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 25
- 238000001514 detection method Methods 0.000 claims abstract description 15
- 238000009499 grossing Methods 0.000 claims abstract description 10
- 238000005516 engineering process Methods 0.000 claims abstract description 9
- 230000004888 barrier function Effects 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000004590 computer program Methods 0.000 claims description 7
- 238000012545 processing Methods 0.000 claims description 5
- 238000010276 construction Methods 0.000 claims description 4
- 230000008569 process Effects 0.000 claims description 4
- 238000005259 measurement Methods 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 2
- 230000005855 radiation Effects 0.000 claims 1
- 238000013507 mapping Methods 0.000 description 4
- 230000004927 fusion Effects 0.000 description 3
- 238000013459 approach Methods 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000013135 deep learning Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
- 238000012876 topography Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明公开一种越野环境中路径规划方法、系统及设备，涉及路径规划技术领域，方法包括：基于目标越野环境局部栅格地图、当前车辆位置及当前车辆图像，确定当前车辆行驶路面特征，包括车辆行驶道路宽度特征、车辆周围障碍物特征；基于预设采样规则及当前车辆行驶路面特征，进行自适应调节采样，以确定路径采样点集合，进而采用碰撞检测技术，构建路径网络图；以当前车辆位置作为起始点，基于路径网络图，以总路径代价值最小且总路径长度最短为目标，采用Dijkstra算法确定出待用路径；对待用路径进行平滑处理，以得到最终行驶路径。本发明提高了越野环境下的路径规划效率，保证实时性。
Description
技术领域
本发明涉及路径规划技术领域，特别是涉及一种越野环境中路径规划方法、系统及设备。
背景技术
近年来，针对城市环境的自动驾驶领域已经出现了大批可靠的路径规划方案，然而针对越野环境路径规划的研究还不够成熟。在越野环境中，无人车辆往往面临着复杂的地形和多样化的场景，然而目前的路径规划方法大多没有与越野环境的场景结合起来。在现有的PRM（概率路线图，Probabilistic Roadmap）算法中，对环境进行采样时采取的策略是均匀采样，而这种做法会使得算法耗费大量时间在采样和碰撞检测上。
发明内容
本发明的目的是提供一种越野环境局部路径规划方法、系统及设备，提高越野环境下的路径规划效率，保证实时性。
为实现上述目的，本发明提供了如下方案。
第一方面，本发明提供一种越野环境中路径规划方法，包括。
获取目标越野环境局部栅格地图、当前车辆位置及当前车辆图像；所述目标越野环境局部栅格地图中包括障碍物和道路。
基于所述目标越野环境局部栅格地图、所述当前车辆位置及所述当前车辆图像，确定当前车辆行驶路面特征；所述当前车辆行驶路面特征包括车辆行驶道路宽度特征、车辆周围障碍物特征。
基于预设采样规则及所述当前车辆行驶路面特征，进行自适应调节采样，以确定路径采样点集合。
基于所述路径采样点集合，采用碰撞检测技术，构建无向图；所述无向图为路径网络图。
以所述当前车辆位置作为起始点，基于所述路径网络图，以总路径代价值最小且总路径长度最短为目标，采用Dijkstra算法确定出待用路径；所述总路径长度指所述起始点与预设车辆行驶终点之间的路径长度；所述总路径代价值为基于障碍物的代价值及基于距离的代价值之和。
对所述待用路径进行平滑处理，以得到最终行驶路径。
第二方面，本发明提供一种越野环境中路径规划系统，包括。
地图及车辆位置获取模块，用于获取目标越野环境局部栅格地图、当前车辆位置及当前车辆图像；所述目标越野环境局部栅格地图中包括障碍物和道路。
路面特征确定模块，用于基于所述目标越野环境局部栅格地图、所述当前车辆位置及所述当前车辆图像，确定当前车辆行驶路面特征；所述当前车辆行驶路面特征包括车辆行驶道路宽度特征、车辆周围障碍物特征。
路径采样点确定模块，用于基于预设采样规则及所述当前车辆行驶路面特征，进行自适应调节采样，以确定路径采样点集合。
路径网络图确定模块，用于基于所述路径采样点集合，采用碰撞检测技术，构建无向图；所述无向图为路径网络图。
待用路径确定模块，用于以所述当前车辆位置作为起始点，基于所述路径网络图，以总路径代价值最小且总路径长度最短为目标，采用Dijkstra算法确定出待用路径；所述总路径长度指所述起始点与预设车辆行驶终点之间的路径长度；所述总路径代价值为基于障碍物的代价值及基于距离的代价值之和。
最终路径确定模块，用于对所述待用路径进行平滑处理，以得到最终行驶路径。
第三方面，本发明提供一种电子设备，包括存储器及处理器，存储器用于存储计算机程序，处理器运行计算机程序以使电子设备执行越野环境中路径规划方法。
根据本发明提供的具体实施例，本发明公开了以下技术效果：本发明公开一种越野环境中路径规划方法、系统及设备，基于目标越野环境局部栅格地图、当前车辆位置及当前车辆图像，确定当前车辆行驶路面特征，包括车辆行驶道路宽度特征、车辆周围障碍物特征，进而结合预设采样规则，进行自适应调节采样，以确定路径采样点集合。这样的处理能够实现能够根据越野环境自适应调节采样，避免了无意义的采样点的生成以及对应的碰撞检测，能够减少构建路径网络图所耗费的时间，提高路径规划算法的效率，增强了其实时性。然后，构建无向图，以总路径代价值最小且总路径长度最短为目标、采用Dijkstra算法（迪杰斯特拉算法）确定出待用路径，最后进行平滑处理，从而得到更高效、更精确及更安全的最终行驶路径。本发明针对越野环境，可以提高PRM算法构建路径网络图的效率，增强局部路径规划的实时性，以保证无人车辆在越野环境中的安全性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明越野环境中路径规划方法的流程示意图。
图2为本发明虚拟车道线边界的第一示例图。
图3为本发明虚拟车道线边界的第二示例图。
图4为本发明坐标变换示意图。
图5为本发明越野环境中路径规划系统的示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
传统的PRM算法在采样并构建路径网络图的过程中会消耗大量时间，相比之下搜索目标路径的过程耗费时间较短。PRM算法所需的大量时间主要耗费在随机采样和碰撞检测上。传统PRM算法通常使用均匀采样的方式，即在自由空间中每个点的采样概率是相同的。
基于此，本发明提供了一种越野环境中路径规划方法、系统及设备，主要包括局部地图构建、行驶场景识别、路径网络图构建、路径搜索、路径平滑这五个部分，尤其对采样策略进行改进，首先判断车辆所处的环境是否位于道路上，如果在道路上则设置虚拟车道线边界，仅在虚拟车道线边界内的区域进行采样；如果车辆不在道路上，则在车辆周围均匀采样，若车辆在开阔道路上，则增大采样间距，若车辆不在开阔道路上则减小采样间距。根据这种自适应的采样策略进行采样，减少了不必要的采样点和其对应的碰撞检测，有效提高了构建路径网络图的效率，因而提高了该路径规划算法的效率，增强了其实时性。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
实施例一
如图1所示，本发明提供一种越野环境中路径规划方法，包括以下步骤。
步骤100，获取目标越野环境局部栅格地图、当前车辆位置及当前车辆图像；所述目标越野环境局部栅格地图中包括障碍物和道路；其中，所述目标越野环境局部栅格地图采用激光雷达进行建图，激光雷达通过对环境扫描提供环境信息，融合IMU（惯性测量单元，Inertial Measurement Unit）和GPS（全球定位系统，Global Positioning System）传感器数据对地图进行维护更新，具体地，其构建过程，包括以下步骤。
（1）获取激光雷达对目标越野环境进行扫描后得到的环境扫描数据。
（2）基于SLAM（即时定位与地图构建，Simultaneous Localization and Mapping）算法，根据所述环境扫描数据，构建初始目标越野环境栅格地图。具体地，建图过程采用2.5D地图，可以更好地考虑地形因素。
（3）获取惯性测量部件采集的车辆位姿数据。
（4）获取全球定位系统采集的车辆经纬度及车辆高程数据。
（5）基于所述车辆位姿数据、所述车辆经纬度及所述车辆高程数据，对所述初始目标越野环境栅格地图进行局部更新，以得到目标越野环境局部栅格地图。
进一步来说，目标越野环境局部栅格地图的建图步骤主要包括点云障碍物标记、多帧地图融合和栅格地图输出。其中，点云障碍物标记、多帧地图融合主要由上层感知层（IMU和GPS传感器）输出，占据栅格以一维数组的方式输出，根据地图的分辨率和宽度，对栅格地图进行排序并建立索引。通过激光雷达对障碍物的检测结果进行栅格属性判断，并对栅格属性赋值，考虑到定位建图算法的运行效率和任务需求，设置地图的分辨率为0.2米。以第一帧的GPS信息作为地图坐标系原点，在接收到上层多传感器更新的环境数据（同一时刻下的环境扫描数据、车辆位姿数据、车辆经纬度及车辆高程数据）后对局部栅格地图进行更新，完成局部栅格地图的建立。
所述当前车辆位置是通过设置在车辆上的IMU和GPS传感器实时确定的，且当前车辆位置的确定与目标越野环境局部栅格地图的维护更新并不冲突，二者可同时进行。
所述当前车辆图像是通过设置在车辆上的摄像头实时采集得到。
步骤200，基于所述目标越野环境局部栅格地图、所述当前车辆位置及所述当前车辆图像，确定当前车辆行驶路面特征；所述当前车辆行驶路面特征包括车辆行驶道路宽度特征、车辆周围障碍物特征。
其中，对于车辆行驶道路宽度特征：当车辆行驶在越野道路上时，根据透视关系，其前方路径区域在远处会收窄，因此可以利用当前车辆图像，结合当前车辆位置及目标越野环境局部栅格地图内的环境扫描数据，通过构建多传感器融合深度学习可通行区域分割模型，将道路进行分割，从而可以计算出路面宽度。通过上述路面宽度的变化来判断车辆当前是否在道路上行驶，从而得到车辆行驶路面特征。换而言之，本发明是基于多传感器数据（包括摄像头、激光雷达、IMU和GPS传感器等）融合越野场景路面类型识别模型的结果确定道路宽度的特征。
对于车辆周围障碍物特征：当车辆在越野环境中行驶时，基于当前车辆位置，在目标越野环境局部栅格地图中统计车辆周围20米范围内的障碍物栅格占比，将障碍物栅格占比超过0.2的场景视为障碍物较密集的场景，当低于这个阈值时，视为开阔越野环境。
步骤300，基于预设采样规则及所述当前车辆行驶路面特征，进行自适应调节采样，以确定路径采样点集合；所述预设采样规则包括第一预设规则、第二预设规则及第三预设规则。
步骤300，具体包括以下步骤。
（1）基于所述车辆行驶道路宽度特征，确定车辆道路状态；所述车辆道路状态为车辆行驶在道路上或车辆未行驶在道路上。
（2）当所述车辆道路状态为车辆行驶在道路上时，基于所述第一预设规则进行采样，以得到多个路径采样点；所述第一预设规则为在所述当前车辆位置的周围设置虚拟车道线边界，并在所述虚拟车道线边界内、以第一预设采样间距值进行采样。具体如图2所示，根据车辆行驶方向，在车前方虚拟车道线范围内可进行采样。在一个实际应用中，第一预设采样间距值设为0.6m。
（3）当所述车辆道路状态为车辆未行驶在道路上时，基于所述车辆周围障碍物特征（具体为车辆周围障碍物栅格占比），确定车辆环境状态；所述车辆环境状态为车辆处在开阔越野环境区域或车辆未处在开阔越野环境区域。
（4）当所述车辆环境状态为车辆处在开阔越野环境区域时，基于所述第二预设规则进行采样，以得到多个路径采样点；所述第二预设规则为在所述当前车辆位置的周围以第二预设采样间距值进行均匀采样。具体如图3所示，当车辆在开阔区域时，为了确保能搜索到最优路径，在车周围进行均匀采样。在一个实际应用中，第二预设采样间距值设为0.6m。
（5）当所述车辆环境状态为车辆未处在开阔越野环境区域时，基于所述第三预设规则进行采样，以得到多个路径采样点；所述第三预设规则为在所述当前车辆位置的周围以第三预设采样间距值进行均匀采样；多个路径采样点构成路径采样点集合。在一个实际应用中，第三预设采样间距值设为1.2m。
进一步地，所述路径采样点的确定过程，具体包括以下步骤。
建立车辆坐标系及目标越野环境局部栅格地图坐标系，并采用下述公式将所述当前车辆位置在所述车辆坐标系中的坐标变换至所述目标越野环境局部栅格地图坐标系中。
。
其中，表示当前车辆位置在目标越野环境局部栅格地图坐标系的坐标，/>表示当前车辆位置在车辆坐标系（base系）的坐标，/>表示车辆坐标系到目标越野环境局部栅格地图坐标系的变换关系。
将当前车辆位置在目标越野环境局部栅格地图坐标系的坐标设为地图中心坐标，公式如下。
。
采用如下公式确定路径采样点。
。
。
其中，为地图中心坐标，mapCenter.x为地图中心坐标的x值，mapCenter.y为地图中心坐标的y值，/>和/>代表路径采样点的坐标的x值和y值，/>和/>表示取值范围为/>和/>之间的随机数，/>和/>表示目标越野环境局部栅格地图坐标系中x方向和y方向的随机采样范围，需要根据车辆大小与道路情况以及激光雷达建图的距离确定，在一个具体实例中，取/>，/>。
进一步地，考虑到车辆行驶时车辆横摆角会不断改变，采样区域应该随着横摆角的变化而变化。基于此，将车辆初始横摆角设为0，x轴指向正东方向，设置采样区域的坐标的边界条件后，确定所需的采样点；当车辆横摆角发生改变时，将已通过上文确定的采样点的坐标绕车辆坐标位置旋转和横摆角相同的角度。
如图4所示，设采样点坐标为，当前车辆位置对应的坐标为/>，横摆角为/>，旋转后的采样点的坐标为/>，则对应计算公式为。
。
。
当车辆不在路面上行驶时，应当在环境内随机均匀采样，但是在非道路环境下环境特点可能大不相同，在开阔的场景中障碍物较少，而有灌木丛的场景中障碍物较多。若区域较开阔，则可以适当增大采样点之间的距离，减少算法运行时间。在采样时需要对采样点的最小间距做出限制，若采样点间距过小，则会大幅增加计算量，过密的随机采样点使路径的转角增多；采样点间距不能过大，间距过大导致采样点密度降低会导致搜索不到有效路径。经过试验，若当前行驶环境为道路，则采样点间距（即第一预设采样间距值）设为0.6m；若当前在非道路环境行驶时，障碍物栅格占比高于阈值则采样点间距设（即第二预设采样间距值）为0.6m，若低于阈值则将采样点间距（即第三预设采样间距值）设为1.2m。
步骤400，基于所述路径采样点集合，采用碰撞检测技术，构建无向图；所述无向图为路径网络图。通过构建路径网络图为后续路径搜索提供被搜索的对象。该采样方式能够有效提升构建路径网络图的速度，增强算法的实时性。
根据以上采样点生成规则进行采样以后，把采样点加入无向图G中，这些路径采样点在无向图中作为顶点，搜索G中的顶点邻近的顶点，将每一个顶点与其周围的邻点进行连线，通过对连线进行碰撞检测，舍弃无效的连线，若路径无碰撞则将两个顶点之间的连线加入无向图中作为一条无向边，并计算该无向边的代价。车辆在越野环境中实时构建无向图作为路径网络图，基于此，步骤400，具体包括以下步骤。
（1）针对所述路径采样点集合中的任一路径采样点，当所述路径采样点与所述目标越野环境局部栅格地图中的障碍物有碰撞时，舍弃所述路径采样点；当所述路径采样点与所述目标越野环境局部栅格地图中的障碍物无碰撞时，将所述路径采样点标记为顶点；多个所述顶点构成顶点集。
（2）基于所述顶点集，构建初始无向图。
（3）针对所述顶点集中的任一顶点，在所述初始无向图中搜索与所述顶点相邻的顶点，然后确定所述顶点与相邻的顶点之间的连线，并对所述连线进行碰撞检测。
（4）若所述连线与所述目标越野环境局部栅格地图中的障碍物有碰撞，则舍弃所述连线；若所述连线与所述目标越野环境局部栅格地图中的障碍物无碰撞，则将所述连线加入至连线集。
（5）基于所述连线集，对所述初始无向图进行更新，以得到最终的无向图。
步骤500，以所述当前车辆位置作为起始点，基于所述路径网络图，以总路径代价值最小且总路径长度最短为目标，采用Dijkstra算法确定出待用路径；所述总路径长度指所述起始点与预设车辆行驶终点之间的路径长度；所述总路径代价值为基于障碍物的代价值（具体表征使车辆远离障碍物的程度，从而避免碰撞）及基于距离的代价值之和。
具体地，接收上一步骤传来的全局路径点信息（路径网络图），将全局路径点设为局部规划的目标点，通过接收车辆GPS信息来获取车辆实时位置（即当前车辆位置），当车辆和当前的局部目标点的距离在一定范围内时，删去当前全局参考点，设置下一个全局参考点为局部目标点，引导车辆继续前进，直至到达预设车辆行驶终点。
而在为局部路径规划设置起始点和预设车辆行驶终点后，使用Dijkstra算法来搜索局部最短路径。Dijkstra算法基于路径网络图，根据总路径代价值对应的函数来计算每个顶点（即采样路径点）的代价，利用其中各个节点相邻的关系，依次遍历邻接节点，直至找到目标节点。Dijkstra算法运行步骤如下。
（1）将起始节点加入队列。
（2）判断队列是否为空集，若队列非空，则找出队列中代价最小的采样路径点n，并将其从队列中删除并标记。
（3）遍历n的所有邻接的采样路径点，将其加入队列中并标记，转向步骤（2）。
所述总路径代价值的计算公式为。
。
其中，表示路径网络图中第n个采样路径点的总路径代价值，/>代表第n个采样路径点的基于距离的代价值，/>表示第n个采样路径点的基于障碍物的代价值，表示基于距离的代价值的权重，/>表示基于障碍物的代价值的权重。
第n个采样路径点的基于障碍物的代价值的计算公式为。
。
其中，为斥力势场的增益系数，为预设值；/>为当前车辆位置与目标越野环境局部栅格地图中障碍物的距离，/>为斥力辐射半径。
第n个采样路径点的基于距离的代价值的计算公式为。
。
其中，是第n个采样路径点的父节点，/>代表第n个采样路径点的父节点的基于距离的代价值，设起始点的基于距离的代价值为0；/>为第n个采样路径点与第n个采样路径点的父节点之间的距离。
步骤600，对所述待用路径进行平滑处理，以得到最终行驶路径，从而有利于车辆跟踪该路径。
步骤600，具体包括以下步骤。
（1）计算所述待用路径中任意两个路径点之间的欧几里得距离。
（2）基于多个欧几里得距离，计算累计行驶距离，计算公式如下。
。
其中，i为路径点的序号，ti为序号为i的路径点的累计行驶距离，ti-1为序号为i-1的路径点的累计行驶距离，和/>为序号为i的路径点的坐标，xi-1和yi-1为序号为i-1的路径点的坐标。
（3）基于所述累计行驶距离，构造三次样条曲线方程，方程如下。
。
其中，c0、c1、c2、c3、d0、d1、d2、d3均表示系数。
（4）采用所述三次样条曲线方程，对所述待用路径进行插值优化，以得到最终行驶路径。在每一段曲线中，可以通过对上述公式求解得到x和y关于累计行驶距离的三次多项式方程，选取边界条件为自然边界条件，即给定端点的二阶导数值，就可以得到每一段曲线的参数方程，在每一曲线上根据曲线的长度确定插值点数，根据曲线的参数方程确定插值点的坐标，完成对局部路径的插值优化。
实施例二
如图5所示，为了实现实施例一中的技术方案，以达到相应的功能和技术效果，本实施例还提供了一种越野环境中路径规划系统，包括以下模块。
地图及车辆位置获取模块，用于获取目标越野环境局部栅格地图、当前车辆位置及当前车辆图像；所述目标越野环境局部栅格地图中包括障碍物和道路。
路面特征确定模块，用于基于所述目标越野环境局部栅格地图、所述当前车辆位置及所述当前车辆图像，确定当前车辆行驶路面特征；所述当前车辆行驶路面特征包括车辆行驶道路宽度特征、车辆周围障碍物特征。
路径采样点确定模块，用于基于预设采样规则及所述当前车辆行驶路面特征，进行自适应调节采样，以确定路径采样点集合。
路径网络图确定模块，用于基于所述路径采样点集合，采用碰撞检测技术，构建无向图；所述无向图为路径网络图。
待用路径确定模块，用于以所述当前车辆位置作为起始点，基于所述路径网络图，以总路径代价值最小且总路径长度最短为目标，采用Dijkstra算法确定出待用路径；所述总路径长度指所述起始点与预设车辆行驶终点之间的路径长度；所述总路径代价值为基于障碍物的代价值及基于距离的代价值之和。
最终路径确定模块，用于对所述待用路径进行平滑处理，以得到最终行驶路径。
实施例三
本实施例提供一种电子设备，包括存储器及处理器，存储器用于存储计算机程序，处理器运行计算机程序以使电子设备执行实施例一的越野环境中路径规划方法。可选地，上述电子设备可以是服务器。
另外，本发明实施例还提供一种计算机可读存储介质，其存储有计算机程序，该计算机程序被处理器执行时实现实施例一的越野环境中路径规划方法。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。
Claims (8)
1.一种越野环境中路径规划方法，其特征在于，方法包括：
获取目标越野环境局部栅格地图、当前车辆位置及当前车辆图像；所述目标越野环境局部栅格地图中包括障碍物和道路；
基于所述目标越野环境局部栅格地图、所述当前车辆位置及所述当前车辆图像，确定当前车辆行驶路面特征；所述当前车辆行驶路面特征包括车辆行驶道路宽度特征、车辆周围障碍物特征；
基于预设采样规则及所述当前车辆行驶路面特征，进行自适应调节采样，以确定路径采样点集合；所述预设采样规则包括第一预设规则、第二预设规则及第三预设规则；
基于预设采样规则及所述当前车辆行驶路面特征，进行自适应调节采样，以确定路径采样点集合，具体包括：
基于所述车辆行驶道路宽度特征，确定车辆道路状态；所述车辆道路状态为车辆行驶在道路上或车辆未行驶在道路上；当所述车辆道路状态为车辆行驶在道路上时，基于所述第一预设规则进行采样，以得到多个路径采样点；所述第一预设规则为在所述当前车辆位置的周围设置虚拟车道线边界，并在所述虚拟车道线边界内、以第一预设采样间距值进行采样；当所述车辆道路状态为车辆未行驶在道路上时，基于所述车辆周围障碍物特征，确定车辆环境状态；所述车辆环境状态为车辆处在开阔越野环境区域或车辆未处在开阔越野环境区域；当所述车辆环境状态为车辆处在开阔越野环境区域时，基于所述第二预设规则进行采样，以得到多个路径采样点；所述第二预设规则为在所述当前车辆位置的周围以第二预设采样间距值进行均匀采样；当所述车辆环境状态为车辆未处在开阔越野环境区域时，基于所述第三预设规则进行采样，以得到多个路径采样点；所述第三预设规则为在所述当前车辆位置的周围以第三预设采样间距值进行均匀采样；多个路径采样点构成路径采样点集合；
基于所述路径采样点集合，采用碰撞检测技术，构建无向图；所述无向图为路径网络图；
以所述当前车辆位置作为起始点，基于所述路径网络图，以总路径代价值最小且总路径长度最短为目标，采用Dijkstra算法确定出待用路径；所述总路径长度指所述起始点与预设车辆行驶终点之间的路径长度；所述总路径代价值为基于障碍物的代价值及基于距离的代价值之和；
对所述待用路径进行平滑处理，以得到最终行驶路径。
2.根据权利要求1所述的越野环境中路径规划方法，其特征在于，所述路径采样点的确定过程，具体包括：
建立车辆坐标系及目标越野环境局部栅格地图坐标系，并采用下述公式将所述当前车辆位置在所述车辆坐标系中的坐标变换至所述目标越野环境局部栅格地图坐标系中：
；
其中，表示当前车辆位置在目标越野环境局部栅格地图坐标系的坐标，/>表示当前车辆位置在车辆坐标系的坐标，/>表示车辆坐标系到目标越野环境局部栅格地图坐标系的变换关系；
将当前车辆位置在目标越野环境局部栅格地图坐标系的坐标设为地图中心坐标，并采用如下公式确定路径采样点：
；
；
其中，mapCenter.x为地图中心坐标的x值，mapCenter.y为地图中心坐标的y值，和/>代表路径采样点的坐标的x值和y值，/>和表示取值范围为/>和/>之间的随机数，/>和/>表示目标越野环境局部栅格地图坐标系中x方向和y方向的随机采样范围。
3.根据权利要求1所述的越野环境中路径规划方法，其特征在于，基于所述路径采样点集合，采用碰撞检测技术，构建无向图，具体包括：
针对所述路径采样点集合中的任一路径采样点，当所述路径采样点与所述目标越野环境局部栅格地图中的障碍物有碰撞时，舍弃所述路径采样点；当所述路径采样点与所述目标越野环境局部栅格地图中的障碍物无碰撞时，将所述路径采样点标记为顶点；多个所述顶点构成顶点集；
基于所述顶点集，构建初始无向图；
针对所述顶点集中的任一顶点，在所述初始无向图中搜索与所述顶点相邻的顶点，然后确定所述顶点与相邻的顶点之间的连线，并对所述连线进行碰撞检测；
若所述连线与所述目标越野环境局部栅格地图中的障碍物有碰撞，则舍弃所述连线；若所述连线与所述目标越野环境局部栅格地图中的障碍物无碰撞，则将所述连线加入至连线集；
基于所述连线集，对所述初始无向图进行更新，以得到最终的无向图。
4.根据权利要求1所述的越野环境中路径规划方法，其特征在于，所述总路径代价值的计算公式为：
；
其中，表示路径网络图中第n个采样路径点的总路径代价值，/>代表第n个采样路径点的基于距离的代价值，/>表示第n个采样路径点的基于障碍物的代价值，/>表示基于距离的代价值的权重，/>表示基于障碍物的代价值的权重；
第n个采样路径点的基于障碍物的代价值的计算公式为：
；
其中，为斥力势场的增益系数，为预设值；/>为当前车辆位置与目标越野环境局部栅格地图中障碍物的距离，/>为斥力辐射半径；
第n个采样路径点的基于距离的代价值的计算公式为：
；
其中，是第n个采样路径点的父节点，/>代表第n个采样路径点的父节点的基于距离的代价值，/>为第n个采样路径点与第n个采样路径点的父节点之间的距离。
5.根据权利要求1所述的越野环境中路径规划方法，其特征在于，对所述待用路径进行平滑处理，以得到最终行驶路径，具体包括：
计算所述待用路径中任意两个路径点之间的欧几里得距离；
基于多个欧几里得距离，计算累计行驶距离；
基于所述累计行驶距离，构造三次样条曲线方程；
采用所述三次样条曲线方程，对所述待用路径进行插值优化，以得到最终行驶路径。
6.根据权利要求1所述的越野环境中路径规划方法，其特征在于，所述目标越野环境局部栅格地图的构建过程，具体包括：
获取激光雷达对目标越野环境进行扫描后得到的环境扫描数据；
基于SLAM算法，根据所述环境扫描数据，构建初始目标越野环境栅格地图；
获取惯性测量部件采集的车辆位姿数据；
获取全球定位系统采集的车辆经纬度及车辆高程数据；
基于所述车辆位姿数据、所述车辆经纬度及所述车辆高程数据，对所述初始目标越野环境栅格地图进行局部更新，以得到目标越野环境局部栅格地图。
7.一种越野环境中路径规划系统，其特征在于，系统包括：
地图及车辆位置获取模块，用于获取目标越野环境局部栅格地图、当前车辆位置及当前车辆图像；所述目标越野环境局部栅格地图中包括障碍物和道路；
路面特征确定模块，用于基于所述目标越野环境局部栅格地图、所述当前车辆位置及所述当前车辆图像，确定当前车辆行驶路面特征；所述当前车辆行驶路面特征包括车辆行驶道路宽度特征、车辆周围障碍物特征；
路径采样点确定模块，用于基于预设采样规则及所述当前车辆行驶路面特征，进行自适应调节采样，以确定路径采样点集合；所述预设采样规则包括第一预设规则、第二预设规则及第三预设规则；
基于预设采样规则及所述当前车辆行驶路面特征，进行自适应调节采样，以确定路径采样点集合，具体包括：
基于所述车辆行驶道路宽度特征，确定车辆道路状态；所述车辆道路状态为车辆行驶在道路上或车辆未行驶在道路上；当所述车辆道路状态为车辆行驶在道路上时，基于所述第一预设规则进行采样，以得到多个路径采样点；所述第一预设规则为在所述当前车辆位置的周围设置虚拟车道线边界，并在所述虚拟车道线边界内、以第一预设采样间距值进行采样；当所述车辆道路状态为车辆未行驶在道路上时，基于所述车辆周围障碍物特征，确定车辆环境状态；所述车辆环境状态为车辆处在开阔越野环境区域或车辆未处在开阔越野环境区域；当所述车辆环境状态为车辆处在开阔越野环境区域时，基于所述第二预设规则进行采样，以得到多个路径采样点；所述第二预设规则为在所述当前车辆位置的周围以第二预设采样间距值进行均匀采样；当所述车辆环境状态为车辆未处在开阔越野环境区域时，基于所述第三预设规则进行采样，以得到多个路径采样点；所述第三预设规则为在所述当前车辆位置的周围以第三预设采样间距值进行均匀采样；多个路径采样点构成路径采样点集合；
路径网络图确定模块，用于基于所述路径采样点集合，采用碰撞检测技术，构建无向图；所述无向图为路径网络图；
待用路径确定模块，用于以所述当前车辆位置作为起始点，基于所述路径网络图，以总路径代价值最小且总路径长度最短为目标，采用Dijkstra算法确定出待用路径；所述总路径长度指所述起始点与预设车辆行驶终点之间的路径长度；所述总路径代价值为基于障碍物的代价值及基于距离的代价值之和；
最终路径确定模块，用于对所述待用路径进行平滑处理，以得到最终行驶路径。
8.一种电子设备，其特征在于，包括存储器及处理器，存储器用于存储计算机程序，处理器运行计算机程序以使电子设备执行权利要求1-6任一项所述的越野环境中路径规划方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410043887.6A CN117553820B (zh) | 2024-01-12 | 2024-01-12 | 一种越野环境中路径规划方法、系统及设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410043887.6A CN117553820B (zh) | 2024-01-12 | 2024-01-12 | 一种越野环境中路径规划方法、系统及设备 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117553820A CN117553820A (zh) | 2024-02-13 |
| CN117553820B true CN117553820B (zh) | 2024-04-05 |
Family
ID=89813311
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410043887.6A Active CN117553820B (zh) | 2024-01-12 | 2024-01-12 | 一种越野环境中路径规划方法、系统及设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117553820B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118464054B (zh) * | 2024-07-09 | 2024-09-06 | 北京理工大学 | 一种崎岖路面路径规划方法、装置、介质及产品 |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE10316822A1 (de) * | 2002-05-14 | 2003-12-24 | Volkswagen Ag | Off-Road-Navigationssystem |
| EP3742125A1 (en) * | 2019-05-20 | 2020-11-25 | Tata Consultancy Services Limited | Method and system for path planning |
| WO2021164277A1 (zh) * | 2020-07-31 | 2021-08-26 | 平安科技（深圳）有限公司 | 应用于路径规划的空间采样方法、装置、设备及介质 |
| WO2022007350A1 (zh) * | 2020-07-08 | 2022-01-13 | 格力电器（武汉）有限公司 | 一种全局路径规划方法、装置、终端及可读存储介质 |
| CN114812581A (zh) * | 2022-06-23 | 2022-07-29 | 中国科学院合肥物质科学研究院 | 一种基于多传感器融合的越野环境导航方法 |
| CN115061467A (zh) * | 2022-06-24 | 2022-09-16 | 合肥工业大学 | 基于改进高斯采样prm算法和狭窄通道识别的路径规划方法 |
| CN115468567A (zh) * | 2022-09-28 | 2022-12-13 | 中国人民解放军陆军装甲兵学院 | 一种面向越野环境的激光视觉融合slam方法 |
| EP4174609A1 (en) * | 2021-07-30 | 2023-05-03 | INTEL Corporation | Path planning system and method with intelligent sampling and two-way search (its) |
| CN116107300A (zh) * | 2022-12-08 | 2023-05-12 | 中国船舶集团有限公司第七一六研究所 | 用于越野环境下的基于先验知识适用于无人驾驶的路径规划方法 |
| CN117109625A (zh) * | 2023-10-20 | 2023-11-24 | 湖南大学 | 一种基于改进prm算法的无人驾驶路径规划方法 |
-
2024
- 2024-01-12 CN CN202410043887.6A patent/CN117553820B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE10316822A1 (de) * | 2002-05-14 | 2003-12-24 | Volkswagen Ag | Off-Road-Navigationssystem |
| EP3742125A1 (en) * | 2019-05-20 | 2020-11-25 | Tata Consultancy Services Limited | Method and system for path planning |
| WO2022007350A1 (zh) * | 2020-07-08 | 2022-01-13 | 格力电器（武汉）有限公司 | 一种全局路径规划方法、装置、终端及可读存储介质 |
| WO2021164277A1 (zh) * | 2020-07-31 | 2021-08-26 | 平安科技（深圳）有限公司 | 应用于路径规划的空间采样方法、装置、设备及介质 |
| EP4174609A1 (en) * | 2021-07-30 | 2023-05-03 | INTEL Corporation | Path planning system and method with intelligent sampling and two-way search (its) |
| CN114812581A (zh) * | 2022-06-23 | 2022-07-29 | 中国科学院合肥物质科学研究院 | 一种基于多传感器融合的越野环境导航方法 |
| CN115061467A (zh) * | 2022-06-24 | 2022-09-16 | 合肥工业大学 | 基于改进高斯采样prm算法和狭窄通道识别的路径规划方法 |
| CN115468567A (zh) * | 2022-09-28 | 2022-12-13 | 中国人民解放军陆军装甲兵学院 | 一种面向越野环境的激光视觉融合slam方法 |
| CN116107300A (zh) * | 2022-12-08 | 2023-05-12 | 中国船舶集团有限公司第七一六研究所 | 用于越野环境下的基于先验知识适用于无人驾驶的路径规划方法 |
| CN117109625A (zh) * | 2023-10-20 | 2023-11-24 | 湖南大学 | 一种基于改进prm算法的无人驾驶路径规划方法 |
Non-Patent Citations (2)
| Title |
|---|
| 基于分层2.5D地图的无人履带车辆路径规划;刘海鸥等;北京理工大学学报;20231018;1-9 * |
| 基于距离变换的PRM路径规划算法;李敏;周远远;黄鲁;;信息技术与网络安全;20180310(第03期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117553820A (zh) | 2024-02-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111857160B (zh) | 一种无人车路径规划方法和装置 | |
| Xuexi et al. | SLAM algorithm analysis of mobile robot based on lidar | |
| US10696300B2 (en) | Vehicle tracking | |
| CN114812581B (zh) | 一种基于多传感器融合的越野环境导航方法 | |
| CN107144285B (zh) | 位姿信息确定方法、装置和可移动设备 | |
| CN105404844B (zh) | 一种基于多线激光雷达的道路边界检测方法 | |
| CN117553820B (zh) | 一种越野环境中路径规划方法、系统及设备 | |
| Weng et al. | Pole-based real-time localization for autonomous driving in congested urban scenarios | |
| CN110816548A (zh) | 传感器融合 | |
| CN112184736B (zh) | 一种基于欧式聚类的多平面提取方法 | |
| CN113345008B (zh) | 考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法 | |
| CN106054900A (zh) | 基于深度摄像头的机器人临时避障方法 | |
| CN106541945A (zh) | 一种基于icp算法的无人车自动泊车方法 | |
| CN112346463B (zh) | 一种基于速度采样的无人车路径规划方法 | |
| Jaspers et al. | Multi-modal local terrain maps from vision and lidar | |
| CN113592891B (zh) | 无人车可通行域分析方法及导航栅格地图制作方法 | |
| Schreier et al. | From grid maps to parametric free space maps—A highly compact, generic environment representation for ADAS | |
| CN115371662B (zh) | 一种基于概率栅格移除动态对象的静态地图构建方法 | |
| CN115861968A (zh) | 一种基于实时点云数据的动态障碍物剔除方法 | |
| CN117389305A (zh) | 一种无人机巡检路径规划方法、系统、设备及介质 | |
| CN116858269A (zh) | 基于激光slam的烟草行业成品库平库盘点机器人路径优化方法 | |
| CN113554705B (zh) | 一种变化场景下的激光雷达鲁棒定位方法 | |
| CN113178091B (zh) | 安全行驶区域方法、装置和网络设备 | |
| CN113551679A (zh) | 一种示教过程中的地图信息构建方法、构建装置 | |
| CN117289301A (zh) | 一种未知越野场景下空地无人平台协同路径规划方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |