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

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

【技术实现步骤摘要】
一种基于全站仪的激光雷达数据融合的点云地图构建方法
本专利技术属于测绘
，特别是涉及一种基于全站仪的激光雷达数据融合的点云地图构建方法。
技术介绍
当前无人系统的主流导航方法是先构建激光雷达高精度点云定位地图，以高精度地图为基准能够有效避免位姿解算发散的问题，并通过将传感器采集的实时信息与高精度地图配准的方法实现无人系统的长航时高精度自主定位。而激光雷达点云地图精度又是保证无人系统自主定位精度的基础，传统的点云地图构建算法通常以卫星定位信息为基础，在无卫星基准的情况下传统算法可靠性较差。因此，急需一种能够提高地图精度与算法可控性的地图构建方法成为研究人员关注的问题。
技术实现思路
为了解决上述技术问题，本专利技术提供一种基于全站仪的激光雷达数据融合的点云地图构建方法，通过全站仪对激光雷达的信息辅助以及先验特征对局部地图航向偏差的抑制，保证了构建地图的全局一致性，相比传统算法具有构图精度高、算法可控性好的特点。为实现上述目的，本专利技术提出一种基于全站仪的激光雷达数据融合的点云地图构建方法，具体包括以下步骤：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个特征区间，并排列组合形成m3个三维特征区间，得到法线特征直方图；S22.3、将所述初始帧点云根据法线特征参数投放到不同的特征区间，得到激光雷达三维扫描特征直方图。优选地，所述步骤S23具体为：对特征区间内点云进行平面拟合形成平面方程，计算拟合的若干个平面的偏差，当所述偏差小于设定阈值时，将两特征区间内的点云数据合并；然后根据设定的空间模型平面数量选取数据量最大的一组特征区间内的点云通过RANSAC算法滤波去除奇异值，最后对其进行粗拟合形成平面方程。优选地，所述步骤S3具体为：S31、利用全站仪测量棱镜测量每个局部点云地图坐标系到全局地图坐标系的位置偏差；S32、将步骤S31测得的每个局部点云地图坐标系到全局地图坐标系的位置偏差和各个局部点云地图之间的航向偏差进行联合，得到局部点云地图到全局点云地图的转换矩阵；S33、利用所述转换矩阵，将局部地图进行变换统一到全局地图坐标系中，得到全局地图。优选地，所述步骤S33具体为：其中，ξglobal为全局地图点云，为第k幅局部地图点云，为第k个局部地图坐标系到全局地图坐标系的转换矩阵。与现有技术相比，本专利技术的有益效果在于：本专利技术以图优化算法为基础，采用全站仪定位信息对激光雷达进行信息辅助，构建高精度局部地图，并通过先验特征匹配及站点测量相结合的方式实现局部地图融合，构建激光雷达点云全局地图，能够解决传统地图构建算法因定位误差的累积，进而带来的地图构建精度低的问题；与传统地图构建算法相比，本专利技术精度高、可控性好。附图说明为了更清楚地说明本专利技术实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本专利技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。图1为本专利技术的方法流程示意图。具体实施方式下面将结合本专利技术实施例中的附图，对本专利技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本专利技术一部分实施例，而不是全部的实施例。基于本专利技术中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本专利技术保护的范围。为使本专利技术的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本专利技术作进一步详细的说明。实施例1参照图1所示，本专利技术提供一种基于全站仪的激光雷达数据融合的点云地图构建方法，具体包括以下步骤：S1、搭载激光雷达及全站仪测量棱镜的无人系统按照预定直线轨迹行驶，采集全站仪测量数据与激光雷达点云数据，通过图优化算法进行数据融合，实现局部地图的构建；S1、按照无人系统运行轨迹，等间隔选取激光雷达点云作为关键帧，并构建相邻关键帧位姿的相对约束Fodom(Xl)，其表达式为：其中，为第i个关键帧位姿；为第j个关键帧位姿；dij表示第i、j关键帧位姿之间的观测值，由直接匹配两帧点云获取；为误差模型，表示激光里程计递推获得的关键帧位姿以及观测值dij之间的偏差；Σij为待优化变量Xl中各元素之间的协方差，其表达式为：其中，I为单位矩阵，为待优化变量Xl中各元素之间的位置的协方差；为待优化变量Xl中各元素之间的旋转角的协方差；σpos、σrot表示人为设置的状态可信度；eij表示经过转换矩阵T(dij)转换后的输入点云pi到目标点云中相应最近点pj的距离之和，表征了当前观测值配准点云的不匹配度，其表达式为：其中，N为匹配的点云的点的个数；S12、根据全站仪测量棱镜采集的位置数据为关键帧位姿提供观测约束Fob(Xl)，其表达式为：其中，z本文档来自技高网...

【技术保护点】
1.一种基于全站仪的激光雷达数据融合的点云地图构建方法，其特征在于，具体包括以下步骤：/nS1、搭载着激光雷达及全站仪测量棱镜的无人系统在按照预定直线轨迹行驶的过程中采集全站仪测量数据与激光雷达点云数据，并对所述全站仪测量数据与激光雷达点云数据进行融合，得到一幅局部地图；/nS2、按照不同的直线行驶轨迹多次重复步骤S1，得到若干幅局部地图；然后在若干幅所述局部地图中分别提取初始帧点云的先验平面特征，并计算各局部点云地图之间的航向偏差；/nS3、通过所述全站仪测量棱镜测量各局部地图的位置偏差和所述步骤S1中的各局部点云地图之间的航向偏差，对各幅局部地图进行融合，得到全局地图。/n

【技术特征摘要】
1.一种基于全站仪的激光雷达数据融合的点云地图构建方法，其特征在于，具体包括以下步骤：
S1、搭载着激光雷达及全站仪测量棱镜的无人系统在按照预定直线轨迹行驶的过程中采集全站仪测量数据与激光雷达点云数据，并对所述全站仪测量数据与激光雷达点云数据进行融合，得到一幅局部地图；
S2、按照不同的直线行驶轨迹多次重复步骤S1，得到若干幅局部地图；然后在若干幅所述局部地图中分别提取初始帧点云的先验平面特征，并计算各局部点云地图之间的航向偏差；
S3、通过所述全站仪测量棱镜测量各局部地图的位置偏差和所述步骤S1中的各局部点云地图之间的航向偏差，对各幅局部地图进行融合，得到全局地图。


2.根据权利要求1所述的基于全站仪的激光雷达数据融合的点云地图构建方法，其特征在于，所述步骤S1具体为：
S11、无人系统按照预定的直线轨迹行驶，并通过所述激光雷达等间隔采集激光雷达点云作为关键帧，得到相邻两个关键帧位姿之间的相对约束；
S12、通过所述全站仪测量棱镜采集全站仪测量数据，得到每个关键帧位姿的观测约束；
S13、将所述相对约束和所述观测约束进行联合优化，构建联合优化函数；
S14、对所述联合优化函数进行线性化处理，得到线性优化函数；
S15、基于所述线性优化函数，求解出每个关键帧点云的优化增量；并利用所述优化增量对原有的关键帧位姿进行更新，得到一幅局部地图。


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


4.根据权利要求1所述的基于全站仪的激光雷达数据融合的点云地图构建方法，其特征在于，所述步骤S2具体为：
S21、根据待建图环境大小设定多条无人系统数据采集的直线轨迹，并重复步骤S1，得到若干幅局部地图；
S22、根据平面特征特性，对若干个所述局部地图中的的初始帧点云进行点云分隔，得到若干个平面点云；
S2...

【专利技术属性】
技术研发人员：石昌俊，赖际舟，吕品，蔡玉良，向林浩，蔡小飞，沙建东，温烨贝，何洪磊，李志敏，
申请(专利权)人：南京鹏畅科技实业有限公司，南京航空航天大学，中国船级社，
类型：发明
国别省市：江苏;32