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

说明书

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

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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