CN116228969A - 一种基于多传感器融合的林区定位与三维重建方法及系统 - Google Patents

Abstract

本发明实施例公开了一种基于多传感器融合的林区定位与三维重建方法及系统。该系统由单目相机、惯性测量单元和单线激光雷达组成，将单线激光雷达采集到的位于极坐标系下的二维激光数据，投影至雷达坐标系下生成三维激光点；继而通过螺旋线修正模型消除运动畸变误差，再结合单目视觉与惯性测量单元融合的全局定位信息，转换至世界坐标系下，生成位于关键帧处的三维点云数据；随后采用点云稠密化模型补齐帧间缺失的点云数据，得到林区初始点云；最后利用融合点云数据改进的校正优化算法，检测回还，消除累计误差，生成全局一致的林区三维点云地图。本发明可以获取林区树木的生长以及分布情况并在林区资源的保护中发挥着至关重要的作用。

Description

一种基于多传感器融合的林区定位与三维重建方法及系统

技术领域

本发明涉及林区信息监测技术领域，具体涉及一种基于多传感器融合的林区定位与三维重建方法及系统。

背景技术

森林一直以来是地球上分布范围最广，物质结构层次最为复杂的陆地生态系统。森林的重要性不仅在于调节气候、保持水土、维持全球生态系统平衡，还作为野生动物赖以生存的环境，并能为人类提供大量的林产品资源，已然成为国家战略性资源。同时，随着“数字地球”等概念的提出，“智慧林业”也逐步步入大家的视野。其最大的特征即是数字化，而林区的环境信息采集则是构成“智慧林业”数字化的重点。

在人工智能的背景下，三维重建技术的兴起，可以有效解决传统的林区环境获取信息方式效率且精度较低的问题。三维重建是指对三维物体建立适合计算机表示和处理的数学模型，具有很高的实用价值，在军事、医学、航空航天、机器人、农林业等领域被广泛应用。在林业领域中，基于三维重建技术重构林区树木表型特征，从而获取树木胸径以及森林树高等信息，提高了林业环境监测的精度与作业效率。然而，林区环境的三维重建与其他领域并不相同，其因为场景范围大、环境复杂、枝干树叶等其他特征物的干扰，导致需对其设计专门的三维重建设备。

随着同步定位与地图构建技术发展，激光雷达、深度相机等硬件技术的蓬勃进步，三维重建开始逐步利用点云数据，形成林区树木完整的三维模型。其中，激光雷达因其稳定性好，精度高，受周边环境影响小，获得林业监测装备领域的广泛关注。但是单线激光雷达建图因其硬件设备限制，只能获取当前激光雷达所处平面的二维信息，而无法得到空间三维信息。

发明内容

针对现有技术存在的上述缺陷，本发明实施例的目的在于提供一种基于多传感器融合的林区定位与三维重建方法及系统。

为实现上述目的，第一方面，本发明实施例提供了一种基于多传感器融合的林区定位与三维重建方法，包括：

S1，通过单目相机获取图像信息，并基于所述图像信息构建视觉重投影误差，为位姿估价函数提供视觉约束；

S2，通过惯性测量单元获取测量数据，并基于所述测量数据得到IMU预积分误差，为位姿估价函数提供惯性约束；

S3：将所述视觉投影误差和IMU预积分误差进行紧耦合融合，并经非线性优化后得到位姿估计结果；

S4：通过单线激光雷达获取原始雷达数据，并根据所述原始雷达数据生成雷达坐标系下的三维激光点；

S5：根据所述位姿估计结果和三维激光点进行位姿转换，得到世界坐标系下的三维点云数据；

S6：基于回环校正算法和所述三维点云数据生成林区的全局三维地图。

作为本申请的一种具体实现方式，步骤S1具体为：

通过单目相机获取图像信息，对所述图像信息进行特征点提取及追踪，建立相邻帧之间的视觉投影误差。

作为本申请的一种具体实现方式，步骤S4具体为：

通过单线激光雷达在二维扫描平面内采集角度、距离；

将角度、距离进行空间投影、螺旋线矫正，得到雷达坐标系下的三维激光点。

作为本申请的一种具体实现方式，步骤S5具体为：

根据所述位姿估计结果，结合单目相机与激光雷达的相对位姿关系，计算单线激光雷达坐标系{L}相对于世界坐标系{W}的位姿变换矩阵

即可实现将当前雷达坐标系的三维激光点转换至世界坐标系下形成三维点云数据。

作为本申请的一种具体实现方式，步骤S6具体为：

针对所述三维点云数据，在关键帧之间将空间点云进行插值稠密化处理，形成林区初始点云；

根据来自单目相机的图像信息和林区初始点云进行回环校正，生成林区的全局三维地图。

第二方面，本发明实施例提供了一种基于多传感器融合的林区定位与三维重建系统，包括：

单目相机，用于系统在林区移动时，获取图像信息，并基于所述图像信息构建视觉重投影误差，为位姿估价函数提供视觉约束；

惯性测量单元，用于系统在林区移动时，获取测量数据，并基于所述测量数据得到IMU预积分误差，为位姿估价函数提供惯性约束；

所述惯性测量单元，还用于将所述视觉投影误差和IMU预积分误差进行紧耦合融合，并经非线性优化后得到位姿估计结果；

单线激光雷达，用于：

获取原始雷达数据，并根据所述原始雷达数据生成雷达坐标系下的三维激光点；

根据所述位姿估计结果和三维激光点进行位姿转换，得到世界坐标系下的三维点云数据；

基于回环校正算法和所述三维点云数据生成林区的全局三维地图。

第三方面，本发明实施例提供了另一种基于多传感器融合的林区定位与三维重建系统，包括：

单目相机，用于提供单目视觉信息，基于该单目视觉信息构建空间三维坐标点和像素坐标点之间的映射关系，形成相邻帧之间的匹配特征点，以帮助系统粗略估计相机运动，以及构建视觉重投影误差，为位姿估价函数提供视觉约束；

惯性测量单元，用于在视觉约束的基础上，增加高频惯性测量数据提供的惯性约束，形成预积分误差；

单线激光雷达，用于与单目相机和惯性测量单元协同，建立林区的全景三维地图。

与现有技术相比，本发明实施例的有益效果体现在：

围绕单线激光雷达进行林区三维点云重建展开研究，同时对整个林区SLAM系统的准确性与鲁棒性进行了提升。

此外，本发明可以获取林区树木的生长以及分布情况并在林区资源的保护中发挥着至关重要的作用。

附图说明

为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍。在所有附图中，类似的元件或部分一般由类似的附图标记标识。附图中，各元件或部分并不一定按照实际的比例绘制。

图1为本发明实施例提供的基于多传感器融合的林区定位与三维重建方法的一种流程图；

图2为本发明实施例提供的基于多传感器融合的林区定位与三维重建方法的另一种流程图；

图3为世界坐标系与像素坐标系的转换示意图；

图4为视觉重投影误差示意图；

图5为林区三维点云重建过程示意图；

图6为三维点云稠密化模型示意图；

图7为三维点云回环模型示意图；

图8为本发明实施例提供的基于多传感器融合的林区定位与三维重建系统的结构图。

图9为本发明实施例提供的基于多传感器融合的林区定位与三维重建系统的工作示意图。

具体实施方式

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

应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”和“包含”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。

为了解决背景技术中所描述的问题，本发明围绕单线激光雷达进行林区三维点云重建展开研究，同时对整个林区SLAM系统的准确性与鲁棒性进行提升。本文系统采用单目相机、惯性测量单元传感器和单线激光雷达多传感器的融合，进而引出了本文所提出的一种融合单目相机、惯性测量单元传感器和单线激光雷达数据的林区三维点云重建系统，从理论上分析了本文系统在林区建图上的精度与鲁棒性。其运用到了视觉观测模型和惯性测量单元观测模型，并基于两种观测模型计算其相应误差项，即视觉重投影误差与惯性测量单元预积分误差；随后计算融合两者误差项的位姿代价函数，运用非线性优化算法，得到三维点云重建系统的全局定位信息。结合基于单目视觉与惯性测量单元融合的定位结果，本文将平面信息进行投影、螺旋线修正，接着转换至世界坐标系下形成三维点云数据，采用稠密化模型，增加融合点云数据改进的回环检测校正，最终生成全局一致的三维点云地图。

请参考图1及图2，本发明实施例提供的基于多传感器融合的林区定位与三维重建方法，可以包括如下步骤：

S1，通过单目相机获取图像信息，并基于所述图像信息构建视觉重投影误差，为位姿估价函数提供视觉约束。

具体地，通过单目相机获取图像信息，对所述图像信息进行特征点提取及追踪，建立相邻帧之间的视觉投影误差。

S2，通过惯性测量单元获取测量数据，并基于所述测量数据得到IMU预积分误差，为位姿估价函数提供惯性约束。

具体地，根据惯性测量单元传感器采集到的陀螺仪与加速度计算其预积分误差。

S3：将所述视觉投影误差和IMU预积分误差进行紧耦合融合，并经非线性优化后得到位姿估计结果。

在本实施例中，涉及三个坐标系，分别是：单目相机坐标系{C}、IMU坐标系{B}和激光雷达坐标系{L}。这三个坐标系相对固定，可以通过外参标定直接获得{C}、{B}、{L}之间的相对位姿变换

同时各传感器通过系统时间打上相应时间戳，进行时间同步。

请参考图3，其为世界坐标系与像素坐标系的转换过程，视觉SLAM根据相机返回的图像数据进行处理从而获得位姿信息，首先需要经过相机模型的映射，将空间中的三维坐标点投影至二维像素平面。而不同相机所采用的映射模型也有差异，如针孔映射模型、鱼眼映射模型。本文根据林区点云重建系统所采用的单目相机型号，选用基于针孔模型的空间映射关系。

如图3所示，以采集设备初始时刻位姿作为固定的世界坐标系{W}，设在此坐标系下存在一空间点P_W＝[X,Y,Z]^T。

首先通过采集设备自身的全局位姿变换矩阵T_w，将此空间点转换至相机坐标系{C}下形成三维坐标P_C＝[X′,Y′,Z′]^T。

随后，将其在Z轴方向上进行归一化处理，即在Z轴方向上坐标化为1，形成归一化平面坐标P_N＝[X″,Y″,1]^T。

然而，相机的归一化平面与理论上并不可能完全重合，其原因有二：1、透镜形状的不同会改变光传播的路线，造成径向畸变，真实环境中的直线在图像中变成了曲线；2、透镜与成像平面无法保持完全平行，造成切向畸变，成像点与实际点产生偏移。因此，归一化平面坐标还需进行畸变校正：

其中，

k₁、p₁、p₂表示去畸变参数，可通过相机标定获得。

经上述过程得到去畸变后的归一化平面坐标，最后利用相机内参矩阵K投影到像素平面，形成像素平面坐标p＝[u,v]^T。

于是，将空间点P_W经视觉观测模型映射至像素坐标p的过程可表示为：

sp＝K(RP_w+t)＝KT_wP_w

其中，s表示空间点深度信息，即为空间点Z坐标；矩阵K称为相机的内参矩阵，其表示相机出厂时自身带有的性质，相机内参K一般通过棋盘法离线标定所得，也可通过在线估计的方式来获取；T_w称为相机外参矩阵，其包括一个3x3旋转矩阵R和一个3x1的平移向量t，R和t称为相机外参数。

上述过程展示了相机二维图像的成像过程，即将世界坐标系下的空间点P_w映射形成二维像素坐标[u,v]^T。同时，也是林区点云重建系统对林区环境信息的观测模型。

进一步地，视觉重投影误差可获得帧与帧之间的位姿变换矩阵，但由于视觉里程计快速运动时易造成特征点丢失，惯性测量单元存在偏差，且积分时位姿易发散。因此，本实施例中，采用紧耦合的方式，联合视觉与预积分误差构建代价函数。误差项的度量采用了二范数，将位姿优化问题转化为了最小二乘问题，最终采用高斯牛顿法，将代价函数中每个误差项式进行一阶泰勒展开，得到其增量方程从而进行计算。

预积分误差具体为惯性测量单元在相邻两帧视觉关键帧之间的观测模型，并以第k帧和第k+1帧的状态量作为优化变量，其两帧之间的位置、速度、姿态积分作为约束引导优化方向。本实施例也将采用VINS-Mono中预积分的方式。

其中，对于重投影误差的处理请参考图4，在林区信息采集过程中，假设存在世界坐标系下三维点P，被前后两帧k，k+1观测到，经单目相机模型投影至像素点p₁、p₂，且通过经光流法组成匹配特征点对，两帧之间的位姿变换即是需要求解的变量。

如图4所示，空间点P映射在第k帧图像上的像素点为p₁，与其匹配的在第k+1帧图像上的像素点为p₂，与此同时，p₁经由两帧间变换矩阵

在第k+1帧图像上形成重投影点p₂ ^′。由于相邻帧的变换矩阵是不确定的，p₂、p₂ ^′则存在一个误差e，即重投影误差：

e＝p₂-p₂ ^′

视觉重投影误差是位姿估计代价函数中重要的视觉约束，同一空间点P在相邻两帧中存在误差e。若是多帧中同时观测到空间点P，即每一关键帧时刻都将存在重投影误差e。在实际林区三维点云重建中，将每一帧每一个特征点的误差项式求和，构建视觉误差的最小二乘代价函数

式中,

为第k帧第p个特征点的三维空间坐标；/>

为第k+1帧第p个特征点像素平面下的坐标，以齐次坐标表示；/>

表示从第k帧重投影到第k+1帧的转换矩阵，由位置/>

与姿态/>

组成；K为相机内参；F表示所有图像关键帧，P表示在所有图像帧中提取的特征点。于是，通过上述过程，利用林区图像得到视觉重投影误差，为多传感器融合的位姿估计提供了视觉约束。/>

需要说明的是，上述过程提到的基于单目视觉与惯性测量单元紧耦合的定位方式，相较于基于单一视觉传感器的定位而言，融合了惯性测量单元运动信息，增加了一种数据观测，可提高位姿估计精度。同时，当光照条件恶劣或是快速运动时导致缺少视觉特征，帧间位姿追踪失败，惯性测量单元则可在短时间内以高频率发布位姿，维持系统定位，提升了系统的鲁棒性。

且，融合后的定位信息作用于单线激光雷达，实现了将二维平面信息转化为整个三维空间点云，算法复杂度低保证了实时性。

S4：通过单线激光雷达获取原始雷达数据，并根据所述原始雷达数据生成雷达坐标系下的三维激光点。

具体地，通过单线激光雷达的陀螺仪、加速度计等在二维扫描平面内采集角度、距离，对其进行空间投影、螺旋线矫正等预处理，得到雷达坐标系下的三维激光点。

S5：根据所述位姿估计结果和三维激光点进行位姿转换，得到世界坐标系下的三维点云数据。

具体地，根据所述位姿估计结果，结合单目相机与激光雷达的相对位姿关系，计算单线激光雷达坐标系{L}相对于世界坐标系{W}的位姿变换矩阵

即可实现将当前雷达坐标系的三维激光点转换至世界坐标系下形成三维点云数据。

S6：基于回环校正算法和所述三维点云数据生成林区的全局三维地图。

具体地，针对所述三维点云数据，在关键帧之间将空间点云进行插值稠密化处理，形成林区初始点云；

根据来自单目相机的图像信息和林区初始点云进行回环校正，生成林区的全局三维地图。

综上，林区三维点云的重建过程可如图5所示，结合基于单目视觉与IMU融合的实时定位结果，再结合激光雷达与其他传感器之间通过外参标定已有的相对位姿关系，得到单线激光雷达自身在空间世界坐标系下的位姿信息。于是结合全局位姿估计结果，将二维激光数据点进行处理，即可增量式获得全局一致的三维激光点云地图。

进一步地，请参考图5，先将视觉关键帧处的二维激光数据进行投影得到激光雷达坐标系下的三维激光点，利用位姿估计矩阵对初始三维激光点进行螺旋线修正，并结合位姿估计结果转换至世界坐标系下的三维点云数据；

随后在关键帧之间将空间点云进行插值稠密化，形成林区初始点云地图；最后融合视觉与点云数据进行回环修正，生成全局一致的林区三维点云地图。

需要说明的是，前文所选择的方法只在视觉关键帧之处接收当前点云数据，这就造成了点云在相邻两帧关键帧第k帧和第k+1帧之间的数据缺失。于是，本发明提出相邻帧之间的点云稠密化模型，如图6所示，获取相邻关键帧之处的位姿信息与相应林区空间点云数据，并计算得出两帧间共缺失的激光点云数量。

针对位姿矩阵中的R和t，采用不同插值算法，作用于两帧处点云的局部特征，以此对两帧间的缺失点云数据进行拟合，获得林区的初始点云。

单线激光雷达在位于第k帧和第k+1帧处时，分别基于单目视觉与惯性测量单元融合获取当前帧的定位信息，且利用前述的空间坐标转换形成位于相邻帧处的三维点云数据。但在关键帧之间，虽然激光数据得以保留，但都是极坐标系下的角度与距离，无法获取其全局定位信息，也就无法转换至世界坐标系下形成三维点云，这就造成了相邻帧之间的点云数据缺失，只能获取稀疏的林区三维点云地图。

为了更好地获取林区环境特征，且能较好地对树木进行三维重建，形成连续的树木表面，本实施例提出了一种位于视觉关键帧之间的点云稠密化模型，如图6所示。实线表示真实存在的位姿，即是单线激光雷达位于第k帧和第k+1帧时的位姿状态，而虚线表示的两帧间的位姿状态，由首尾两帧计算获得。同时，将第k帧处的点云数据，通过两帧间不同的位姿状态转换至世界坐标系下，以此补充帧间环境信息。

其中，在每一帧关键帧处，单线激光雷达在世界坐标系下的全局位姿变换矩阵均可知。设在第k帧和第k+1帧处单线激光雷达所处的全局位姿为T_k和T_k+1，位姿矩阵包含旋转矩阵R与平移向量k。为了获取相邻两帧间每一激光数据的位姿信息，本文采用不同插值策略对T_k、T_k+1插值。针对旋转矩阵R，其常采用四元数表示，因此采用非线性球面插值；针对平移向量t，则采用线性插值。于是，两帧间激光数据所处于的位姿为：

其中，R_k表示在第k帧关键帧扫描初始时刻系统所处的旋转矩阵，t_k表示在第k帧关键帧扫描初始时刻系统所处的平移向量；n表示在两关键帧之间需补齐的点云总数；θ_k表示第k帧关键帧出激光雷达所处的角度；l表示在当前第k帧和第k帧之间第l个点云。于是，两帧间的激光数据在世界坐标系下形成三维点云数据，补充两帧间的数据缺失，形成初始点云地图。

进一步地，上述提及的三维点云的回环校正如图7所示，在检测到与当前帧满足一致性检验的回环帧之后，先构建当前帧与回环帧之间位姿约束，得到两帧的位姿变化；随后将当前帧与回环帧之间的各关键帧位置进行校正优化至正确位置。检测到闭环帧之后，建立当前帧K_cur与回环帧K_loop之间精准匹配的特征点对，将K_cur的位姿与K_loop位姿对齐，并根据本文所提出的位姿优化，将K_cur与K_loop间形成的重投影误差与惯性测量单元预积分误差作为约束条件，进行紧耦合优化并得到校正矩阵

于是当前帧K_cur处的三维点云数据校正至：

/>

上述过程只校正了当前帧的点云信息，但K_cur与K_loop之间的点云信息依旧存在长时间的累计误差。本发明采用VINS-Mono中4自由度位姿优化，以当前帧与回环帧之间关键帧处的位置t与航向角ψ作为优化变量，以快速收敛获取准确值。这是由于VINS-Mono中惯性测量单元传感器可提供世界坐标系下的重力g，横滚角φ与俯仰角θ均是可观测的，不存在漂移现象，而t与ψ在世界参考系下只是相对的，易造成累计误差。假设在K_loop与K_cur两帧中间任意一帧K_j的位姿状态经由视觉与惯性测量单元融合的位姿估计已知，本文在第K_j关键帧与相邻K_j+1关键帧之间建立位姿约束，分别获取其准确的旋转R与平移t，航向角ψ则由旋转矩阵获取。建立第K_j关键帧与相邻K_j+1关键帧关于航向角与平移之间的误差：

上式中，优化变量(t_j,ψ_j)表示在第K_j关键帧处需参与优化的平移向量与航向角；

表示只优化航向角的旋转矩阵；/>

θ、ψ分别表示横滚角、俯仰角和航向角；上角标/>

表示为准确值不参与优化过程；/>

表示第j与第j+1帧之间由多传感器融合的位姿估计获得的平移向量，/>

则由位姿估计中旋转矩阵获取其航向角。

同样，在当前帧与回环帧之间由于回环帧修正，也会产生位姿约束。由两帧间需参与优化的平移与航向角(t_cur,ψ_cur)、(t_loop,ψ_loop)，与校正矩阵

获得的两帧间准确的平移与航向角/>

形成位姿误差函数：

上述过程只是基于视觉关键帧的回环约束，也是当前视觉SLAM中传统的回环校正过程。但是运用在三维点云重建系统中会遇到位姿产生回环优化，但点云无法得到快速收敛的问题。究其原因还是点云数据与位姿信息松耦合导致的。

于是，本发明提出了一种紧耦合点云数据的回环校正优化算法。在检测到回环之后，取当前帧时刻n圈点云数据构成的点云单元，与过去回环帧时刻n圈点云数据构成的点云单元，进行点云配准，采用ICP算法中point-to-point计算匹配点对之间距离，构建当前帧三维点云与回环帧三维点云误差函数，为点云回环校正提供三维点云约束：

上式中，

是当前帧处点云单元与回环帧处点云单元中相匹配的最邻近点；/>

则是需优化的第n个局部点云中第i个匹配点对之间平移向量与航向角。于是，本文通过以平移向量t和航向角ψ作为优化变量，构建整个回环优化的目标函数：/>

其中，大括号内即为回环校正的损失函数，其包括三种约束：一种是在回环过程中相邻关键帧之间的位姿约束，一种是当前关键帧与回环关键帧之间的位姿约束，最后一种是当前关键帧处局部点云与回环关键帧处局部点云配准约束。S是回环过程中间相邻帧的集合，L是回环帧的集合，N是局部点云集合，I是局部点云中所有匹配点的集合。ρ()表示Huber损失函数，可排除因为错误闭环约束引起的游离点对位姿校正的影响。通过高斯牛顿法求解，即可得到每一帧的校正矩阵T_j(cor)。在校正位姿的同时，也同时将偏移的三维点云校正至：

这样，当视觉帧形成回环优化更新后，关键帧处所有对应的点云都将进行相应的更新，再次投影至正确的点云单元，消除累计误差，从而使整个点云地图得到校正优化，生成全局一致的三维点云地图，进一步提高地图构建的精度。

从以上描述可以得知，本发明实施例的有益效果主要体现在：

围绕单线激光雷达进行林区三维点云重建展开研究，同时对整个林区SLAM系统的准确性与鲁棒性进行了提升。

请参考图8，本发明实施例提供了一种基于多传感器融合的林区定位与三维重建系统，包括：

单目相机，用于系统在林区移动时，获取图像信息，并基于所述图像信息构建视觉重投影误差，为位姿估价函数提供视觉约束；

惯性测量单元，用于系统在林区移动时，获取测量数据，并基于所述测量数据得到IMU预积分误差，为位姿估价函数提供惯性约束；

所述惯性测量单元，还用于将所述视觉投影误差和IMU预积分误差进行紧耦合融合，并经非线性优化后得到位姿估计结果；

单线激光雷达，用于：

获取原始雷达数据，并根据所述原始雷达数据生成雷达坐标系下的三维激光点；

根据所述位姿估计结果和三维激光点进行位姿转换，得到世界坐标系下的三维点云数据；

基于回环校正算法和所述三维点云数据生成林区的全局三维地图。

其中，根据所述原始雷达数据生成雷达坐标系下的三维激光点，具体为：

通过单线激光雷达在二维扫描平面内采集角度、距离；

将角度、距离进行空间投影、螺旋线矫正，得到雷达坐标系下的三维激光点。

其中，根据所述位姿估计结果和三维激光点进行位姿转换，具体为：

根据所述位姿估计结果，结合单目相机与激光雷达的相对位姿关系，计算单线激光雷达坐标系{L}相对于世界坐标系{W}的位姿变换矩阵

即可实现将当前雷达坐标系的三维激光点转换至世界坐标系下形成三维点云数据。

可选地，在本发明的另一优选实施例中，基于多传感器融合的林区定位与三维重建系统，包括：

单目相机，用于提供单目视觉信息，基于该单目视觉信息构建空间三维坐标点和像素坐标点之间的映射关系，形成相邻帧之间的匹配特征点，以帮助系统粗略估计相机运动，以及构建视觉重投影误差，为位姿估价函数提供视觉约束；

惯性测量单元，用于在视觉约束的基础上，增加高频惯性测量数据提供的惯性约束，并进行紧耦合融合，同时在两帧间进行积分传播，转变为基于前一视觉帧的预积分量的传播，从而为位姿估计代价函数提供惯性约束；

单线激光雷达，用于与单目相机和惯性测量单元协同，建立林区的全景三维地图。主要通过发射激光点束并接收，计算时间差得到距离值，利用三角测距获取控件特征物离传感器自身的角度与距离，并以激光点的数据格式进行发布，随后通过点云配准和点云回环等处理手段进行建图。

具体而言，是利用非线性优化算法融合视觉重投影误差和预积分误差，得到世界坐标系下的实时位姿估计，并将此定位结果与单线激光雷达获得的激光数据结合，构建三维点云地图。

需要说明的是，系统在林区移动时，其工作示意图如图9所示，单目相机和惯性测量单元分别获取图像信息和惯性测量单元信息，并由两者融合得到系统的全局里程计信息，单线激光雷达则获得激光点云平面。为保证三维点云地图疏密分布均匀，本文将相机水平放置获取正前方图像，单线激光雷达竖直放置纵向扫描，以使激光点云平面与全局里程计信息始终保持垂直。

针对回环校正问题，本实施例中是基于词袋模型实现的。所述词袋模型的回环检测算法需构建与视觉特征相关的字典，字典由很多单词组成，每一个单词代表一个概念，是多个相似的特征组成的一个集合，用以描述两幅图像的相似性。在进行林区三维点云重建前，先于林区空间采集多张图像，采用视觉特征描述子提取图像特征，并集合多个相似的特征点作为单词，构建多个单词形成离线检索字典。同时，为提高图像相似性判断效率，将词典问题转换为特征聚类问题，并以树的结构展开，形成特征点的KD-Tree。

此外，该系统的更为详细的工作流程，请参考前述方法实施例部分，在此不再赘述。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (10)

1.一种基于多传感器融合的林区定位与三维重建方法，其特征在于，包括：

S1，通过单目相机获取图像信息，并基于所述图像信息构建视觉重投影误差，为位姿估价函数提供视觉约束；

S2，通过惯性测量单元获取测量数据，并基于所述测量数据得到IMU预积分误差，为位姿估价函数提供惯性约束；

S3：将所述视觉投影误差和IMU预积分误差进行紧耦合融合，并经非线性优化后得到位姿估计结果；

S4：通过单线激光雷达获取原始雷达数据，并根据所述原始雷达数据生成雷达坐标系下的三维激光点；

S5：根据所述位姿估计结果和三维激光点进行位姿转换，得到世界坐标系下的三维点云数据；

S6：基于回环校正算法和所述三维点云数据生成林区的全局三维地图。

2.如权利要求1所述的方法，其特征在于，步骤S1具体为：

通过单目相机获取图像信息，对所述图像信息进行特征点提取及追踪，建立相邻帧之间的视觉投影误差。

3.如权利要求1所述的方法，其特征在于，所述预积分误差具体为惯性测量单元在相邻两帧视觉关键帧之间的观测模型，并以第k帧和第k+1帧的状态量作为优化变量，其两帧之间的位置、速度、姿态积分作为约束引导优化方向。

4.如权利要求1所述的方法，其特征在于，步骤S4具体为：

通过单线激光雷达在二维扫描平面内采集角度、距离；

将角度、距离进行空间投影、螺旋线矫正，得到雷达坐标系下的三维激光点。

5.如权利要求1所述的方法，其特征在于，步骤S5具体为：

根据所述位姿估计结果，结合单目相机与激光雷达的相对位姿关系，计算单线激光雷达坐标系{L}相对于世界坐标系{W}的位姿变换矩阵

即可实现将当前雷达坐标系的三维激光点转换至世界坐标系下形成三维点云数据。

6.如权利要求1所述的方法，其特征在于，步骤S6具体为：

针对所述三维点云数据，在关键帧之间将空间点云进行插值稠密化处理，形成林区初始点云；

根据来自单目相机的图像信息和林区初始点云进行回环校正，生成林区的全局三维地图。

7.一种基于多传感器融合的林区定位与三维重建系统，其特征在于，包括：

单目相机，用于系统在林区移动时，获取图像信息，并基于所述图像信息构建视觉重投影误差，为位姿估价函数提供视觉约束；

惯性测量单元，用于系统在林区移动时，获取测量数据，并基于所述测量数据得到IMU预积分误差，为位姿估价函数提供惯性约束；

所述惯性测量单元，还用于将所述视觉投影误差和IMU预积分误差进行紧耦合融合，并经非线性优化后得到位姿估计结果；

单线激光雷达，用于：

获取原始雷达数据，并根据所述原始雷达数据生成雷达坐标系下的三维激光点；

根据所述位姿估计结果和三维激光点进行位姿转换，得到世界坐标系下的三维点云数据；

基于回环校正算法和所述三维点云数据生成林区的全局三维地图。

8.如权利要求7所述的系统，其特征在于，根据所述原始雷达数据生成雷达坐标系下的三维激光点，具体为：

通过单线激光雷达在二维扫描平面内采集角度、距离；

将角度、距离进行空间投影、螺旋线矫正，得到雷达坐标系下的三维激光点。

9.如权利要求7所述的系统，其特征在于，根据所述位姿估计结果和三维激光点进行位姿转换，具体为：

根据所述位姿估计结果，结合单目相机与激光雷达的相对位姿关系，计算单线激光雷达坐标系{L}相对于世界坐标系{W}的位姿变换矩阵

即可实现将当前雷达坐标系的三维激光点转换至世界坐标系下形成三维点云数据。

10.一种基于多传感器融合的林区定位与三维重建系统，其特征在于，包括：

单目相机，用于提供单目视觉信息，基于该单目视觉信息构建空间三维坐标点和像素坐标点之间的映射关系，形成相邻帧之间的匹配特征点，以帮助系统粗略估计相机运动，以及构建视觉重投影误差，为位姿估价函数提供视觉约束；

惯性测量单元，用于在视觉约束的基础上，增加高频惯性测量数据提供的惯性约束，形成预积分误差；

单线激光雷达，用于与单目相机和惯性测量单元协同，建立林区的全景三维地图。

Images