[发明专利]一种基于视觉惯性融合的无人机状态参数初始化方法

权利要求书

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。