CN115877349B - 一种基于激光雷达的交叉路口车辆定位方法及系统 - Google Patents

一种基于激光雷达的交叉路口车辆定位方法及系统 Download PDF

Info

Publication number
CN115877349B
CN115877349B CN202310132154.5A CN202310132154A CN115877349B CN 115877349 B CN115877349 B CN 115877349B CN 202310132154 A CN202310132154 A CN 202310132154A CN 115877349 B CN115877349 B CN 115877349B
Authority
CN
China
Prior art keywords
point cloud
intersection
dimensional point
real
residual value
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
Application number
CN202310132154.5A
Other languages
English (en)
Other versions
CN115877349A (zh
Inventor
刘仕杰
邹渊
张旭东
李圆圆
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202310132154.5A priority Critical patent/CN115877349B/zh
Publication of CN115877349A publication Critical patent/CN115877349A/zh
Application granted granted Critical
Publication of CN115877349B publication Critical patent/CN115877349B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

本发明公开一种基于激光雷达的交叉路口车辆定位方法及系统，涉及车辆定位技术领域，利用NDT算法对实时环境三维点云和参考环境三维点云进行配准，得到第一定位结果和第一残差值，利用ICP算法对实时环境三维点云和参考环境三维点云进行配准，得到第二定位结果和第二残差值，最后在第一残差值和第二残差值的最小值小于设定阈值时，得到车辆定位结果，从而同时采用NDT算法和ICP算法对通过激光雷达获取的实时环境三维点云与参考环境三维点云进行配准，可实现车辆在交叉路口的精准鲁棒的定位。

Description

一种基于激光雷达的交叉路口车辆定位方法及系统
技术领域
本发明涉及车辆定位技术领域，特别是涉及一种基于激光雷达的交叉路口车辆定位方法及系统。
背景技术
目前交叉路口车辆定位技术主要是基于卫星信号的GNSS定位，但基于卫星信号的GNSS定位只有在能看到天空的地方才能够保证定位精准，在诸如高楼密集的城市、森林、高架桥下、室内或者地下车库等其他地方GPS信号很差，定位鲁棒性差。此外，GNSS定位很容易受天气影响，如果天气不好，信号强度大打折扣。
基于此，亟需一种精准鲁棒的交叉路口车辆定位技术。
发明内容
本发明的目的是提供一种基于激光雷达的交叉路口车辆定位方法及系统，可实现车辆在交叉路口的精准鲁棒的定位。
为实现上述目的，本发明提供了如下方案：
一种基于激光雷达的交叉路口车辆定位方法，所述交叉路口车辆定位方法包括：
获取通过车载激光雷达得到的交叉路口的实时环境三维点云；
利用NDT算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准，得到第一定位结果和第一残差值；
利用ICP算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准，得到第二定位结果和第二残差值；
判断所述第一残差值和所述第二残差值的最小值是否小于设定阈值；
若是，则在所述第一残差值小于所述第二残差值时，以所述第一定位结果作为车辆定位结果，否则，以所述第二定位结果作为车辆定位结果；所述车辆定位结果包括车辆在所述交叉路口的坐标和姿态；
若否，则返回“利用NDT算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准”的步骤。
在一些实施例中，所述利用NDT算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准，得到第一定位结果和第一残差值具体包括：
对所述交叉路口的参考环境三维点云进行网格划分，得到多个网格；计算每一所述网格内的所有扫描点的坐标的均值和协方差，得到每一所述网格对应的均值和协方差；
基于变换矩阵对所述实时环境三维点云进行变换，得到第一变换后点云；
基于每一所述网格对应的均值和协方差构建所述第一变换后点云的扫描点落在所述网格的概率分布函数；
基于所有所述网格的概率分布函数构建目标函数；
对所述目标函数进行优化求解，得到优化后变换矩阵；
基于所述优化后变换矩阵计算第一初始残差值；
判断所述第一初始残差值是否小于第一预设阈值；若是，则结束迭代，以所述优化后变换矩阵对应的姿态参数作为第一定位结果，以所述第一初始残差值作为第一残差值；若否，则继续迭代，以所述优化后变换矩阵作为下一迭代的变换矩阵，以所述第一变换后点云作为下一迭代的实时环境三维点云，返回“基于变换矩阵对所述实时环境三维点云进行变换，得到第一变换后点云”的步骤。
在一些实施例中，第一次迭代时，所述变换矩阵基于初始姿态参数构建得到；所述初始姿态参数包括侧倾角度、俯仰角度、横摆角度和三维坐标。
在一些实施例中，所述初始姿态参数为给定值或者0。
在一些实施例中，所述对所述目标函数进行优化求解具体包括：利用牛顿法对所述目标函数进行优化求解。
在一些实施例中，所述利用ICP算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准，得到第二定位结果和第二残差值具体包括：
对于所述实时环境三维点云的每一个扫描点，在所述交叉路口的参考环境三维点云中确定与所述扫描点相对应的参考点，得到对应点对；所述对应点对包括所述扫描点和与所述扫描点相对应的参考点；
计算使所有所述对应点对的平均距离最小的平移参数和旋转参数；
基于所述平移参数和所述旋转参数计算第二初始残差值；
判断所述第二初始残差值是否小于第二预设阈值；若是，则结束迭代，以所述平移参数和所述旋转参数作为第二定位结果，以所述第二初始残差值作为第二残差值；若否，则继续迭代，基于所述平移参数和所述旋转参数对所述实时环境三维点云进行变换，得到第二变换后点云，并以所述第二变换后点云作为下一迭代的实时环境三维点云，返回“对于所述实时环境三维点云的每一个扫描点，在所述交叉路口的参考环境三维点云中确定与所述扫描点相对应的参考点，得到对应点对”的步骤。
在一些实施例中，所述在所述交叉路口的参考环境三维点云中确定与所述扫描点相对应的参考点具体包括：利用K-D Tree方法在所述交叉路口的参考环境三维点云中确定与所述扫描点相对应的参考点。
在一些实施例中，在得到车辆定位结果并进行下一次定位时，判断所述设定阈值是否等于设定值，若是，则返回“获取通过车载激光雷达得到的交叉路口的实时环境三维点云”的步骤；若否，则将所述设定阈值减1，得到新的设定阈值，并以所述新的设定阈值作为下一次定位时所用的设定阈值，返回“获取通过车载激光雷达得到的交叉路口的实时环境三维点云”的步骤。
在一些实施例中，所述设定值为2。
一种基于激光雷达的交叉路口车辆定位系统，所述交叉路口车辆定位系统包括：
点云数据模块，用于获取通过车载激光雷达得到的交叉路口的实时环境三维点云；
NDT配准模块，用于利用NDT算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准，得到第一定位结果和第一残差值；
ICP配准模块，用于利用ICP算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准，得到第二定位结果和第二残差值；
判断模块，用于判断所述第一残差值和所述第二残差值的最小值是否小于设定阈值；
车辆定位模块，用于若是，则在所述第一残差值小于所述第二残差值时，以所述第一定位结果作为车辆定位结果，否则，以所述第二定位结果作为车辆定位结果；所述车辆定位结果包括车辆在所述交叉路口的坐标和姿态；
返回模块，用于若否，则返回“利用NDT算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准”的步骤。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明用于提供一种基于激光雷达的交叉路口车辆定位方法及系统，先获取通过车载激光雷达得到的交叉路口的实时环境三维点云，然后利用NDT算法对实时环境三维点云和交叉路口的参考环境三维点云进行配准，得到第一定位结果和第一残差值，利用ICP算法对实时环境三维点云和交叉路口的参考环境三维点云进行配准，得到第二定位结果和第二残差值，最后在第一残差值和第二残差值的最小值小于设定阈值时，若第一残差值小于第二残差值，以第一定位结果作为车辆定位结果，否则，以第二定位结果作为车辆定位结果，从而同时采用NDT算法和ICP算法对通过激光雷达获取的实时环境三维点云与参考环境三维点云进行配准，可实现车辆在交叉路口的精准鲁棒的定位。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例1所提供的交叉路口车辆定位方法的方法流程图；
图2为本发明实施例1所提供的交叉路口车辆定位方法的原理框图；
图3为本发明实施例2所提供的交叉路口车辆定位系统的系统框图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种基于激光雷达的交叉路口车辆定位方法及系统，通过激光雷达获取的实时环境三维点云与预先获取的参考环境三维点云的一系列配准，可实现车辆在交叉路口的精准鲁棒的定位。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
实施例1：
本实施例用于提供一种基于激光雷达的交叉路口车辆定位方法，如图1和图2所示，所述交叉路口车辆定位方法包括：
S1：获取通过车载激光雷达得到的交叉路口的实时环境三维点云；
本实施例是在车辆驶入交叉路口后，利用车载激光雷达对交叉路口进行扫描，以得到交叉路口的实时环境三维点云，实时环境三维点云包括多个扫描点，每一个扫描点均具有点云坐标x，y，z。
S2：利用NDT算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准，得到第一定位结果和第一残差值；
本实施例中，交叉路口的参考环境三维点云是通过车载激光雷达预先获取并保存的交叉路口处的三维点云，参考环境三维点云包括多个扫描点，每一个扫描点均具有点云坐标x，y，z。
NDT是正态分布变换，其通过标准最优化技术来确定两个点云间的最优的匹配，因为在配准过程中不需利用对应点的特征来进行计算和匹配，所以匹配速度比其他方法快，NDT算法耗时稳定，对定位结果的初值依赖小，初值误差大时，也能很好的纠正过来。具体的，S2可以包括：
（1）对交叉路口的参考环境三维点云进行网格划分，得到多个网格；计算每一网格内的所有扫描点的坐标的均值和协方差，得到每一网格对应的均值和协方差。
对参考环境三维点云进行网格划分，即将参考环境三维点云的整个空间划分成多个小立方体，每个小立方体即为一个网格，从而将参考环境三维点云的全部扫描点分别划分到各个网格内，以将参考环境三维点云网格化。
对于每一网格，统计该网格内扫描点的数量m以及该网格内每一个扫描点的位置
Figure SMS_1
，/>
Figure SMS_2
即包括扫描点的三维坐标，k=1，2，...，m。
则均值的计算公式为：
Figure SMS_3
；
其中，
Figure SMS_4
为均值。
协方差的计算公式为：
Figure SMS_5
；
其中，
Figure SMS_6
为协方差。
（2）基于变换矩阵对实时环境三维点云进行变换，得到第一变换后点云。
第一次迭代时，本实施例的变换矩阵基于初始姿态参数构建得到，初始姿态参数包括三个欧拉角和三维坐标，三个欧拉角包括侧倾角度r、俯仰角度h和横摆角度w，三维坐标包括x，y，z坐标。初始姿态参数为给定值或者0，即如果给定初始姿态参数，则用给定值，如果没有给定，则设为0。除第一次迭代之外，其余迭代的变换矩阵均选用上一次迭代所得到的优化后变换矩阵。
在得到变换矩阵后，则将实时环境三维点云的每一个扫描点均基于变换矩阵进行变换，即利用变换矩阵对实时环境三维点云的每一个扫描点的三维坐标进行变换，得到每一个扫描点的转换后三维坐标，所有扫描点的转换后三维坐标组成转移到网格上的第一变换后点云。
（3）基于每一网格对应的均值和协方差构建第一变换后点云的扫描点落在网格的概率分布函数。
对于每一网格，该网格对应的概率分布函数为：
Figure SMS_7
；
其中，
Figure SMS_8
为第一变换后点云的扫描点的转换后三维坐标。
基于此，即可建立每一网格对应的概率分布函数，根据该概率分布函数即可计算第一变换后点云的扫描点落于该网格的概率。
（4）基于所有网格的概率分布函数构建目标函数。
目标函数如下：
Figure SMS_9
；
其中，
Figure SMS_10
为目标函数；n为网格总数量；T为变换矩阵；/>
Figure SMS_11
为变换矩阵对应的姿态参数（r，h，w，x，y，z）；/>
Figure SMS_12
为实时环境三维点云；/>
Figure SMS_13
是指将/>
Figure SMS_14
作用在/>
Figure SMS_15
上所得到的转移到网格上的第一变换后点云。
（5）对目标函数进行优化求解，得到优化后变换矩阵。
本实施例通过寻找最优的变换矩阵，以使目标函数最大化，目标函数最大化时的变换矩阵即为优化后变换矩阵。
优选的，本实施例可利用牛顿法对目标函数进行优化求解，但由于利用牛顿法寻找极值，一般找的是极小值，但优化目标是寻找目标函数的最大值，所以需要一个负号对目标函数进行逻辑转换，转换后得到负对数似然函数，最大化目标函数即为最小化负对数似然函数，利用牛顿法对该负对数似然函数进行优化求解，负对数似然函数最小化时的变换矩阵即为优化后变换矩阵。
负对数似然函数如下：
Figure SMS_16
。
（6）基于优化后变换矩阵计算第一初始残差值。
第一初始残差值的计算公式为：
Figure SMS_17
；
其中，E(T)N为第一初始残差值；p为参考环境三维点云的扫描点的三维坐标；q为第一变换后点云的扫描点的转换后三维坐标；k表示扫描点的索引，其覆盖参考环境三维点云的所有扫描点以及第一变换后点云的所有扫描点；T为优化后变换矩阵。
（7）判断第一初始残差值是否小于第一预设阈值；若是，则结束迭代，以优化后变换矩阵对应的姿态参数作为第一定位结果，以第一初始残差值作为第一残差值；若否，则继续迭代，以优化后变换矩阵作为下一迭代的变换矩阵，以第一变换后点云作为下一迭代的实时环境三维点云，返回“基于变换矩阵对实时环境三维点云进行变换，得到第一变换后点云”的步骤。
第一定位结果包括车辆在交叉路口中的x，y，z坐标以及车辆姿态（侧倾角度r、俯仰角度h、横摆角度w）。
S3：利用ICP算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准，得到第二定位结果和第二残差值；
ICP算法是一种数据配准方法，利用最近点搜索法，从而解决基于自由形态曲面的配准问题，以点集对点集（PSTPS）配准方法为基础，阐述了一种曲面拟合算法，该算法是基于四元数的点集到点集配准方法，从测量点集中确定其对应的就近点点集后，运用K-DTree方法计算新的就近点点集，进行迭代计算。具体的，S3可以包括：
（1）对于实时环境三维点云的每一个扫描点，在交叉路口的参考环境三维点云中确定与扫描点相对应的参考点，得到对应点对，对应点对包括扫描点和与扫描点相对应的参考点。
其中，可利用K-D Tree方法在交叉路口的参考环境三维点云中确定与扫描点相对应的参考点。
（2）计算使所有对应点对的平均距离最小的平移参数和旋转参数。
本实施例的平移参数即指三维坐标，三维坐标包括x，y，z坐标，旋转参数即指三个欧拉角，三个欧拉角包括侧倾角度r、俯仰角度h和横摆角度w。本实施例以所有对应点对的平均距离作为目标函数，利用牛顿法对该目标函数进行优化求解，得到使目标函数最小的平移参数和旋转参数。
（3）基于平移参数和旋转参数计算第二初始残差值。
第二初始残差值的计算公式为：
Figure SMS_18
；/>
其中，E(T)I为第二初始残差值；p为参考环境三维点云的扫描点的三维坐标；q为实时环境三维点云的扫描点的三维坐标；k表示扫描点的索引，其覆盖参考环境三维点云的所有扫描点以及实时环境三维点云的所有扫描点；T为基于平移参数和旋转参数所构建的变换矩阵。
（4）判断第二初始残差值是否小于第二预设阈值；若是，则结束迭代，以平移参数和旋转参数作为第二定位结果，以第二初始残差值作为第二残差值；若否，则继续迭代，基于平移参数和旋转参数对实时环境三维点云进行变换，得到第二变换后点云，并以第二变换后点云作为下一迭代的实时环境三维点云，返回“对于实时环境三维点云的每一个扫描点，在交叉路口的参考环境三维点云中确定与扫描点相对应的参考点”的步骤。
S4：判断所述第一残差值和所述第二残差值的最小值是否小于设定阈值；
S5：若是，则在所述第一残差值小于所述第二残差值时，以所述第一定位结果作为车辆定位结果，否则，以所述第二定位结果作为车辆定位结果；所述车辆定位结果包括车辆在所述交叉路口的坐标和姿态；
比较E(T)N与E(T)I大小，若
Figure SMS_19
，更新残差值/>
Figure SMS_20
，以NDT配准结果作为当前配准结果，更新参数r，h，w，x，y，z；否则，更新/>
Figure SMS_21
，以ICP配准结果为当前配准结果，更新参数r，h，w，x，y，z。
S6：若否，则返回“利用NDT算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准”的步骤。
本实施例并行使用NDT和ICP两种配准方法对实时环境三维点云和参考环境三维点云进行配准，得到两个残差值，取两个残差值中较小的残差值与设定阈值比较，若小于设定阈值，则配准过程结束，输出车辆定位结果，否则，则再次配准。通过并行使用ICP和NDT两种配准方法对实时环境三维点云和参考环境三维点云进行配准，NDT配准在初次配准时即可快速、准确地得到车辆定位结果，ICP配准在后续的连续定位中可以得到精度较高的车辆定位结果，通过将以上两种算法结合，所提出的交叉路口车辆定位方法在快速性和鲁棒性上均具有良好的表现，能够实现车辆在交叉路口的精准、鲁棒的定位。
优选的，本实施例在得到车辆定位结果并进行下一次定位时，判断设定阈值是否等于设定值，若是，则返回“获取通过车载激光雷达得到的交叉路口的实时环境三维点云”的步骤，开始下一次定位；若否，则将设定阈值减1，得到新的设定阈值，并以新的设定阈值作为下一次定位时所用的设定阈值，返回“获取通过车载激光雷达得到的交叉路口的实时环境三维点云”的步骤，开始下一次定位。也即在当前定位成功后，将当前的设定阈值减1（设定阈值减至设定值之后不再减小），进行下一次的配准定位，进行连续定位。本实施例中，设定值可为2。
本实施例设计了一个在配准过程中逐次减小，直至达到设定值的自适应阈值，这一设定使算法在前期配准中可以容忍较大的误差，并在后续的配准过程中逐步将误差缩小，保持连续定位的高精度。本实施例的交叉路口车辆定位方法在快速性和鲁棒性上均具有良好的表现，能够实现车辆在交叉路口的精准、鲁棒定位，且该交叉路口车辆定位方法不需要提供位置和姿态的初始值即可使用。
实施例2：
本实施例用于提供一种基于激光雷达的交叉路口车辆定位系统，如图3所示，所述交叉路口车辆定位系统包括：
点云数据模块M1，用于获取通过车载激光雷达得到的交叉路口的实时环境三维点云；
NDT配准模块M2，用于利用NDT算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准，得到第一定位结果和第一残差值；
ICP配准模块M3，用于利用ICP算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准，得到第二定位结果和第二残差值；
判断模块M4，用于判断所述第一残差值和所述第二残差值的最小值是否小于设定阈值；
车辆定位模块M5，用于若是，则在所述第一残差值小于所述第二残差值时，以所述第一定位结果作为车辆定位结果，否则，以所述第二定位结果作为车辆定位结果；所述车辆定位结果包括车辆在所述交叉路口的坐标和姿态；
返回模块M6，用于若否，则返回“利用NDT算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准”的步骤。
本说明书中每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (9)

1.一种基于激光雷达的交叉路口车辆定位方法，其特征在于，所述交叉路口车辆定位方法包括：
获取通过车载激光雷达得到的交叉路口的实时环境三维点云；
利用NDT算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准，得到第一定位结果和第一残差值；
利用ICP算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准，得到第二定位结果和第二残差值；
判断所述第一残差值和所述第二残差值的最小值是否小于设定阈值；
若是，则在所述第一残差值小于所述第二残差值时，以所述第一定位结果作为车辆定位结果，否则，以所述第二定位结果作为车辆定位结果；所述车辆定位结果包括车辆在所述交叉路口的坐标和姿态；
若否，则返回“利用NDT算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准”的步骤；
在得到车辆定位结果并进行下一次定位时，判断所述设定阈值是否等于设定值，若是，则返回“获取通过车载激光雷达得到的交叉路口的实时环境三维点云”的步骤；若否，则将所述设定阈值减1，得到新的设定阈值，并以所述新的设定阈值作为下一次定位时所用的设定阈值，返回“获取通过车载激光雷达得到的交叉路口的实时环境三维点云”的步骤。
2.根据权利要求1所述的交叉路口车辆定位方法，其特征在于，所述利用NDT算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准，得到第一定位结果和第一残差值具体包括：
对所述交叉路口的参考环境三维点云进行网格划分，得到多个网格；计算每一所述网格内的所有扫描点的坐标的均值和协方差，得到每一所述网格对应的均值和协方差；
基于变换矩阵对所述实时环境三维点云进行变换，得到第一变换后点云；
基于每一所述网格对应的均值和协方差构建所述第一变换后点云的扫描点落在所述网格的概率分布函数；
基于所有所述网格的概率分布函数构建目标函数；
对所述目标函数进行优化求解，得到优化后变换矩阵；
基于所述优化后变换矩阵计算第一初始残差值；
判断所述第一初始残差值是否小于第一预设阈值；若是，则结束迭代，以所述优化后变换矩阵对应的姿态参数作为第一定位结果，以所述第一初始残差值作为第一残差值；若否，则继续迭代，以所述优化后变换矩阵作为下一迭代的变换矩阵，以所述第一变换后点云作为下一迭代的实时环境三维点云，返回“基于变换矩阵对所述实时环境三维点云进行变换，得到第一变换后点云”的步骤。
3.根据权利要求2所述的交叉路口车辆定位方法，其特征在于，第一次迭代时，所述变换矩阵基于初始姿态参数构建得到；所述初始姿态参数包括侧倾角度、俯仰角度、横摆角度和三维坐标。
4.根据权利要求3所述的交叉路口车辆定位方法，其特征在于，所述初始姿态参数为给定值或者0。
5.根据权利要求2所述的交叉路口车辆定位方法，其特征在于，所述对所述目标函数进行优化求解具体包括：利用牛顿法对所述目标函数进行优化求解。
6.根据权利要求1所述的交叉路口车辆定位方法，其特征在于，所述利用ICP算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准，得到第二定位结果和第二残差值具体包括：
对于所述实时环境三维点云的每一个扫描点，在所述交叉路口的参考环境三维点云中确定与所述扫描点相对应的参考点，得到对应点对；所述对应点对包括所述扫描点和与所述扫描点相对应的参考点；
计算使所有所述对应点对的平均距离最小的平移参数和旋转参数；
基于所述平移参数和所述旋转参数计算第二初始残差值；
判断所述第二初始残差值是否小于第二预设阈值；若是，则结束迭代，以所述平移参数和所述旋转参数作为第二定位结果，以所述第二初始残差值作为第二残差值；若否，则继续迭代，基于所述平移参数和所述旋转参数对所述实时环境三维点云进行变换，得到第二变换后点云，并以所述第二变换后点云作为下一迭代的实时环境三维点云，返回“对于所述实时环境三维点云的每一个扫描点，在所述交叉路口的参考环境三维点云中确定与所述扫描点相对应的参考点，得到对应点对”的步骤。
7.根据权利要求6所述的交叉路口车辆定位方法，其特征在于，所述在所述交叉路口的参考环境三维点云中确定与所述扫描点相对应的参考点具体包括：利用K-D Tree方法在所述交叉路口的参考环境三维点云中确定与所述扫描点相对应的参考点。
8.根据权利要求1所述的交叉路口车辆定位方法，其特征在于，所述设定值为2。
9.一种基于激光雷达的交叉路口车辆定位系统，其特征在于，所述交叉路口车辆定位系统包括：
点云数据模块，用于获取通过车载激光雷达得到的交叉路口的实时环境三维点云；
NDT配准模块，用于利用NDT算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准，得到第一定位结果和第一残差值；
ICP配准模块，用于利用ICP算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准，得到第二定位结果和第二残差值；
判断模块，用于判断所述第一残差值和所述第二残差值的最小值是否小于设定阈值；
车辆定位模块，用于若是，则在所述第一残差值小于所述第二残差值时，以所述第一定位结果作为车辆定位结果，否则，以所述第二定位结果作为车辆定位结果；所述车辆定位结果包括车辆在所述交叉路口的坐标和姿态；
返回模块，用于若否，则返回“利用NDT算法对所述实时环境三维点云和所述交叉路口的参考环境三维点云进行配准”的步骤；
在得到车辆定位结果并进行下一次定位时，判断所述设定阈值是否等于设定值，若是，则返回“获取通过车载激光雷达得到的交叉路口的实时环境三维点云”的步骤；若否，则将所述设定阈值减1，得到新的设定阈值，并以所述新的设定阈值作为下一次定位时所用的设定阈值，返回“获取通过车载激光雷达得到的交叉路口的实时环境三维点云”的步骤。
CN202310132154.5A 2023-02-20 2023-02-20 一种基于激光雷达的交叉路口车辆定位方法及系统 Active CN115877349B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310132154.5A CN115877349B (zh) 2023-02-20 2023-02-20 一种基于激光雷达的交叉路口车辆定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310132154.5A CN115877349B (zh) 2023-02-20 2023-02-20 一种基于激光雷达的交叉路口车辆定位方法及系统

Publications (2)

Publication Number Publication Date
CN115877349A CN115877349A (zh) 2023-03-31
CN115877349B true CN115877349B (zh) 2023-05-23

Family

ID=85761268

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310132154.5A Active CN115877349B (zh) 2023-02-20 2023-02-20 一种基于激光雷达的交叉路口车辆定位方法及系统

Country Status (1)

Country Link
CN (1) CN115877349B (zh)

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111522043A (zh) * 2020-04-30 2020-08-11 北京联合大学 一种无人车激光雷达快速重新匹配定位方法
WO2023272964A1 (zh) * 2021-06-28 2023-01-05 上海科技大学 用于无人驾驶中激光点云定位的正态分布变换方法

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR102257610B1 (ko) * 2019-10-02 2021-05-28 고려대학교 산학협력단 자율 주행 시스템을 위한 복수의 3차원 라이다 센서의 외부 파리미터 보정 방법
US11521329B2 (en) * 2019-11-22 2022-12-06 Baidu Usa Llc Updated point cloud registration pipeline based on ADMM algorithm for autonomous vehicles
CN111812658B (zh) * 2020-07-09 2021-11-02 北京京东乾石科技有限公司 位置确定方法、装置、系统和计算机可读存储介质
CN114063090A (zh) * 2020-07-29 2022-02-18 北京图森未来科技有限公司 一种可移动设备的定位方法、装置及可移动设备
CN112731334B (zh) * 2020-12-10 2023-09-12 东风汽车集团有限公司 一种激光定位车辆的方法及装置
CN113325389A (zh) * 2021-08-03 2021-08-31 北京理工大学 一种无人车激光雷达定位方法、系统及存储介质
CN114820749A (zh) * 2022-04-27 2022-07-29 西安优迈智慧矿山研究院有限公司 无人车井下定位方法、系统、设备及介质
CN115372989A (zh) * 2022-08-19 2022-11-22 中国人民解放军陆军工程大学 基于激光雷达的越野自动小车长距离实时定位系统及方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111522043A (zh) * 2020-04-30 2020-08-11 北京联合大学 一种无人车激光雷达快速重新匹配定位方法
WO2023272964A1 (zh) * 2021-06-28 2023-01-05 上海科技大学 用于无人驾驶中激光点云定位的正态分布变换方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
"Research on 3D LiDAR Point Cloud Matching Localization Method";YiFei Xue et al.;《2022 5th World Conference on Mechanical Engineering and Intelligent Manufacturing (WCMEIM)》;第878-881页 *
"基于NDT和ICP融合的点云配准方法";张桂杨等;《北京测绘》;第33卷(第12期);第1465-1469页 *

Also Published As

Publication number Publication date
CN115877349A (zh) 2023-03-31

Similar Documents

Publication Publication Date Title
CN111427026B (zh) 激光雷达的标定方法、装置、存储介质及自移动设备
CN111508021B (zh) 一种位姿确定方法、装置、存储介质及电子设备
CN110609290B (zh) 激光雷达匹配定位方法及装置
CN110930495A (zh) 基于多无人机协作的icp点云地图融合方法、系统、装置及存储介质
CN112086010B (zh) 地图生成方法、装置、设备及存储介质
CN113587933B (zh) 一种基于分支定界算法的室内移动机器人定位方法
CN111273312B (zh) 一种智能车辆定位与回环检测方法
CN111080682B (zh) 点云数据的配准方法及装置
CN111505662B (zh) 一种无人驾驶车辆定位方法及系统
CN111862215B (zh) 一种计算机设备定位方法、装置、计算机设备和存储介质
CN112346104A (zh) 一种无人机信息融合定位方法
CN113325389A (zh) 一种无人车激光雷达定位方法、系统及存储介质
CN111913169A (zh) 激光雷达内参、点云数据的修正方法、设备及存储介质
CN115728803A (zh) 一种城市驾驶车辆连续定位系统及方法
CN113012206B (zh) 一种顾及房檐特征的机载与车载LiDAR点云配准方法
CN111678516B (zh) 一种基于激光雷达的有界区域快速全局定位方法
CN115877349B (zh) 一种基于激光雷达的交叉路口车辆定位方法及系统
CN113959437A (zh) 一种用于移动测量设备的测量方法及系统
CN113503883B (zh) 采集用于构建地图的数据的方法、存储介质及电子设备
CN114842074A (zh) 一种基于模型匹配的无人机影像定位方法
CN115166702A (zh) 一种基于高精度定位信息的激光雷达到车体坐标系的自动标定方法及装置
CN114742141A (zh) 基于icp点云的多源信息数据融合研判方法
CN111275748B (zh) 动态环境下基于激光雷达的点云配准方法
CN117075171B (zh) 激光雷达的位姿信息确定方法、装置、设备及存储介质
CN117058323A (zh) 一种基于机载LiDAR点云数据的建筑物三维重建方法

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