CN113138395A - 一种基于全站仪的激光雷达数据融合的点云地图构建方法 - Google Patents

Abstract

本发明公开了一种基于全站仪的激光雷达数据融合的点云地图构建方法，以图优化算法为基础，采用全站仪定位信息对激光雷达进行信息辅助，构建高精度局部地图，并通过先验特征匹配及站点测量相结合的方式实现局部地图融合，构建激光雷达点云全局地图。本发明能够解决传统地图构建算法因定位误差的累积，进而带来的地图构建精度低的问题；与传统地图构建算法相比，本发明精度高、可控性好。

Description

一种基于全站仪的激光雷达数据融合的点云地图构建方法

技术领域

本发明属于测绘技术领域，特别是涉及一种基于全站仪的激光雷达数据融合的点云地图构建方法。

背景技术

当前无人系统的主流导航方法是先构建激光雷达高精度点云定位地图，以高精度地图为基准能够有效避免位姿解算发散的问题，并通过将传感器采集的实时信息与高精度地图配准的方法实现无人系统的长航时高精度自主定位。

而激光雷达点云地图精度又是保证无人系统自主定位精度的基础，传统的点云地图构建算法通常以卫星定位信息为基础，在无卫星基准的情况下传统算法可靠性较差。

因此，急需一种能够提高地图精度与算法可控性的地图构建方法成为研究人员关注的问题。

发明内容

为了解决上述技术问题，本发明提供一种基于全站仪的激光雷达数据融合的点云地图构建方法，通过全站仪对激光雷达的信息辅助以及先验特征对局部地图航向偏差的抑制，保证了构建地图的全局一致性，相比传统算法具有构图精度高、算法可控性好的特点。

为实现上述目的，本发明提出一种基于全站仪的激光雷达数据融合的点云地图构建方法，具体包括以下步骤：

S1、搭载着激光雷达及全站仪测量棱镜的无人系统在按照预定直线轨迹行驶的过程中采集全站仪测量数据与激光雷达点云数据，并对所述全站仪测量数据与激光雷达点云数据进行融合，得到一幅局部地图；

S2、按照不同的直线行驶轨迹多次重复步骤S1，得到若干幅局部地图；然后在若干幅所述局部地图中分别提取初始帧点云的先验平面特征，并计算各局部点云地图之间的航向偏差；

S3、通过所述全站仪测量棱镜测量各局部地图的位置偏差和所述步骤S1中的各局部点云地图之间的航向偏差，对各幅局部地图进行融合，得到全局地图。

优选地，所述步骤S1具体为：

S11、无人系统按照预定的直线轨迹行驶，并通过所述激光雷达等间隔采集激光雷达点云作为关键帧，得到相邻两个关键帧位姿之间的相对约束；

S12、通过所述全站仪测量棱镜采集全站仪测量数据，得到每个关键帧位姿的观测约束；

S13、将所述相对约束和所述观测约束进行联合优化，构建联合优化函数；

S14、对所述联合优化函数进行线性化处理，得到线性优化函数；

S15、基于所述线性优化函数，求解出每个关键帧点云的优化增量；并利用所述优化增量对原有的关键帧位姿进行更新，得到一幅局部地图。

优选地，所述步骤S13是采用图优化算法对所述相对约束和所述观测约束进行联合优化的。

优选地，所述步骤S2具体为：

S21、根据待建图环境大小设定多条无人系统数据采集的直线轨迹，并重复步骤S1，得到若干幅局部地图；

S22、根据平面特征特性，对若干个所述局部地图中的的初始帧点云进行点云分隔，得到若干个平面点云；

S23、在所述若干个平面点云中提取出占比最大的平面点云，并对其进行平面拟合；

S24、基于步骤S23，计算各局部地图之间的航向偏差。

优选地，所述步骤S22具体为：

S22.1、计算所述初始帧点云的法线，并采用特征直方图思想对所述法线的三维特征进行分析，得到所述初始帧点云的粗聚类；

S22.2、将所述法线的三维特征区间分别等分为m个特征区间，并排列组合形成m³个三维特征区间，得到法线特征直方图；

S22.3、将所述初始帧点云根据法线特征参数投放到不同的特征区间，得到激光雷达三维扫描特征直方图。

优选地，所述步骤S23具体为：

对特征区间内点云进行平面拟合形成平面方程，计算拟合的若干个平面的偏差，当所述偏差小于设定阈值时，将两特征区间内的点云数据合并；然后根据设定的空间模型平面数量选取数据量最大的一组特征区间内的点云通过RANSAC算法滤波去除奇异值，最后对其进行粗拟合形成平面方程。

优选地，所述步骤S3具体为：

S31、利用全站仪测量棱镜测量每个局部点云地图坐标系到全局地图坐标系的位置偏差；

S32、将步骤S31测得的每个局部点云地图坐标系到全局地图坐标系的位置偏差和各个局部点云地图之间的航向偏差进行联合，得到局部点云地图到全局点云地图的转换矩阵；

S33、利用所述转换矩阵，将局部地图进行变换统一到全局地图坐标系中，得到全局地图。

优选地，所述步骤S33具体为：

其中，ξ^global为全局地图点云，

为第k幅局部地图点云，

为第k个局部地图坐标系到全局地图坐标系的转换矩阵。

与现有技术相比，本发明的有益效果在于：

本发明以图优化算法为基础，采用全站仪定位信息对激光雷达进行信息辅助，构建高精度局部地图，并通过先验特征匹配及站点测量相结合的方式实现局部地图融合，构建激光雷达点云全局地图，能够解决传统地图构建算法因定位误差的累积，进而带来的地图构建精度低的问题；与传统地图构建算法相比，本发明精度高、可控性好。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明的方法流程示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

实施例1

参照图1所示，本发明提供一种基于全站仪的激光雷达数据融合的点云地图构建方法，具体包括以下步骤：

S1、搭载激光雷达及全站仪测量棱镜的无人系统按照预定直线轨迹行驶，采集全站仪测量数据与激光雷达点云数据，通过图优化算法进行数据融合，实现局部地图的构建；

S1、按照无人系统运行轨迹，等间隔选取激光雷达点云作为关键帧，并构建相邻关键帧位姿的相对约束F^odom(X^l)，其表达式为：

其中，

为第i个关键帧位姿；

为第j个关键帧位姿；d_ij表示第i、j关键帧位姿之间的观测值，由直接匹配两帧点云获取；

为误差模型，表示激光里程计递推获得的关键帧位姿

以及观测值d_ij之间的偏差；Σ_ij为待优化变量X^l中各元素之间的协方差，其表达式为：

其中，I为单位矩阵，

为待优化变量X^l中各元素之间的位置的协方差；

为待优化变量X^l中各元素之间的旋转角的协方差；σ_pos、σ_rot表示人为设置的状态可信度；e_ij表示经过转换矩阵T(d_ij)转换后的输入点云p_i到目标点云中相应最近点p_j的距离之和，表征了当前观测值配准点云的不匹配度，其表达式为：

其中，N为匹配的点云的点的个数；

S12、根据全站仪测量棱镜采集的位置数据为关键帧位姿提供观测约束F^ob(X^l)，其表达式为：

其中，z_i表示全站仪对第i帧关键帧点位的观测值。

为误差模型，表示第i帧关键帧点位的观测值与第i帧关键帧位姿的偏差。Σ_i表示待优化变量X^l中各元素之间的协方差。具体如下：

其中，

为人为设置的状态可信度。

S13、根据相对约束与观测约束，构建联合优化函数为：

目标即求解F(X^l)最小时对应的待优化变量X^l的状态X^MAP：

其中，F(X^l)是非线性函数。

S14、将联合优化函数进行线性化处理，得到线性优化函数F(X^l+ΔX^l)，其表达式为：

其中，ΔX^l为为关键帧点位的优化增量；

为设定误差函数的初值，则将其进行一阶泰勒展开，可得：

式中：

可将F(X^l+ΔX^l)线性化表示为：

令：

则：

F(X^l+ΔX^l)＝c+2b^TΔX^l+ΔX^lTHΔX^l

S15、求解线性函数F(X^l+ΔX^l)：

按如下式对线性函数F(X^l+ΔX^l)进行求解：

可得：

HΔX^l＝-b

通过上式求取关键帧点位的优化增量ΔX^l，对原关键帧点位进行更新，从而实现对地图的更新，最终构建出高精度局部地图。

S2、在待建图环境中按照不同轨迹多次重复步骤S1，构建多幅局部地图后，提取各局部地图初始帧点云的先验平面特征，计算局部地图之间的航向偏差。

S21、根据待建图环境大小设定多条无人系统数据采集的直线轨迹，并重复步骤一，从而形成多幅局部地图。

S22、根据平面特征特性，对各局部地图初始帧点云(即构建局部地图过程中的第一帧关键帧点云)进行点云分割。

首先对激光雷达点云的法线n_k进行计算。并采用特征直方图思想对法线三维特征快速分析以实现点云的粗聚类。将点云法线的三维特征区间a^sec、b^sec、c^sec分别等分为m个特征区间，即根据法线x,y,z三个轴特征值的范围(即-1～1)人为划分区间，并排列组合形成m³(m×m×m)个三维特征区间，最终构建出法线特征直方图。

第i个特征char_i(0≤i≤m³-1)表示的特征区间为：

其中，

表示点云法向量第一个特征参数的第n₁个特征区间，

表示点云法向量第二个特征参数的第n₂个特征区间，

表示点云法向量第三个特征参数的第n₃个特征区间。并且特征区间的划分遵循归一化原则，有：

将点云根据其法向量特征的数值投放到不同的特征区间，形成激光雷达三维扫描特征直方图，从而实现三维点云的快速粗分割。

S23、提取占比最大的平面点云，并进行平面拟合。

按照下式对特征区间内点云进行平面拟合形成平面方程：

a^plx+b^ply+c^plz+1＝0

其中，x，y，z表示平面方程的参数；

表示一个特征区间内点云各个点的法向量的三个参数；

表示一个特征区间内点云拟合成的平面的法向量的三个参数；t为char_i内点云数据量，d为方程归一化常数。

计算拟合的λ个平面的偏差

其表达式为：

其中，ξ_i、ξ_j分别标识第i个点云和第j个点云表示第拟合出来的平面的法向量。

当

小于阈值δ时，将两特征区间内的点云数据合并。根据设定的空间模型平面数量选取数据量最大的一组特征区间内的点云通过RANSAC算法滤波去除奇异值，随后对其进行粗拟合形成平面方程。

S24、通过平面特征匹配解算局部地图之间的航向偏差。

定义两帧初始帧提取的特征平面系数为

与

则其夹角θ应当满足：

则这两帧局部地图的航向误差角Δθ为：

Δθ＝min(|θ|,|θ-90|)

S3、通过所述全站仪测量棱镜测量各局部地图的位置偏差和所述步骤S1中的各局部点云地图之间的航向偏差，对各幅局部地图进行融合，得到全局地图。

利用全站仪测量棱镜分别确定局部地图坐标系到全局地图坐标系的位置偏差，并联合步骤S2中确定的航向偏差形成转换矩阵，将局部地图通过对应转换矩阵进行变换统一到全局地图坐标系中，形成全局地图。

式中，ξ^global表示全局地图点云，

表示第k幅局部地图点云，

表示第k个局部地图坐标系到全局地图坐标系的转换矩阵。

式中，Δx_k、Δy_k、Δθ_k分别表示第k幅局部地图与全局地图之间的位置、航向偏差。

以上所述的实施例仅是对本发明的优选方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (8)

1.一种基于全站仪的激光雷达数据融合的点云地图构建方法，其特征在于，具体包括以下步骤：

S1、搭载着激光雷达及全站仪测量棱镜的无人系统在按照预定直线轨迹行驶的过程中采集全站仪测量数据与激光雷达点云数据，并对所述全站仪测量数据与激光雷达点云数据进行融合，得到一幅局部地图；

S2、按照不同的直线行驶轨迹多次重复步骤S1，得到若干幅局部地图；然后在若干幅所述局部地图中分别提取初始帧点云的先验平面特征，并计算各局部点云地图之间的航向偏差；

S3、通过所述全站仪测量棱镜测量各局部地图的位置偏差和所述步骤S1中的各局部点云地图之间的航向偏差，对各幅局部地图进行融合，得到全局地图。

2.根据权利要求1所述的基于全站仪的激光雷达数据融合的点云地图构建方法，其特征在于，所述步骤S1具体为：

S11、无人系统按照预定的直线轨迹行驶，并通过所述激光雷达等间隔采集激光雷达点云作为关键帧，得到相邻两个关键帧位姿之间的相对约束；

S12、通过所述全站仪测量棱镜采集全站仪测量数据，得到每个关键帧位姿的观测约束；

S13、将所述相对约束和所述观测约束进行联合优化，构建联合优化函数；

S14、对所述联合优化函数进行线性化处理，得到线性优化函数；

S15、基于所述线性优化函数，求解出每个关键帧点云的优化增量；并利用所述优化增量对原有的关键帧位姿进行更新，得到一幅局部地图。

3.根据权利要求2所述的基于全站仪的激光雷达数据融合的点云地图构建方法，其特征在于，所述步骤S13是采用图优化算法对所述相对约束和所述观测约束进行联合优化的。

4.根据权利要求1所述的基于全站仪的激光雷达数据融合的点云地图构建方法，其特征在于，所述步骤S2具体为：

S21、根据待建图环境大小设定多条无人系统数据采集的直线轨迹，并重复步骤S1，得到若干幅局部地图；

S22、根据平面特征特性，对若干个所述局部地图中的的初始帧点云进行点云分隔，得到若干个平面点云；

S23、在所述若干个平面点云中提取出占比最大的平面点云，并对其进行平面拟合；

S24、基于步骤S23，计算各局部地图之间的航向偏差。

5.根据权利要求4所述的基于全站仪的激光雷达数据融合的点云地图构建方法，其特征在于，所述步骤S22具体为：

S22.1、计算所述初始帧点云的法线，并采用特征直方图思想对所述法线的三维特征进行分析，得到所述初始帧点云的粗聚类；

S22.2、将所述法线的三维特征区间分别等分为m个特征区间，并排列组合形成m³个三维特征区间，得到法线特征直方图；

S22.3、将所述初始帧点云根据法线特征参数投放到不同的特征区间，得到激光雷达三维扫描特征直方图。

6.根据权利要求4所述的基于全站仪的激光雷达数据融合的点云地图构建方法，其特征在于，所述步骤S23具体为：

对特征区间内点云进行平面拟合形成平面方程，计算拟合的若干个平面的偏差，当所述偏差小于设定阈值时，将两特征区间内的点云数据合并；然后根据设定的空间模型平面数量选取数据量最大的一组特征区间内的点云通过RANSAC算法滤波去除奇异值，最后对其进行粗拟合形成平面方程。

7.根据权利要求1所述的基于全站仪的激光雷达数据融合的点云地图构建方法，其特征在于，所述步骤S3具体为：

S31、利用全站仪测量棱镜测量每个局部点云地图坐标系到全局地图坐标系的位置偏差；

S32、将步骤S31测得的每个局部点云地图坐标系到全局地图坐标系的位置偏差和各个局部点云地图之间的航向偏差进行联合，得到局部点云地图到全局点云地图的转换矩阵；

S33、利用所述转换矩阵，将局部地图进行变换统一到全局地图坐标系中，得到全局地图。

8.根据权利要求1所述的基于全站仪的激光雷达数据融合的点云地图构建方法，其特征在于，所述步骤S33具体为：

其中，ξ^global为全局地图点云，

为第k幅局部地图点云，

为第k个局部地图坐标系到全局地图坐标系的转换矩阵。

Images