一种基于CAMERA/MIMU室内组合导航系统的四元数测量方法

摘要

本发明属于室内组合导航导航系统技术领域，尤其涉及解决现有的MIMU误差漂移大导致的导航精度低，可靠性差问题的一种基于CAMERA/MIMU室内组合导航系统的四元数测量方法。本发明包括：采集陀螺输出的角速度和CAMERA拍摄的图像；对CAMERA拍摄的图像进行平滑滤波；得到原始亮度数据的梯度值，定位物体的边缘；确定中心灭点、水平灭点和垂直灭点位置矩阵；测量出标定矩阵；得到的标定矩阵，得到CAMERA数学模型；测量矩阵：得到MIMU的离散域四元数更新模型；确定CAMERA/MIMU室内组合导航系统的状态；估计出四元数。本发明给出了一种组合系统四元数测量方法，有效的提高了导航精度。

说明书

技术领域

本发明属于室内组合导航导航系统技术领域，尤其涉及解决现有的MIMU误差漂移大导致 的导航精度低，可靠性差问题的一种基于CAMERA/MIMU室内组合导航系统的四元数测量方法。

背景技术

微机械系统(MEMS)惯性器件存在着测量精度低、误差随时间漂移大及温度稳定性差等缺 点，MEMS构成的微惯性测量单元(MIMU)的导航参数估计存在严重误差漂移，误差发散导致系 统无法正确进行正确导航，最终失去导航的意义。如何抑制误差漂移，提高导航精度一直是 室内导航的难题，多传感器融合是解决此难题的方法，其中，CAMERA辅助的MIMU组合系统 是抑制误差漂移的一种有效方法，采用卡尔曼滤波算法对系统四元数进行估计，能够克服MIMU 误差漂移的缺点，提高四元数精度，进而提高导航精度。

发明内容

本发明实施例的目的在于提供一种提高导航精度和可靠性的基于CAMERA/MIMU室内组合 导航系统的四元数测量方法。

本发明的目的是这样实现的：

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

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

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

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

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

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

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

B＝KA；

式中，A为CAMERA旋转矩阵：

$A=\left(\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right);$

其中，为四元数，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：

$W_{k-1}=I+\left[{\Phi }_a\right]+\frac{1}{2}{\left[{\Phi }_a\right]}^2;$

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

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

$Q_k=\frac{1}{2}{\mathrm{QW}}_{k-1}{Q}_{k-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。

本发明的有益效果在于：

本发明通过对CAMERA和MIMU建立模型，CAMERA模型为组合系统量测方程，MIMU四元数 更新模型为状态方程，采用卡尔曼滤波算法实现对组合系统的四元数估计，克服了单独MIMU 误差漂移大的缺点，提高了导航精度；本发明方法简单，稳定性和可靠性高，给出了一种组 合系统四元数测量方法，有效的提高了导航精度。

附图说明

图1是基于CAMERA/MIMU室内组合导航系统的四元数测量方法的流程图。

具体实施方式

下面结合附图对本发明做进一步描述。

本发明涉及室内组合导航导航系统技术领域，为解决单兵在室内长时间导航时，微惯性 测量单元(MIMU，miniature inertial measurement unit)导航误差发散问题，本发明提出了 一种基于CAMERA/MIMU室内组合导航系统的四元数测量方法，通过建立摄像机(CAMERA)的数 学模型和MIMU四元数更新模型，以摄像机数学模型为系统量测方程，以MIMU四元数更新模 型为系统状态更新方程，采用卡尔曼滤波器对四元数进行估计，提高四元数的测量精度，进 而提高导航精度。

本发明实施例是这样实现的，一种基于CAMERA/MIMU室内组合导航系统的四元数测量方 法，该基于CAMERA/MIMU室内组合导航系统的四元数测量方法包括以下步骤：

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

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

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

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

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

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

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

B＝KA(1)

式中，A为CAMERA旋转矩阵，其测量公式为：

$A=\left(\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right)---\left(2\right)$

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

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

B_k＝KA_k+v_k(3)

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

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

$W_{k-1}=I+\left[{\Phi }_a\right]+\frac{1}{2}{\left[{\Phi }_a\right]}^2---\left(4\right)$

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

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

$Q_k=\frac{1}{2}{\mathrm{QW}}_{k-1}{Q}_{k-1}---\left(5\right)$

式中，φ_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(6)

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

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

本发明提供的基于CAMERA/MIMU室内组合导航系统的四元数测量方法，通过对CAMERA和 MIMU建立模型，CAMERA模型为组合系统量测方程，MIMU四元数更新模型为状态方程，采用 卡尔曼滤波算法实现对组合系统的四元数估计，克服了单独MIMU误差漂移大的缺点，提高了 导航精度；本发明方法简单，稳定性和可靠性高，给出了一种组合系统四元数测量方法，有 效的提高了导航精度。