[发明专利]一种基于CAMERA/MIMU室内组合导航系统的四元数测量方法

权利要求书

1.一种基于CAMERA/MIMU室内组合导航系统的四元数测量方法，其特征在于：

步骤一：连接CAMERA/MIMU室内组合导航系统，系统上电后进行预热；

步骤二：采集陀螺输出的角速度和CAMERA拍摄的图像；

步骤三：采用空间滤波方法生成3×3模板的高斯卷积滤波器对步骤二中CAMERA拍摄的图像进行平滑滤波；

步骤四：采用边缘检测器追踪步骤三中经平滑滤波后图像中亮度快速变化的点，得到原始亮度数据的梯度值，并利用局部梯度值的最大值定位物体的边缘；

步骤五：采用随机采样一致性方法找到步骤三中经平滑滤波后图像中的灭点位置，确定中心灭点、水平灭点和垂直灭点位置矩阵B；

步骤六：根据CAMERA焦距、像主点和透镜畸变参数测量出标定矩阵K；

步骤七：根据步骤五中得到的灭点位置矩阵B和步骤六中得到的标定矩阵K，得到CAMERA数学模型：

B＝KA；

式中，A为CAMERA旋转矩阵：

A=q02+q12-q22-q322(q1q2-q0q3)2(q1q3+q0q2)2(q1q2+q0q3)q02-q12+q22-q322(q2q3-q0q1)2(q1q3-q0q2)2(q2q3+q0q1)q02-q12-q22+q32;]]>

其中，为四元数，q₀，q₁，q₂，q₃分别为四个实数，为载体坐标系三个正交坐标轴上的单位矢量；

步骤八：利用步骤七中获得的CAMERA数学模型确定CAMERA/MIMU室内组合导航系统的量测方程：

B_k＝KA_k+v_k；

式中，B_k为t_k时刻灭点位置矩阵、A_k为t_k时刻与四元数相关的CAMERA旋转矩阵，v_k为量测噪声序列；

步骤九：根据步骤二中陀螺输出的角速度测量矩阵W_k-1：

Wk-1=I+[Φa]+12[Φa]2;]]>

式中，I为单位阵，[Φ_a]为由旋转矢量构造的反对称矩阵，Δt＝t_k-t_k-1为更新周期；

步骤十：根据步骤八中得到的矩阵W_k-1得到MIMU的离散域四元数Q_k更新模型：

Qk=12QWk-1Qk-1;]]>

式中，φ_x，φ_y，φ_z为旋转矢量Φ在载体坐标系三个正交坐标轴上的分量；

步骤十一：选取四元数Q_k＝[q_0,k q_1,k q_2,k q_3,k]^T作为CAMERA/MIMU室内组合导航系统的状态向量，利用步骤十中获得的四元数更新模型确定CAMERA/MIMU室内组合导航系统的状态方程为：

x_k＝F_k-1x_k-1+w_k-1；

式中，q_i,k(i＝0,1,2,3)为t_k时刻四元数的分量值，w_k-1为系统激励噪声序列，系统状态向量x_k＝Q_k，一步转移矩阵

步骤十二：根据步骤八和步骤十一建立的CAMERA/MIMU室内组合导航系统的状态方程和量测方程进行卡尔曼滤波，估计出四元数Q_k。