CN102506857A - 一种基于双imu/dgps组合的相对姿态测量实时动态滤波方法 - Google Patents

Abstract

本发明公开基于双IMU/DGPS组合的相对姿态测量实时动态滤波方法，采用双光纤捷联惯导系统，通过捷联惯导实时解算，得到主惯导与从惯导系统的导航信息；判断DGPS信息是否更新，产生两种情况：当更新时，主惯导与从惯导系统进行滤波修正，建立组合导航滤波器的量测方程；当没有更新时，使用主惯导系统对从惯导系统进行滤波修正，建立组合导航滤波器的量测方程；两种情况得到的组合滤波方程通过离散化，建立离散型卡尔曼滤波器的递推方程，解算得到主惯导与从惯导系统的俯仰、横滚和航向角；随后进行相对姿态矩阵解算，得到主惯导与从惯导相对姿态角的主值。本发明提高了导航系统的稳定性，且可实时输出导航系统在测量过程中的速度、位置、姿态信息，测量范围广。

Description

一种基于双IMU/DGPS组合的相对姿态测量实时动态滤波方法

技术领域

本发明涉及惯性导航领域，具体来说，是一种基于双IMU/DGPS组合的相对姿态测量实时动态滤波方法。

背景技术

具有弹性变形的载体在受到外力、外力矩、湍流的作用时会产生结构振动变形。载体在运动过程中的形变测量，对测量设备的精度、数据更新频率、动态环境下的可靠性均有较高要求，但传统的测量方法难以满足。特别在高精度实时测量中，由于载体的不同部位所处的环境状况、所受的外力矩都是有差别的，因而分别安装在载体不同位置间的主从惯导安装位置间的相对形变会影响其正常工作。这就需要一种可以满足要求的实时动态相对姿态测量系统。

捷联惯导是一种完全自主的导航方式，它具有不依赖外界信息，隐蔽性强，机动灵活等优点，但是存在误差随时间迅速积累的问题，导航精度随时间而发散，在精度要求较高的情况下，捷联惯导系统不能单独长时间工作，必须不断以其他信息加以修正。

GPS(Global Positioning System)是一种适用于测距的空间交会定点导航系统，它不仅具有全球性、全天候和连续的精密三维定位能力，而且能实时的对运载体的速度、姿态进行测定以及精确授时，但其存在着信号易受干扰、输出数据更新频率低等缺点，且难以独立提供姿态信息。DGPS即差分GPS，是改善GPS定位或授时性能的一种方法。在GPS精确定位中占据重要角色。其原理为：在一个或若干个已知精确坐标的位置设置GPS接收机作为基准站，连续跟踪观测视野内所有可见的GPS卫星的伪距，通过与已知距离的对比，求出差分修正参数，并发送给用户。用户在观测GPS卫星伪距的同时，接受基准站发送的伪距修正参数，对观测的伪距结果进行修正，并利用修正后的伪距进行定位解算。

在现有技术中，传递对准的方法在目前的工程实践中应用较多。传递对准法是指载体上需要对准的子惯导利用已对准好的主惯导的信息进行对准的一种方法。传递对准也是动基座对准中的一种方法。由于子惯导未对准以前，平台失准角对惯导性能产生很大误差影响。而主、子惯导之间性能参数的差值能不同程度地反映了失准角的大小。因此，利用这些差值运用卡尔曼滤波方法可进行传递对准。传递对准匹配方法可以归纳为两大类，一类是计算参数匹配法，它包括速度匹配和位置匹配，一类为测量参数匹配法，包括加速度匹配、姿态匹配和角速度匹配。计算参数匹配法利用主、子惯导各自计算的导航参数(速度或位置)各分量的差值作为测量值。由于速度或位置信息不是直接从测量元件得到，不能在量测方程中直接反映出相对失准角与这些差值的关系，故必须将相对失准角和速度差都列为状态，它们之间的关系在状态方程中描述。测量参数匹配法依靠物理矢量在主、子惯导各自测量轴上分量的差异来进行对准。方法直接，对准时间短，但其精度因受载体挠性变形的影响而受限，对准时载体需作小量的机动动作。(《基于测量矢量匹配的传递对准方法研究》，夏家和、秦永元、赵长山，系统工程与电子技术，2009，31(12)，《基于惯性传感器输出匹配的舰船变形估计方法》，柳爱利、戴洪德，传感技术学报，2011，24(1))。

现有的传递对准法在应用中有着如下不足之处：

1、载体上的各器件存在误差，且传递对准的方法动态变形建模复杂；

2、传递对准的方法一般对准时间要求较短，需快速收敛；

3、传递对准的方法和动态环境、系统精度相关，需针对不同的应用背景专门设计。

发明内容

为了解决上述问题，本发明给出了一种基于双IMU/DGPS组合的相对测姿系统滤波方法，采用两个具有耐冲击、耐加速、高动态范围、高灵敏度等优点的光纤陀螺和石英挠性加速度计构成光纤捷联惯导系统，将其作为基本导航方式，并引入DGPS信号辅助惯导，可以充分利用惯导与DGPS的互补特性，发挥各自的优点，弥补各自的不足。高精度的DGPS信息可用来补偿捷联惯导系统随时间快速发散的高度通道以及随积分计算不断积累的速度、位置误差等；且在DGPS卫星信号不理想或是卫星失锁的情况下，光纤捷联惯导可以暂时独立的提供导航信息，并用所处环境条件较好的惯导数据去修正所处条件相对较差的惯导数据。因而，组合导航系统的误差要比单独的DGPS导航或单独的捷联惯导系统可能达到的误差都小；在满足同样的精度要求的情况下，加入DGPS导航信息可以降低对惯导系统的精度要求，大大降低系统成本，并且能够实现在高动态环境下实时的、高精度的导航定位和姿态计算。

一种基于双IMU/DGPS组合的相对姿态测量实时动态滤波方法，其特征在于：采用主惯导系统与从惯导系统两个光纤捷联惯导系统与一个DGPS进行组合，具体滤波方法由下述步骤来完成：

步骤1：主惯导系统和从惯导系统根据各自的惯性测量单元测得的载体加速度数据和角速度数据，分别通过捷联惯导实时解算，得到主惯导系统和从惯导系统的导航信息；

步骤2：判断DGPS信息是否有更新；

当DGPS信息更新时，进入步骤3；当DGPS信息丢失或未更新时，则进入步骤4。

步骤3：根据由DGPS获取的载体的位置、速度等信息，与步骤一中得到的主惯导系统与从惯导系统的导航信息，分别对主惯导系统与从惯导系统进行滤波修正，建立组合导航滤波器的量测方程，随后进入步骤5；

步骤4：使用主惯导系统的速度、位置信息对从惯导系统进行滤波修正，建立双惯导组合滤波器方程，随后进入步骤5；

步骤5：将惯导系统组合滤波器量测方程和状态方程离散化，建立离散型卡尔曼滤波器的递推方程；

步骤6：通过步骤5中建立的离散型卡尔曼滤波器的递推方程，进行Kalman滤波解算，得到主惯导系统与从惯导系统的俯仰、横滚和航向角；

步骤7：根据步骤6得到的主惯导系统的俯仰、横滚和航向角ψ_AE、ψ_AN和ψ_AU，与从惯导系统的俯仰、横滚和航向角ψ_BE、ψ_BN和ψ_BU，建立由地理坐标系到主惯导系统载体坐标系的转换矩阵为由地理坐标系到从惯导系统载体坐标系的转换矩阵为则：

$C_A^n=\left(\begin{array}{ccc}\cos {\mathrm{\ψ}}_{\mathrm{AN}}\cos {\mathrm{\ψ}}_{\mathrm{AU}}-\sin {\mathrm{\ψ}}_{\mathrm{AN}}\sin {\mathrm{\ψ}}_{\mathrm{AE}}\sin {\mathrm{\ψ}}_{\mathrm{AU}}& -\cos {\mathrm{\ψ}}_{\mathrm{AE}}\sin {\mathrm{\ψ}}_{\mathrm{AU}}& \sin {\mathrm{\ψ}}_{\mathrm{AN}}\cos {\mathrm{\ψ}}_{\mathrm{AU}}+\cos {\mathrm{\ψ}}_{\mathrm{AN}}\sin {\mathrm{\ψ}}_{\mathrm{AE}}\sin {\mathrm{\ψ}}_{\mathrm{AU}}\\ \cos {\mathrm{\ψ}}_{\mathrm{AN}}\sin {\mathrm{\ψ}}_{\mathrm{AU}}+\sin {\mathrm{\ψ}}_{\mathrm{AN}}\sin {\mathrm{\ψ}}_{\mathrm{AE}}\cos {\mathrm{\ψ}}_{\mathrm{AU}}& \cos {\mathrm{\ψ}}_{\mathrm{AE}}\cos {\mathrm{\ψ}}_{\mathrm{AU}}& \sin {\mathrm{\ψ}}_{\mathrm{AN}}\sin {\mathrm{\ψ}}_{\mathrm{AU}}-\cos {\mathrm{\ψ}}_{\mathrm{AN}}\sin {\mathrm{\ψ}}_{\mathrm{AE}}\cos {\mathrm{\ψ}}_{\mathrm{AU}}\\ -\sin {\mathrm{\ψ}}_{\mathrm{AN}}\cos {\mathrm{\ψ}}_{\mathrm{AE}}& \sin {\mathrm{\ψ}}_{\mathrm{AE}}& \cos {\mathrm{\ψ}}_{\mathrm{AN}}\cos {\mathrm{\ψ}}_{\mathrm{AE}}\end{array}\right)$

$C_B^n=\left(\begin{array}{ccc}\cos {\mathrm{\ψ}}_{\mathrm{BN}}\cos {\mathrm{\ψ}}_{\mathrm{BU}}-\sin {\mathrm{\ψ}}_{\mathrm{BN}}\sin {\mathrm{\ψ}}_{\mathrm{BE}}\sin {\mathrm{\ψ}}_{\mathrm{BU}}& -\cos {\mathrm{\ψ}}_{\mathrm{BE}}\sin {\mathrm{\ψ}}_{\mathrm{BU}}& \sin {\mathrm{\ψ}}_{\mathrm{BN}}\cos {\mathrm{\ψ}}_{\mathrm{BU}}+\cos {\mathrm{\ψ}}_{\mathrm{BN}}\sin {\mathrm{\ψ}}_{\mathrm{BE}}\sin {\mathrm{\ψ}}_{\mathrm{BU}}\\ \cos {\mathrm{\ψ}}_{\mathrm{BN}}\sin {\mathrm{\ψ}}_{\mathrm{BU}}+\sin {\mathrm{\ψ}}_{\mathrm{BN}}\sin {\mathrm{\ψ}}_{\mathrm{BE}}\cos {\mathrm{\ψ}}_{\mathrm{BU}}& \cos {\mathrm{\ψ}}_{\mathrm{BE}}\cos {\mathrm{\ψ}}_{\mathrm{BU}}& \sin {\mathrm{\ψ}}_{\mathrm{BN}}\sin {\mathrm{\ψ}}_{\mathrm{BU}}-\cos {\mathrm{\ψ}}_{\mathrm{BN}}\sin {\mathrm{\ψ}}_{\mathrm{BE}}\cos {\mathrm{\ψ}}_{\mathrm{BU}}\\ -\sin {\mathrm{\ψ}}_{\mathrm{BN}}\cos {\mathrm{\ψ}}_{\mathrm{BE}}& \sin {\mathrm{\ψ}}_{\mathrm{BE}}& \cos {\mathrm{\ψ}}_{\mathrm{BN}}\cos {\mathrm{\ψ}}_{\mathrm{BE}}\end{array}\right)$

得到由主惯导系统载体坐标系到从惯导系统载体坐标系的坐标转换矩阵

为：

$C_A^B=C_n^B\·C_A^n={\left(C_B^n\right)}^T\·C_A^n$

则主惯导系统与从惯导相对姿态角的主值为：

${\mathrm{\ψ}}_{\mathrm{EAB}0}=\arctan \frac{C_A^B\left(\mathrm{3,2}\right)}{\sqrt{1-{\left(C_A^B\left(\mathrm{3,2}\right)\right)}^2}}$

${\mathrm{\ψ}}_{\mathrm{NAB}0}=\arctan (-\frac{C_A^B(3,1)}{C_A^B\left(\mathrm{3,3}\right)})$

${\mathrm{\ψ}}_{\mathrm{UAB}0}=\arctan (-\frac{C_A^B\left(\mathrm{1,2}\right)}{C_A^B\left(\mathrm{2,2}\right)})$

步骤8：主惯导系统与从惯导系统间的相对姿态置信度估计。

本发明的优点在于：

1、本发明方法提出了对具有DGPS和无DGPS两种不同情况下的滤波方法，有效解决了在DGPS信号缺失这种可能性下的解算问题，提高了导航系统的稳定性，且可实时输出导航系统在测量过程中的速度、位置、姿态信息，测量范围广；

2、本发明方法采用双IMU/DGPS组合导航系统对动载体姿态变形误差进行测量，得出主惯导系统安装点与从惯导安装点之间相对姿态，同时采用了高精度的DGPS信息作为外部信息源，实现组合测量；

3、本发明方法实现了导航信息的优化，提高了导航系统精度，而且导航输出具有实时性，能满足高精度实时测量要求；

4、本发明方法适用于所有的动载体形变测量，具有通用性。

附图说明

图1是本发明的方法流程图。

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明一种基于双IMU/DGPS组合的相对姿态测量实时动态滤波方法，采用两个光纤捷联惯导系统(主惯导系统与从惯导系统)分别与一个DGPS进行组合，对两个光纤捷联惯导系统的误差进行校正，其中主惯导系统安装于载体环境条件变化较小的部分，从惯导系统安装于载体温度和振动环境相对比较恶劣的部位。其滤波方法由以下步骤来完成：

步骤1：主惯导系统和从惯导系统根据各自的惯性测量单元测得的载体加速度数据和角速度数据，分别通过捷联惯导实时解算，得到主惯导系统和从惯导系统的导航信息，包括位置信息、速度信息和姿态信息；

设惯导系统的导航坐标系为当地地理坐标系，水平位置误差为δL，δλ，δh，δL为惯导系统经度误差，δλ为惯导系统纬度误差，δh为惯导系统高度误差，水平速度误差为δV_E，δV_N，δV_U，姿态误差为ψ_E，ψ_N，ψ_U，陀螺漂移项ε_E，ε_N，ε_U，加速度计零偏

则捷联惯导误差方程为：

状态矢量：

$X\left(t\right)={[\mathrm{\δL},\mathrm{\δ\λ},\mathrm{\δh},\mathrm{\δ}{V}_E,\mathrm{\δ}{V}_N,\mathrm{\δ}{V}_U,{\mathrm{\ψ}}_E,{\mathrm{\ψ}}_N,{\mathrm{\ψ}}_U,{\mathrm{\ϵ}}_E,{\mathrm{\ϵ}}_N,{\mathrm{\ϵ}}_U,{\▿}_E,{\▿}_N,{\▿}_U]}^T---\left(1\right)$

姿态误差方程：

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ψ}}}_E=-\frac{{\mathrm{\δV}}_E}{R_M+h}+({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L){\mathrm{\ψ}}_N-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}){\mathrm{\ψ}}_U+{\mathrm{\ϵ}}_E\\ {\stackrel{\·}{\mathrm{\ψ}}}_N=\frac{\mathrm{\δ}{V}_E}{R_N+h}-{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}-({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L){\mathrm{\ψ}}_E-\frac{V_N}{R_M+h}{\mathrm{\ψ}}_U+{\mathrm{\ϵ}}_N\\ {\stackrel{\·}{\mathrm{\ψ}}}_U=\frac{\mathrm{\δ}{V}_E}{R_N+h}\tan L+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)\mathrm{\δL}+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}){\mathrm{\ψ}}_E+\frac{V_N}{R_N+h}{\mathrm{\ψ}}_N+{\mathrm{\ϵ}}_U\end{array}\right.---\left(2\right)$

速度误差方程：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{V}}_E=f_N{\mathrm{\ψ}}_U-f_U{\mathrm{\ψ}}_N+2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)\mathrm{\δ}{V}_N\\ +(\frac{V_N}{R_M+h}\tan L-\frac{V_U}{R_M+h})\mathrm{\δ}{V}_E+(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\mathrm{\δ}{V}_U\\ +(2{\mathrm{\ω}}_{\mathrm{ie}}{V}_N\cos L+\frac{V_E{V}_N}{R_N+h}{\sec }^{2}L)\mathrm{\δL}+{\▿}_E\\ \mathrm{\δ}{\stackrel{\·}{V}}_N=f_U{\mathrm{\ψ}}_E-f_E{\mathrm{\ψ}}_U-2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)\mathrm{\δ}{V}_E\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)V_E\mathrm{\δL}+{\▿}_E\\ \mathrm{\δ}{\stackrel{\·}{V}}_U=f_E{\mathrm{\ψ}}_N-f_N{\mathrm{\ψ}}_E+2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\mathrm{\δ}{V}_E+2\frac{V_N}{R_M+h}\mathrm{\δ}{V}_N\\ -2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{V}_E\mathrm{\δL}+{\▿}_E\end{array}\right.---\left(3\right)$

水平位置误差方程：

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{L}=\frac{1}{R_M+h}\mathrm{\δ}{V}_N\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{\sec L}{R_N+h}\mathrm{\δ}{V}_E+\frac{V_E}{R_N+h}\sec L\tan \mathrm{L\δL}\\ \mathrm{\δ}\stackrel{\·}{h}=\mathrm{\δ}{V}_U\end{array}\right.---\left(4\right)$

上述式(1)～(4)中，ω_ie为地球自转角速度，ω_ie＝7.29×10^-5rad/s；R_E为参考椭球体的长半径，R_E＝6378137m；f为地球扁率，f＝1/298.257；R_M为子午圈主曲率半径R_M＝R_E(1-2f+3fsin²L)；R_N为卯酉圈主曲率半径R_N＝R_E(1+fsin²L)；

根据主惯导系统和从惯导系统根据各自的惯性测量单元测得的载体加速度数据和角速度数据，通过上述捷联惯导误差方程，可分别得到主惯导系统与从惯导系统的水平位置误差δL_A，δλ_A，δh_A与δL_B，δλ_B，δh_B，水平速度误差δV_AE，δV_AN，δV_AU与δV_BE，δV_BN，δV_BU，姿态误差ψ_AE，ψ_AN，ψ_AU与ψ_BE，ψ_BN，ψ_BU，并分别对主惯导系统与从惯导系统的水平位置误差，水平速度误差，姿态误差进行迭代运算，从而得到主惯导系统与从惯导系统的位置、速度和姿态值。

步骤2：判断DGPS信息是否有更新；

当DGPS信息更新时，进入步骤3；当DGPS信息丢失或未更新时，则进入步骤4。

步骤：3：根据由DGPS获取的载体的位置、速度等信息，与步骤一中得到的主惯导系统与从惯导系统的导航信息，分别对主惯导系统与从惯导系统进行滤波修正，建立组合导航滤波器的量测方程，采用位置/速度组合模式，随后进入步骤5；

惯导系统的位置信息可表示为：

$\left\{\begin{array}{c}{L}_I=L_t+\mathrm{\δL}\\ {\mathrm{\λ}}_I={\mathrm{\λ}}_t+\mathrm{\δ\λ}\\ h_I=h_t+\mathrm{\δt}\end{array}\right.---\left(5\right)$

DGPS测得的载体位置信息可表示为：

$\left\{\begin{array}{c}{L}_{\mathrm{GPS}}=L_t-\frac{N_N}{R_E}\\ {\mathrm{\λ}}_{\mathrm{GPS}}={\mathrm{\λ}}_t-\frac{N_E}{R_E\cos L}\\ h_{\mathrm{GPS}}=h_t-N_U\end{array}\right.---\left(6\right)$

式(5)、式(6)中：L_t，λ_t，h_t为惯导系统的真实位置信息，N_E，N_N，N_U分别为DGPS测量得到的载体沿东、北、天方向的位置误差。

则惯导系统的位置量测矢量为：

$Z_p\left(t\right)=\left(\begin{array}{c}(L_I-L_{\mathrm{GPS}})R_E\\ ({\mathrm{\λ}}_I-{\mathrm{\λ}}_{\mathrm{GPS}})R_E\cos L\\ h_I-h_G\end{array}\right)=\left(\begin{array}{c}{R}_E\mathrm{\δL}+N_N\\ R_E\mathrm{\δ\λ}\cos L+N_E\\ \mathrm{\δh}+N_U\end{array}\right)=H_p\left(t\right)X\left(t\right)+V_p\left(t\right)---\left(7\right)$

式(7)中，H_p(t)＝(diag(R RcosL 1)0_3×12)_3×15，V_p(t)＝(N_N N_E N_U)^T。设DGPS接收机伪距测量误差为σ_ρ，平面位置精度因子为HDOP，高程精度因子为VDOP，将DGPS量测噪声矢量V(t)作为白噪声处理，则DGPS位置噪声方差为

为：

$\left\{\begin{array}{c}{\mathrm{\σ}}_{\mathrm{pN}}={\mathrm{\σ}}_{\mathrm{pE}}={\mathrm{\σ}}_{\mathrm{\ρ}}\·\mathrm{HDOP}\\ {\mathrm{\σ}}_{\mathrm{pU}}={\mathrm{\σ}}_{\mathrm{\ρ}}\·\mathrm{VDOP}\end{array}\right.---\left(8\right)$

惯导系统的速度信息可表示为：

$\left\{\begin{array}{c}{V}_{\mathrm{IN}}=V_N+{\mathrm{\δV}}_N\\ V_{\mathrm{IE}}=V_E+{\mathrm{\δV}}_E\\ V_{\mathrm{IU}}=V_U+{\mathrm{\δV}}_U\end{array}\right.---\left(9\right)$

DGPS测得的载体速度信息可表示为：

$\left\{\begin{array}{c}{V}_{\mathrm{GN}}=V_N-M_N\\ V_{\mathrm{GE}}=V_E-M_E\\ V_{\mathrm{GU}}=V_U-M_U\end{array}\right.---\left(10\right)$

式中，V_N，V_E，V_U为惯导系统的真实速度信息，M_E，M_N，M_U分别为DGPS沿东、北、天方向的载体速度误差。

则惯导系统的速度量测矢量为：

$Z_V\left(t\right)=\left(\begin{array}{c}{V}_{\mathrm{IN}}-V_{\mathrm{GN}}\\ V_{\mathrm{IE}}-V_{\mathrm{GE}}\\ V_{\mathrm{IU}}-V_{\mathrm{GU}}\end{array}\right)=\left(\begin{array}{c}\mathrm{\δ}{V}_N+M_N\\ {\mathrm{\δV}}_E+M_E\\ \mathrm{\δ}{V}_U+M_U\end{array}\right)=H_V\left(t\right)X\left(t\right)+V_V\left(t\right)---\left(11\right)$

式中，H_V(t)＝(0_3×3diag(111)0_3×9)_3×15。

设DGPS接收机伪距率测量误差为

则DGPS速度噪声方差为

为：

$\left\{\begin{array}{c}{\mathrm{\σ}}_{\mathrm{VN}}={\mathrm{\σ}}_{\mathrm{VE}}={\mathrm{\σ}}_{\stackrel{\·}{\mathrm{\ρ}}}\·\mathrm{HDOP}\\ {\mathrm{\σ}}_{\mathrm{VU}}={\mathrm{\σ}}_{\stackrel{\·}{\mathrm{\ρ}}}\·\mathrm{VDOP}\end{array}\right.---\left(12\right)$

综上可得在DGPS信号正常时惯导系统组合滤波器量测方程如下：

$Z\left(t\right)=\left(\begin{array}{c}{Z}_p\left(t\right)\\ Z_V\left(t\right)\end{array}\right)=\left(\begin{array}{c}{H}_p\left(t\right)\\ H_V\left(t\right)\end{array}\right)X\left(t\right)+\left(\begin{array}{c}{V}_p\left(t\right)\\ V_V\left(t\right)\end{array}\right)=H\left(t\right)X\left(t\right)+V\left(t\right)---\left(13\right)$

步骤4：当DGPS信息丢失或未更新时，无法使用DGPS信息对主惯导系统与从惯导系统进行滤波修正，此时主惯导系统工作在纯惯导模式下，而从惯导系统工作在主惯导系统与从惯导系统组合模式下，由于从惯导系统位于温度和振动环境比较恶劣的部位，为避免其精度随时间过快发散，因此使用主惯导系统的速度、位置信息对从惯导系统进行滤波修正，建立双惯导组合滤波器方程，随后进入步骤5。

从惯导系统的位置量测信息可表示为：

$\left\{\begin{array}{c}{L}_B=L_{\mathrm{Bt}}+{\mathrm{\δL}}_B\\ {\mathrm{\λ}}_B={\mathrm{\λ}}_{\mathrm{Bt}}+{\mathrm{\δ\λ}}_B\\ h_B=h_{\mathrm{Bt}}+{\mathrm{\δh}}_B\end{array}\right.---\left(14\right)$

式(14)中，δL_B、δλ_R、δh_B分别为从惯导系统经度、纬度、高度上的位置误差。主惯导系统的位置量测信息可表示为：

$\left\{\begin{array}{c}{L}_A=L_{\mathrm{Bt}}-\mathrm{\δ}{L}_A\\ {\mathrm{\λ}}_A={\mathrm{\λ}}_{\mathrm{Bt}}-\mathrm{\δ}{\mathrm{\λ}}_A\\ h_A=h_{\mathrm{Bt}}-\mathrm{\δ}{h}_A\end{array}\right.---\left(15\right)$

式(15)中，L_Bt，λ_Bt，h_Bt为从惯导系统的真实位置信息。

则惯导系统位置量测矢量定义为：

$Z_p^{\′}\left(t\right)=\left(\begin{array}{c}(L_B-L_A)R\\ ({\mathrm{\λ}}_B-{\mathrm{\λ}}_A)R\cos L\\ h_B-h_A\end{array}\right)=\left(\begin{array}{c}\mathrm{R\δ}{L}_B+\mathrm{R\δ}{L}_A\\ \mathrm{R\δ}{\mathrm{\λ}}_B\cos L+\mathrm{R\δ}{\mathrm{\λ}}_A\cos L\\ \mathrm{\δ}{h}_B+\mathrm{\δ}{h}_A\end{array}\right)=H_p^{\′}\left(t\right)X\left(t\right)+V_p^{\′}\left(t\right)---\left(16\right)$

式(16)中，H′_p(t)＝(diag(RRcosL1)0_3×12)_3×15，V′_p(t)＝(RδL_A Rδλ_A cosLδh_A)^T

主惯导系统与从惯导系统组合滤波器的位置噪声的最优估计由主惯导系统估计误差的协方差阵P_A(t)为：

$\left\{\begin{array}{c}{\widehat{\mathrm{\σ}}}_{\mathrm{pN}}^2=P_A\left(\mathrm{1,1}\right)\·R^2\\ {\widehat{\mathrm{\σ}}}_{\mathrm{pE}}^2=P_A\left(\mathrm{2,2}\right)\·R^2{\cos }^{2}L\\ {\widehat{\mathrm{\σ}}}_{\mathrm{pU}}^2=P_A\left(\mathrm{3,3}\right)\end{array}\right.---\left(17\right)$

式(17)中，

为主惯导位置误差方差的估计值。

从惯导系统的速度信息可表示为：

$\left\{\begin{array}{c}{V}_{\mathrm{BN}}=V_{\mathrm{BNt}}+\mathrm{\δ}{V}_{\mathrm{BN}}\\ V_{\mathrm{BE}}=V_{\mathrm{BEt}}+{\mathrm{\δV}}_{\mathrm{BE}}\\ V_{\mathrm{BU}}=V_{\mathrm{BUt}}+\mathrm{\δ}{V}_{\mathrm{BU}}\end{array}\right.---\left(18\right)$

主惯导系统的速度量测信息可表示为：

$\left\{\begin{array}{c}{V}_{\mathrm{AN}}=V_{\mathrm{BNt}}-\mathrm{\δ}{V}_{\mathrm{AN}}\\ V_{\mathrm{AE}}=V_{\mathrm{BEt}}-\mathrm{\δ}{V}_{\mathrm{AE}}\\ V_{\mathrm{AU}}=V_{\mathrm{BUt}}-\mathrm{\δ}{V}_{\mathrm{AU}}\end{array}\right.---\left(19\right)$

式(19)中，V_BNt，V_BEt，V_BUt为从惯导系统真实速度信息。

则惯导系统位置量测速度量测矢量为：

$Z_V^{\′}\left(t\right)=\left(\begin{array}{c}{V}_{\mathrm{BN}}-V_{\mathrm{AN}}\\ V_{\mathrm{BE}}-V_{\mathrm{AE}}\\ V_{\mathrm{BU}}-V_{\mathrm{AU}}\end{array}\right)=\left(\begin{array}{c}\mathrm{\δ}{V}_{\mathrm{BN}}+\mathrm{\δ}{V}_{\mathrm{AN}}\\ {\mathrm{\δV}}_{\mathrm{BE}}+\mathrm{\δ}{V}_{\mathrm{AE}}\\ \mathrm{\δ}{V}_{\mathrm{BU}}+\mathrm{\δ}{V}_{\mathrm{AU}}\end{array}\right)=H_V^{\′}\left(t\right)X\left(t\right)+V_V^{\′}\left(t\right)---\left(20\right)$

式中，H_V(t)＝(0_3×3 diag(111)0_3×9)_3×15。

主惯导系统与从惯导系统组合滤波器的速度量测噪声的最优估计由主惯导系统估计误差的协方差阵P_A(t)给出：

$\left\{\begin{array}{c}{\widehat{\mathrm{\σ}}}_{\mathrm{VE}}^2=P_A\left(\mathrm{4,4}\right)\\ {\widehat{\mathrm{\σ}}}_{\mathrm{VN}}^2=P_A\left(\mathrm{5,5}\right)\\ {\widehat{\mathrm{\σ}}}_{\mathrm{VU}}^2=P_A\left(\mathrm{6,6}\right)\end{array}\right.---\left(21\right)$

式中，

为主惯导系统速度误差方差的估计值。

综上可得在无DGPS信号时惯导系统组合滤波器量测方程如下：

$Z^{\′}\left(t\right)=\left(\begin{array}{c}{Z}_p^{\′}\left(t\right)\\ Z_V^{\′}\left(t\right)\end{array}\right)=\left(\begin{array}{c}{H}_p^{\′}\left(t\right)\\ H_V^{\′}\left(t\right)\end{array}\right)X\left(t\right)+\left(\begin{array}{c}{V}_p^{\′}\left(t\right)\\ V_V^{\′}\left(t\right)\end{array}\right)=H^{\′}\left(t\right)X\left(t\right)+V^{\′}\left(t\right)---\left(22\right)$

步骤5：将惯导系统组合滤波器量测方程与步骤1中的状态方程离散化，建立离散型卡尔曼滤波器的递推方程；

离散型卡尔曼滤波器的递推方程建立过程如下：

首先，给定惯导系统的一阶线性状态方程和量测方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(23\right)$

Z(t)＝H(t)X(t)+V(t)(24)

进而将状态方程(23)和量测方程(24)离散化可得：

X_k＝Ф_k，k-1X_k-1+Γ_k-1W_k-1(25)

Z_k＝H_kX_k+V_k(26)

其中，X_k为状态向量，Ф_k，k-1为状态转移矩阵，Z_k为量测向量，H_k为量测矩阵，Γ_k-1为系统噪声矩阵，W_k-1为系统噪声向量，V_k为量测噪声向量。W_k-1、V_k是不相关高斯白噪声列。

状态预测估计方程为：

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(27\right)$

方差预测方程为：

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(28\right)$

状态预测估计方程为：

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})---\left(29\right)$

方差迭代方程：

$P_k=P_{k/k-1}-P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}{H}_k{P}_{k/k-1}$

$=(I-K_k{H}_k)P_{k/k-1}---\left(30\right)$

滤波增益方程为：

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(31\right)$

初始条件为：

${\hat{X}}_0=E\left(X_0\right)={\mathrm{\μ}}_0,\mathrm{var}{\tilde{X}}_0=\mathrm{var}{X}_0=P_0---\left(32\right)$

验前统计量为：

E[W_k]＝0，Cov[W_k，W_j]＝E[W_kW_j ^T]＝Q_kδ_kj(33)

E[V_K]＝0，Cov[V_k，V_j]＝E[V_kV_j ^T]＝R_kδ_kj(34)

Cov[W_k，V_j]＝E[W_kV_j ^T]＝0(35)

${\mathrm{\δ}}_{\mathrm{kj}}=\left\{\begin{array}{cc}0,& k\≠j\\ 1,& k=j\end{array}\right.---\left(36\right)$

步骤6：通过步骤5中建立的离散型卡尔曼滤波器的递推方程，进行Kalman滤波解算，得到主惯导系统的俯仰、横滚和航向角分别为ψ_AE、ψ_AN和ψ_AU，从惯导系统的俯仰、横滚和航向角分别为ψ_BE、ψ_BN和ψ_BU。

步骤7：相对姿态矩阵解算；

根据步骤6得到的主惯导系统与从惯导系统的俯仰、横滚和航向角，由地理坐标系到主惯导系统载体坐标系的转换矩阵为

由地理坐标系到从惯导系统载体坐标系的转换矩阵为

则：

$C_A^n=\left(\begin{array}{ccc}\cos {\mathrm{\ψ}}_{\mathrm{AN}}\cos {\mathrm{\ψ}}_{\mathrm{AU}}-\sin {\mathrm{\ψ}}_{\mathrm{AN}}\sin {\mathrm{\ψ}}_{\mathrm{AE}}\sin {\mathrm{\ψ}}_{\mathrm{AU}}& -\cos {\mathrm{\ψ}}_{\mathrm{AE}}\sin {\mathrm{\ψ}}_{\mathrm{AU}}& \sin {\mathrm{\ψ}}_{\mathrm{AN}}\cos {\mathrm{\ψ}}_{\mathrm{AU}}+\cos {\mathrm{\ψ}}_{\mathrm{AN}}\sin {\mathrm{\ψ}}_{\mathrm{AE}}\sin {\mathrm{\ψ}}_{\mathrm{AU}}\\ \cos {\mathrm{\ψ}}_{\mathrm{AN}}\sin {\mathrm{\ψ}}_{\mathrm{AU}}+\sin {\mathrm{\ψ}}_{\mathrm{AN}}\sin {\mathrm{\ψ}}_{\mathrm{AE}}\cos {\mathrm{\ψ}}_{\mathrm{AU}}& \cos {\mathrm{\ψ}}_{\mathrm{AE}}\cos {\mathrm{\ψ}}_{\mathrm{AU}}& \sin {\mathrm{\ψ}}_{\mathrm{AN}}\sin {\mathrm{\ψ}}_{\mathrm{AU}}-\cos {\mathrm{\ψ}}_{\mathrm{AN}}\sin {\mathrm{\ψ}}_{\mathrm{AE}}\cos {\mathrm{\ψ}}_{\mathrm{AU}}\\ -\sin {\mathrm{\ψ}}_{\mathrm{AN}}\cos {\mathrm{\ψ}}_{\mathrm{AE}}& \sin {\mathrm{\ψ}}_{\mathrm{AE}}& \cos {\mathrm{\ψ}}_{\mathrm{AN}}\cos {\mathrm{\ψ}}_{\mathrm{AE}}\end{array}\right)$

$C_B^n=\left(\begin{array}{ccc}\cos {\mathrm{\ψ}}_{\mathrm{BN}}\cos {\mathrm{\ψ}}_{\mathrm{BU}}-\sin {\mathrm{\ψ}}_{\mathrm{BN}}\sin {\mathrm{\ψ}}_{\mathrm{BE}}\sin {\mathrm{\ψ}}_{\mathrm{BU}}& -\cos {\mathrm{\ψ}}_{\mathrm{BE}}\sin {\mathrm{\ψ}}_{\mathrm{BU}}& \sin {\mathrm{\ψ}}_{\mathrm{BN}}\cos {\mathrm{\ψ}}_{\mathrm{BU}}+\cos {\mathrm{\ψ}}_{\mathrm{BN}}\sin {\mathrm{\ψ}}_{\mathrm{BE}}\sin {\mathrm{\ψ}}_{\mathrm{BU}}\\ \cos {\mathrm{\ψ}}_{\mathrm{BN}}\sin {\mathrm{\ψ}}_{\mathrm{BU}}+\sin {\mathrm{\ψ}}_{\mathrm{BN}}\sin {\mathrm{\ψ}}_{\mathrm{BE}}\cos {\mathrm{\ψ}}_{\mathrm{BU}}& \cos {\mathrm{\ψ}}_{\mathrm{BE}}\cos {\mathrm{\ψ}}_{\mathrm{BU}}& \sin {\mathrm{\ψ}}_{\mathrm{BN}}\sin {\mathrm{\ψ}}_{\mathrm{BU}}-\cos {\mathrm{\ψ}}_{\mathrm{BN}}\sin {\mathrm{\ψ}}_{\mathrm{BE}}\cos {\mathrm{\ψ}}_{\mathrm{BU}}\\ -\sin {\mathrm{\ψ}}_{\mathrm{BN}}\cos {\mathrm{\ψ}}_{\mathrm{BE}}& \sin {\mathrm{\ψ}}_{\mathrm{BE}}& \cos {\mathrm{\ψ}}_{\mathrm{BN}}\cos {\mathrm{\ψ}}_{\mathrm{BE}}\end{array}\right)---\left(38\right)$

得到由主惯导系统载体坐标系到从惯导系统载体坐标系的坐标转换矩阵

为：

$C_A^B=C_n^B\·C_A^n={\left(C_B^n\right)}^T\·C_A^n---\left(39\right)$

则主惯导系统与从惯导相对姿态角的主值为：

${\mathrm{\ψ}}_{\mathrm{EAB}0}=\arctan \frac{C_A^B\left(\mathrm{3,2}\right)}{\sqrt{1-{\left(C_A^B\left(\mathrm{3,2}\right)\right)}^2}}---\left(40\right)$

${\mathrm{\ψ}}_{\mathrm{NAB}0}=\arctan (-\frac{C_A^B(3,1)}{C_A^B\left(\mathrm{3,3}\right)})---\left(41\right)$

${\mathrm{\ψ}}_{\mathrm{UAB}0}=\arctan (-\frac{C_A^B\left(\mathrm{1,2}\right)}{C_A^B\left(\mathrm{2,2}\right)})---\left(42\right)$

步骤7：主惯导系统与从惯导系统间的相对姿态置信度估计；

主惯导系统与从惯导系统各自的姿态误差分别由协方差阵P_A(t)和P_B(t)给出最优估计。设主惯导系统在地理坐标系下(x、y、z)三个坐标轴方向上姿态角的噪声方差估计值分别

从惯导系统为

则：

$\left\{\begin{array}{c}{\widehat{\mathrm{\δ}}}_{\mathrm{A\ψx}}^2=P_A\left(\mathrm{7,7}\right)\\ {\widehat{\mathrm{\δ}}}_{\mathrm{A\ψy}}^2=P_A\left(\mathrm{8,8}\right)\\ {\widehat{\mathrm{\δ}}}_{\mathrm{A\ψz}}^2=P_A\left(\mathrm{9,9}\right)\\ {\widehat{\mathrm{\δ}}}_{\mathrm{B\ψx}}^2=P_B\left(\mathrm{7,7}\right)\\ {\widehat{\mathrm{\δ}}}_{\mathrm{B\ψy}}^2=P_B\left(\mathrm{8,8}\right)\\ {\widehat{\mathrm{\δ}}}_{\mathrm{B\ψz}}^2=P_B\left(\mathrm{9,9}\right)\end{array}\right.---\left(43\right)$

在主惯导系统与从惯导系统精度近似不相关的情况下，相对姿态的置信度估计值为：

$\left\{\begin{array}{c}{\widehat{\mathrm{\δ}}}_{\mathrm{AB\ψx}}=\sqrt{{\widehat{\mathrm{\δ}}}_{\mathrm{A\ψx}}^2+{\widehat{\mathrm{\δ}}}_{\mathrm{B\ψx}}^2}\\ {\widehat{\mathrm{\δ}}}_{\mathrm{AB\ψy}}=\sqrt{{\widehat{\mathrm{\δ}}}_{\mathrm{A\ψy}}^2+{\widehat{\mathrm{\δ}}}_{\mathrm{B\ψy}}^2}\\ {\widehat{\mathrm{\δ}}}_{\mathrm{AB\ψz}}=\sqrt{{\widehat{\mathrm{\δ}}}_{\mathrm{A\ψz}}^2+{\widehat{\mathrm{\δ}}}_{\mathrm{B\ψz}}^2}\end{array}\right.---\left(44\right)$

式(44)中，

分别表示两惯导相对姿态置信度估计值。

在动态载体上的高精度相对姿态测量中，通过上述方法有效解决了在DGPS信号缺失这种可能性下的解算问题，提高了导航系统的稳定性，可实时输出导航系统在测量过程中的速度、位置、姿态信息，测量范围广，且实现了导航信息的优化，提高了导航系统精度，能满足高精度实时测量要求。

Claims (6)

1.一种基于双IMU/DGPS组合的相对姿态测量实时动态滤波方法，其特征在于：采用主惯导系统与从惯导系统两个光纤捷联惯导系统与一个DGPS进行组合，具体滤波方法由下述步骤来完成：

步骤1：主惯导系统和从惯导系统根据各自的惯性测量单元测得的载体加速度数据和角速度数据，分别通过捷联惯导实时解算，得到主惯导系统和从惯导系统的导航信息；

步骤2：判断DGPS信息是否有更新；

当DGPS信息更新时，进入步骤3；当DGPS信息丢失或未更新时，则进入步骤4。

步骤3：根据由DGPS获取的载体的位置、速度等信息，与步骤一中得到的主惯导系统与从惯导系统的导航信息，分别对主惯导系统与从惯导系统进行滤波修正，建立组合导航滤波器的量测方程，随后进入步骤5；

步骤4：使用主惯导系统的速度、位置信息对从惯导系统进行滤波修正，建立双惯导组合滤波器方程，随后进入步骤5；

步骤5：将惯导系统组合滤波器量测方程和状态方程离散化，建立离散型卡尔曼滤波器的递推方程；

步骤6：通过步骤5中建立的离散型卡尔曼滤波器的递推方程，进行Kalman滤波解算，得到主惯导系统与从惯导系统的俯仰、横滚和航向角；

步骤7：根据步骤6得到的主惯导系统的俯仰、横滚和航向角ψ_AE、ψ_AN和ψ_AU，与从惯导系统的俯仰、横滚和航向角ψ_BE、ψ_BM和ψ_BU，建立由地理坐标系到主惯导系统载体坐标系的转换矩阵为

由地理坐标系到从惯导系统载体坐标系的转换矩阵为

则：

$C_A^n=\left(\begin{array}{ccc}\cos {\mathrm{\ψ}}_{\mathrm{AN}}\cos {\mathrm{\ψ}}_{\mathrm{AU}}-\sin {\mathrm{\ψ}}_{\mathrm{AN}}\sin {\mathrm{\ψ}}_{\mathrm{AE}}\sin {\mathrm{\ψ}}_{\mathrm{AU}}& -\cos {\mathrm{\ψ}}_{\mathrm{AE}}\sin {\mathrm{\ψ}}_{\mathrm{AU}}& \sin {\mathrm{\ψ}}_{\mathrm{AN}}\cos {\mathrm{\ψ}}_{\mathrm{AU}}+\cos {\mathrm{\ψ}}_{\mathrm{AN}}\sin {\mathrm{\ψ}}_{\mathrm{AE}}\sin {\mathrm{\ψ}}_{\mathrm{AU}}\\ \cos {\mathrm{\ψ}}_{\mathrm{AN}}\sin {\mathrm{\ψ}}_{\mathrm{AU}}+\sin {\mathrm{\ψ}}_{\mathrm{AN}}\sin {\mathrm{\ψ}}_{\mathrm{AE}}\cos {\mathrm{\ψ}}_{\mathrm{AU}}& \cos {\mathrm{\ψ}}_{\mathrm{AE}}\cos {\mathrm{\ψ}}_{\mathrm{AU}}& \sin {\mathrm{\ψ}}_{\mathrm{AN}}\sin {\mathrm{\ψ}}_{\mathrm{AU}}-\cos {\mathrm{\ψ}}_{\mathrm{AN}}\sin {\mathrm{\ψ}}_{\mathrm{AE}}\cos {\mathrm{\ψ}}_{\mathrm{AU}}\\ -\sin {\mathrm{\ψ}}_{\mathrm{AN}}\cos {\mathrm{\ψ}}_{\mathrm{AE}}& \sin {\mathrm{\ψ}}_{\mathrm{AE}}& \cos {\mathrm{\ψ}}_{\mathrm{AN}}\cos {\mathrm{\ψ}}_{\mathrm{AE}}\end{array}\right)$

$C_B^n=\left(\begin{array}{ccc}\cos {\mathrm{\ψ}}_{\mathrm{BN}}\cos {\mathrm{\ψ}}_{\mathrm{BU}}-\sin {\mathrm{\ψ}}_{\mathrm{BN}}\sin {\mathrm{\ψ}}_{\mathrm{BE}}\sin {\mathrm{\ψ}}_{\mathrm{BU}}& -\cos {\mathrm{\ψ}}_{\mathrm{BE}}\sin {\mathrm{\ψ}}_{\mathrm{BU}}& \sin {\mathrm{\ψ}}_{\mathrm{BN}}\cos {\mathrm{\ψ}}_{\mathrm{BU}}+\cos {\mathrm{\ψ}}_{\mathrm{BN}}\sin {\mathrm{\ψ}}_{\mathrm{BE}}\sin {\mathrm{\ψ}}_{\mathrm{BU}}\\ \cos {\mathrm{\ψ}}_{\mathrm{BN}}\sin {\mathrm{\ψ}}_{\mathrm{BU}}+\sin {\mathrm{\ψ}}_{\mathrm{BN}}\sin {\mathrm{\ψ}}_{\mathrm{BE}}\cos {\mathrm{\ψ}}_{\mathrm{BU}}& \cos {\mathrm{\ψ}}_{\mathrm{BE}}\cos {\mathrm{\ψ}}_{\mathrm{BU}}& \sin {\mathrm{\ψ}}_{\mathrm{BN}}\sin {\mathrm{\ψ}}_{\mathrm{BU}}-\cos {\mathrm{\ψ}}_{\mathrm{BN}}\sin {\mathrm{\ψ}}_{\mathrm{BE}}\cos {\mathrm{\ψ}}_{\mathrm{BU}}\\ -\sin {\mathrm{\ψ}}_{\mathrm{BN}}\cos {\mathrm{\ψ}}_{\mathrm{BE}}& \sin {\mathrm{\ψ}}_{\mathrm{BE}}& \cos {\mathrm{\ψ}}_{\mathrm{BN}}\cos {\mathrm{\ψ}}_{\mathrm{BE}}\end{array}\right)$

得到由主惯导系统载体坐标系到从惯导系统载体坐标系的坐标转换矩阵

为：

$C_A^B=C_n^B\·C_A^n={\left(C_B^n\right)}^T\·C_A^n$

则主惯导系统与从惯导相对姿态角的主值为：

${\mathrm{\ψ}}_{\mathrm{EAB}0}=\arctan \frac{C_A^B\left(\mathrm{3,2}\right)}{\sqrt{1-{\left(C_A^B\left(\mathrm{3,2}\right)\right)}^2}}$

${\mathrm{\ψ}}_{\mathrm{NAB}0}=\arctan (-\frac{C_A^B(3,1)}{C_A^B\left(\mathrm{3,3}\right)})$

${\mathrm{\ψ}}_{\mathrm{UAB}0}=\arctan (-\frac{C_A^B\left(\mathrm{1,2}\right)}{C_A^B\left(\mathrm{2,2}\right)})$

步骤8：主惯导系统与从惯导系统间的相对姿态置信度估计。

2.如权利要求1所述一种基于双IMU/DGPS组合的相对姿态测量实时动态滤波方法，其特征在于：所述步骤1中：

设惯导系统的导航坐标系为当地地理坐标系，水平位置误差为δL，δλ，δh，δL为惯导系统经度误差，δλ为惯导系统纬度误差，δh为惯导系统高度误差，水平速度误差为δV_E，δV_N，δV_U，姿态误差为ψ_E，ψ_N，ψ_U，陀螺漂移项ε_E，ε_N，ε_U，加速度计零偏

则捷联惯导误差方程为：

状态矢量：

$X\left(t\right)={[\mathrm{\δL},\mathrm{\δ\λ},\mathrm{\δh},\mathrm{\δ}{V}_E,\mathrm{\δ}{V}_N,\mathrm{\δ}{V}_U,{\mathrm{\ψ}}_E,{\mathrm{\ψ}}_N,{\mathrm{\ψ}}_U,{\mathrm{\ϵ}}_E,{\mathrm{\ϵ}}_N,{\mathrm{\ϵ}}_U,{\▿}_E,{\▿}_N,{\▿}_U]}^T---\left(1\right)$

姿态误差方程：

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ψ}}}_E=-\frac{{\mathrm{\δV}}_E}{R_M+h}+({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L){\mathrm{\ψ}}_N-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}){\mathrm{\ψ}}_U+{\mathrm{\ϵ}}_E\\ {\stackrel{\·}{\mathrm{\ψ}}}_N=\frac{\mathrm{\δ}{V}_E}{R_N+h}-{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}-({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L){\mathrm{\ψ}}_E-\frac{V_N}{R_M+h}{\mathrm{\ψ}}_U+{\mathrm{\ϵ}}_N\\ {\stackrel{\·}{\mathrm{\ψ}}}_U=\frac{\mathrm{\δ}{V}_E}{R_N+h}\tan L+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)\mathrm{\δL}+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}){\mathrm{\ψ}}_E+\frac{V_N}{R_N+h}{\mathrm{\ψ}}_N+{\mathrm{\ϵ}}_U\end{array}\right.---\left(2\right)$

速度误差方程：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{V}}_E=f_N{\mathrm{\ψ}}_U-f_U{\mathrm{\ψ}}_N+2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)\mathrm{\δ}{V}_N\\ +(\frac{V_N}{R_M+h}\tan L-\frac{V_U}{R_M+h})\mathrm{\δ}{V}_E+(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\mathrm{\δ}{V}_U\\ +(2{\mathrm{\ω}}_{\mathrm{ie}}{V}_N\cos L+\frac{V_E{V}_N}{R_N+h}{\sec }^{2}L)\mathrm{\δL}+{\▿}_E\\ \mathrm{\δ}{\stackrel{\·}{V}}_N=f_U{\mathrm{\ψ}}_E-f_E{\mathrm{\ψ}}_U-2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)\mathrm{\δ}{V}_E\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)V_E\mathrm{\δL}+{\▿}_E\\ \mathrm{\δ}{\stackrel{\·}{V}}_U=f_E{\mathrm{\ψ}}_N-f_N{\mathrm{\ψ}}_E+2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\mathrm{\δ}{V}_E+2\frac{V_N}{R_M+h}\mathrm{\δ}{V}_N\\ -2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{V}_E\mathrm{\δL}+{\▿}_E\end{array}\right.---\left(3\right)$

水平位置误差方程：

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{L}=\frac{1}{R_M+h}\mathrm{\δ}{V}_N\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{\sec L}{R_N+h}\mathrm{\δ}{V}_E+\frac{V_E}{R_N+h}\sec L\tan \mathrm{L\δL}\\ \mathrm{\δ}\stackrel{\·}{h}=\mathrm{\δ}{V}_U\end{array}\right.---\left(4\right)$

其中，ω_ie为地球自转角速度，ω_ie＝7.29×10^-5rad/s；R_E＝6378137m，R_E为参考椭球体的长半径；f为地球扁率，f＝1/298.257；R_M＝R_M(1-2f+3fsin²L)，R_M为子午圈主曲率半径；R_N为卯酉圈主曲率半径R_N＝R_R(1+fsin²L)；

根据主惯导系统和从惯导系统根据各自的惯性测量单元测得的载体加速度数据和角速度数据，通过上述捷联惯导误差方程，可分别得到主惯导系统与从惯导系统的水平位置误差δL_A，δλ_A，δh_A与δL_B，δλ_B，δh_B，水平速度误差δV_AE，δV_AN，δV_AU与δV_BE，δV_BN，δV_BU，姿态误差ψ_AE，ψ_AN，ψ_AU与ψ_BE，ψ_BN，ψ_BU，并分别对主惯导系统与从惯导系统的水平位置误差，水平速度误差，姿态误差进行迭代运算，从而得到主惯导系统与从惯导系统的位置、速度和姿态值。

3.如权利要求1所述一种基于双IMU/DGPS组合的相对姿态测量实时动态滤波方法，其特征在于：所述步骤3中组合导航滤波器的量测方程的建立方法为：

惯导系统的位置信息可表示为：

$\left\{\begin{array}{c}{L}_I=L_t+\mathrm{\δL}\\ {\mathrm{\λ}}_I={\mathrm{\λ}}_t+\mathrm{\δ\λ}\\ h_I=h_t+\mathrm{\δt}\end{array}\right.---\left(5\right)$

其中，惯导系统水平位置误差为γL，γλ，δh，L_t，λ_t，h_t为惯导系统的真实位置信息；DGPS测得的载体位置信息可表示为：

$\left\{\begin{array}{c}{L}_{\mathrm{GPS}}=L_t-\frac{N_N}{R_E}\\ {\mathrm{\λ}}_{\mathrm{GPS}}={\mathrm{\λ}}_t-\frac{N_E}{R_E\cos L}\\ h_{\mathrm{GPS}}=h_t-N_U\end{array}\right.---\left(6\right)$

其中，N_E，N_N，N_U分别为DGPS测量得到的载体沿东、北、天方向的位置误差；

则惯导系统的位置量测矢量为：

$Z_p\left(t\right)=\left(\begin{array}{c}(L_I-L_{\mathrm{GPS}})R_E\\ ({\mathrm{\λ}}_I-{\mathrm{\λ}}_{\mathrm{GPS}})R_E\cos L\\ h_I-h_G\end{array}\right)=\left(\begin{array}{c}{R}_E\mathrm{\δL}+N_N\\ R_E\mathrm{\δ\λ}\cos L+N_E\\ \mathrm{\δh}+N_U\end{array}\right)=H_p\left(t\right)X\left(t\right)+V_p\left(t\right)---\left(7\right)$

其中，H_p(t)＝(diag(RRcosL1)0_3×12)_3×15，V_p(t)＝(N_N N_E N_U)^T；设DGPS接收机伪距测量误差为σ_ρ，平面位置精度因子为HDOP，高程精度因子为VDOP，将DGPS量测噪声矢量V(t)作为白噪声处理，则DGPS位置噪声方差为

为：

$\left\{\begin{array}{c}{\mathrm{\σ}}_{\mathrm{pN}}={\mathrm{\σ}}_{\mathrm{pE}}={\mathrm{\σ}}_{\mathrm{\ρ}}\·\mathrm{HDOP}\\ {\mathrm{\σ}}_{\mathrm{pU}}={\mathrm{\σ}}_{\mathrm{\ρ}}\·\mathrm{VDOP}\end{array}\right.---\left(8\right)$

惯导系统的速度信息可表示为：

$\left\{\begin{array}{c}{V}_{\mathrm{IN}}=V_N+{\mathrm{\δV}}_N\\ V_{\mathrm{IE}}=V_E+{\mathrm{\δV}}_E\\ V_{\mathrm{IU}}=V_U+{\mathrm{\δV}}_U\end{array}\right.---\left(9\right)$

其中，惯导系统的水平速度误差为δV_E，δV_N，δV_U，V_N，V_R，V_U为惯导系统的真实速度信息；

DGPS测得的载体速度信息可表示为：

$\left\{\begin{array}{c}{V}_{\mathrm{GN}}=V_N-M_N\\ V_{\mathrm{GE}}=V_E-M_E\\ V_{\mathrm{GU}}=V_U-M_U\end{array}\right.---\left(10\right)$

其中，M_E，M_N，M_U分别为DGPS沿东、北、天方向的载体速度误差；

则惯导系统的速度量测矢量为：

$Z_V\left(t\right)=\left(\begin{array}{c}{V}_{\mathrm{IN}}-V_{\mathrm{GN}}\\ V_{\mathrm{IE}}-V_{\mathrm{GE}}\\ V_{\mathrm{IU}}-V_{\mathrm{GU}}\end{array}\right)=\left(\begin{array}{c}\mathrm{\δ}{V}_N+M_N\\ {\mathrm{\δV}}_E+M_E\\ \mathrm{\δ}{V}_U+M_U\end{array}\right)=H_V\left(t\right)X\left(t\right)+V_V\left(t\right)---\left(11\right)$

其中，H_V(t)＝(0_3×3 diag(111)0_3×9)_3×15；

设DGPS接收机伪距率测量误差为

则DGPS速度噪声方差为

为：

$\left\{\begin{array}{c}{\mathrm{\σ}}_{\mathrm{VN}}={\mathrm{\σ}}_{\mathrm{VE}}={\mathrm{\σ}}_{\stackrel{\·}{\mathrm{\ρ}}}\·\mathrm{HDOP}\\ {\mathrm{\σ}}_{\mathrm{VU}}={\mathrm{\σ}}_{\stackrel{\·}{\mathrm{\ρ}}}\·\mathrm{VDOP}\end{array}\right.---\left(12\right)$

综上可得在DGPS信号正常时惯导系统组合滤波器量测方程如下：

$Z\left(t\right)=\left(\begin{array}{c}{Z}_p\left(t\right)\\ Z_V\left(t\right)\end{array}\right)=\left(\begin{array}{c}{H}_p\left(t\right)\\ H_V\left(t\right)\end{array}\right)X\left(t\right)+\left(\begin{array}{c}{V}_p\left(t\right)\\ V_V\left(t\right)\end{array}\right)=H\left(t\right)X\left(t\right)+V\left(t\right)---\left(13\right)$

4.如权利要求1所述一种基于双IMU/DGPS组合的相对姿态测量实时动态滤波方法，其特征在于：所述步骤4中使用主惯导系统的速度、位置信息对从惯导系统进行滤波修正方法为：

从惯导系统的位置量测信息表示为：

$\left\{\begin{array}{c}{L}_B=L_{\mathrm{Bt}}+{\mathrm{\δL}}_B\\ {\mathrm{\λ}}_B={\mathrm{\λ}}_{\mathrm{Bt}}+{\mathrm{\δ\λ}}_B\\ h_B=h_{\mathrm{Bt}}+{\mathrm{\δh}}_B\end{array}\right.---\left(14\right)$

其中，δL_B、δλ_B、δh_B分别为从惯导系统经度、纬度、高度上的位置误差；L_Bt，λ_Bt，h_Bt为从惯导系统的真实位置信息；

主惯导系统的位置量测信息可表示为：

$\left\{\begin{array}{c}{L}_A=L_{\mathrm{Bt}}-\mathrm{\δ}{L}_A\\ {\mathrm{\λ}}_A={\mathrm{\λ}}_{\mathrm{Bt}}-\mathrm{\δ}{\mathrm{\λ}}_A\\ h_A=h_{\mathrm{Bt}}-\mathrm{\δ}{h}_A\end{array}\right.---\left(15\right)$

其中，δL_A，δλ_A，δh_A为主惯导系统与从惯导系统的水平位置误差；

则惯导系统位置量测矢量定义为：

$Z_p^{\′}\left(t\right)=\left(\begin{array}{c}(L_B-L_A)R\\ ({\mathrm{\λ}}_B-{\mathrm{\λ}}_A)R\cos L\\ h_B-h_A\end{array}\right)=\left(\begin{array}{c}\mathrm{R\δ}{L}_B+\mathrm{R\δ}{L}_A\\ \mathrm{R\δ}{\mathrm{\λ}}_B\cos L+\mathrm{R\δ}{\mathrm{\λ}}_A\cos L\\ \mathrm{\δ}{h}_B+\mathrm{\δ}{h}_A\end{array}\right)=H_p^{\′}\left(t\right)X\left(t\right)+V_p^{\′}\left(t\right)---\left(16\right)$

其中，H′_p(t)＝(diag(RRcosL1)0_3×12)_3×15，V′_p(t)＝(RδL _ARδλ_A cosLδh_A)^T；

主惯导系统与从惯导系统组合滤波器的位置噪声的最优估计由主惯导系统估计误差的协方差阵P_A(t)为：

$\left\{\begin{array}{c}{\widehat{\mathrm{\σ}}}_{\mathrm{pN}}^2=P_A\left(\mathrm{1,1}\right)\·R^2\\ {\widehat{\mathrm{\σ}}}_{\mathrm{pE}}^2=P_A\left(\mathrm{2,2}\right)\·R^2{\cos }^{2}L\\ {\widehat{\mathrm{\σ}}}_{\mathrm{pU}}^2=P_A\left(\mathrm{3,3}\right)\end{array}\right.---\left(17\right)$

式(17)中，为主惯导位置误差方差的估计值；

从惯导系统的速度信息可表示为：

$\left\{\begin{array}{c}{V}_{\mathrm{BN}}=V_{\mathrm{BNt}}+\mathrm{\δ}{V}_{\mathrm{BN}}\\ V_{\mathrm{BE}}=V_{\mathrm{BEt}}+{\mathrm{\δV}}_{\mathrm{BE}}\\ V_{\mathrm{BU}}=V_{\mathrm{BUt}}+\mathrm{\δ}{V}_{\mathrm{BU}}\end{array}\right.---\left(18\right)$

其中，δV_BE，δV_BN，δV_BU为主惯导系统水平速度误差；V_BNt，V_BEt，V_BUt为从惯导系统真实速度信息；主惯导系统的速度量测信息可表示为：

$\left\{\begin{array}{c}{V}_{\mathrm{AN}}=V_{\mathrm{BNt}}-\mathrm{\δ}{V}_{\mathrm{AN}}\\ V_{\mathrm{AE}}=V_{\mathrm{BEt}}-\mathrm{\δ}{V}_{\mathrm{AE}}\\ V_{\mathrm{AU}}=V_{\mathrm{BUt}}-\mathrm{\δ}{V}_{\mathrm{AU}}\end{array}\right.---\left(19\right)$

其中，δV_AE，δV_AN，δV_AU为主惯导系统水平速度误差；

则惯导系统位置量测速度量测矢量为：

$Z_V^{\′}\left(t\right)=\left(\begin{array}{c}{V}_{\mathrm{BN}}-V_{\mathrm{AN}}\\ V_{\mathrm{BE}}-V_{\mathrm{AE}}\\ V_{\mathrm{BU}}-V_{\mathrm{AU}}\end{array}\right)=\left(\begin{array}{c}\mathrm{\δ}{V}_{\mathrm{BN}}+\mathrm{\δ}{V}_{\mathrm{AN}}\\ {\mathrm{\δV}}_{\mathrm{BE}}+\mathrm{\δ}{V}_{\mathrm{AE}}\\ \mathrm{\δ}{V}_{\mathrm{BU}}+\mathrm{\δ}{V}_{\mathrm{AU}}\end{array}\right)=H_V^{\′}\left(t\right)X\left(t\right)+V_V^{\′}\left(t\right)---\left(20\right)$

式中，H_V(t)＝(0_3×3diag(111)0_3×9)_3×15；

主惯导系统与从惯导系统组合滤波器的速度量测噪声的最优估计由主惯导系统估计误差的协方差阵P_A(t)给出：

$\left\{\begin{array}{c}{\widehat{\mathrm{\σ}}}_{\mathrm{VE}}^2=P_A\left(\mathrm{4,4}\right)\\ {\widehat{\mathrm{\σ}}}_{\mathrm{VN}}^2=P_A\left(\mathrm{5,5}\right)\\ {\widehat{\mathrm{\σ}}}_{\mathrm{VU}}^2=P_A\left(\mathrm{6,6}\right)\end{array}\right.---\left(21\right)$

式中，

为主惯导系统速度误差方差的估计值；

综上可得在无DGPS信号时惯导系统组合滤波器量测方程如下：

$Z^{\′}\left(t\right)=\left(\begin{array}{c}{Z}_p^{\′}\left(t\right)\\ Z_V^{\′}\left(t\right)\end{array}\right)=\left(\begin{array}{c}{H}_p^{\′}\left(t\right)\\ H_V^{\′}\left(t\right)\end{array}\right)X\left(t\right)+\left(\begin{array}{c}{V}_p^{\′}\left(t\right)\\ V_V^{\′}\left(t\right)\end{array}\right)=H^{\′}\left(t\right)X\left(t\right)+V^{\′}\left(t\right)---\left(22\right)$

5.如权利要求1所述一种基于双IMU/DGPS组合的相对姿态测量实时动态滤波方法，

其特征在于：所述步骤5中：离散型卡尔曼滤波器的递推方程建立过程如下：

首先，给定惯导系统的一阶线性状态方程和量测方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(23\right)$

Z(t)＝H(t)X(t)+V(t)(24)

进而将状态方程(5)和量测方程(6)离散化可得：

X_k＝Ф_k，k-1X_k-1+Γ_k-1W_k-1(25)

Z_k＝H_kX_k+V_k(26)

其中，X_k为状态向量，Ф_k，k-1为状态转移矩阵，Z_k为量测向量，H_k为量测矩阵，Γ_k-1为系统噪声矩阵，W_k-1为系统噪声向量，V_k为量测噪声向量。W_k-1、V_k是不相关高斯白噪声列；

状态预测估计方程为：

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(27\right)$

方差预测方程为：

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(28\right)$

状态预测估计方程为：

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})---\left(29\right)$

方差迭代方程：

$P_k=P_{k/k-1}-P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}{H}_k{P}_{k/k-1}$

$=(I-K_k{H}_k)P_{k/k-1}---\left(30\right)$

滤波增益方程为：

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(31\right)$

初始条件为：

${\hat{X}}_0=E\left(X_0\right)={\mathrm{\μ}}_0,\mathrm{var}{\tilde{X}}_0=\mathrm{var}{X}_0=P_0---\left(32\right)$

验前统计量为：

E[W_k]＝0，Cov[W_k，W_j]＝E[W_kW_j ^T]＝Q_kδ_kj(33)

E[V_k]＝0，Cov[V_k，V_j]＝E[V_kV_j ^T]＝R_kδ_kj(34)

Cov[W_k，V_j]＝E[W_kV_j ^T]＝0(35)

${\mathrm{\δ}}_{\mathrm{kj}}=\left\{\begin{array}{cc}0,& k\≠j\\ 1,& k=j\end{array}\right.---\left(36\right)$

6.如权利要求1所述一种基于双IMU/DGPS组合的相对姿态测量实时动态滤波方法，其特征在于：所述步骤8中，主惯导系统与从惯导系统间的相对姿态置信度估计具体方法为：

由主惯导系统与从惯导系统各自的姿态误差分别由协方差阵P_A(t)和P_B(t)给出最优估计；设主惯导系统在地理坐标系下(x、y、z)三个坐标轴方向上姿态角的噪声方差估计值分别从惯导系统为

则：

$\left\{\begin{array}{c}{\widehat{\mathrm{\δ}}}_{\mathrm{A\ψx}}^2=P_A\left(\mathrm{7,7}\right)\\ {\widehat{\mathrm{\δ}}}_{\mathrm{A\ψy}}^2=P_A\left(\mathrm{8,8}\right)\\ {\widehat{\mathrm{\δ}}}_{\mathrm{A\ψz}}^2=P_A\left(\mathrm{9,9}\right)\\ {\widehat{\mathrm{\δ}}}_{\mathrm{B\ψx}}^2=P_B\left(\mathrm{7,7}\right)\\ {\widehat{\mathrm{\δ}}}_{\mathrm{B\ψy}}^2=P_B\left(\mathrm{8,8}\right)\\ {\widehat{\mathrm{\δ}}}_{\mathrm{B\ψz}}^2=P_B\left(\mathrm{9,9}\right)\end{array}\right.---\left(37\right)$

在主惯导系统与从惯导系统精度近似不相关的情况下，相对姿态的置信度估计值为：

$\left\{\begin{array}{c}{\widehat{\mathrm{\δ}}}_{\mathrm{AB\ψx}}=\sqrt{{\widehat{\mathrm{\δ}}}_{\mathrm{A\ψx}}^2+{\widehat{\mathrm{\δ}}}_{\mathrm{B\ψx}}^2}\\ {\widehat{\mathrm{\δ}}}_{\mathrm{AB\ψy}}=\sqrt{{\widehat{\mathrm{\δ}}}_{\mathrm{A\ψy}}^2+{\widehat{\mathrm{\δ}}}_{\mathrm{B\ψy}}^2}\\ {\widehat{\mathrm{\δ}}}_{\mathrm{AB\ψz}}=\sqrt{{\widehat{\mathrm{\δ}}}_{\mathrm{A\ψz}}^2+{\widehat{\mathrm{\δ}}}_{\mathrm{B\ψz}}^2}\end{array}\right.---\left(38\right)$

其中，

分别表示两惯导相对姿态置信度估计值。

Images