CN100593689C - 基于捷联惯性导航系统的姿态估计和融合的方法 - Google Patents

Abstract

本发明公开了一种基于捷联惯性导航系统的姿态估计和融合的方法，包括：(1)利用六自由度惯性测量组件(简称IMU)中的传感器感应载体运动特性；(2)进行捷联惯性解算；(3)在进行步骤(2)同时利用加速度估计系统的姿态信号；(4)判断姿态估计值的可信度；(5)进行姿态信息融合；(6)输出导航参数；本发明的方法具有以下优点：(1)不增加任何硬件成本并且不改变硬件安装结构；(2)具有完全的自主性，不受外界环境的影响，能够全天候工作；(3)能够有效提高系统的导航精度，特别是姿态精度；(4)系统能够为控制显示装置提供每秒钟50次以上的导航信号。

Description

基于捷联惯性导航系统的姿态估计和融合的方法

一、技术领域

本发明涉及一种捷联惯性导航系统的导航技术，尤其涉及一种捷联惯性导航系统利用加速度估计姿态的方法。

二、背景技术

捷联惯性导航系统(Strapdown Inertial Navigation System简称SINS)是根据牛顿提出的相对惯性空间的力学定律，利用陀螺仪、加速度计等惯性元件感受运行体的角速度和加速度，通过计算机进行积分运算得到运动体的姿态、速度和位置等导航参数。在这些导航参数中，姿态参数误差会直接影响其它参数的精度，所以提高姿态测量精度具有更为重要的意义。

SINS的制导精度主要取决于惯性器件(陀螺仪和加速度计)的精度。SINS的精度在开始工作和较短的时间内是优良的。但是从初始对准之后，由于陀螺仪漂移误差的累积，精度会随着时间的推移而降低。目前提高SINS的精度，主要采取两个技术进行改进：(1)采用更高精度的惯性器件；(2)采用制导误差不累积的外部参考信息对SINS进行修正。

上述两种方法均能够提高SINS的导航精度。但是采用更高精度的惯性器件，会成倍增加系统的成本，这是许多应用领域都不能接受的。如果采用外部信息参考源，不仅会增加系统成本、降低系统的自主性，而且会受到更多的干扰导致系统可靠性的降低。此外，在一些复杂的应用环境中无法有效使用外部信息源。如何在不增加系统成本的情况下，更充分的利用惯性元器件信息尽量提高导航系统的精度，对很多应用场合具有重要意义。

三、发明内容

1、发明目的：本发明的目的是提供一种能够有效提高系统导航参数精度的捷联惯性导航系统的导航控制方法。

2、技术方案：为了达到上述的发明目的，本发明包括下列步骤：

(1)利用六自由度惯性测量组件(简称IMU)中的传感器感应载体运动特性：IMU通过陀螺仪敏感运动载体沿其轴向的角速度信号，通过加速度计测量沿载体轴向的线加速度信号，并将信号传输给导航计算机；

(2)进行捷联惯性解算：导航计算机对陀螺仪敏感的角速度信号进行姿态解算，对姿态矩阵进行三角函数计算即提取出载体的姿态角和方位角，将加速度计测量的线加速度由载体坐标系变换至地理坐标系，进而后积分得到速度、位置等导航参数；

(3)在进行步骤(2)的同时进行本步骤，利用加速度估计系统的姿态信号：根据三轴加速度计输出、地理坐标系和载体坐标系之间的关系，当系统加速度很小时，忽略速度和加速度对姿态的影响，得到加速度计输出矢量

重力加速度g、系统横滚角的估计值γ_D、俯仰角估计值θ_D和航向角估计值ψ_D(中间变量)之间的关系：

$\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\\ f_{\mathrm{bz}}\end{array}\right]\≈\left[\begin{array}{ccc}{\cos \mathrm{\ψ}}_D{\cos \mathrm{\θ}}_D& {\sin \mathrm{\ψ}}_D{\cos \mathrm{\θ}}_D& -\sin {\mathrm{\θ}}_D\\ \begin{array}{c}{\cos \mathrm{\ψ}}_D\sin {\mathrm{\θ}}_D\sin {\mathrm{\γ}}_D\\ -\sin {\mathrm{\ψ}}_D\cos {\mathrm{\γ}}_D\end{array}& \begin{array}{c}\sin {\mathrm{\ψ}}_D\sin {\mathrm{\θ}}_D{\sin \mathrm{\γ}}_D\\ +\cos {\mathrm{\ψ}}_D{\cos \mathrm{\γ}}_D\end{array}& \cos {\mathrm{\θ}}_D{\sin \mathrm{\γ}}_D\\ \begin{array}{c}{\cos \mathrm{\ψ}}_D{\sin \mathrm{\θ}}_D{\cos \mathrm{\γ}}_D\\ +{\sin \mathrm{\ψ}}_D{\sin \mathrm{\γ}}_D\end{array}& \begin{array}{c}\sin {\mathrm{\ψ}}_D\sin {\mathrm{\θ}}_D{\cos \mathrm{\γ}}_D\\ -\cos {\mathrm{\ψ}}_D{\sin \mathrm{\γ}}_D\end{array}& \cos {\mathrm{\θ}}_D{\cos \mathrm{\γ}}_D\end{array}\right]\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]---\left(1\right)$

从而得到：

$\left\{\begin{array}{c}{f}_{\mathrm{bx}}=\sin {\mathrm{\θ}}_D\·g\\ f_{\mathrm{by}}=-\cos {\mathrm{\θ}}_D\·\sin {\mathrm{\γ}}_D\·g\\ f_{\mathrm{bz}}=-\cos {\mathrm{\θ}}_D\·\cos {\mathrm{\γ}}_D\·g\end{array}\right.---\left(2\right)$

由此，得到内阻尼姿态估计的公式为：

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_D=\arcsin \left(\frac{f_{\mathrm{bx}}}{\sqrt{f_{\mathrm{bx}}^2+f_{\mathrm{by}}^2+f_{\mathrm{bz}}^2}}\right)\\ {\mathrm{\&gamma;}}_D=\left\{\begin{array}{c}\arctan (f_{\mathrm{by}}/f_{\mathrm{bz}});\left|f_{\mathrm{bz}}\right|\&GreaterEqual;0.1g\\ \arccos (-\frac{f_{\mathrm{bz}}}{\cos {\mathrm{\&theta;}}_D\&CenterDot;\sqrt{f_{\mathrm{bx}}^2+f_{\mathrm{by}}^2+f_{\mathrm{bz}}^2}});\left|f_{\mathrm{bz}}\right|<0.1g\end{array}\right.\end{array}\right.---\left(3\right)$

(4)判断姿态估计值的可信度：先利用上式(2)得到加速度计测量值变换到地理坐标系，判断系统加速运动的情况，再确定根据式(3)得到的姿态估计值的可信度；设置系统姿态估计法使用的基本条件为：

式中，f_nx和f_ny表示地理坐标系下的系统水平加速度。X₁、X₂、Y₁、Y₂分别表示系统使用姿态估计值的阈值；

(5)进行姿态信息融合：当根据式(4)得到的式(3)的姿态估计值是可信的，则进行卡尔曼滤波器的构建进行姿态信息融合，否则，直接跳到步骤(6)；本步骤包括如下步骤：

①建立卡尔曼滤波的状态方程：

使用一阶线性随机微分方程描述捷联航姿系统的状态误差如下：

$\stackrel{\&CenterDot;}{X}{\left(t\right)}_{8\&times;1}=A{\left(t\right)}_{8\&times;8}X\left(t\right)+G{\left(t\right)}_{8\&times;9}W{\left(t\right)}_{9\&times;1}---\left(5\right)$

式中，X(t)为t时刻系统的状态向量；A(t)，G(t)分别为系统状态矩阵和噪声矩阵；W(t)为系统的噪声向量；

系统的状态矢量为：

X＝[φ_n φ_e φ_d δv_n δv_e ε_rx ε_ry ε_rz]^T    (6)

系统的白噪声矢量为：

W＝[ω_gx ω_gy ω_gz ω_rx ω_ry ω_rz ω_ax ω_ay ω_az]^T    (7)

其中，φ_n、φ_e、φ_d分别表示系统的北向、东向和地向的姿态误差；δv_n、φv_e分别表示系统的北向和东向的速度误差；ε_rx、ε_ry、ε_rz分别表示X、Y、Z轴陀螺的误差；ω_gx、ω_gy、ω_gz分别表示X、Y、Z轴陀螺误差模型的一阶马尔可夫过程；ω_rx、ω_ry、ω_rz分别表示X、Y、Z轴陀螺误差模型的白噪声误差；ω_ax、ω_ay、ω_az分别表示X、Y、Z轴加速度计误差模型的白噪声误差；

系统噪声系数矩阵为：

$G\left(t\right)={\left[\begin{array}{ccc}{-C}_b^n& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{2\&times;3}& 0_{2\&times;3}& C_{b2\&times;3}^n\\ 0_{3\&times;3}& I_{3\&times;3}& 0_{3\&times;3}\end{array}\right]}_{8\&times;9}---\left(8\right)$

系统的状态转移阵为：

$A\left(t\right)={\left[\begin{array}{cc}{\left(A_{\mathrm{INS}}\right)}_{5\&times;5}& {\left(A_S\right)}_{5\&times;3}\\ 0_{3\&times;5}& {\left(A_{\mathrm{IMU}}\right)}_{3\&times;3}\end{array}\right]}_{8\&times;8}---\left(9\right)$

上式中，

$A_{\mathrm{IMU}}=\mathrm{diag}\left[\begin{array}{ccc}-\frac{1}{T_{\mathrm{rx}}}& -\frac{1}{T_{\mathrm{ry}}}& -\frac{1}{T_{\mathrm{rz}}}\end{array}\right]---\left(10\right)$

式中T_rx、T_ry、T_ry分别为陀螺仪一阶马尔可夫误差的相关时间

$A_S={\left[\begin{array}{c}-C_b^n\\ 0_{2\&times;3}\end{array}\right]}_{5\&times;3}---\left(11\right)$

A_INS是对应5个基本导航参数的矩阵，其非零项元素为：

$\left\{\begin{array}{cc}A\left(\mathrm{1,3}\right)=\frac{v_N}{R_M+h}& A\left(\mathrm{1,5}\right)=\frac{1}{R_N+h}\\ A\left(\mathrm{2,3}\right)={\mathrm{\&omega;}}_{\mathrm{ie}}+\frac{v_E}{R_N+h}& A\left(\mathrm{2,4}\right)=-\frac{1}{R_M+h}\\ A\left(\mathrm{3,1}\right)=-\frac{v_N}{R_M+h}& A\left(\mathrm{3,2}\right)=-{\mathrm{\&omega;}}_{\mathrm{ie}}-\frac{v_E}{R_N+h}\\ A\left(\mathrm{4,2}\right)=-f_D& A\left(\mathrm{4,3}\right)=f_E\\ A\left(\mathrm{5,1}\right)=f_D& A\left(\mathrm{5,3}\right)=-f_E\end{array}\right.---\left(12\right)$

式(12)中，v_N、v_E分别为惯性导航系统输出的北向和东向速度，ω_ie为地球自转角速度，R_N为卯酉圈半径，R_M为子午圈半径，h为飞行高度，f_E、f_D分别为东向和地向比力输出。

对8阶状态方程对应的白噪声矢量，取白噪声方差阵为：

$Q\left(t\right)=\mathrm{diag}\left[\begin{array}{ccccccccc}{\mathrm{\&sigma;}}_{\mathrm{gx}}^2& {\mathrm{\&sigma;}}_{\mathrm{gy}}^2& {\mathrm{\&sigma;}}_{\mathrm{gz}}^2& \frac{2{\mathrm{\&beta;}}_x{\mathrm{\&sigma;}}_{\mathrm{rx}}^2}{T_{\mathrm{kf}}}& \frac{2{\mathrm{\&beta;}}_y{\mathrm{\&sigma;}}_{\mathrm{ry}}^2}{T_{\mathrm{kf}}}& \frac{2{\mathrm{\&beta;}}_z{\mathrm{\&sigma;}}_{\mathrm{rz}}^2}{T_{\mathrm{kf}}}& {\mathrm{\&sigma;}}_{\mathrm{ax}}^2& {\mathrm{\&sigma;}}_{\mathrm{ay}}^2& {\mathrm{\&sigma;}}_{\mathrm{az}}^2\end{array}\right]---\left(13\right)$

其中，σ_gx，σ_gy，σ_gz为陀螺的白噪声漂移均方根值，σ_rx，σ_ry，σ_rz为陀螺的一阶马尔可夫漂移均方根值，β_x、β_x、β_x为相关频率，σ_ax、σ_ay、σ_az为加速度计白噪声均方根值，T_kf是离散化的周期时间；

②建立卡尔曼滤波的观测方程：

使用一阶线性随机微分方程来描述航姿系统的测量误差，其方程如下：

$Z\left(t\right)\stackrel{\mathrm{\&Delta;}}{=}\left[\begin{array}{c}{\mathrm{\&theta;}}_I-{\mathrm{\&theta;}}_D\\ {\mathrm{\&gamma;}}_I-{\mathrm{\&gamma;}}_D\end{array}\right]=\left[\begin{array}{c}\mathrm{\&delta;}{\mathrm{\&theta;}}_I+\mathrm{\&delta;}{\mathrm{\&theta;}}_D\\ \mathrm{\&delta;}{\mathrm{\&gamma;}}_I+\mathrm{\&delta;}{\mathrm{\&gamma;}}_D\end{array}\right]\stackrel{\mathrm{\&Delta;}}{=}H\left(t\right)X\left(t\right)+N\left(t\right)---\left(14\right)$

式中，Z(t)为t时刻系统的量测向量；H(t)为系统量测矩阵；N(t)为系统的量测噪声向量。θ_I、γ_I表示惯性系统姿态角，δθ_I、δγ_I表示惯性系统姿态误差角；θ_D、γ_D表示由步骤(3)估计的姿态角，δθ_D、δγ_D表示姿态估计值的误差角度。

系统量测矩阵为：

H(t)＝[I_2×2 0_2×6]    (15)

内阻尼姿态的测量噪声虽然和系统加速度、速度、纬度都有关系，但是经过补偿后的内阻尼姿态误差没有呈现明显的线性相关性，所以近似为白噪声矢量：

N(t)＝[M_N M_E]^T    (16)

式中M_N、M_E分别表示北向和东向估计姿态的白噪声误差。

(6)输出导航参数：导航计算机按照规定的格式和速率，将计算得到的导航参数传输给相应的控制单元或者显示仪器。

上述的步骤(4)中设置系统姿态估计法使用的基本条件也可以为：

式中，f_x、f_y、f_z表示三轴加速度计的输出值；f_nz表示地理坐标系下的系统垂直加速度，X₁、X₂、Y₁、Y₂分别表示系统使用姿态估计值的阈值。同时，也可以根据实际需要设置系统姿态估计法使用的基本条件。

本发明使用的捷联惯性导航系统中硬件包括六自由度惯性测量组件(简称IMU；包括三个相互垂直的陀螺和三个相互垂直的加速度计)、导航计算机和相关的控制显示装置，具体硬件组成如附图1所示；捷联惯性导航系统的基本组成元件中，IMU必须安装在运载体的几何中心位置，通过数据线与导航计算机相连，导航计算机将导航参数输出到相应的控制装置或者显示装置，图2所示为以飞机为例的系统硬件安装示意图；图3所示为以大型拖车为例的系统硬件安装示意图。

3、有益效果；本发明的方法具有以下优点：(1)不增加任何硬件成本并且不改变硬件安装结构；(2)具有完全的自主性，不受外界环境的影响，能够全天候工作；(3)能够有效提高系统的导航精度，特别是姿态精度；(4)系统能够为控制显示装置提供每秒钟50次以上的导航信号。

对本发明的有益效果说明如下：

(1)根据静态试验采用的惯性元器件精度，0.02秒的采样频率下分别对十秒平均和百秒平均的IMU精度表如下：

表1十秒平均和百秒平均的IMU标准偏差

利用上述精度的IMU搭建了系统原理样机，然后将IMU放置在水平的转台上，进行3个小时的水平静态试验，试验结果如图7和图8所示。利用本发明所述方法得到新的捷联惯性导航系统横滚角曲线如图7(a)和图8(a)所示。为了将本发明的方法和传统的捷联惯性导航控制方法进行比较，利用同一组IMU数据文件进行传统的纯捷联解算，得到姿态的对比曲线如图7(b)和图8(b)。由图7和图8可知，在水平静态情况下，当仅仅利用陀螺和加速度计信息时，利用传统的捷联惯性导航系统解算方法得到的姿态角有明显的漂移发散现象，并且具有明显的舒拉周期振荡特性，而采用本发明的方法能够有效抑制系统姿态角的漂移和振荡，并且静态姿态精度提高到了0.05°以内。

(2)在三轴转台上的试验：

使用的IMU精度同表1。

1)横滚角转动试验：

将IMU放置在三轴转台上，转台调平。初始时刻转台水平，每隔两分种转台内框增加5°或10°。新的捷联惯性导航系统横滚角曲线如图9(a)所示。为了将新系统和传统的捷联惯性导航进行比较，利用同一组IMU数据文件进行传统的纯捷联解算，得到系统俯仰角的对比曲线图如图9(b)；

2)俯仰角转动试验：

将IMU放置在三轴转台上，将转台调平。初始时刻转台水平，每隔两分种令转台中框减小5°或10°。得到新的捷联惯性导航系统俯仰角曲线如图10(a)所示。为了将新系统和传统的捷联惯性导航进行比较，利用同一组IMU数据文件进行传统的纯捷联解算，得到系统俯仰角的对比曲线图如图10(b)；

3)横滚角摇摆试验：

将IMU放置在水平三轴转台上，转台保持水平静止7分钟后，转台内框进行频率为0.1Hz，幅度为±10°的摇摆运动。得到新的捷联惯性导航系统横滚角曲线如图11(a)所示。系统利用同一组IMU数据文件进行传统的纯捷联解算，得到横滚角曲线如图11(b)；

4)俯仰角圆周试验

将IMU放置在水平三轴转台上，初始时刻转台水平，静止状态7分钟作后转台中框进行速率为20°/s的圆周运动，得到新的捷联惯性导航系统俯仰角曲线如图12(a)所示。为了明显看到运动特性，给出某一段时间内俯仰角曲线图如图12(b)。

系统在三轴转台上的实验表明，当系统具有一定的姿态角度、具有一定的角速度或角加速度时，采用加速度进行姿态估计的捷联惯性导航系统的姿态精度明显高于传统的捷联惯性导航系统精度。

(3)系统的跑车试验

动态跑车试验在一段较为平直的公路上进行。车辆的姿态角大致水平、航向基本保持为330°。车辆初始静止，经两分钟加速运动后，基本保持36km/h的速度，实验时间为400s。以车辆上另外一套高精度组合导航系统提供的数据为参考，得到了跑车过程中系统航姿误差角，分别以本发明的方法和传统的捷联惯性导航系统算法进行解算，得到系统的横滚角对比曲线如图13(a)，得到俯仰角对比曲线如图13(b)所示。

动态跑车试验结果表明，当系统处于加速状况下(0～100s)，加速度估计姿态不可用，当系统进入匀速平稳行驶阶段(100～400s)，系统采用加速度估计的姿态值，将系统的姿态误差减小了一倍。大量动态实验表明，如果系统能够在长时间内保持匀速，即使用加速度计姿态估计值的时间越长，则系统精度的提高就越为明显。

四、附图说明

图1为捷联惯性导航系统硬件框图；

图2为飞机中系统硬件安装示意图；

图3为大型拖车中系统硬件安装示意图；

图4为传统的捷联惯性导航系统原理图；

图5为本发明的捷联惯性导航系统原理图；

图6为新型的捷联惯性导航系统软件流程图；

图7为静态系统横滚曲线图

图8为静态系统俯仰曲线图；

图9为横滚角位置测试曲线图；

图10为俯仰角位置测试曲线图；

图11为横滚角摇摆测试曲线图；

图12为俯仰角圆周测试曲线图；

图13为动态跑车系统姿态曲线图；

图14为新型的组合导航系统软件流程图。

五、具体实施方式

实施例一：

本实施例中捷联惯性导航系统硬件组成与传统的捷联惯性导航系统硬件组成相同。如图5所示，本实施例包括下列步骤：

(1)利用六自由度惯性测量组件(简称IMU)中的传感器感应载体运动：IMU通过陀螺仪敏感运动载体沿其轴向的角速度信号，通过加速度计测量沿其轴向的线加速度信号，并将信号传输给导航计算机；

(2)进行捷联惯性解算：导航计算机对陀螺仪敏感的角速度信号进行姿态解算，对姿态矩阵进行三角函数计算即提取出载体的姿态角和方位角，用加速度计测量的加速度进行由载体坐标系至地理坐标系的坐标变换计算，进而在地理坐标系中进行速度、位置等导航参数计算；

(3)在进行步骤(2)的同时进行本步骤，利用加速度估计系统的姿态信号：根据三轴加速度计输出、地理坐标系和载体坐标系之间的关系，当系统加速度很小时，忽略速度和加速度对姿态的影响，得到加速度计输出矢量

重力加速度g、系统横滚角的估计值γ_D、俯仰角估计值θ_D和航向角估计值ψ_D之间的关系：

$\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\\ f_{\mathrm{bz}}\end{array}\right]\&ap;\left[\begin{array}{ccc}{\cos \mathrm{\&psi;}}_D{\cos \mathrm{\&theta;}}_D& {\sin \mathrm{\&psi;}}_D{\cos \mathrm{\&theta;}}_D& -\sin {\mathrm{\&theta;}}_D\\ \begin{array}{c}{\cos \mathrm{\&psi;}}_D\sin {\mathrm{\&theta;}}_D\sin {\mathrm{\&gamma;}}_D\\ -\sin {\mathrm{\&psi;}}_D\cos {\mathrm{\&gamma;}}_D\end{array}& \begin{array}{c}\sin {\mathrm{\&psi;}}_D\sin {\mathrm{\&theta;}}_D{\sin \mathrm{\&gamma;}}_D\\ +\cos {\mathrm{\&psi;}}_D{\cos \mathrm{\&gamma;}}_D\end{array}& \cos {\mathrm{\&theta;}}_D{\sin \mathrm{\&gamma;}}_D\\ \begin{array}{c}{\cos \mathrm{\&psi;}}_D{\sin \mathrm{\&theta;}}_D{\cos \mathrm{\&gamma;}}_D\\ +{\sin \mathrm{\&psi;}}_D{\sin \mathrm{\&gamma;}}_D\end{array}& \begin{array}{c}\sin {\mathrm{\&psi;}}_D\sin {\mathrm{\&theta;}}_D{\cos \mathrm{\&gamma;}}_D\\ -\cos {\mathrm{\&psi;}}_D{\sin \mathrm{\&gamma;}}_D\end{array}& \cos {\mathrm{\&theta;}}_D{\cos \mathrm{\&gamma;}}_D\end{array}\right]\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]---\left(1\right)$

从而可以得到：

$\left\{\begin{array}{c}{f}_{\mathrm{bx}}=\sin {\mathrm{\&theta;}}_D\&CenterDot;g\\ f_{\mathrm{by}}=-\cos {\mathrm{\&theta;}}_D\&CenterDot;\sin {\mathrm{\&gamma;}}_D\&CenterDot;g\\ f_{\mathrm{bz}}=-\cos {\mathrm{\&theta;}}_D\&CenterDot;\cos {\mathrm{\&gamma;}}_D\&CenterDot;g\end{array}\right.---\left(2\right)$

由此，可以得到内阻尼姿态估计的公式为：

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_D=\arcsin \left(\frac{f_{\mathrm{bx}}}{\sqrt{f_{\mathrm{bx}}^2+f_{\mathrm{by}}^2+f_{\mathrm{bz}}^2}}\right)\\ {\mathrm{\&gamma;}}_D=\left\{\begin{array}{c}\arctan (f_{\mathrm{by}}/f_{\mathrm{bz}});\left|f_{\mathrm{bz}}\right|\&GreaterEqual;0.1g\\ \arccos (-\frac{f_{\mathrm{bz}}}{\cos {\mathrm{\&theta;}}_D\&CenterDot;\sqrt{f_{\mathrm{bx}}^2+f_{\mathrm{by}}^2+f_{\mathrm{bz}}^2}});\left|f_{\mathrm{bz}}\right|<0.1g\end{array}\right.\end{array}\right.---\left(3\right)$

(4)判断姿态估计值的可信度：先利用上式(2)得到加速度计测量值变换到地理坐标系，判断系统加速运动的情况，再确定根据式(3)得到的姿态估计值的可信度；设置系统姿态估计法使用的基本条件为：

式中，f_nx和f_ny表示地理坐标系下的系统水平加速度。X₁、X₂、Y₁、Y₂分别表示系统使用姿态估计值的阈值；

(5)进行姿态信息融合：如果根据式(4)得到的式(3)的姿态估计值是可信的，则进行卡尔曼滤波器的构建进行姿态信息融合，否则，直接跳到步骤(6)；本步骤包括如下步骤：

①建立卡尔曼滤波的状态方程：

使用一阶线性随机微分方程来描述捷联航姿系统的状态误差如下：

$\stackrel{\&CenterDot;}{X}{\left(t\right)}_{8\&times;1}=A{\left(t\right)}_{8\&times;8}X\left(t\right)+G{\left(t\right)}_{8\&times;9}W{\left(t\right)}_{9\&times;1}---\left(5\right)$

式中，X(t)为t时刻系统的状态向量；A(t)，G(t)分别为系统状态矩阵和噪声矩阵；W(t)为系统的噪声向量；

系统的状态矢量为：

X＝[φ_n φ_e φ_d δv_n δv_e ε_rx ε_ry ε_rz]^T    (6)

系统的白噪声矢量为：

W＝[ω_gx ω_gy ω_gz ω_rx ω_ry ω_rz ω_ax ω_ay ω_az]^T    (7)

其中，φ_n、φ_e、φ_d分别表示系统的北向、东向和地向的姿态误差；δv_n、δv_e分别表示系统的北向和东向的速度误差；ε_rx ε_ry ε_rz分别表示X、Y、Z轴陀螺的误差；ω_gx、ω_gy、ω_gz分别表示X、Y、Z轴陀螺误差模型的一阶马尔可夫过程；ω_rx ω_ry ω_rz分别表示X、Y、Z轴陀螺误差模型的白噪声误差；ω_ax、ω_ay、ω_az分别表示X、Y、Z轴加速度计误差模型的白噪声误差。

系统噪声系数矩阵为：

$G\left(t\right)={\left[\begin{array}{ccc}{-C}_b^n& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{2\&times;3}& 0_{2\&times;3}& C_{b2\&times;3}^n\\ 0_{3\&times;3}& I_{3\&times;3}& 0_{3\&times;3}\end{array}\right]}_{8\&times;9}---\left(8\right)$

系统的状态转移阵为：

$A\left(t\right)={\left[\begin{array}{cc}{\left(A_{\mathrm{INS}}\right)}_{5\&times;5}& {\left(A_S\right)}_{5\&times;3}\\ 0_{3\&times;5}& {\left(A_{\mathrm{IMU}}\right)}_{3\&times;3}\end{array}\right]}_{8\&times;8}---\left(9\right)$

上式中，

$A_{\mathrm{IMU}}=\mathrm{diag}\left[\begin{array}{ccc}-\frac{1}{T_{\mathrm{rx}}}& -\frac{1}{T_{\mathrm{ry}}}& -\frac{1}{T_{\mathrm{rz}}}\end{array}\right]---\left(10\right)$

$A_S={\left[\begin{array}{c}-C_b^n\\ 0_{2\&times;3}\end{array}\right]}_{5\&times;3}---\left(11\right)$

A_INS是对应5个基本导航参数的矩阵，其非零项元素为：

$\left\{\begin{array}{cc}A\left(\mathrm{1,3}\right)=\frac{v_N}{R_M+h}& A\left(\mathrm{1,5}\right)=\frac{1}{R_N+h}\\ A\left(\mathrm{2,3}\right)={\mathrm{\&omega;}}_{\mathrm{ie}}+\frac{v_E}{R_N+h}& A\left(\mathrm{2,4}\right)=-\frac{1}{R_M+h}\\ A\left(\mathrm{3,1}\right)=-\frac{v_N}{R_M+h}& A\left(\mathrm{3,2}\right)=-{\mathrm{\&omega;}}_{\mathrm{ie}}-\frac{v_E}{R_N+h}\\ A\left(\mathrm{4,2}\right)=-f_D& A\left(\mathrm{4,3}\right)=f_E\\ A\left(\mathrm{5,1}\right)=f_D& A\left(\mathrm{5,3}\right)=-f_E\end{array}\right.---\left(12\right)$

对8阶状态方程对应的白噪声矢量，取白噪声方差阵为：

$Q\left(t\right)=\mathrm{diag}\left[\begin{array}{ccccccccc}{\mathrm{\&sigma;}}_{\mathrm{gx}}^2& {\mathrm{\&sigma;}}_{\mathrm{gy}}^2& {\mathrm{\&sigma;}}_{\mathrm{gz}}^2& \frac{2{\mathrm{\&beta;}}_x{\mathrm{\&sigma;}}_{\mathrm{rx}}^2}{T_{\mathrm{kf}}}& \frac{2{\mathrm{\&beta;}}_y{\mathrm{\&sigma;}}_{\mathrm{ry}}^2}{T_{\mathrm{kf}}}& \frac{2{\mathrm{\&beta;}}_z{\mathrm{\&sigma;}}_{\mathrm{rz}}^2}{T_{\mathrm{kf}}}& {\mathrm{\&sigma;}}_{\mathrm{ax}}^2& {\mathrm{\&sigma;}}_{\mathrm{ay}}^2& {\mathrm{\&sigma;}}_{\mathrm{az}}^2\end{array}\right]---\left(13\right)$

其中，σ_gx、σ_gy、σ_gz为陀螺的白噪声漂移均方根值，σ_rx、σ_ry、σ_rz为陀螺的一阶马尔可夫漂移均方根值，β_x、β_y、β_z为相关频率，σ_ax、σ_ay、σ_az为加速度计白噪声均方根值，T_kf是离散化的周期时间。

②建立卡尔曼滤波的观测方程：

使用一阶线性随机微分方程来描述航姿系统的测量误差，其方程如下：

$Z\left(t\right)\stackrel{\mathrm{\&Delta;}}{=}\left[\begin{array}{c}{\mathrm{\&theta;}}_I-{\mathrm{\&theta;}}_D\\ {\mathrm{\&gamma;}}_I-{\mathrm{\&gamma;}}_D\end{array}\right]=\left[\begin{array}{c}\mathrm{\&delta;}{\mathrm{\&theta;}}_I+\mathrm{\&delta;}{\mathrm{\&theta;}}_D\\ \mathrm{\&delta;}{\mathrm{\&gamma;}}_I+\mathrm{\&delta;}{\mathrm{\&gamma;}}_D\end{array}\right]\stackrel{\mathrm{\&Delta;}}{=}H\left(t\right)X\left(t\right)+N\left(t\right)---\left(14\right)$

式中，Z(t)为t时刻系统的量测向量；H(t)为系统量测矩阵；N(t)为系统的量测噪声向量。θ_I、γ_I表示惯性系统姿态角，δθ_I、δγ_I表示惯性系统姿态误差角，θ_D、γ_D表示由步骤(3)估计的姿态角，δθ_D、δγ_D表示姿态估计值的误差角度。

系统量测矩阵为：

H(t)＝[I_2×2 0_2×6]    (15)

内阻尼姿态的测量噪声虽然和系统加速度、速度、纬度都有关系，但是经过补偿后的内阻尼姿态误差没有呈现明显的线性相关性，所以近似为白噪声矢量：

N(t)＝[M_N M_E]^T    (16)

式中M_N、M_E分别表示北向和东向估计姿态的白噪声误差。

(6)输出导航参数：导航计算机按照规定的格式和速率，将计算得到的导航参数传输给相应的控制单元或者显示仪器。

如图6所示，本实施例的工作流程如下：本实施例仅有IMU传感器信号先待处理：

(1)系统初始化模块：

SINS一般应在静态条件下正常启动。首先进行系统的初始化，这包括导航计算机硬件自检，接口扩展板各个接口的初始化，对导航传感器信号的检测，和控制显示器之间的通讯，相应的状态设置，以及卡尔曼滤波器的初始化等等。

(2)初始对准：

在静基座下，SINS采用两种初始对准方法：一是由控制显示器直接给出系统的初始姿态；二是在接收到外界给出的控制信号和初始信息后，进行自对准。根据外界条件，可以自由选用初始对准方法

(3)捷联惯性航姿解算：

捷联航姿解算模块是较为重要的一部分，主要包含以下几个子模块；

1)惯性测量装置数据的采集处理。主要是采集陀螺仪和加速度计的信号，条件具备的情况下，还采集温度等信号。

2)系数和坐标转换。采集到的惯性器件的输出值，需要乘以相应的标定因子就可以得到对应的物理量。此外，由于惯性器件的坐标系和实际的导航坐标系可能不一致，所以还需要进行一定的坐标变换，把惯性器件的物理量转化为地理坐标系的物理量。

3)求解四元数姿态方程。根据姿态算法中的四元数方法，利用四元数和载体姿态之间的关系，求取姿态阵。

4)比力变换。比力是单位质量上对应的外作用力，是相对惯性空间的加速度与引力加速度之差，也称“非引力加速度”。加速度计直接测量的不是载体的加速度，所以不能直接把加速度计的测量值拿来做运算，它需要经过适当的变换才能带入运算。

5)提取姿态角度。根据四元数求得的姿态阵方程，从中提取出载体的姿态角度信息。

6)求解比力方程。利用获得的姿态角度信息和比力，建立比力方程获得载体运动时候的真实加速度，进而获得载体的速度和位置。

(4)利用加速度计输出估计姿态模块：

系统每次采集IMU信号，均使用公式3进行对系统姿态的估计。根据不同于捷联惯性航姿解算的原理，得到系统横滚角和俯仰角的估计数值。

(5)姿态估计可信度判断模块：

根据捷联惯性航姿解算过程中得到的地理坐标系下的系统加速度的值，根据公式4，判断系统姿态估计的可信度。

(6)姿态组合卡尔曼滤波模块：

根据公式5～公式15，建立卡尔曼滤波器。当判断姿态估计值可信时，进行卡尔曼滤波解算，解算频率可以达到50Hz。当系统判断滤波器不可信时，跳过该模块，不进行卡尔曼滤波。

(7)数据输出模块：

系统需要根据外界设备的需求，对不同的接口设备，设置导航信息的输出格式、频率、校验方式以及状态位设定等等。

实施例二：本实施例中的步骤与实施例中的步骤基本相同，区别在于步骤(4)中设置系统姿态估计法使用的基本条件为：

式中，f_x、f_y、f_z表示三轴加速度计的输出值；f_nz表示地理坐标系下的系统垂直加速度，X₁、X₂、Y₁、Y₂分别表示系统使用姿态估计值的阈值。

实施例三：

当系统具备其它的外界导航信息源，例如GPS、里程仪、大气速度计算机或者磁航向传感器等信息时，系统仍可采用加速度计的姿态估计值来进一步提高系统姿态精度，基本步骤与实施例一基本相同，不同的是本实施例中，系统可以和一个或多个外导航信息进行数据融合，从而进一步提高系统的精度，以满足更多应用场合的要求。当捷联惯性导航系统具备外导航信息辅助时，通常称之为组合导航系统。本发明的加速度计估计姿态方法也可以应用于组合导航系统中，其系统软件流程图如图14所示。

Claims (3)

1、一种基于捷联惯性导航系统的姿态估计和融合的方法，包括下列步骤：

步骤(1)利用六自由度惯性测量组件中的传感器感应载体运动特性：通过陀螺仪敏感运动载体沿其轴向的角速度信号，通过加速度计测量沿载体轴向的线加速度信号，并将信号传输给导航计算机；

步骤(2)进行捷联惯性解算：导航计算机对陀螺仪敏感的角速度信号进行姿态解算，对姿态矩阵进行三角函数计算即提取出载体的姿态角和方位角，用加速度计测量的加速度进行由载体坐标系至地理坐标系的坐标变换计算，进而在地理坐标系中进行导航参数计算；

其特征在于，在进行上述步骤(2)的同时进行下列步骤：

步骤(3)利用线加速度估计系统的姿态信号：根据三轴加速度计输出、地理坐标系和载体坐标系之间的关系，当系统加速度很小时，忽略速度和加速度对姿态的影响，得到加速度计输出矢量

重力加速度g、系统横滚角的估计值γ_D、俯仰角估计值θ_D和航向角估计值ψ_D之间的关系：

$\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\\ f_{\mathrm{bz}}\end{array}\right]\&ap;\left[\begin{array}{ccc}\cos {\mathrm{\&psi;}}_D\cos {\mathrm{\&theta;}}_D& \sin {\mathrm{\&psi;}}_D\cos {\mathrm{\&theta;}}_D& -\sin {\mathrm{\&theta;}}_D\\ \cos {\mathrm{\&psi;}}_D\sin {\mathrm{\&theta;}}_D\sin {\mathrm{\&gamma;}}_D& \sin {\mathrm{\&psi;}}_D\sin {\mathrm{\&theta;}}_D\sin {\mathrm{\&gamma;}}_D& \\ & & \cos {\mathrm{\&theta;}}_D\sin {\mathrm{\&gamma;}}_D\\ -\sin {\mathrm{\&psi;}}_D\cos {\mathrm{\&gamma;}}_D& +\cos {\mathrm{\&psi;}}_D\cos {\mathrm{\&gamma;}}_D& \\ \cos {\mathrm{\&psi;}}_D\sin {\mathrm{\&theta;}}_D\cos {\mathrm{\&gamma;}}_D& \sin {\mathrm{\&psi;}}_D\sin {\mathrm{\&theta;}}_D\cos {\mathrm{\&gamma;}}_D& \\ & & \cos {\mathrm{\&theta;}}_D\cos {\mathrm{\&gamma;}}_D\\ +\sin {\mathrm{\&psi;}}_D\sin {\mathrm{\&gamma;}}_D& -\cos {\mathrm{\&psi;}}_D\sin {\mathrm{\&gamma;}}_D& \end{array}\right]\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]---\left(1\right)$

从而得到：

$\left\{\begin{array}{c}{f}_{\mathrm{bx}}=\sin {\mathrm{\&theta;}}_D\&CenterDot;g\\ f_{\mathrm{by}}=-\cos {\mathrm{\&theta;}}_D\&CenterDot;\sin {\mathrm{\&gamma;}}_D\&CenterDot;g\\ f_{\mathrm{bz}}=-\cos {\mathrm{\&theta;}}_D\&CenterDot;\cos {\mathrm{\&gamma;}}_D\&CenterDot;g\end{array}\right.---\left(2\right)$

由此，得到内阻尼姿态估计的公式为：

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_D=\arcsin \left(\frac{f_{\mathrm{bx}}}{\sqrt{f_{\mathrm{bx}}^2+f_{\mathrm{by}}^2+f_{\mathrm{bz}}^2}}\right)\\ {\mathrm{\&gamma;}}_D=\left\{\begin{array}{c}\arcsin (f_{\mathrm{by}}/f_{\mathrm{bz}});\left|f_{\mathrm{bz}}\right|\&GreaterEqual;0.1g\\ \arccos (-\frac{f_{\mathrm{bz}}}{\cos {\mathrm{\&theta;}}_D\&CenterDot;\sqrt{f_{\mathrm{bx}}^2+f_{\mathrm{by}}^2+f_{\mathrm{bz}}^2}});\left|f_{\mathrm{bz}}\right|<0.1g\end{array}\right.\end{array}\right.---\left(3\right)$

步骤(4)判断姿态估计值的可信度：根据步骤(1)中加速度计测量值判断系统加速运动的情况，确定根据式(3)得到的姿态估计值的可信度；

步骤(5)进行姿态信息融合：当所述的姿态估计值是可信的，则通过卡尔曼滤波器的构建进行姿态信息融合，否则，直接跳到步骤(6)；本步骤包括如下步骤：

①建立卡尔曼滤波的状态方程：

使用一阶线性随机微分方程来描述捷联航姿系统的状态误差如下：

X(t)_8×1＝A(t)_8×8X(t)+G(t)_8×9W(t)_9×1   (5)

式中，X(t)为t时刻系统的状态向量；A(t)，G(t)分别为系统状态转移矩阵和噪声系数矩阵；W(t)为系统的噪声向量；

系统的状态矢量为：

X＝[φ_n φ_e φ_d δv_n δv_e ε_rx ε_ry ε_rz]^T    (6)

系统的白噪声矢量为：

W＝[ω_gx ω_gy ω_gz ω_rx ω_ry ω_rz ω_ax ω_ay ω_az]^T    (7)

其中，φ_n、φ_e、φ_d分别表示系统的北向、东向和地向的姿态误差；δv_n、δv_e分别表示系统的北向和东向的速度误差；ε_rx、ε_ry、ε_rz分别表示X、Y、Z轴陀螺的误差；ω_gx、ω_gy、ω_gz分别表示X、Y、Z轴陀螺误差模型的一阶马尔可夫过程；ω_rx、ω_ry、ω_rz分别表示X、Y、Z轴陀螺误差模型的白噪声误差；ω_ax、ω_ay、ω_az分别表示X、Y、Z轴加速度计误差模型的白噪声误差；

系统噪声系数矩阵为：

$G\left(t\right)={\left[\begin{array}{ccc}-C_b^n& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{2\&times;3}& 0_{2\&times;3}& C_{b2\&times;3}^n\\ 0_{3\&times;3}& I_{3\&times;3}& 0_{3\&times;3}\end{array}\right]}_{8\&times;9}---\left(8\right)$

系统的状态转移阵为：

$A\left(t\right)={\left[\begin{array}{cc}{\left(A_{\mathrm{INS}}\right)}_{5\&times;5}& {\left(A_S\right)}_{5\&times;3}\\ 0_{3\&times;5}& {\left(A_{\mathrm{IMU}}\right)}_{3\&times;3}\end{array}\right]}_{8\&times;8}---\left(9\right)$

上式中，

$A_{\mathrm{IMU}}=\mathrm{diag}\left[\begin{array}{ccc}-\frac{1}{T_{\mathrm{rx}}}& -\frac{1}{T_{\mathrm{ry}}}& -\frac{1}{T_{\mathrm{rz}}}\end{array}\right]---\left(10\right)$

$A_S={\left[\begin{array}{c}-C_b^n\\ 0_{2\&times;3}\end{array}\right]}_{5\&times;3}---\left(11\right)$

式(10)中，T_rx、T_xy、T_rz分别为陀螺仪一阶马尔可夫误差的相关时间；A_INS是对应5个基本导航参数的矩阵，其非零项元素为：

$\left\{\begin{array}{cc}A\left(\mathrm{1,3}\right)=\frac{v_N}{R_M+h}& A\left(\mathrm{1,5}\right)=\frac{1}{R_N+h}\\ A\left(\mathrm{2,3}\right)={\mathrm{\&omega;}}_{\mathrm{ie}}+\frac{v_E}{R_N+h}& A\left(\mathrm{2,4}\right)=-\frac{1}{R_M+h}\\ A\left(\mathrm{3,1}\right)=-\frac{v_N}{R_M+h}& A\left(\mathrm{3,2}\right)=-{\mathrm{\&omega;}}_{\mathrm{ie}}-\frac{v_E}{R_N+h}\\ A\left(\mathrm{4,2}\right)=-f_D& A\left(\mathrm{4,3}\right)=f_E\\ A\left(\mathrm{5,1}\right)=f_D& A\left(\mathrm{5,3}\right)=-f_E\end{array}\right.---\left(12\right)$

式(12)中，v_N、v_E分别为惯性导航系统输出的北向和东向速度，ω_ie为地球自转角速度，R_N为卯酉圈半径，R_M为子午圈半径，h为飞行高度，f_E、f_D分别为东向和地向比力输出；

对8阶状态方程对应的白噪声矢量，取白噪声方差阵为：

$Q\left(t\right)=\mathrm{diag}\left[\begin{array}{ccccccccc}{\mathrm{\&sigma;}}_{\mathrm{gx}}^2& {\mathrm{\&sigma;}}_{\mathrm{gy}}^2& {\mathrm{\&sigma;}}_{\mathrm{gz}}^2& \frac{2{\mathrm{\&beta;}}_x{\mathrm{\&sigma;}}_{\mathrm{rx}}^2}{T_{\mathrm{kf}}}& \frac{2{\mathrm{\&beta;}}_y{\mathrm{\&sigma;}}_{\mathrm{ry}}^2}{T_{\mathrm{kf}}}& \frac{2{\mathrm{\&beta;}}_z{\mathrm{\&sigma;}}_{\mathrm{rz}}^2}{T_{\mathrm{kf}}}& {\mathrm{\&sigma;}}_{\mathrm{ax}}^2& {\mathrm{\&sigma;}}_{\mathrm{ay}}^2& {\mathrm{\&sigma;}}_{\mathrm{az}}^2\end{array}\right]---\left(13\right)$

其中，σ_gx、σ_gy、σ_gz为陀螺的白噪声漂移均方根值，σ_rx、σ_ry、σ_rz为陀螺的一阶马尔可夫漂移均方根值，β_x、β_y、β_z为相关频率，σ_ax、σ_at、σ_az速度计白噪声均方根值，T_kf是离散化的周期时间；

②建立卡尔曼滤波的观测方程：

使用一阶线性随机微分方程来描述航姿系统的测量误差，其方程如下：

$Z\left(t\right)\stackrel{\mathrm{\&Delta;}}{=}\left[\begin{array}{c}{\mathrm{\&theta;}}_I-{\mathrm{\&theta;}}_D\\ {\mathrm{\&gamma;}}_I-{\mathrm{\&gamma;}}_D\end{array}\right]=\left[\begin{array}{c}{\mathrm{\&delta;\&theta;}}_I+{\mathrm{\&delta;\&theta;}}_D\\ {\mathrm{\&delta;\&gamma;}}_I+{\mathrm{\&delta;\&gamma;}}_D\end{array}\right]\stackrel{\mathrm{\&Delta;}}{=}H\left(t\right)X\left(t\right)+N\left(t\right)---\left(14\right)$

式中，Z(t)为t时刻系统的量测向量；H(t)为系统量测矩阵；N(t)为系统的量测噪声向量；θ₁、γ₁表示惯性系统姿态角，δθ₁、δγ₁表示惯性系统姿态误差角；θ_D、γ_D表示由步骤(3)估计的姿态角，δθ_D、δγ_D表示姿态估计值的误差角度；

系统量测矩阵为：

H(t)＝[I_2×2 0_2×6]  (15)

内阻尼姿态的测量噪声虽然和系统加速度、速度、纬度都有关系，但是经过补偿后的内阻尼姿态误差没有呈现明显的线性相关性，所以近似为白噪声矢量：

N(t)＝[M_N M_E]^T          (16)

式中M_N、M_E分别表示北向和东向估计姿态的白噪声误差；

步骤(6)输出导航参数：导航计算机按照规定的格式和速率，将计算得到的导航参数传输给相应的控制单元或者显示仪器。

2、如权利要求1所述的基于捷联惯性导航系统的姿态估计和融合的方法，其特征在于，步骤(4)中设置系统姿态估计法使用的基本条件为：

式中，f_nx和f_ny表示地理坐标系下的系统水平加速度，它是系统加速度计输出值变换到地理坐标系后得到的；X₁、X₂、Y₁、Y₂分别表示系统使用姿态估计值的阈值。

3、如权利要求1所述的基于捷联惯性导航系统的姿态估计和融合的方法，其特征在于，步骤(4)中设置系统姿态估计法使用的基本条件为：

式中，f_x、f_y、f_z表示三轴加速度计的输出值；f_nz表示地理坐标系下的系统垂直加速度，X₁、X₂、Y₁、Y₂分别表示系统使用姿态估计值的阈值。

Images