CN110874569B - 一种基于视觉惯性融合的无人机状态参数初始化方法 - Google Patents

Abstract

本发明公开了一种基于视觉惯性融合的无人机状态参数初始化方法，针对纯视觉初始化阶段，对将要进入最终全局优化的特征点进行筛选，保留处于图像中部的特征点，对靠近图像边缘的特征点不进行优化，减少需要优化的特征点数目与该阶段的运算规模，保留特征点的跟踪准确度较高，系统精度变化可接受。针对视觉‑惯性校准阶段，在该阶段不再估计各个初始关键帧的速度向量，只估计重力向量与尺度，将原算法的34维方程降为16维方程，降低了方程的求解压力，减少初始化运行时间。本发明在优化迭代之前加入简单闭式求解模型，以减少迭代次数，大幅减少计算时长，具有较强的通用性，可以适用于各种结构的视觉惯性初始化系统，以提升运算效率。

Description

一种基于视觉惯性融合的无人机状态参数初始化方法

技术领域

本发明属于无人机状态估计算法设计技术领域，具体涉及一种基于视觉惯性融合的无人机状态参数初始化方法。

背景技术

随着近年来无人机技术的迅猛发展，针对估计自身位姿的相关技术也在同步发展。其中，典型技术是通过将相机数据与惯性测量单元数据融合建立联立方程，求解初始参数，并通过后端求解器不断迭代实现精准位姿估计。因此，具备短时间、高精度的参数初始化方法在自身状态估计过程中尤为重要，也成为一个热点问题。

目前获得精确的初始状态参数一般分两大类：闭式解法和优化迭代算法。闭式解法采用解析求解线性方程组的方式进行初始状态参数估计，优化迭代算法一般使用最小二乘法迭代求解参数的最优值。

现有闭式求解初始化参数的方法，只需要3帧图和1个特征点即可完成部分初始状态参数的估计，该方法之后又增加了陀螺仪偏置模型，对原闭式解进行校正，提升了参数估计的精度。但单纯的闭式求解方法一般都存在着精度不够的问题，如果低精度的初始状态参数加入到了基于优化框架的估计方法中，可能会使得结果不收敛或者结果收敛很慢。

相对于闭式解法，使用优化的方法迭代次数更少、收敛更快、精度更高。现有一种基于ORB-SLAM框架的视觉惯性融合初始化方法，利用IMU预积分值以及视觉测量值之间的关系建立约束函数，通过迭代求解机身初始位姿以及IMU初始状态的所有参数。但该初始化算法时间很长，一般需要10秒钟左右，不适用于在一开始就需要进行里程计工作的应用场合。现有一种使用线性估计的初始化方法，忽略了陀螺仪偏置的作用，而且未构造传感器噪声模型，借鉴ORB-SLAM的做法，分步来求解陀螺仪偏差、尺度、速度与重力等信息，同时考虑到加速度计偏置相对于重力加速度影响很小，在初始化过程中忽略了加速度计偏置的影响，对之前的初始化算法进行了改进，在精度和鲁棒性方面取得了较好的效果。在初始化过程中没有使用IMU预积分方法，而是将视觉估计出的位姿进行微分，得到角速度与加速度，并与IMU测量值进行比对，构建误差，并提出在频域上进行数据对齐，但这同时也存在着需要检验待微分函数是否可以微分计算的问题。

发明内容

本发明所要解决的技术问题在于针对上述现有技术中的不足，提供一种基于视觉惯性融合的无人机状态参数初始化方法，通过将相机图像及惯性测量单元(InertialMeasurement Unit，IMU)数据融合，能够更准确地估计机体位姿。

本发明采用以下技术方案：

一种基于视觉惯性融合的无人机状态参数初始化方法，包括纯视觉初始化阶段和视觉惯性校准阶段两步，其中，在纯视觉初始化阶段，将要进入最终全局BundleAdjustment优化的特征点进行筛选，只保留处于图像中部的特征点；在视觉惯性校准阶段，只估计重力向量与尺度，取连续3个关键帧k、k+1和k+2作为一组关键帧组建立计算方程，并在一个滑动窗口内选取最新的6帧关键帧作为建立关键帧组的基础对象，确定最终计算维度为16维，采用LDLT矩阵分解法求解方程，完成初始化。

具体的，纯视觉初始化阶段具体为：

先对极约束求解两帧间位姿；然后三角化两帧间特征点；再使用PnP方法求解位姿；对三角化其余特征点；使用PnP方法求解其他位姿；对特征点进行筛选；组合采用全局Bundle Adjustment进行优化。

进一步的，对特征点进行筛选中，设置筛选阈值将图像长/宽维度上距离图像边缘不足1/10图像长/宽的特征点全部筛掉，保留剩余特征点。

进一步的，采用全局Bundle Adjustment进行优化具体为：

S1071、进入函数后，首先判断是否已遍历所有特征点，如果已遍历，则直接求解BA优化函数后结束；如果没有遍历所有特征点，进入步骤S1072；

S1072、选取一个未经处理的特征点，选取观测到该特征点并未在该点下处理的关键帧，进行筛选；

S1073、判断步骤S1072的特征点是否在该关键帧的边缘，如果不是，则将数据写入BZ优化函数，判断是否已遍历所有关键帧，如果已遍历，则返回步骤S1071，如果未遍历，则返回步骤S1072。

具体的，视觉惯性校准阶段具体为：

S201、通过陀螺仪偏置校准；

S202、分别对重力向量和尺度进行初始化；

S203、重新进行关联模型推导，仅估算重力向量和尺度，待估计状态量仅为(3+1)维，较原算法的(3n+3+1)维数大幅减少；

S204、采用LDLT矩阵分解法求解方程，完成初始化。

进一步的，步骤S204具体为：

S2041、进入函数，判断是否已遍历所有关键帧，如果已遍历所有关键帧，则使用LDLT方法求解总方程组，然后取关键帧组求解结果的平均值后结束；

S2042、如果没有遍历所有关键帧，选取下一个关键帧；

S2043、对步骤S2042确定的当前关键帧列写方程，左乘系数矩阵并写入到最终系数矩阵的对应位置，然后返回步骤S2041。

与现有技术相比，本发明至少具有以下有益效果：

本发明一种基于视觉惯性融合的无人机状态参数初始化方法，对将要进入BA优化的特征点进行筛选，只保留在图像中部的特征点，对靠近图像边缘的特征点不进行优化，减少了需要优化的特征点数，减少了纯视觉初始化阶段的运算规模与时间，并且保留下来的特征点跟踪准确度较高，系统的精度下降可接受；通过分析原算法缺点，不估算原算法中的冗余量，并提出关键帧组的思路进行参数计算，能够在保证精度的同时，将系统求解维度降低1倍；根据仿真结果，本发明算法不会对原算法稳定性产生影响，鲁棒性依然有保证。

进一步的，通过使用Lucas-Kanade光流法用于特征点跟踪。但Lucas-Kanade光流法是基于“灰度不变假设”，这使得特征点在跟踪过程中很容易发生丢失、误跟踪等情况。通过对多种图像集的仿真对比后得知，LK光流特征点在图像边缘丢失现象尤为严重，这种特征点没有经过连续帧跟踪，无法保证正确性，一旦进入系统，将占用大量计算资源且影响参数估计精度。本专利将不足1/10图像长/宽的特征点全部筛选掉，一方面减少了计算复杂度，另一方面确保进入系统的特征点是可靠、正确的，保证了精度可观。

进一步的，通过BA方法，对所有关键帧的所有特征点及位姿进行优化，提升纯视觉初始化的精度。

进一步的，对于视觉惯性校准阶段第2步估计结果，只有重力这一个3维向量被第3步重力细化承接，其他的状态量如各个初始关键帧的速度以及尺度并未传递到下一步，而是在第3步中被再次估计。因此在新的视觉惯性校准阶段中，在S202中仅估算重力初始值，再在S203中细化重力向量并估算尺度，能够简化求解方程，缩减未知状态向量维数。

进一步的，在建立完成新方程后，需要对形如Ax＝b的方程求解得到参数x，即必须进行矩阵A的逆运算。但矩阵A并非时刻可逆，不一定存在逆矩阵，因此需要进行转化解算，LDLT方法是一种高效的求解方法，能够有效提高求解时间，缩短初始化过程。

综上所述，本发明在优化迭代之前加入简单闭式求解模型，以减少迭代次数，大幅减少计算时长，具有较强的通用性，可以适用于各种结构的视觉惯性初始化系统，以提升运算效率。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1为本发明涉及的原VINS-Mono算法流程图；

图2为本发明中纯视觉初始化算法流程图；

图3为本发明中“特征点筛选+全局BA优化”算法框图；

图4为本发明中视觉惯性校准阶段新算法框图。

具体实施方式

本发明提供了一种基于视觉惯性融合的无人机状态参数初始化方法，借鉴ORB-SLAM初始化算法思路，在VINS-Mono算法基础上进行改进设计，如图1所示。在精度损失较小的前提下，加快了系统初始化进程，减少了计算耗时。

本发明一种基于视觉惯性融合的无人机状态参数初始化方法，分别对纯视觉初始化阶段和视觉-惯性校准阶段进行优化。首先对将要进入最终全局BundleAdjustment(BA)优化的特征点进行筛选，只保留处于图像中部的特征点，对靠近图像边缘的特征点不进行优化，减少了需要优化的特征点数目与该阶段的运算规模，并且所保留特征点的跟踪准确度较高，因此系统精度变化可接受；其次，不再估计各个初始关键帧的速度向量，只估计重力向量与尺度，将原算法的34维方程降为16维方程，降低了方程的求解压力，减少了初始化运行时间。

S1、针对纯视觉初始化阶段

请参阅图2，具体步骤如下：

S101、对极约束求解两帧间位姿；

先在已经填满的滑动窗口(设滑动窗口的容量为n)中选取与最新关键帧有足够视差且有足够多共视特征点的某个关键帧k，在关键帧k与最新关键帧之间建立对极几何约束，使用“五点法”求解出两者之间的相对旋转与平移。

S102、三角化两帧间特征点；

将关键帧k与最新关键帧进行共视特征点的三角化，获得两者共视特征点的深度信息，初步确定特征点的3维坐标。

S103、使用PnP方法求解位姿；

已知多个3维空间点的初始坐标及其投影位置，通过构建的3维到2维点对运动方程，求解从第k+1关键帧一直到最新关键帧的旋转与位移，得到第k+1、k+2等关键帧位姿。

S104、三角化其余特征点；

利用S102步骤方法，三角化从最旧关键帧到第k关键帧之间的所有共视特征点。

S105、使用PnP方法求解其他位姿；

在S103步骤上，使用PnP方法求解从最旧关键帧一直到第k关键帧的旋转与位移，得到第1、2…k-1关键帧的位姿，完成所有位姿估计。

S106、对特征点进行筛选；

注意到边特征点很可能产生误跟踪并占用储存资源与计算资源导致运算速度下降。因此在纯视觉初始化过程中，本发明增加了特征点筛选步骤，即步骤S106，并设置筛选阈值将图像长/宽维度上距离图像边缘约不足1/10图像长/宽的特征点全部筛掉，仅保留剩余特征点进入到步骤S107。同时，考虑到每帧图像须有一定数量特征点满足方程计算需要，在纯视觉初始化阶段步骤S107中，对每一帧图像位姿及每一个特征点坐标进行全局BA优化，提升精度。

S107、采用全局Bundle Adjustment(BA)进行优化。

请参阅图3，全局Bundle Adjustment(BA)优化具体为：

S1071、进入函数后，首先判断是否已遍历所有特征点，如果已遍历，则直接求解BA优化函数后结束；如果没有遍历所有特征点，进入步骤S1072；

S1072、选取一个未经处理的特征点，选取观测到该特征点并未在该点下处理的关键帧，进行筛选；

S1073、判断步骤S1072的特征点是否在该关键帧的边缘，如果不是，则将数据写入BZ优化函数，判断是否已遍历所有关键帧，如果已遍历，则返回步骤S1071，如果未遍历，则返回步骤S1072。

S2、针对视觉-惯性校准阶段

请参阅图1，具体步骤如下：

S201、通过陀螺仪偏置校准；

S202、分别对重力向量和尺度进行初始化；

S203、重新进行关联模型推导，仅估算重力向量和尺度，待估计状态量仅为(3+1)维，较原算法的(3n+3+1)维数大幅减少；

考虑到用4组方程约束4个未知量难以满足精度要求，本发明取连续3个关键帧(k，k+1，k+2)为一组关键帧组建立计算方程，并在一个滑动窗口内选取最新的6帧关键帧作为建立关键帧组的基础对象，这样最终计算维度降至16维，远小于原算法的34维。

S204、采用LDLT矩阵分解法求解方程，进一步降低计算时长，完成初始化。

请参阅图4，具体步骤如下：

S2041、进入函数，判断是否已遍历所有关键帧，如果已遍历所有关键帧，则使用LDLT方法求解总方程组，然后取关键帧组求解结果的平均值后结束；

S2042、如果没有遍历所有关键帧，选取下一个关键帧；

S2043、对步骤S2042确定的当前关键帧列出方程，左乘系数矩阵并写入到最终系数矩阵的对应位置，然后返回步骤S2041。

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中的描述和所示的本发明实施例的组件可以通过各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

仿真实验

请参阅表1，本发明使用MH_01_esay、MH_03_medium、MH_05_difficult共3种公开数据集对改进前后的初始化算法进行仿真验证，结果表明，改进后算法在3种数据集上运行时间的减幅分别为52.5％、46.4％与37.2％，均方根误差增加分别为0.0257m、0.0477m与0.0003m，计算时长大幅降低，误差增加在可接受范围。

本发明在纯视觉初始化阶段，对将要进入BA优化的特征点进行筛选，只保留在图像中部的特征点，滤除靠近图像边缘的特征点，减少了纯视觉初始化阶段的运算规模与时间，且保留下来的特征点跟踪准确度较高，保证了系统精度。在视觉惯性校准阶段，将原来34维方程降为16维，大大减少了方程的求解压力，减少了初始化在该阶段的运行时间。通过公开数据集进行了仿真验证，可以看出本专利方法在保持精度可观的前提下，能够大幅缩短初始化过程，有效改善当前工程化应用阶段的痛点。

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。

Claims (3)

1.一种基于视觉惯性融合的无人机状态参数初始化方法，其特征在于，包括纯视觉初始化阶段和视觉惯性校准阶段两步，其中，在纯视觉初始化阶段，将要进入最终全局BundleAdjustment优化的特征点进行筛选，只保留处于图像中部的特征点；在视觉惯性校准阶段，只估计重力向量与尺度，取连续3个关键帧k、k+1和k+2作为一组关键帧组建立计算方程，并在一个滑动窗口内选取最新的6帧关键帧作为建立关键帧组的基础对象，确定最终计算维度为16维，采用LDLT矩阵分解法求解方程，完成初始化后用于机体位姿估计；

纯视觉初始化阶段具体为：

先对极约束求解两帧间位姿；然后三角化两帧间特征点；再使用PnP方法求解位姿；对三角化其余特征点；使用PnP方法求解其他位姿；对特征点进行筛选；组合采用全局BundleAdjustment进行优化，采用全局Bundle Adjustment进行优化具体为：

S1071、进入函数后，首先判断是否已遍历所有特征点，如果已遍历，则直接求解BA优化函数后结束；如果没有遍历所有特征点，进入步骤S1072；

S1072、选取一个未经处理的特征点，选取观测到该特征点并未在该点下处理的关键帧，进行筛选；

S1073、判断步骤S1072的特征点是否在该关键帧的边缘，如果不是，则将数据写入BZ优化函数，判断是否已遍历所有关键帧，如果已遍历，则返回步骤S1071，如果未遍历，则返回步骤S1072；

视觉惯性校准阶段具体为：

S201、通过陀螺仪偏置校准；

S202、分别对重力向量和尺度进行初始化；

S203、重新进行关联模型推导，仅估算重力向量和尺度，待估计状态量仅为(3+1)维，较原算法的(3n+3+1)维数大幅减少；

S204、采用LDLT矩阵分解法求解方程，完成初始化。

2.根据权利要求1所述的基于视觉惯性融合的无人机状态参数初始化方法，其特征在于，对特征点进行筛选中，设置筛选阈值将图像长/宽维度上距离图像边缘不足1/10图像长/宽的特征点全部筛掉，保留剩余特征点。

3.根据权利要求1所述的基于视觉惯性融合的无人机状态参数初始化方法，其特征在于，步骤S204具体为：

S2041、进入函数，判断是否已遍历所有关键帧，如果已遍历所有关键帧，则使用LDLT方法求解总方程组，然后取关键帧组求解结果的平均值后结束；

S2042、如果没有遍历所有关键帧，选取下一个关键帧；

S2043、对步骤S2042确定的当前关键帧列写方程，左乘系数矩阵并写入到最终系数矩阵的对应位置，然后返回步骤S2041。

Images