[发明专利]基于惯性引导采样的视觉IMU方向估计方法

权利要求书

1.一种基于惯性引导采样的视觉IMU方向估计方法，其特征在于，包括以下步骤：

步骤1：基于增益自适应互补滤波器的IMU方向估计；

步骤2：基于尺度旋转不变的特征检测方法，来检测与匹配两幅图像间的特征点；

步骤3：基于视觉IMU融合的方向估计；

所述步骤1包括以下步骤：

步骤1.1：计算陀螺仪的方向估计；

步骤1.2：计算加速计与磁力计的方向估计；

步骤1.3：基于互补滤波算法的方向估计；

步骤1.4：IMU姿态估计；

所述步骤1.1包括以下步骤：

步骤1.1.1：求解t+Δt时刻描述IMU方向的四元数的变化率

其中，四元数中的q₁、q₂、q₃分量分别对应旋转轴在X、Y、Z方向的投影，q₀分量对应IMU围绕旋转轴旋转的角度；符号表示四元数乘积，表示t时刻IMU陀螺仪的四元数估计值，ω_t+Δt表示t+Δt时刻陀螺仪的输出值。

步骤1.1.2：求解t+Δt时刻四元数的估计值

步骤1.1.3：归一化处理四元数估计值，得出t+Δt时刻由陀螺仪估计出的四元数值

2.根据权利要求1所述的一种基于惯性引导采样的视觉IMU方向估计方法，其特征在于，所述步骤1.2通过高斯牛顿的迭代优化算法计算加速计与磁力计的方向估计值首先确定初始时刻的加速度值z_α和磁场强度值z_m，再根据t时刻加速计和磁力计的输出值α(t)和m(t)，以最小化估计值误差为目标，建立目标函数：

其中，和分别表示由加速计和磁力计估计值的误差，计算过程为：

其中，表示的共轭四元数。

3.根据权利要求1所述的一种基于惯性引导采样的视觉IMU方向估计方法，其特征在于，所述步骤1.3联合t+Δt时刻陀螺仪的方向估计值以及加速计和磁力计的方向估计值采用增益自适应互补滤波算法求解IMU坐标系下IMU的方向信息

其中，k_t表示自适应的增益系数，表示t时刻加速计、磁力计、陀螺仪融合后的方向估计信息，表示t时刻加速计与磁力计的方向估计值。

4.根据权利要求1所述的一种基于惯性引导采样的视觉IMU方向估计方法，其特征在于，所述步骤1.4采用运动力学中的扭转表示和指数映射算法，初步估计IMU的姿态信息，具体包括以下步骤：

步骤1.4.1：根据IMU估计出的四元数得到与四元数相对应的旋转轴ω＝(cosω_x,cosω_y,cosω_z)和绕轴旋转的角度θ，求解过程是：θ＝2cos^-1q₀

步骤1.4.2：根据扭转与指数映射理论，将(ω,θ)其映射为坐标系间的旋转和平移；已知初始时刻的位置g_global,n(0)与坐标系间的转换关系(R,T)后，计算n时刻IMU的位置g_global,n(θ)，其映射关系如下所示：

其中，表示与ω相对应的扭转参数，ν表示绕轴旋转的角度；

ν＝-ω×q_g

其中，q_g表示初始时刻坐标系原点与载体之间的向量。

5.根据权利要求1所述的一种基于惯性引导采样的视觉IMU方向估计方法，其特征在于，所述步骤2包含以下步骤：

步骤2.1：基于SURF算法的特征点检测；

步骤2.2：相邻帧间图像特征点的匹配。