基于三维激光雷达的无人机多场景定位建图方法

摘要

本发明提供一种基于三维激光雷达的无人机多场景定位及三维地图建图方法，在室内环境下无GPS信号时搭建基于三维激光传感器的SLAM算法进行定位与三维地图构建，在室外环境下以RTK提供的位置信息为主进行三维地图构建，在RTK丢失GPS信号后由SLAM进行辅助定位。实现室内外三维地图的快速构建，为环境三维地图获取提供了便捷高效的方法，很大程度提高室内外环境三维地图构建的效率；在图优化SLAM算法的理论基础上，完成了基于三维激光传感器的无人机平台SLAM算法构建，实现了无GPS信号环境下环境三维地图的构建，解决了无人机在室内定位以及室内室外定位融合问题，实现了多场景无人机定位的需求，在获取高精度位姿信息的基础上，融合激光数据，完成三维地图的构建。

权利要求

1.基于三维激光雷达的无人机多场景定位建图方法，其特征在于，包括：前端初步定位，将三维激光传感器采集的每一帧点云数据按照定义的算法进行提取边缘和面特征对相邻两帧数据的特征进行匹配，求解出相邻两帧数据的位姿变化量，进而对无人机的位姿进行初步计算；

后端优化定位，将前端数据按位姿和特征构建图，添加图中节点的约束关系，依据优化算法，优化前端定位数据；

回环检测，通过三维激光传感器所获取数据提取特征，与已建立的地图特征之间进行相似性检测与匹配，通过与高相似性的地图进行匹配，求解位姿变换，优化无人机全局定位误差；

所述后端优化定位的过程包括：

对无人机的观测方程与运动方程进行泰勒展开，并将泰勒展开式的二次型部分与一次型部分进行整合得到误差函数表达式 ；

对误差函数表达式中的逆矩阵 进行稀疏化，去除逆矩阵 中的环境特征信息，构建一个只与位姿信息有关的矩阵 ；

通过g2o库以Levenberg-Marquardt算法对误差函数进行优化，优化变量为无人机的姿态，通过优化使误差函数达到最小值以获取最优姿态估计。

2.根据权利要求1所述的基于三维激光雷达的无人机多场景定位建图方法，其特征在于，还包括：当无人机处于室外环境且具有GPS信号时，RTK进入固定解，将RTK差分定位系统数据、惯性测量单元数据、三维激光传感器数据进行融合，融合过程包括时间对齐及空间对齐；

时间对齐，即给传感器组的每帧数据赋予时间戳，若传感器组中任意两个传感器时间差小于设定值，则认为三个传感器数据是同一时刻数据，所述传感器组包括：三维激光传感器、惯性测量单元及RTK差分定位系统；

空间对齐，即将三维激光传感器的每一帧数据由机体坐标系变换到固定的世界坐标系。

3.根据权利要求2所述的基于三维激光雷达的无人机多场景定位建图方法，其特征在于，还包括：当无人机处于室外环境时且GPS信号丢失时，SLAM定位与RTK定位依据RTK解的精度和SLAM特征数量进行融合，当RTK无解时SLAM定位的权重为1，当RTK解的状态由无解逐步到达固定解时逐步增大RTK定位的权重，当RTK固定解时RTK定位的权重为1，当RTK无解且SLAM特征匹配80%收敛时，位置信息由SLAM提供，当SLAM特征匹配不收敛与RTK无GPS信号情况下，停止三维地图构建。

4.根据权利要求3所述的基于三维激光雷达的无人机多场景定位建图方法，其特征在于，所述前端初步定位的过程包括：提取边缘点和平面点；

依据提取的边缘点特征集及平面点特征集并通过迭代最近邻算法求解相邻两帧的位姿变换矩阵，其中，迭代初始变换值通过惯性测量单元经扩展卡尔曼滤波后获取。

说明书

基于三维激光雷达的无人机多场景定位建图方法

技术领域

[0001] 本发明涉及一种基于三维激光雷达的无人机多场景定位建图方法，用于无人机多场景三维点云地图构建，适用于无人机室内定位与三维地图构建、室外定位与模型构建、用于无人机救援、无人机测绘技术领域。

背景技术

[0002] 随着无人机的兴起，基于无人机平台的三维地图构建也逐步走向人们的视野，环境的3D点云模型能够很好的表述环境中物体的位置信息，传统复杂大场景三维地图的获取需要借助专业的设备，不仅耗时而且成本昂贵。无人机因其易控制灵活的特征，在三维地图建立中具有巨大潜力，在无人机三维场景构建中，以建模场景划分可大致分为室外有GPS信号和室内无GPS信号两种情况。

[0003] 室外位置信息可由GPS获取，而室内无GPS信号的环境下的定位一直是一个难点，一般通过复杂的算法进行定位，其中，SLAM算法占据主导地位，但是传统基于滤波的SLAM算法难以适应复杂大场景。除此之外，在室外环境下不得不面临一个问题就是GPS信号丢失问题，GPS信号丢失导致无人机失去位置信息将无法构建环境三维地图，为克服这一问题，需要其他定位信息来辅助定位。因此，目前利用无人机的环境三维地图构建方法由于过于复杂导致存在构建效率低的技术问题，且室内定位与室内室外无法进行融合，无法适应多场景。目前无人机定位及三维地图构建尚处于起步阶段，适应多场景更是一个十分具有前景的研究，将无人机用在三维地图构建，在一定程度上会推进三维地图构建行业的发展。

发明内容

[0004] 为解决上述技术问题，本发明提供一种基于三维激光雷达的无人机多场景定位建图方法。为了对披露的实施例的一些方面有一个基本的理解，下面给出了简单的概括。该概括部分不是泛泛评述，也不是要确定关键／重要组成元素或描绘这些实施例的保护范围。其唯一目的是用简单的形式呈现一些概念，以此作为后面的详细说明的序言。

[0005] 本发明采用如下技术方案：

[0006] 在一些可选的实施例中，提供一种基于三维激光雷达的无人机多场景定位建图方法，包括：

[0007] 前端初步定位，将三维激光传感器采集的每一帧点云数据按照定义的算法进行提取边缘和面特征对相邻两帧数据的特征进行匹配，求解出相邻两帧数据的位姿变化量，进而对无人机的位姿进行初步计算；

[0008] 后端优化定位，将前端数据按位姿和特征构建图，添加图中节点的约束关系，依据优化算法，优化前端定位数据；

[0009] 回环检测，通过三维激光传感器所获取数据提取特征，与已建立的地图特征之间进行相似性检测与匹配，通过与高相似性的地图进行匹配，求解位姿变换，优化无人机全局定位误差。

[0010] 在一些可选的实施例中，所述的基于三维激光雷达的无人机多场景定位建图方法，还包括：当无人机处于室外环境且具有GPS信号时，RTK进入固定解，将RTK差分定位系统数据、惯性测量单元数据、三维激光传感器数据进行融合，融合过程包括时间对齐及空间对齐；时间对齐，即给传感器组的每帧数据赋予时间戳，若传感器组中任意两个传感器时间差小于设定值，则认为三个传感器数据是同一时刻数据，所述传感器组包括：三维激光传感器、惯性测量单元及RTK差分定位系统；空间对齐，即将三维激光传感器的每一帧数据由机体坐标系变换到固定的世界坐标系。

[0011] 在一些可选的实施例中，所述的基于三维激光雷达的无人机多场景定位建图方法，还包括：当无人机处于室外环境时且GPS信号丢失时，SLAM定位与RTK定位依据RTK解的精度和SLAM特征数量进行融合，当RTK无解时SLAM定位的权重为1，当RTK解的状态由无解逐步到达固定解时逐步增大RTK定位的权重，当RTK固定解时RTK定位的权重为1，当RTK无解且SLAM特征匹配80%收敛时，位置信息由SLAM提供，当SLAM特征匹配不收敛与RTK无GPS信号情况下，停止三维地图构建。

[0012] 在一些可选的实施例中，所述前端初步定位的过程包括：提取边缘点和平面点；依据提取的边缘点特征集及平面点特征集并通过迭代最近邻算法求解相邻两帧的位姿变换矩阵，其中，迭代初始变换值通过惯性测量单元经扩展卡尔曼滤波后获取。

[0013] 在一些可选的实施例中，所述后端优化定位的过程包括：对无人机的观测方程与运动方程进行泰勒展开，并将泰勒展开式的二次型部分与一次型部分进行整合得到误差函数表达式 ；对误差函数表达式中的逆矩阵 进行稀疏化，去除逆矩阵 中的环境特征信息，构建一个只与位姿信息有关的矩阵 ；通过g2o库以Levenberg-Marquardt算法对误差函数进行优化，优化变量为无人机的姿态，通过优化使误差函数达到最小值以获取最优姿态估计。

[0014] 在一些可选的实施例中，所述的基于三维激光雷达的无人机多场景定位建图方法，还包括：三维激光传感器采集环境点云信息；惯性测量单元采集无人机姿态信息；RTK差分定位系统采集无人机处于室外环境时的位置信息。

[0015] 本发明所带来的有益效果：通过三维激光传感器对室内外环境进行探测结合定位算法实现三维场景的快速构建，为环境三维地图获取提供了简单高效的方法，很大程度提高室内外环境三维地图构建的效率；在图优化SLAM算法的理论基础上，完成了无人机平台的SLAM算法构建，实现了无GPS信号环境下环境三维地图的构建，解决了无人机在室内定位以及室内室外定位融合问题，适应多场景定位与建图作业。

[0016] 为了上述以及相关的目的，一个或多个实施例包括后面将详细说明并在权利要求中特别指出的特征。下面的说明以及附图详细说明某些示例性方面，并且其指示的仅仅是各个实施例的原则可以利用的各种方式中的一些方式。其它的益处和新颖性特征将随着下面的详细说明结合附图考虑而变得明显，所公开的实施例是要包括所有这些方面以及它们的等同。

附图说明

[0017] 图1是本发明基于三维激光雷达的无人机多场景定位建图方法的流程示意图。

[0018] 图2是本发明算法结构示意图。

具体实施方式

[0019] 以下描述和附图充分地示出本发明的具体实施方案，以使本领域的技术人员能够实践它们。

[0020] 其他实施方案可以包括结构的、逻辑的、电气的、过程的以及其他的改变，实施例仅代表可能的变化，除非明确要求，否则单独的部件和功能是可选的，并且操作的顺序可以变化，一些实施方案的部分和特征可以被包括在或替换其他实施方案的部分和特征。

[0021] 本发明的实施方案的范围包括权利要求书的整个范围，以及权利要求书的所有可获得的等同物。

[0022] 如图1和2所示，在一些说明性的实施例中，提供一种基于三维激光雷达的无人机多场景定位建图方法。

[0023] 三维地图是通过激光的点云数据描述环境的几何结构，实现点云对空间精确表述，构建环境三维地图，需要时刻获取无人机的高精度位姿信息，在获取位姿信息的基础上融合激光的点云的数据进行三维场景的构建。

[0024] 无人机高精度的位姿信息的获取，分为室内无GPS信号和室外有GPS信号两个基础场景。在室内无GPS信号的情况下，以GraphSLAM为基础理论搭建基于三维激光传感器的无人机SLAM算法完成无人机的室内位姿信息的获取；在室外场景下，通过RTK差分定位系统与惯性测量单元获取位姿信息；RTK定位与SLAM算法定位融合，通过RTK解的状态和SLAM提取的特征数量进行融合，主要实现在室外环境下RTK丢失信号后由SLAM进行辅助定位。

[0025] 所述的基于三维激光雷达的无人机多场景定位建图方法，包括：

[0026] 101：采集数据。三维激光传感器采集环境点云信息，惯性测量单元采集无人机姿态信息，RTK差分定位系统采集无人机处于室外环境时的位置信息。

[0027] 三维激光传感器的传感器信息通过网口传输至工控机，惯性测量单元的传感器信息通过串口传输至工控机，RTK差分定位系统的传感器信息通过USB连接至工控机。

[0028] 通过工控机内定位建图算法对传感器数据进行处理得到位姿和环境地图信息。

[0029] 102：室内定位建图。当无人机处于室内环境下时，无GPS信号，因此无法提供准确的定位信息，且室内环境下点云的线面特征丰富，因此搭建SLAM算法为核心进行定位与三维地图构建。

[0030] 室内定位建图主要分为三个部分，分别为前端初步定位、后端优化定位、回环检测。

[0031] 室内定位建图以图优化SLAM算法为核心，前端依据迭代最近邻算法增量式的求解位姿，后端负责优化前端的位姿数据以减小累积误差。

[0032] 前端初步定位，将三维激光传感器采集的每一帧点云数据按照定义的算法进行提取边缘和面特征对相邻两帧数据的特征进行匹配，求解出相邻两帧数据的位姿变化量，进而对无人机的位姿进行初步计算。在前端部分，三维激光传感器的数据采集频率设为10Hz，每帧数据的点云数量可以达到相邻两帧激光数据具有很高的特征相似度。

[0033] 前端初步定位的具体过程如下：

[0034] 首先，提取边缘点和平面点。特征线和面提取按照点的平滑度 来提取，当时将该点选为边缘点，当 时将该点选为平面点，其中， 为线特征点平滑度阈值， 为面特征点平滑度阈值。

[0035] 然后，依据提取的边缘点特征集 及平面点特征集 ，通过迭代最近邻算法求解相邻两帧的位姿变换矩阵 。

[0036] 其中，在迭代最近邻算法最近点的搜索过程中利用k－d tree提升搜索效率，迭代初始变换值通过惯性测量单元经扩展卡尔曼滤波后获取。

[0037] 最后，在迭代最近邻算法框架下，通过矩阵奇异值分解方法对所述位姿变换矩阵进行求解，其中，误差函数表达式为：

[0038] ；

[0039] 。

[0040] 当误差函数表达式达到预设迭代次数或误差小于预设阈值停止迭代。

[0041] 后端优化定位，将前端数据按位姿和特征构建图，添加图中节点的约束关系，依据优化算法，优化前端定位数据。后端主要负责优化前端数据以减小全局的累积误差，依据图论算法，通过前端数据构建图。图中的节点为前端的位姿和在每个位姿下三维激光传感器观测到的环境特征，节点之间的约束分为位姿节点之间约束以及位姿与环境特征之间的约束。

[0042] 后端优化定位的具体过程如下：

[0043] 假设无人机运动和观测方程为高斯模型，则待优化总的误差函数如下式：。

[0044] 无人机的运动方程 如下：

[0045] ；

[0046] ； 。

[0047] 无人机的观测方程h如下：

[0048] ；

[0049] ；

[0050] 。

[0051] 其中 和 为运动与观测的高斯噪声, 为ICP求解的位姿， 为 时刻三维激光传感器观测到的第 个环境特征的位置信息。

[0052] 首先，对无人机的观测方程与运动方程进行泰勒展开，并将泰勒展开式的二次型部分与一次型部分进行整合得到简化的误差函数表达式：

[0053] 。

[0054] 其次，对误差函数表达式中的逆矩阵 进行稀疏化，去除逆矩阵 中的环境特征信息，构建一个只与位姿信息有关的矩阵 。

[0055] 误差协方差矩阵的逆矩阵 度量了运动模型和观测模型的不确定性即运动与观测噪声的大小，其包括位姿与位姿之间的关系以及位姿和环境特征之间的误差相关性。

[0056] 随着环境特征增多矩阵的维数会急剧增加，为降低计算量满足实时性要求，需要对逆矩阵 进行稀疏化，主要通过去除逆矩阵 中的环境特征信息，构建一个只与位姿信息有关的矩阵 ，从而达到降低计算量保持实时性。依据舒尔消元， 逆矩阵稀疏化如下所示：

[0057] ；

[0058] 。

[0059] 最后，通过g2o库以Levenberg-Marquardt算法对误差函数进行优化，优化变量为无人机的姿态，通过优化使误差函数达到最小值以获取最优姿态估计。

[0060] 当前端迭代最近邻算法匹配失败时会产生较大定位误差，这对后端优化会产生很大影响从而影响定位精度。回环检测，通过三维激光传感器所获取数据提取特征，与已建立的地图特征之间进行相似性检测与匹配，通过与高相似性的地图进行匹配，求解位姿变换，优化无人机全局定位误差，获取一个相对固定的参考变换，达到降低累积误差的目的。在建图过程中为提升效率，将匹配较好的数帧地图构建成一个局部地图，建图的过程即可视为局部地图累积构建的过程，回环检测通过当前帧数据与已存在的局部地图进行相似性检测，以达到回环检测减小全局误差。

[0061] 进行坐标系变换，坐标系变换涉及两个坐标系即机体坐标系与世界坐标系-站心坐标系，其中站心坐标系的原点设在无人机起飞的位置，两个坐标系之间的关系如下式：

[0062] 。

[0063] 其中旋转矩阵 三个旋转变量roll、pitch、yaw与平移矩阵 三个变量x、y、z均由经过优化过后的所得位姿的变化量。

[0064] 103：室外定位建图。

[0065] 传感器组包括：三维激光传感器、惯性测量单元及RTK差分定位系统，输出频率分别设为10HZ、50HZ、5HZ。

[0066] 在室外有GPS信号环境下进行三维地图构建，当RTK进入固定解时即可看成定位已知的建图问题，将RTK差分定位系统数据、惯性测量单元数据、三维激光传感器数据进行融合。

[0067] 融合过程包括时间对齐及空间对齐。

[0068] 其中，时间对齐，即给传感器组的每帧数据赋予时间戳，若传感器组中任意两个传感器时间差小于设定值，则认为三个传感器数据是同一时刻数据。

[0069] 其中，空间对齐，即将三维激光传感器的每一帧数据由机体坐标系变换到固定的世界坐标系。

[0070] 104：无人机处于室外且GPS信号丢失时的定位建图，SLAM定位与RTK定位两者依据RTK解的精度和SLAM特征数量进行融合以适应场景变化。当无人机处于室外环境时且GPS信号丢失时，SLAM定位与RTK定位依据RTK解的精度和SLAM特征数量进行融合，当RTK无解时SLAM定位的权重为1，当RTK解的状态由无解逐步到达固定解时逐步增大RTK定位的权重，当RTK固定解时RTK定位的权重为1，当RTK无解且SLAM特征匹配80%收敛时，位置信息由SLAM提供，当SLAM特征匹配不收敛与RTK无GPS信号情况下，停止三维地图构建。

[0071] 融合的表达式如下：

[0072] 。

[0073] 公式中 是融合后的位置信息， 是RTK位置信息， 是SLAM算法的位置信息， 是权重。

[0074] 本领域技术人员还应当理解，结合本文的实施例描述的各种说明性的逻辑框、模块、电路和算法步骤均可以实现成电子硬件、计算机软件或其组合。

[0075] 为了清楚地说明硬件和软件之间的可交换性，上面对各种说明性的部件、框、模块、电路和步骤均围绕其功能进行了一般地描述。

[0076] 至于这种功能是实现成硬件还是实现成软件，取决于特定的应用和对整个系统所施加的设计约束条件。

[0077] 熟练的技术人员可以针对每个特定应用，以变通的方式实现所描述的功能，但是，这种实现决策不应解释为背离本公开的保护范围。