一种基于多IMU的点线融合定位方法及系统技术方案

本发明专利技术提供一种基于多IMU的点线融合定位方法及系统，方法包括：通过相机和4个相同型号的IMU分别进行图像采集和惯性数据采集；将步骤S1中相机获取到的图像帧进行特征提取；将步骤S1中得到的四个IMU的多组数据分别进行预积分，计算出两帧图像对应的IMU的位置和速度；视觉和IMU初始化；将步骤S4中多个IMU(A、B、C、D)初始化后得到的测量值概率的映射到虚拟IMU(E)上；将线重投影误差建模为线段两端点到投影线的距离，然后最小化点线的重投影误差和IMU残差，获得精确的位姿估计；将本文设计的算法在公共数据集上以及采集的数据集上进行测试。本发明专利技术解决了定位及融合视觉精度低、低纹理场景处理效果差以及线段提取质量差的技术问题。问题。问题。

【技术实现步骤摘要】
一种基于多IMU的点线融合定位方法及系统


[0001]本专利技术涉及SLAM技术中多传感器融合定位领域，具体涉及一种基于多IMU的点线融合定位方法及系统。

技术介绍

[0002]SLAM(Simultaneous Localization and Mapping同步定位和地图构建)技术自提出以来到现在已经有十几年的发展。从当初的基于单特征的纯视觉SLAM方法到现在的基于多传感器信息融合以及基于多特征融合的SLAM方法，SLAM技术逐渐发展成熟起来。SLAM技术主要用于机器人、无人机以及无人车的导航和定位，是人工智能邻域的重要技术之一。目前主流的视觉SLAM技术主要有基于间接法(点特征)的ORB_SLAM和基于直接法的LSD
‑
SLAM、SVO和DSO。其中ORB_SLAM提供了单目、双目和RGB
‑
D三种相机类型接口，包含了跟踪、局部建图和回环检测三大线程，可以保证地图与运动轨迹的全局一致性，是目前视觉SLAM算法中较为完备和成熟的框架，缺点是使用场景有限，定位鲁棒性较差。基于直接法的LSD
‑
SLAM是对图像灰度值进行位姿估计，是直接对图像像素进行操作，而不再对图像进行特征点的提取与匹配，可在CPU上构建半稠密地图，适用于大规模的场景，缺点是对环境的光照变化鲁棒性不高。SVO和DSO都是基于稀疏直接法的SLAM方案，不同的是SVO方案使用混合模型，将直接法和间接法相结合实现位姿估计，而DSO方案没有回环检测功能，不可避免会产生闭环误差。
[0003]随着场景的复杂化以及对精度和实时性上的要求不断提升，纯视觉的SLAM技术出现瓶颈。因此基于多传感器和多特征的SLAM技术快速发展起来。目前使用较多的传感器有相机(单、双目、RGB
‑
D、事件相机)、IMU(惯性测量单元)、轮速计和激光雷达等。场景特征方面除了点特征外还增加了线特征、面特征以及点线融合的特征方案。目前基于优化的视觉和IMU数据融合的VIO(Visual Inertial Odometry)已经有比较成熟的框架，例如香港科技大学的单目VINS
‑
mono方案和ORB_SLAM3中的单目/IMU、双目/IMU方案等。除了这些VIO算法框架外，还有基于多特征融合的SLAM算法，例如PL
‑
SLAM、PL
‑
VINS等等都是点线融合方法的代表。
[0004]以往的方案中大多只是应用单个IMU和点特征的视觉融合来估计机器人的位姿。尽管这样的方案可以为不同的使用案例提供可接受的准确性和鲁棒性，但是通过多个IMU和点线特征融合的视觉融合方案会进一步提高整体性能，使算法在低纹理环境中更具鲁棒性。
[0005]与本专利技术相关的现有技术一——基于点线特征融合的PL
‑
SLAM
[0006]现有技术一的技术方案：
[0007]为了解决低纹理区域中传统特征点法鲁棒性差的问题，2019年Ruben在PL
‑
StVO的基础上更新了PL
‑
SLAM。该算法的点特征提取采用的是ORB特征，线特征提取部分采用的是LSD算法提取特征，采用高斯
‑
牛顿法最小化点和线特征的重投影误差，并且通过统计误差来调整点线的权重。该算法在之前的基础上实现了较为完整的双目视觉SLAM系统。
[0008]现有技术一的缺点：
[0009]该方法没有对特征匹配精度进行分析，用同一种误差模型对两种特征进行误差优化问题的构建，导致不同特征在位姿估计中效果一致。
[0010]此方案依然是纯视觉的SLAM方案，在定位精度上还有上升的空间。
[0011]与本专利技术相关的现有技术二——基于单IMU的VIO方案(ORB_SLAM3和VINS
‑
mono)
[0012]现有技术二的技术方案：
[0013]ORB_SLAM3是一个支持视觉、视觉惯导融合以及混合地图的SLAM系统，可以在单目、双目和RGB
‑
D相机上利用针孔或者鱼眼相机模型运行。它是第一个基于特征的紧耦合的VIO系统，仅依赖于最大后验估计(即使是在IMU初始化时)。该系统不论是在大场景还是小场景下，都能够鲁棒实时运行，并且精度较上一版提升了2到5倍。在建图方面，通过改进的新的重定位模块来构建混合地图，这样使得系统能够在场景中长时间的稳定运行。
[0014]VINS
‑
mono方案是一种基于紧耦合滑动窗口非线性优化方法的单目视觉—惯性系统。该方案提出了包括效果最佳的IMU预积分方法、估计器初始化机制、故障检测和复原机制以及外参在线校订等内容。
[0015]现有技术二的缺点：
[0016]1)两种方案都是目前VIO中比较优秀的、全面的系统框架。但是在传感器融合方面使用的仍是单个的IMU和视觉的融合，虽然目前的精度已经在预期理想范围内，但还可以进一步提升。
[0017]2)两种方案均在视觉特征方面均采用单一的点特征进行跟踪，这对于结构化的低纹理场景不够友好。
[0018]与本专利技术相关的现有技术三——PL
‑
VINS方案
[0019]现有技术三的技术方案：
[0020]PL
‑
VINS方案是一种实时基于点线的单目惯导SLAM方案。该方案依托的是基于点特征的VINS
‑
mono的框架，并在此基础上加入了线特征的提取与匹配。通过对LSD算法的改进，使得运行速度提升为原来的三倍。此外线重投影残差被建模为中点到线的距离。
[0021]现有技术三的缺点：
[0022]1)改进后的LSD算法虽然在直线提取速度上提升较高，但是在线段质量上有所欠缺，还是容易将长线段分割成很多短线段，使得构建的场景地图不够精确。
[0023]2)单个IMU融合视觉，定位精度可以进一步提升。
[0024]此外，申请号为CN202110138834.9的现有专利技术专利《一种基于点线特征融合的双目惯性同时定位与地图构建方法》包括S1、前端视觉惯性里程计；S2、后端优化；S3三维环境建图；在所述前端视觉惯性里程计中，采用双目相机采集相机前方左、右目图片，作为视觉惯性同时定位与地图构建系统的检测分析输入图像。该现有专利基于优化的滑动窗口中有效融合了点、线和IMU数据信息，解决弱纹理环境下点特征视觉SLAM算法精度不高且不稳定的问题，以实现更高精度的位姿估计；该现有专利利用长度抑制、近线合并和断线拼接策略，在保证快速提取的同时，优化传统算法线段提取质量，降低系统线特征的误匹配率，以提取能适应室内弱纹理和无纹理场景；该现有专利在弱纹理和无纹理的室内结构化场景下，提取更多有效的线特征，并与点特征和IMU预积分数据进行多源信息融合，以得到全局一致的相机位姿数据。该现有专利虽然采用双目相机以及点特征和IMU预积分数据进行多
源信息融合，但该现有专利采用长度抑制、近线合并和断线拼接策略，本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于多IMU的点线融合定位方法，其特征在于，所述方法包括：S1、将4IMU阵列集成模块连接相机，采集获取环境图像数据及运动惯性数据；S2、利用ORB算法提取并匹配所述环境图像数据中的点特征，利用线段组合方法定位组合短线段，据以提取出适用长线段，以获取适用LSD算法，所述步骤S2包括：S21、调整预置LSD算法的隐藏参数；S22、设LSD算法输出线段组为：L＝{l1，l2，...，l
n
}(n＝1，2，3...)，其中，l
b
为L中的基线，预置矩形区域R为所述基线l
b
的影响区域；S23、将所述LSD算法输出线段组L中的所有线段按长度降序排列；S24、按照线段直方图的主方向，以下述逻辑选出方向相似的组内线段作为组合预选线段，所述步骤S24包括：S241、设定所述线段的近似标准Φ
l
：Φ
l
＝μ|l
b
|，其中，μ为近似系数；S242、如果计算距离大于所述近似标准Φ
l
，则取消组合机制；S243、否则计算l
b
和l
i
的角度差θ；S244、以下述逻辑确定角度近似标准Φ
θ
，若角度差θ满足角度近似标准Φ
θ
，则组合所述短线段：，式中x、y和ξ均为实验设定参数，α是归一化系数；S25、拼接所述组合预选线段，以得到适用长线段；S3、以预置逻辑获取IMU测量模型，从所述运动惯性数据中获取初始旋转数据，根据所述IMU测量模型及所述初始旋转数据进行IMU预积分；S4、根据所述环境图像数据及所述运动惯性数据初始化视觉和IMU；S5、利用最小二乘估计器随机估计所述4IMU阵列集成模块测得的运动惯性数据，以获取并边缘化IMU间旋转加速度概率，据以融合得到虚拟IMU E；S6、构建优化函数，以将所述IMU间旋转加速度概率映射到所述虚拟IMU E，据以优化点、线特征的重投影误差及所述虚拟IMU E的残差，以提升相机位姿；S7、利用前述步骤所设计逻辑测试EuRoC数据集、YorkUrban数据集及室内数据集，以得到点线融合定位验证结果。2.根据权利要求1所述的一种基于多IMU的点线融合定位方法，其特征在于，所述步骤S1中的所述4IMU阵列集成模块的频率包括：200Hz；所述相机的工作频率包括：10Hz。3.根据权利要求1所述的一种基于多IMU的点线融合定位方法，其特征在于，所述步骤S21包括：S211、利用OpenCV以生成的N层高斯金字塔表示原始图像，下采样所述原始图像N
‑
1次，模糊N次；S212、在所述高斯金字塔的每一层使用LSD算法提取直线；S213、如果线段的封闭矩形支持域中的对齐区域点小于预设阈值，利用所述LSD算法设置最小密度阈值以剔除不可靠线段，其中，设置所述最小密度阈值为0.6以加快处理速度。
4.根据权利要求3所述的一种基于多IMU的点线融合定位方法，其特征在于，所述步骤S212中调整所述高斯金字塔的比例为s＝0.5，层数调整为N＝2。5.根据权利要求1所述的一种基于多IMU的点线融合定位方法，其特征在于，所述步骤S3包括：S31、以下述逻辑处理得到IMU测量模型：w
m
＝w
b
+b
g
+n
g
其中，ω
m
和是陀螺仪和加速度计的测量值，w
b
和a
w
分别代表IMU角速度和线加速度在坐标系b系和w系中表示，其中b系即body系，w系即world系，代表坐标系w和坐标系b之间的变换矩阵，n
a
，n
g
为高斯噪声，b
a
，b
g
为随机游走测量偏差模型，而g
w
为w系下已知的重力向量；S32、以下述逻辑对IMU积分：S32、以下述逻辑对IMU积分：S32、以下述逻辑对IMU积分：其中分别是对应IMU从b
k
到b
k+1
时刻的位置，速度和姿态四元数，为四元数乘法，这里假设加速度观测a
m
和旋转矩阵在Δt的时间内是不变的；S33、以下述逻辑使所述步骤S32中的等式两边同时乘以将积分项中的旋转矩阵变成初值为单位阵，以消除对初始旋转的依赖：的依赖：的依赖：，其中，不依赖初始状态的IMU预积分项包括：，其中，不依赖初始状态的IMU预积分项包括：
6.根据权利要求1所述的一种基于多IMU的点线融合定位方法，其特征在于，所述步骤S4中对所述环境图像数据及所述运动惯性数据进行纯视觉的最大后验估计、纯IMU的最大后验估计以及VI的最大后验估计，以找到系统初始惯性系下的重力方向、初始速度及陀螺仪和加速度计偏置。7.根据权利要求1所述的一种基于多IMU的点线融合定位方法，其特征在于，所述步骤S5包括：S51、利用所述IMU测量模型定义IMU状态向量和误差状态定义：其中以下述逻辑定义所述IMU状态向量：，其中，表示从IMU坐标系(b系)到世界坐标系的四元数变换；S52、以下述逻辑描述IMU连续时间运动动力学：S53、以下述逻辑定义所述误差状态：S54、以下述逻辑定义四元数局部误差S54、以下述逻辑定义四元数局部误差，其中，代表反对称矩阵，为的估计值；S55、以下述逻辑处理得到预测状态估计的非线性方程和线性化的描述误差状态的方程以估计IMU概率：以估计IMU概率：，式中，F和G表示连续时间线性化的状态转移矩阵和噪声雅克比矩阵，n
c
是过程噪声，服从高斯分布；S56、以所述虚拟IMU E的测量值代替步骤S55中的陀螺仪测量值ω
m
和加速度计测量值a
m
，以生成所述虚拟IMU E，其中，所述步骤S56中的所述虚拟IMU E的陀螺仪和加速度计的测量值采用下述逻辑生成：ω
mV
＝N
...

【专利技术属性】
技术研发人员：樊渊，樊富友，韩可，杨国志，董翔，周俊，宋铁旭，袁文博，
申请(专利权)人：安徽大学，
类型：发明
国别省市：