CN115900730A - 一种自动驾驶车辆三维点云定位方法、装置及存储介质 - Google Patents
一种自动驾驶车辆三维点云定位方法、装置及存储介质 Download PDFInfo
- Publication number
- CN115900730A CN115900730A CN202210529691.9A CN202210529691A CN115900730A CN 115900730 A CN115900730 A CN 115900730A CN 202210529691 A CN202210529691 A CN 202210529691A CN 115900730 A CN115900730 A CN 115900730A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- matching
- ndt
- positioning
- filtering
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 59
- 238000003860 storage Methods 0.000 title claims abstract description 14
- 238000004364 calculation method Methods 0.000 claims abstract description 68
- 238000001914 filtration Methods 0.000 claims abstract description 49
- 230000009466 transformation Effects 0.000 claims abstract description 29
- 238000009826 distribution Methods 0.000 claims abstract description 14
- 238000004590 computer program Methods 0.000 claims description 21
- 230000008878 coupling Effects 0.000 claims description 11
- 238000010168 coupling process Methods 0.000 claims description 11
- 238000005859 coupling reaction Methods 0.000 claims description 11
- 230000001133 acceleration Effects 0.000 claims description 10
- 238000013507 mapping Methods 0.000 claims description 10
- 230000008859 change Effects 0.000 claims description 9
- 230000004927 fusion Effects 0.000 claims description 5
- 238000012935 Averaging Methods 0.000 claims description 4
- 238000001514 detection method Methods 0.000 claims description 4
- 238000000605 extraction Methods 0.000 claims description 4
- 230000036544 posture Effects 0.000 claims description 4
- 230000003993 interaction Effects 0.000 claims description 3
- 230000004807 localization Effects 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 12
- 230000006870 function Effects 0.000 description 7
- 230000008569 process Effects 0.000 description 6
- 230000000694 effects Effects 0.000 description 5
- 230000010354 integration Effects 0.000 description 5
- 238000012545 processing Methods 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 206010000372 Accident at work Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000002360 explosive Substances 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000002420 orchard Substances 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
- 239000000725 suspension Substances 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Images
Landscapes
- Navigation (AREA)
Abstract
本申请公开了一种自动驾驶车辆三维点云定位方法、装置及存储介质，用于在复杂环境下提高定位的精度。本申请公开的自动驾驶车辆三维点云定位方法包括：确定先验地图和正态分布变换NDT计算参数；对三维3D点云进行畸变去除和动态点滤除，得到第一点云；根据所述先验地图，所述NDT计算参数和所述第一点云，进行匹配定位。本申请还提供了一种自动驾驶车辆三维点云定位装置及存储介质。
Description
技术领域
本申请涉及自动驾驶领域，尤其涉及一种自动驾驶车辆三维点云定位方法、装置和存储介质。
背景技术
随着自动驾驶技术的蓬勃发展，越来越多的行业开始对运输工具进行自动化的转型升级。对不同行业的运输需求进行自动驾驶车辆相关场景与需求的研发有利于提高作业自动化程度，减少行业事故发生的同时提升作业效率。但实际中，由于车辆面对的作业环境复杂多变，依赖卫星导航定位系统、惯性导航系统和靶标定位等方式往往难以满足复杂场景下的定位需求。例如，面对矿山，果园等复杂的场景，现有的三维点云匹配方法定位精度低。因此，如何提高三维点云定位方法的精度是一个亟待解决的技术问题。
发明内容
针对上述技术问题，本申请实施例提供了一种自动驾驶车辆三维点云定位方法、装置及存储介质，用以提高三维点云定位的精度。
第一方面，本申请实施例提供的一种自动驾驶车辆三维点云定位方法，包括：
确定先验地图和正态分布变换NDT计算参数；
对三维3D点云进行畸变去除和动态点滤除，得到第一点云；
根据所述先验地图，所述NDT计算参数和所述第一点云，进行匹配定位。
其中，述确定先验地图和正态分布变换NDT计算参数包括：
根据初始位姿和NDT单步匹配确定所述先验地图和NDT计算参数。
优选的，所述初始位姿根据以下方式获得：
根据递推里程计确定所述初始位姿；
所述递推里程计包括轮速计和惯性传感器IMU。
优选的，所述根据递推里程计确定所述初始位姿包括：
通过所述轮速计测得脉冲，并通过半径推算得到当前速度；
通过所述IMU积分得到帧间角度，累加历史定位姿态获得当前的角度；
对所述IMU在不同帧间内的N个加速度求平均值；
根据耦合递推模型得到初始位姿；
其中，N为大于等于2的整数。
优选的，所述耦合递推模型为：
Δpx＝vwheel_speedsin(yaw)dt
Δpy＝vwheel_speedcos(yaw)dt，
Δpz＝vwheel_speedsin(pitch)+azdt2/2
Δpx为帧间横向位置变换量，Δpy为帧间纵向位置变换量，Δpz为帧间高度位置变换量，vwheel_speed为轮速计推算得到的帧间平均速度，yaw为IMU累计积分获得的前一帧的yaw角，pitch为IMU累计积分获得的前一帧的pitch角，az为IMU推算得到的平均高度方向加速度，t为两帧间时间变换量。
优选的，所述对三维3D点云进行畸变去除和动态点滤除，得到第一点云包括：
根据所述递推里程计对所述3D点云进行畸变去除和动态点滤除，得到所述第一点云；
所述畸变去除包括：根据所述递推里程计将激光雷达局部坐标系的变换统一到初始时间；
所述动态点滤除包括：根据所述递推里程计得到帧间变换位姿，将所述帧间变换位姿与上一帧的位姿累加得到当前帧的位姿，建立当前帧和上一帧的点云八叉树，根据所述当前帧和上一帧的点云八叉树的栅格占用概率变换得到动态点。
优选的，还包括：
利用高度对地面点进行干扰信息的滤除；
所述利用高度对地面点进行干扰信息的滤除包括：
抽取预设高度阈值以下的点，对抽取的点进行地面提取；
根据平面法向量确定地面以下的点并滤除所述地面以下的点。
优选的，所述根据平面法向量确定地面以下的点并滤除所述地面以下的点还包括：
若地面上的点超过预设的数量阈值，则将所述地面上的点也滤除，否则只滤除所述地面以下的点。
优选的，所述根据所述先验地图，所述NDT计算参数和所述第一点云，进行匹配定位之前，还包括：
建立先验地图，保存匹配计算信息；
所述建立先验地图包括：
将待定位环境点云数据以设定的速度匀速行驶并记录下来；
对记录下来的点云进行离线建图；
所述进行离线建图的方法包括以下之一：
多特征融合匹配；
NDT定位匹配；
回环检测；
全球卫星导航系统GPS真值拼接；
其中，所述匹配计算信息包括：NDT计算参数与地图坐标系下位置的索引表。
所述根据所述先验地图，所述NDT计算参数和所述第一点云，进行匹配定位包括：
根据所述初始位姿、匹配计算信息与所述环境地图进行匹配，若匹配成功则以所述NDT定位位姿为最终定位结果；若匹配失败则以递推位姿作为最终定位结果。
使用本发明提供的自动驾驶车辆三维点云定位方法，融合了IMU、轮速计与3D点云传感器，针对现有的NDT定位技术进行改进，利用欧式聚类在离线建图过程中先验确定NDT匹配计算参数，使用先验起止点位姿优化后的3D点云先验地图进行精确定位，是一种动态计算匹配参数的不依赖特定特征的定位方法，提高了复杂场景下的定位精度。
第二方面，本申请实施例还提供一种自动驾驶车辆三维点云定位装置，包括：
匹配模块，被配置用于确定先验地图和正态分布变换NDT计算参数；
滤除模块，被配置用于对三维3D点云进行畸变去除和动态点滤除，得到第一点云；
定位模块，被配置用于根据所述先验地图，所述NDT计算参数和所述第一点云，进行匹配定位。
第三方面，本申请实施例还提供一种自动驾驶车辆三维点云定位装置，包括：存储器、处理器和用户接口；
所述存储器，用于存储计算机程序；
所述用户接口，用于与用户实现交互；
所述处理器，用于读取所述存储器中的计算机程序，所述处理器执行所述计算机程序时，实现本发明提供的自动驾驶车辆三维点云定位方法。
第四方面，本申请实施例还提供一种处理器可读存储介质，所述处理器可读存储介质存储有计算机程序，所述处理器执行所述计算机程序时实现本发明提供的自动驾驶车辆三维点云定位方法。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简要介绍，显而易见地，下面描述中的附图仅是本申请的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的自动驾驶车辆三维点云定位方法示意图；
图2为本申请实施例提供的初始位姿的确定流程示意图；
图3为本申请实施例提供的先验地图确定流程示意图；
图4为本申请实施例提供的自动驾驶车辆三维点云定位装置示意图；
图5为本申请实施例提供的另一种自动驾驶车辆三维点云定位装置结构示意图。
具体实施方式
为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述，显然，所描述的实施例仅仅是本发明一部份实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
下面对文中出现的一些词语进行解释：
1、本发明实施例中术语“和/或”，描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。字符“/”一般表示前后关联对象是一种“或”的关系。
2、本申请实施例中术语“多个”是指两个或两个以上，其它量词与之类似。
3、NDT，正态分布变换(Normal Distributions Transform)；
4、IMU，惯性测量单元(Inertial Measurement Unit)；
5、yaw，横摆角；
6、roll，翻滚角；
7、pitch，俯仰角。
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，并不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
需要说明的是，本申请实施例的展示顺序仅代表实施例的先后顺序，并不代表实施例所提供的技术方案的优劣。
实施例一
参见图1，本申请实施例提供的一种自动驾驶车辆三维点云定位方法示意图，如图1所示，该方法包括步骤S101到S103：
S101、确定先验地图和正态分布变换NDT计算参数；
作为一种优选示例，可以根据初始位姿和NDT单步匹配确定所述先验地图和NDT计算参数。进一步的，作为一种优选示例，初始位姿可以通过以下方式获得：
根据递推里程计确定所述初始位姿；
所述递推里程计包括轮速计和惯性传感器IMU。
具体的，根据递推里程计确定所述初始位姿包括：
通过所述轮速计测得脉冲，并通过半径推算得到当前速度；
通过所述IMU积分得到帧间角度，累加历史定位姿态获得当前的角度；
对所述IMU在不同帧间内的N个加速度求平均值；
根据耦合递推模型得到初始位姿；
其中，N为大于等于2的整数。
本发明实施例中，作为一种优选示例，耦合递推模型为：
Δpx＝vwheel_speedsin(yaw)dt
Δpy＝vwheel_speedcos(yaw)dt，
Δpz＝vwheel_speedsin(pitch)+azdt2/2
其中，Δpx为帧间横向位置变换量，Δpy为帧间纵向位置变换量，Δpz为帧间高度位置变换量，vwheel_speed为轮速计推算得到的帧间平均速度，yaw为IMU累计积分获得的前一帧的yaw角，pitch为IMU累计积分获得的前一帧的pitch角，az为IMU推算得到的平均高度方向加速度，t为两帧间时间变换量。
为便于后续3D点云帧间匹配和3D点云的预处理，本发明实设计了IMU与轮速计结合的递推里程计。所用递推里程计为IMU与轮速里程计松耦合递推模型，松耦合即分别构建和优化各自参数。根据递推里程计确定所述初始位姿如图2所示，通过所述轮速计测得脉冲，并通过轮速转换和半径推算得到当前速度，将当前速度投影到IMU获得的朝向上，通过所述IMU积分得到帧间角度，累加历史定位姿态获得当前的角度，利用上述耦合递推模型算得角速度、加速度项的积分得到三个轴方向的递推位姿，即初始位姿。
作为另一种优选示例，为了提高精度，还可以对递推里程计做以下约束：IMU误差约束、车辆半径约束、横摆角yaw角约束和翻滚角roll角约束、俯仰角pitch角约束共计五种误差约束。优选的，IMU误差约束即为：通过轮速里程计位姿和IMU预积分模型残差求得IMU误差，对IMU的积分角度与速度进行约束；车辆半径约束即为：通过测量满载和空载时车轮半径可以获得车辆半径范围，根据当前载质量进行插值进行轮半径偏差的约束；yaw角约束即为：当车辆转向时其车轮转角并非车辆航向角，通过车辆转角和轴距等阿克曼转向模型参数可以获得车辆航向角偏置，并对IMU测得航向进行修正；翻滚角roll约束和俯仰角pitch角约束即为：加减速与颠簸会引起roll与picth角的变换，这种误差是悬架、连接处和车身的非刚性性质造成的随机的误差，通过RANSAC(Random Sample Consensus的简称，即随机抽样一致算法)拟合地平面获得法向量进行角度误差的去除。
S102、对三维3D点云进行畸变去除和动态点滤除，得到第一点云；
作为一种优选示例，本步骤具体可以是：
根据所述递推里程计对所述3D点云进行畸变去除和动态点滤除，得到所述第一点云；
所述畸变去除包括：根据所述递推里程计将激光雷达局部坐标系的变换统一到初始时间；
所述动态点滤除包括：根据所述递推里程计得到帧间变换位姿，将所述帧间变换位姿与上一帧的位姿累加得到当前帧的位姿，建立当前帧和上一帧的点云八叉树，根据所述当前帧和上一帧的点云八叉树的栅格占用概率变换得到动态点。
进一步的，本步骤还可以包括：
利用高度对地面点进行干扰信息的滤除；
所述利用高度对地面点进行干扰信息的滤除包括：
抽取预设高度阈值以下的点，对抽取的点进行地面提取；
根据平面法向量确定地面以下的点并滤除所述地面以下的点。
作为一种优选示例，根据平面法向量确定地面以下的点并滤除所述地面以下的点还包括：
若地面上的点超过预设的数量阈值，则将所述地面上的点也滤除，否则只滤除所述地面以下的点。
也就是说，S102中，得到递推里程计后利用该里程计进行3D点云畸变的去除和3D点云动态点的滤除。点云畸变去除是对每个时间下坐标系的统一，利用递推里程计插值地将随时间激光雷达局部坐标系的变换统一到初始时间进行畸变的去除。通过建立前后两帧的点云八叉树，将递推得到的里程计帧间变换位姿用于前一帧点云，通过位姿变换得到当前时刻点云应该所在位置，最后通过当前帧点云八叉树与前一帧点云两颗八叉树栅格占用概率变换得到动态点。
S103、根据所述先验地图，所述NDT计算参数和所述第一点云，进行匹配定位。
作为一种优选示例，进行本步骤的匹配定位之前，还包括建立先验地图的步骤，即：
建立先验地图，保存匹配计算信息；
所述建立先验地图包括：
将待定位环境点云数据以设定的速度匀速行驶并记录下来；
对记录下来的点云进行离线建图；
所述进行离线建图的方法包括以下之一：
多特征融合匹配；
NDT定位匹配；
回环检测；
全球卫星导航系统GPS真值拼接；
其中，所述匹配计算信息包括：NDT计算参数与地图坐标系下位置的索引表。
作为一种优选示例，建立先验地图的方法包括：首先，将待定位环境点云数据以较慢速度匀速行使录制下来，对这些点云进行离线建图，建图方式可以使用多特征融合匹配、ndt定位匹配、回环检测、GPS真值拼接等多种之一或者组合，依照环境以及设备的不同可以选择某一种方法或者多种方法结合。
作为一种优选示例，匹配计算信息通过以下方式得到：在先验地图的建图过程中，根据ndt的计算步骤，建立一个稠密的ndt计算参数与地图坐标系下位置的索引表，在地图中以n为一段，计算重定位ndt匹配所需要的参数，在重定位时可以根据位置索引到相关的ndt匹配计算参数。其中，n为大于0的数，单位为厘米cm。
本步骤中，离线建图的方法以NDT定位匹配为例，如图3所示：
S301a，获取初始位姿。与S101相同，不再赘述；
S301b，点云聚类。例如欧式聚类；
S302，NDT匹配参数计算。利用欧式聚类结果保存先验地图中的环境信息，利用环境信息动态调整NDT计算参数。例如，统计环境欧式聚类包围盒质心位置确定了NDT匹配计算得分公式中的远近距离点的计算系数c1、c2和较远点所占比例P0，计算网格的大小由欧式聚类包围盒大小结果所确定，同时可以得到当前位置点云均值
与协方差
建图时每个定位位姿都会关联c1、c2、P0、
与
五个匹配计算参数，利用定位结果与计算参数形成哈希表，在二次匹配时调用根据初始位姿调用哈希表得到匹配计算参数。NDT匹配计算公式如下：
其中，
为正太分布概率值趋向于0则两帧点云相似，
为当前点云位置向量包括x、y、z位置，
为先验地图模板点云的质心向量包括x、y、z位置，Σ为先验地图点云的协方差矩阵。
需要说明的是，本发明实施例中，包围盒是指能包裹住单个聚类点云内所有点的最小外接六面体。
c1、c2为远近距离点的百分比系数，P0为远点的概率，优选的取55％，优选的c1一般取10％(范围为1％-55％)，c2是与栅格体积成反比的变动项。
S303，点云栅格化。优选的，通过将现有位姿和索引表进行比对，由之前欧式聚类包围盒大小确定的NDT计算栅格大小为参数，作为当前位置NDT划分网格的大小在每个网格内进行NDT的计算；
S304，KNN搜索具备地图最近邻点。优选的，使用i-kdtree搜索局部地图上每个栅格内所有点云的最近邻点将其坐标作为传入S302所述NDT该局计算公式的参数x；
S305，根据S304所述搜索过程得到的局部地图最近邻点和当前初始位姿所对应的NDT计算参数，传入S302所述NDT概率计算公式计算当前时刻NDT匹配概率，对概率取
将其作为最优化的残差将其命名为NDT匹配得分，优化位姿使得上述NDT匹配得分最小计算得出位姿；
S306，得到起止点真值先验优化位姿，即得到先验地图。
建立先验地图后，根据所述先验地图，所述NDT计算参数和所述第一点云，进行匹配定位，具体可以是：
根据所述初始位姿、匹配计算信息与所述环境地图进行匹配，若匹配成功则以所述NDT定位位姿为最终定位结果；若匹配失败则以递推位姿作为最终定位结果。
也就是说，在利用先验地图进行定位时根据匹配得分是否足够小决定是否定位成功，将IMU与轮速计递推位姿作为匹配定位失效时的冗余定位方式。受限于定位结果优化方式，单次匹配定位有可能不成功，当单次匹配定位结果无法收敛时，利用未收敛结果进行二次匹配。当二次匹配依旧无法收敛时，使用递推里程计的初始位姿作为最终匹配定位结果。
使用本发明提供的自动驾驶车辆三维点云定位方法，融合了IMU、轮速计与3D点云传感器，针对现有的NDT定位技术进行改进，利用欧式聚类在离线建图过程中先验确定NDT匹配计算参数，使用先验起止点位姿优化后的3D点云先验地图进行精确定位，是一种动态计算匹配参数的不依赖特定特征的定位方法，提高了复杂场景下的定位精度。
实施例二
基于同一个发明构思，本发明实施例还提供了一种自动驾驶车辆三维点云定位装置，如图4所示，该装置包括：
匹配模块401，被配置用于确定先验地图和正态分布变换NDT计算参数；
滤除模块402，被配置用于对三维3D点云进行畸变去除和动态点滤除，得到第一点云；
定位模块403，被配置用于根据所述先验地图，所述NDT计算参数和所述第一点云，进行匹配定位。
优选的，匹配模块401被配置用于确定先验地图和正态分布变换NDT计算参数包括：
被配置用于根据初始位姿和NDT单步匹配确定所述先验地图和NDT计算参数。
所述初始位姿根据以下方式获得：
根据递推里程计确定所述初始位姿；
所述递推里程计包括轮速计和惯性传感器IMU。
优选的，匹配模块401被配置用于根据递推里程计确定所述初始位姿：
通过所述轮速计测得脉冲，并通过半径推算得到当前速度；
通过所述IMU积分得到帧间角度，累加历史定位姿态获得当前的角度；
对所述IMU在不同帧间内的N个加速度求平均值；
根据耦合递推模型得到初始位姿；
其中，N为大于等于2的整数。
所述耦合递推模型为：
Δpx＝vwheel_speedsin(yaw)dt
Δpy＝vwheel_speedcos(yaw)dt，
Δpz＝vwheel_speedsin(pitch)+azdt2/2
其中，Δpx为帧间横向位置变换量，Δpy为帧间纵向位置变换量，Δpz为帧间高度位置变换量，vwheel_speed为轮速计推算得到的帧间平均速度，yaw为IMU累计积分获得的前一帧的yaw角，pitch为IMU累计积分获得的前一帧的pitch角，az为IMU推算得到的平均高度方向加速度，t为两帧间时间变换量。
优选的，滤除模块402被配置用于对三维3D点云进行畸变去除和动态点滤除，得到第一点云：
根据所述递推里程计对所述3D点云进行畸变去除和动态点滤除，得到所述第一点云；
所述畸变去除包括：根据所述递推里程计将激光雷达局部坐标系的变换统一到初始时间；
所述动态点滤除包括：根据所述递推里程计得到帧间变换位姿，将所述帧间变换位姿与上一帧的位姿累加得到当前帧的位姿，建立当前帧和上一帧的点云八叉树，根据所述当前帧和上一帧的点云八叉树的栅格占用概率变换得到动态点。
优选的，滤除模块402还被配置用于对三维3D点云进行畸变去除和动态点滤除：
利用高度对地面点进行干扰信息的滤除；
所述利用高度对地面点进行干扰信息的滤除包括：
抽取预设高度阈值以下的点，对抽取的点进行地面提取；
根据平面法向量确定地面以下的点并滤除所述地面以下的点。
若地面上的点超过预设的数量阈值，则将所述地面上的点也滤除，否则只滤除所述地面以下的点。
作为一种优选示例，定位模块403，被配置用于在进行匹配定位之前，还包括：
建立先验地图，保存匹配计算信息；
所述建立先验地图包括：
将待定位环境点云数据以设定的速度匀速行驶并记录下来；
对记录下来的点云进行离线建图；
所述进行离线建图的方法包括以下四种：
多特征融合匹配；
NDT匹配定位；
回环检测；
全球卫星导航系统GPS真值拼接；
其中，所述匹配计算信息包括：NDT计算参数与地图坐标系下位置的索引表。
作为一种优选示例，定位模块403，被配置用于根据以下步骤进行匹配定位：
根据所述初始位姿、匹配计算信息与所述环境地图进行匹配，若匹配成功则以所述NDT定位位姿为最终定位结果；若匹配失败则以递推位姿作为最终定位结果。
需要说明的是，本实施例提供的匹配模块401，能实现实施例一中步骤S101包含的全部功能，解决相同技术问题，达到相同技术效果，在此不再赘述；
需要说明的是，本实施例提供的滤除模块402，能实现实施例一中步骤S102包含的全部功能，解决相同技术问题，达到相同技术效果，在此不再赘述；
需要说明的是，本实施例提供的定位模块403，能实现实施例一中步骤S103包含的全部功能，解决相同技术问题，达到相同技术效果，在此不再赘述；
需要说明的是，实施例二提供的装置与实施例一提供的方法属于同一个发明构思，解决相同的技术问题，达到相同的技术效果，实施例二提供的装置能实现实施例一的所有方法，相同之处不再赘述。
实施例三
基于同一个发明构思，本发明实施例还提供了一种自动驾驶车辆三维点云定位装置，如图5所示，该装置包括：
包括存储器502、处理器501和用户接口503；
所述存储器502，用于存储计算机程序；
所述用户接口503，用于与用户实现交互；
所述处理器501，用于读取所述存储器502中的计算机程序，所述处理器501执行所述计算机程序时，实现：
确定先验地图和正态分布变换NDT计算参数；
对三维3D点云进行畸变去除和动态点滤除，得到第一点云；
根据所述先验地图，所述NDT计算参数和所述第一点云，进行匹配定位。
其中，在图5中，总线架构可以包括任意数量的互联的总线和桥，具体由处理器501代表的一个或多个处理器和存储器502代表的存储器的各种电路链接在一起。总线架构还可以将诸如外围设备、稳压器和功率管理电路等之类的各种其他电路链接在一起，这些都是本领域所公知的，因此，本文不再对其进行进一步描述。总线接口提供接口。处理器501负责管理总线架构和通常的处理，存储器502可以存储处理器501在执行操作时所使用的数据。
处理器501可以是CPU、ASIC、FPGA或CPLD，处理器501也可以采用多核架构。
处理器501执行存储器502存储的计算机程序时，实现实施例一中的任一自动驾驶车辆三维点云定位方法。
需要说明的是，实施例三提供的装置与实施例一提供的方法属于同一个发明构思，解决相同的技术问题，达到相同的技术效果，实施例三提供的装置能实现实施例一的所有方法，相同之处不再赘述。
本申请还提出一种处理器可读存储介质。其中，该处理器可读存储介质存储有计算机程序，所述处理器执行所述计算机程序时实现实施例一中的任一自动驾驶车辆三维点云定位方法。
需要说明的是，本申请实施例中对单元的划分是示意性的，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。
Claims (13)
1.一种自动驾驶车辆三维点云定位方法，其特征在于，包括：
确定先验地图和正态分布变换NDT计算参数；
对三维3D点云进行畸变去除和动态点滤除，得到第一点云；
根据所述先验地图，所述NDT计算参数和所述第一点云，进行匹配定位。
2.根据权利要求1所述的方法，其特征在于，所述确定先验地图和正态分布变换NDT计算参数包括：
根据初始位姿和NDT单步匹配确定所述先验地图和NDT计算参数。
3.根据权利要求2所述的方法，其特征在于，所述初始位姿根据以下方式获得：
根据递推里程计确定所述初始位姿；
所述递推里程计包括轮速计和惯性传感器IMU。
4.根据权利要求3所述的方法，其特征在于，所述根据递推里程计确定所述初始位姿包括：
通过所述轮速计测得脉冲，并通过半径推算得到当前速度；
通过所述IMU积分得到帧间角度，累加历史定位姿态获得当前的角度；
对所述IMU在不同帧间内的N个加速度求平均值；
根据耦合递推模型得到初始位姿；
其中，N为大于等于2的整数。
5.根据权利要求4所述的方法，其特征在于，所述耦合递推模型为：
Δpx为帧间横向位置变换量，Δpy为帧间纵向位置变换量，Δpz为帧间高度位置变换量，vwheel_speed为轮速计推算得到的帧间平均速度，yaw为IMU累计积分获得的前一帧的yaw角，pitch为IMU累计积分获得的前一帧的pitch角，az为IMU推算得到的平均高度方向加速度，t为两帧间时间变换量。
6.根据权利要求5所述的方法，其特征在于，所述对三维3D点云进行畸变去除和动态点滤除，得到第一点云包括：
根据所述递推里程计对所述3D点云进行畸变去除和动态点滤除，得到所述第一点云；
所述畸变去除包括：根据所述递推里程计将激光雷达局部坐标系的变换统一到初始时间；
所述动态点滤除包括：根据所述递推里程计得到帧间变换位姿，将所述帧间变换位姿与上一帧的位姿累加得到当前帧的位姿，建立当前帧和上一帧的点云八叉树，根据所述当前帧和上一帧的点云八叉树的栅格占用概率变换得到动态点。
7.根据权利要求6所述的方法，其特征在于，还包括：
利用高度对地面点进行干扰信息的滤除；
所述利用高度对地面点进行干扰信息的滤除包括：
抽取预设高度阈值以下的点，对抽取的点进行地面提取；
根据平面法向量确定地面以下的点并滤除所述地面以下的点。
8.根据权利要求7所述的方法，其特征在于，所述根据平面法向量确定地面以下的点并滤除所述地面以下的点还包括：
若地面上的点超过预设的数量阈值，则将所述地面上的点也滤除，否则只滤除所述地面以下的点。
9.根据权利要求1所述的方法，其特征在于，所述根据所述先验地图，所述NDT计算参数和所述第一点云，进行匹配定位之前，还包括：
建立先验地图，保存匹配计算信息；
所述建立先验地图包括：
将待定位环境点云数据以设定的速度匀速行驶并记录下来；
对记录下来的点云进行离线建图；
所述进行离线建图的方法包括以下之一：
多特征融合匹配；
NDT定位匹配；
回环检测；
全球卫星导航系统GPS真值拼接；
其中，所述匹配计算信息包括：NDT计算参数与地图坐标系下位置的索引表。
10.根据权利要求9所述的方法，其特征在于，所述根据所述先验地图，所述NDT计算参数和所述第一点云，进行匹配定位包括：
根据所述初始位姿、匹配计算信息与所述环境地图进行匹配，若匹配成功则以所述NDT定位位姿为最终定位结果；若匹配失败则以递推位姿作为最终定位结果。
11.一种自动驾驶车辆三维点云定位装置，其特征在于，包括：
匹配模块，被配置用于确定先验地图和正态分布变换NDT计算参数；
滤除模块，被配置用于对三维3D点云进行畸变去除和动态点滤除，得到第一点云；
定位模块，被配置用于根据所述先验地图，所述NDT计算参数和所述第一点云，进行匹配定位。
12.一种自动驾驶车辆三维点云定位装置，其特征在于，包括存储器、处理器和用户接口；
所述存储器，用于存储计算机程序；
所述用户接口，用于与用户实现交互；
所述处理器，用于读取所述存储器中的计算机程序，所述处理器执行所述计算机程序时，实现如权利要求1到10之一所述的自动驾驶车辆三维点云定位方法。
13.一种处理器可读存储介质，其特征在于，所述处理器可读存储介质存储有计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至10之一所述的自动驾驶车辆三维点云定位方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210529691.9A CN115900730A (zh) | 2022-05-16 | 2022-05-16 | 一种自动驾驶车辆三维点云定位方法、装置及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210529691.9A CN115900730A (zh) | 2022-05-16 | 2022-05-16 | 一种自动驾驶车辆三维点云定位方法、装置及存储介质 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115900730A true CN115900730A (zh) | 2023-04-04 |
Family
ID=86486114
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210529691.9A Pending CN115900730A (zh) | 2022-05-16 | 2022-05-16 | 一种自动驾驶车辆三维点云定位方法、装置及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115900730A (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117471513A (zh) * | 2023-12-26 | 2024-01-30 | 合众新能源汽车股份有限公司 | 一种车辆定位方法、定位装置、电子设备及存储介质 |
| CN117671013A (zh) * | 2024-02-01 | 2024-03-08 | 安徽蔚来智驾科技有限公司 | 点云定位方法、智能设备及计算机可读存储介质 |
| CN117968682A (zh) * | 2024-04-01 | 2024-05-03 | 山东大学 | 基于多线激光雷达和惯性测量单元的动态点云去除方法 |
-
2022
- 2022-05-16 CN CN202210529691.9A patent/CN115900730A/zh active Pending
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117471513A (zh) * | 2023-12-26 | 2024-01-30 | 合众新能源汽车股份有限公司 | 一种车辆定位方法、定位装置、电子设备及存储介质 |
| CN117471513B (zh) * | 2023-12-26 | 2024-03-15 | 合众新能源汽车股份有限公司 | 一种车辆定位方法、定位装置、电子设备及存储介质 |
| CN117671013A (zh) * | 2024-02-01 | 2024-03-08 | 安徽蔚来智驾科技有限公司 | 点云定位方法、智能设备及计算机可读存储介质 |
| CN117671013B (zh) * | 2024-02-01 | 2024-04-26 | 安徽蔚来智驾科技有限公司 | 点云定位方法、智能设备及计算机可读存储介质 |
| CN117968682A (zh) * | 2024-04-01 | 2024-05-03 | 山东大学 | 基于多线激光雷达和惯性测量单元的动态点云去除方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115900730A (zh) | 一种自动驾驶车辆三维点云定位方法、装置及存储介质 | |
| CN108444482B (zh) | 一种无人机自主寻路避障方法及系统 | |
| CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
| CN110426051A (zh) | 一种车道线绘制方法、装置及存储介质 | |
| CN111915675B (zh) | 基于粒子漂移的粒子滤波点云定位方法及其装置和系统 | |
| CN109710724A (zh) | 一种构建点云地图的方法和设备 | |
| CN112154303B (zh) | 高精度地图定位方法、系统、平台及计算机可读存储介质 | |
| CN115372989A (zh) | 基于激光雷达的越野自动小车长距离实时定位系统及方法 | |
| CN111427373B (zh) | 一种位姿确定方法、装置、介质和设备 | |
| CN114485698B (zh) | 一种交叉路口引导线生成方法及系统 | |
| CN113819917A (zh) | 自动驾驶路径规划方法、装置、设备及存储介质 | |
| CN114061573B (zh) | 地面无人车辆编队定位装置及方法 | |
| CN115077541A (zh) | 自动驾驶车辆的定位方法、装置及电子设备、存储介质 | |
| CN116858269A (zh) | 基于激光slam的烟草行业成品库平库盘点机器人路径优化方法 | |
| CN116380087A (zh) | 基于多因子图的汽车定位方法、装置、设备及存储介质 | |
| CN116719008A (zh) | 一种自动驾驶车辆的定位方法及设备 | |
| KR102624644B1 (ko) | 벡터 맵을 이용한 이동체의 맵 매칭 위치 추정 방법 | |
| CN116385999A (zh) | 一种停车位的识别方法、装置及设备 | |
| CN113551678A (zh) | 构建地图的方法、构建高精度地图的方法及移动设备 | |
| JPH04205320A (ja) | 移動路データに対する評価方法 | |
| CN118864602A (zh) | 一种基于多传感器融合的地面机器人重定位方法及装置 | |
| CN118067114B (zh) | 一种地图构建方法、装置、电子设备及存储介质 | |
| US20240078750A1 (en) | Parameterization method for point cloud data and map construction method | |
| CN117968682B (zh) | 基于多线激光雷达和惯性测量单元的动态点云去除方法 | |
| CN118169725A (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 |