一种动态环境下地面车辆点云地图快速重定位方法

1.本发明属于自主导航领域，具体涉及一种动态环境下地面车辆点云地图快速重定位方法。


背景技术：

2.大部分的地面车辆都采用卫星导航与惯性导航组合定位，但在作战环境下，卫星信号往往会缺失，需要地面车辆依靠自身传感器进行定位。slam技术解决了地面车辆在移动的同时边建立地图、边定位的问题，但对于事先已经建好的地图，如何在进入地图范围时快速确定车辆在地图内的位置，仍缺少高效的方法，尤其是在复杂动态环境下，移动的物体会加大定位难度。在未来地面车辆作战体系下，卫星信号缺失下车辆如何快速确定自己的位置是迅速发挥作战效能的关键。


技术实现要素：

3.为了解决上述背景技术中提到的技术问题，本发明提供了一种动态环境下地面车辆点云地图快速重定位。
4.为了实现上述技术目的，本发明的技术方案为：
5.动态环境下地面车辆点云地图快速重定位方法，包括以下步骤：
6.(1)采集激光雷达点云，获取每帧点云的位姿，对点云数据进行预处理。
7.(2)设计基于先验信息的点云粗配准算法用以剔除动态点，减小动态点误匹配给激光雷达里程计带来的误差并提供相对精确的转移矩阵初值用于莱温伯格马夸特(lm)精配准。
8.(3)同步进行点云地图构建。在建图阶段构建历史所有关键帧点云的特征描述矩阵、特征描述向量并存储特征点云云地图。
9.(4)匹配定位阶段，利用提取当前帧的描述子与历史关键帧描述子进行匹配实现快速初始定位，最后采用实时激光雷达定位与建图算法进行后续的精准定位。
10.进一步地，在步骤(1)中，具体过程如下：
11.(101)实时采集激光雷达一帧点云的数据，进行地面点和非地面的分割。并将一帧点云投影成距离图像，然后将点云划分为na×
nb段，其中na为激光雷达线数。然后将na×
nb段的距离图像划分为6个子图像，记b
k,l
为k帧点云图像中的第l段。
12.(102)使用图像的分割方法将距离图像分割为很多个聚类。同一个聚类的点被标记上唯一的标识并同时根据曲率c的大小选取角点平面点作为特征点，曲率的计算公式如下：
[0013][0014]
其中，xi，xj分别表示不同两激光点的坐标，s是当前激光点在竖直方向上，左右各
找五个点构成的集合。
[0015]
进一步地，在步骤(2)中，具体过程如下：
[0016]
(201)在当前k帧时刻点云的距离图像中选取所有b
k,l
中两个曲率最大的角点和曲率最小的平面点，构成采样点集p。在采样点集中随机搜取三个不共线的点，同时在k+1帧点云中搜取相对应的点。利用奇异值分解方法svd计算之间的转移矩阵和旋转矩阵。
[0017]
(202)利用求解出来的转移矩阵和旋转矩阵将k帧时刻p点集中剩余的点投影到k+1帧中，并计算与其目标点的欧式距离d
k,n
。采用自适应阈值内点筛选方法设置距离阈值dk，若距离误差d
k,n
小于阈值dk，则判定该点为内点。
[0018][0019]
其中,n为采样点的数量，为所有激光点到模型的平均距离。
[0020]
(203)计算k时刻内点数量，如果内点数量比原模型大，则更新数据集，否则保持不变。
[0021]
(204)根据内点分布权重选取内点构造协方差矩阵c,其计算方法为：
[0022][0023]
其中x
k,l
为b
k,l
的中心，为激光雷达特征点云权重中心，e
k,l
为b
k,l
中内点比重。然后根据方差矩阵c计算其内点分布评分s，若评分比s高时则更新内点集。其中内点分布评分s公式如下：
[0024][0025]
(205)更新迭代次数，设置迭代次数为ks。若k《ks,则继续步骤(2)，否则终止迭代。输出剔除动态点的最优内点集和相对应的转移矩阵。ks的计算公式如下：
[0026][0027]
其中u为整体迭代过程中采集内点的概率，一般根据经验值设置，p
k,l
为前一帧点云中为内点的概率，计算公式为：
[0028][0029]
进一步地，在步骤(3)中，具体过程如下：
[0030]
(301)实时提取当前点云的描述子，与描述子数据库中存储的描述向量寻找相似度最大的关键帧，初步确认当前车辆的位置。
[0031]
(302)选取关键帧构建点云地图并同时提取关键帧点云的描述子构建描述子数据
库。
[0032]
进一步地，在步骤(4)中，具体过程如下：
[0033]
(401)实时提取当前点云的描述子，与描述子数据库中存储的描述向量寻找相似度最大的关键帧，初步确认当前车辆的位置。
[0034]
(402)确定当前车辆的位置后，通过匹配定位算法实现当前关键帧与子地图的匹配并输出当前精准坐标。
[0035]
有益效果：本发明克服了动态环境下动态点对建图和定位精度的影响，寻找了一种能够在前端实时剔除动态点的方法，降低动态物体对定位的影响，能够有效准确地在三维点云地图中精准定位，满足实时性需求。
附图说明
[0036]
图1是本发明的基本流程图。
具体实施方式
[0037]
以下将结合附图，对本发明的技术方案进行详细说明。
[0038]
本发明提出了一种动态环境下地面车辆点云地图快速重定位方法，配准流程图如图1所示，包括如下步骤：
[0039]
步骤一:采集激光雷达点云，获取每帧点云的位姿，对点云数据进行预处理。
[0040]
步骤二：设计基于先验信息的点云粗配准算法用以剔除动态点，减小动态点误匹配给激光雷达里程计带来的误差并提供相对精确的转移矩阵初值用于莱温伯格马夸特(lm)精配准。
[0041]
步骤三：同步进行点云地图构建。在建图阶段构建历史所有关键帧点云的特征描述矩阵、特征描述向量并存储特征点云地图。
[0042]
步骤四：匹配定位阶段，利用提取当前帧的描述子与历史关键帧描述子进行匹配实现快速初始定位，最后采用实时激光雷达定位与建图算法进行后续的精准定位。
[0043]
在本实施例中，步骤一可以采用如下优选方案实现：
[0044]
(1)实时采集激光雷达一帧点云的数据，进行地面点和非地面的分割。并将一帧点云投影成距离图像，然后将点云划分为na×
nb段，其中na为激光雷达线数。然后将na×
nb段的距离图像划分为6个子图像，记b
k,l
为k帧点云子图像中的第l段。
[0045]
使用图像的分割方法将距离图像分割为很多个聚类。同一个聚类的点被标记上唯一的标识并同时根据曲率c的大小选取角点平面点作为特征点，曲率的计算公式如下：
[0046][0047]
其中，分别表示不同两激光点的坐标，s是当前激光点在竖直方向上，左右各找五个点构成的集合。
[0048]
在本实施例中，步骤二可以采用如下优选方案实现：
[0049]
(1)在当前k帧时刻点云的距离图像中选取所有b
k,l
中两个曲率最大的角点和曲率最小的平面点，构成采样点集p。在采样点集中随机搜取三个不共线的点，同时在k+1帧点云
中搜取相匹配的点。利用奇异值分解方法svd计算之间的转移矩阵和旋转矩阵。
[0050]
(2)利用求解出来的转移矩阵和旋转矩阵将k帧时刻p点集中剩余的点投影到k+1帧中，并计算与其目标点的欧式距离d
k,n
。采用自适应阈值内点筛选方法设置距离阈值dk，若距离误差d
k,n
小于阈值dk，则判定该点为内点。
[0051][0052]
其中，n为采样点的数量，为所有激光点到模型的平均距离。
[0053]
(3)计算k时刻内点数量，如果内点数量比原模型大，则更新数据集，否则保持不变。
[0054]
(4)根据内点分布权重选取内点构造协方差矩阵c,其计算方法为：
[0055][0056]
其中x
k,l
为b
k,l
的中心，为激光雷达特征点云权重中心，e
k,l
为b
k,l
中内点比重。然后根据方差矩阵c计算其内点分布评分s，若评分比s高时则更新内点集。其中内点分布评分s计算公式如下：
[0057][0058]
(5)更新迭代次数，设置迭代次数为ks。若k《ks,则继续步骤(2)，否则终止迭代。输出最优内点集和相对应的转移矩阵。
[0059][0060]
其中u为整体迭代过程中采集内点的概率，一般根据经验值设置，p
k,l
为前一帧点云中为内点的概率，其计算公式为：
[0061][0062]
在本实施例中，步骤三可以采用如下优选方案实现：
[0063]
(1)实时提取当前点云的描述子，与描述子数据库中存储的描述向量寻找相似度最大的关键帧，初步确认当前车辆的位置。
[0064]
(2)选取关键帧构建点云地图并同时提取关键帧点云的描述子构建描述子数据库。
[0065]
在本实施例中，步骤四可以采用如下优选方案实现：
[0066]
(1)实时提取当前点云的描述子，与描述子数据库中存储的描述向量寻找相似度最大的关键帧，初步确认当前车辆的位置。
[0067]
(2)确定当前车辆的位置后，通过匹配定位算法实现当前关键帧与子地图的匹配并输出当前精准坐标.
[0068]
以上仅为结合具体的优选实施方式说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

技术特征：
1.一种动态环境下地面车辆点云地图快速重定位方法，其特征在于，包括如下步骤：步骤一:激光雷达采集点云，所述点云包括多个点数据，对所述点云进行预处理；步骤二：设计基于先验信息的点云粗配准算法用以剔除动态的点数据；步骤三：同步进行点云地图构建；在建图阶段构建历史所有关键帧点云的特征描述矩阵、特征描述向量并存储特征点云地图；步骤四：匹配定位阶段，提取当前帧的描述子与历史关键帧描述子进行匹配实现快速初始定位，最后采用实时激光雷达定位与建图算法进行精准定位。2.根据权利要求1所述的一种动态环境下地面车辆点云地图快速重定位方法，其特征在于：步骤一中对所述点云进行预处理的操作具体如下：(1.1)实时采集激光雷达多帧点云，对多帧所述点云进行地面点和非地面点的分割，并将每帧所述点云投影成距离图像，然后将所述距离图像划分为n
a
×
n
b
段，其中n
a
为激光雷达线数，n
b
为投影图像的水平分辨率；再将划分为n
a
×
n
b
段的距离图像划分为6个子图像，记b
k,l
为第k帧距离图像中的第l段；(1.2)使用图像的分割方法将所述距离图像分割为很多个聚类，属于同一个聚类的点数据被标记上唯一的标识并同时根据曲率选取角点数据以及平面点数据作为特征点。3.根据权利要求2所述的一种动态环境下地面车辆点云地图快速重定位方法，其特征在于：步骤二的实现过程为：(2.1)在当前k帧距离图像中选取所有b
k,l
段中两个曲率最大的角点数据和四个曲率最小的平面点数据，构成采样点集；在采样点集中随机搜取三个不共线的点数据，同时在k+1帧距离图像中搜取与这三个不共线点数据相匹配的点数据；利用奇异值分解方法svd计算第k到k+1帧距离图像的帧间转移矩阵和旋转矩阵；(2.2)利用求解出来的转移矩阵和旋转矩阵将第k帧距离图像生成的采样点集中剩余的点数据投影到第k+1帧距离图像生成的采样点集中，同时通过最近邻算法在第k+1帧距离图像搜索与第k帧距离图像距离最近的目标点，并计算投影后的点数据与目标点的欧式距离d
k,n
；采用自适应阈值内点筛选方法设置距离阈值d
k
，若欧式距离d
k,n
小于阈值d
k
，则判定该点数据为内点；其中，n为第k帧距离图像生成的采样点集中点云的数量，为所有激光点到目标点的平均距离；(2.3)计算第k帧距离图像的内点数量，如果内点数量比之前的内点数量多，则更新采样点集，否则保持不变；(2.4)根据内点分布权重选取内点构造协方差矩阵c：
其中x
k,l
为b
k,l
的中心，为激光雷达特征点云权重中心，e
k,l
为b
k,l
中内点比重；然后根据协方差矩阵c计算内点分布评分s，若评分比设定阈值高时则更新采样点集。(2.5)更新迭代次数，设置迭代次数为k
s
，若k<k
s
,则继续步骤(2.2)，若k≥k
s
则终止迭代，输出剔除动态点数据后的内点集和转移矩阵。4.根据权利要求3所述的一种动态环境下地面车辆点云地图快速重定位方法，其特征在于：步骤三的实现过程具体如下：(1)利用剔除动态点数据后的内点集构建点云地图；(2)选取关键帧构建位姿地图并同时提取关键帧点云的描述子构建描述子数据库，其中关键帧是激光雷达采集点云时每隔30米提取具有代表性的点云。5.根据权利要求4所述的一种动态环境下地面车辆点云地图快速重定位方法，其特征在于：步骤四中的点云匹配定位过程具体如下：(1)实时提取当前帧点云的描述子，与所述描述子数据库中存储的描述向量寻找相似度最大的关键帧，初步确认当前车辆的位置；(2)确定当前车辆的位置后，通过匹配定位算法获取当前关键帧与局部地图的匹配定位并输出当前车辆的精准位置。

技术总结
本发明公开了一种动态环境下地面车辆点云地图快速重定位方法。方法包括步骤一：采集激光雷达点云，获取每帧点云的位姿。步骤二：设计基于先验信息的点云粗配准算法用以剔除动态点，并提供相对精确的转移矩阵初值和剔除动态点的点云用于建图。步骤三：同步进行点云地图构建。在建图阶段保存历史所有关键帧点云的描述子，并存储特征点云地图。步骤四：匹配定位阶段，利用当前帧点云的描述符与历史关键帧描述子进行匹配实现快速初始定位，最后采用实时激光雷达定位与建图算法进行后续的精准定位。本发明克服了动态环境下动态点对建图和定位精度的影响，能够有效准确地在三维点云地图中精准定位，满足实时性需求。满足实时性需求。满足实时性需求。


技术研发人员：孟祥华 赵伟
受保护的技术使用者：南京航空航天大学秦淮创新研究院
技术研发日：2023.02.22
技术公布日：2023/7/19