CN104698486A - 一种分布式pos用数据处理计算机系统实时导航方法 - Google Patents

Abstract

本发明提供一种分布式POS用数据处理计算机系统实时导航方法，首先，采用基于PPS实时标定与分频的时间同步方法实现三个IMU原始数据的同时刻采集；其次，将主IMU数据与GPS数据融合得出主POS实时导航结果；子IMU采用基于惯性矢量二次积分的解析对准方法完成摇摆基座初始对准，并采用基于多级杆臂补偿的卡尔曼滤波方法完成分布式POS传递对准，得到子IMU实时导航结果以及主/子IMU之间的相对空间关系；本发明实现一主两子分布式POS实时数据处理，适用于高精度分布式惯性测量、导航及组合导航系统。

Description

一种分布式POS用数据处理计算机系统实时导航方法

技术领域

本发明涉及一种分布式POS用数据处理计算机系统实时导航方法，可以应用于分布式POS(Position and Orientation System,位置姿态测量系统)，也可以应用于惯性网络协同导航、惯性/GPS(Global Position System,全球定位系统)组合导航系统的数据处理与导航计算。

背景技术

高精度POS由惯性测量单元(Inertial Measurement Unit,IMU)和GPS组成。IMU是INS(Inertial Navigation System，惯性导航系统)/GPS、POS等惯性测量系统的核心部件；GPS能够提供位置、速度、时间以及高精度的秒脉冲(pulse per second,PPS)信息。高精度POS可以为高分辨率航空遥感系统提供高频、高精度的时间、空间基准信息，通过运动误差补偿提高成像精度和效率，是实现高分辨率成像的关键。我国在单POS研制方面取得了一定的进展，但是由于对地观测载荷的需求牵引，如集成高分辨率测绘相机、全谱段成像光谱仪、大视场红外扫描仪、SAR于同一载机的多任务载荷，机载分布式阵列天线SAR和柔性多基线干涉SAR以及艇载稀疏阵列成像雷达等，多个或多种观测载荷安装在飞机的不同位置，采用传统的单一POS系统无法实现多点的高精度位置姿态测量以及各载荷数据的时间统一，迫切需要建立起高精度分布式时空基准系统，为高性能航空遥感系统中所有载荷提供高精度的时间、空间信息。

针对一主两子干涉SAR系统，建立高精度分布式POS系统。分布式POS由主POS和两个分布在机翼下的子IMU组成。主POS解算获得飞机的高精度位置、速度和姿态等运动参数，两个子IMU利用主POS提供的信息进行传递对准，最终实现多节点运动信息的精确测量。

分布式POS用数据处理计算机系统实时导航方法，是完成分布式POS实时导航的核心，该方法接收主IMU、两个子IMU和GPS提供的原始信息，并进行数据预处理、初始对准、捷联解算、组合滤波、传递对准、时间同步、数据发送等任务。现有分布式POS传递对准方法在建立状态方程时没有考虑杆臂长度误差的影响，不能实时估计出主/子IMU之间的杆臂长度；且现有方法计算量较大，适用于事后处理，不能应用于分布式POS的实时导航。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提出一种分布式POS用数据处理计算机系统实时导航方法

本发明的技术解决方案为：一种分布式POS用数据处理计算机系统实时导航方法，其步骤如下：

(1)主POS实时导航；首先，采用基于PPS实时标定与分频的时间同步方法实现分布式POS主/子IMU原始数据的同时刻采集，其次，将GPS数据进行杆臂补偿，获得杆臂补偿后的位置和速度信息，利用主IMU数据进行初始对准以及捷联解算，获得主IMU的惯性导航结果；采用卡尔曼滤波的方法估计主IMU位置、速度、姿态和惯性器件误差，并将估计出的误差进行反馈校正，得到高精度的主POS实时导航结果；

(2)子IMU初始对准；根据子IMU数据和主POS的位置信息，采用基于惯性矢量二次积分的解析对准方法完成摇摆基座下子IMU初始对准，通过二次积分的方式，减小了环境扰动引起的加速度误差，得到更精确的子IMU初始位置、速度和姿态信息；

(3)子IMU实时导航；初始对准完成后，利用子IMU数据进行捷联解算，获得子IMU惯性导航结果；采用基于多级杆臂补偿的卡尔曼滤波方法完成分布式POS传递对准，估计主/子IMU之间的基线长度误差以及子IMU位置、速度、姿态和惯性器件误差，得到主/子IMU之间准确的基线长度和高精度的子IMU实时导航结果。

上述步骤(1)中所述的基于PPS实时标定与分频的时间同步方法为：将PPS作为同步脉冲信号引入三个IMU的信号采集电路中，PPS秒脉冲触发主/子IMU在同一时刻启动数据采集；然后根据每秒的PPS标定IMU内部时钟，实时调整数据采集周期分频数，修正IMU内部的时钟漂移，保证各IMU一直在相同时刻进行数据采集。

上述步骤(2)中所述的基于惯性矢量二次积分解析对准的方法，通过下式确定子IMU姿态矩阵

$C_b^n\left(t\right)=C_{e_0}^n{C}_{i_0}^{e_0}\left(t\right)C_{i_{b_0}}^{i_0}{C}_b^{i_{b_0}}\left(t\right)$

式中是地心地球坐标系到导航坐标系的转换矩阵，是地心惯性坐标系到地心地球坐标系的转换矩阵，是基座惯性坐标系到地心惯性坐标系的转换矩阵，是载体坐标系到基座惯性坐标系的转换矩阵。该方法具体步骤如下：

(1)设对准开始时刻t₀载体所在纬度为L，L可由主POS输出的实时导航结果获取，地球自转角速度ω_ie已知，上式中

$C_{e_0}^n=\left[\begin{array}{ccc}0& 1& 0\\ -\sin L& 0& \cos L\\ \cos L& 0& \sin L\end{array}\right]$    $C_{i_0}^{e_0}\left(t\right)=\left[\begin{array}{ccc}\cos {\mathrm{\ω}}_{\mathrm{ie}}(t-t_0)& \sin {\mathrm{\ω}}_{\mathrm{ie}}(t-t_0)& 0\\ -{\sin \mathrm{\ω}}_{\mathrm{ie}}(t-t_0)& \cos {\mathrm{\ω}}_{\mathrm{ie}}(t-t_0)& 0\\ 0& 0& 1\end{array}\right]$

(2)可根据陀螺输出进行实时姿态更新求得，姿态更新的微分方程为如下形式，即

(3)是载体惯性坐标系到经线地心惯性坐标系的变换矩阵，利用重力信息实现的求取，将子IMU敏感到的加速度矢量通过两次积分转化为位移矢量，求取在摇摆过程中，IMU的加速度计感测到的比力中包含重力加速度和杆臂加速度，经过杆臂加速度补偿后的比力在系中的投影可以表示为如下的形式：

${\hat{f}}^{i_{b_0}}={\hat{C}}_b^{i_{b_0}}{\hat{f}}^b$

式中为经过杆臂加速度补偿后的比力，为根据陀螺输出计算得到的b系与系间的变换矩阵。对上式两边积分，即可得到

${\hat{V}}_{(t_1,t_m)}^{i_{b_0}}=-{\∫}_{t_l}^{t_m}{\hat{C}}_b^{i_{b_0}}{\hat{f}}^b\mathrm{dt}$

为t_l时刻到t_m时刻的速度矢量，对上式两边积分，即可得到

${\hat{S}}_{(t_1,t_m)}^{i_{b_0}}=-{\∫}_{t_l}^{t_m}{\hat{V}}^{i_{b_0}}\mathrm{dt}$

为t_l时刻到t_m时刻的位移矢量，又由于：

$g^{i_0}=C_{e_0}^{i_0}{C}_n^{e_0}{g}^n$

对上式两边积分，得：

$V_{(t_r,t_k)}^{i_0}=\left[\begin{array}{c}\frac{g\cos L\sin {\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_k}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ -\frac{g\cos L(\cos {\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_k-1)}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ g\sin \mathrm{L\Δ}{t}_k\end{array}\right]$

对上式两边积分，得：

$S_{(t_r,t_k)}^{i_0}=\left[\begin{array}{c}\frac{g\cos L(1-\cos {\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_k)}{{{\mathrm{\ω}}_{\mathrm{ie}}}^2}\\ \frac{g\cos L({\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_k-\sin {\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_k)}{{{\mathrm{\ω}}_{\mathrm{ie}}}^2}\\ \frac{g\sin \mathrm{L\Δ}{t_k}^2}{2}\end{array}\right]$

式中△t_k＝t_k-t_r。取t₀<t_u≤t_v<t_d，其中t₀为对准开始时刻、t_d为对准结束时刻，得：

$\left\{\begin{array}{c}{\hat{S}}_{(t_0,t_u)}^{i_{b_0}}=C_{i_0}^{i_{b_0}}{S}_{(t_0,t_u)}^{i_0}\\ {\hat{S}}_{(t_v,t_d)}^{i_{b_0}}=C_{i_0}^{i_{b_0}}{S}_{(t_v,t_d)}^{i_0}\end{array}\right.$

由上式可得：

$C_{i_{b_0}}^{i_0}={\left[\begin{array}{c}{\left[S_{(t_0,t_u)}^{i_0}\right]}^T\\ {\left[S_{(t_v,t_d)}^{i_0}\right]}^T\\ {[S_{(t_0,t_u)}^{i_0}\&times;S_{(t_v,t_d)}^{i_0}]}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left[{\hat{S}}_{(t_0,t_u)}^{i_{b_0}}\right]}^T\\ {\left[{\hat{S}}_{(t_v,t_d)}^{i_{b_0}}\right]}^T\\ {[{\hat{S}}_{(t_0,t_u)}^{i_{b_0}}\&times;{\hat{S}}_{(t_v,t_d)}^{i_{b_0}}]}^T\end{array}\right]$

将和代入 $C_b^n\left(t\right)=C_{e_0}^n{C}_{i_0}^{e_0}\left(t\right)C_{i_{b_0}}^{i_0}{C}_b^{i_{b_0}}\left(t\right)$ 即可计算出初始对准确定的姿态阵。

上述步骤(3)中所述的子IMU实时导航包括捷联解算和传递对准两个部分：

(1)捷联解算功能是以子IMU上一时刻的位置、速度和姿态信息作为当前捷联解算的初始值，结合当前时刻的子IMU数据，获得当前时刻的惯性导航结果；

(2)采用基于多级杆臂补偿的卡尔曼滤波方法完成分布式POS传递对准，估计主/子IMU之间的基线长度误差、子IMU位置、速度、姿态和惯性器件误差，并将估计出的误差进行反馈校正，得到高精度的子IMU实时导航结果和主/子IMU之间的相对空间关系；卡尔曼滤波模型中的状态变量X共有18维，包括东、北、天向的失准角 东、北、天向的速度误差δV_E、δV_N、δV_U，纬度、经度、高度误差δL、δλ、δH，x、y、z轴向的陀螺仪常值漂移误差ε_x、ε_y、ε_z，x、y、z轴向加速度计常值漂移误差 x、y、z轴向杆臂长度误差△r_x、△r_y、△r_z；卡尔曼滤波模型中的量测量Z是经过多级杆臂补偿的速度误差和位置误差，多级杆臂分别为GPS与主IMU之间的刚性杆臂和主/子IMU之间的柔性杆臂。传递对准具体步骤如下：

(a)系统状态方程的建立

系统状态方程为：

$\stackrel{\&CenterDot;}{X}=\mathrm{FX}+W$

式中 $X={\left[\begin{array}{cccccccccccccccccc}{\mathrm{\&phi;}}_E& {\mathrm{\&phi;}}_N& {\mathrm{\&phi;}}_U& \mathrm{\&delta;}{V}_E& \mathrm{\&delta;}{V}_N& \mathrm{\&delta;}{V}_U& \mathrm{\&delta;L}& \mathrm{\&delta;\&lambda;}& \mathrm{\&delta;H}& {\mathrm{\&epsiv;}}_E& {\mathrm{\&epsiv;}}_N& {\mathrm{\&epsiv;}}_U& {\&dtri;}_E& {\&dtri;}_N& {\&dtri;}_U& \mathrm{\&Delta;}{r}_x& \mathrm{\&Delta;}{r}_y& \mathrm{\&Delta;}{r}_z\end{array}\right]}^T,$ X为18维状态变量，包括3个姿态误差、3个速度误差、3个位置误差、3个陀螺仪偏置、3个加速度计偏置和3个杆臂长度误差，F为状态转移矩阵；W为系统噪声，并假设其为零均值高斯白噪声；F的表达式：

$F=\left[\begin{array}{cccccc}{F}_1& F_2& F_3& C_b^n& 0_{3\&times;3}& \begin{array}{cc}{C}_b^n& {\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;\end{array}\\ F_4& F_5& F_6& 0_{3\&times;3}& C_b^n& 0_{3\&times;3}\\ 0_{3\&times;3}& F_7& F_8& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right]$

式中：

$F_1=\left[\begin{array}{ccc}0& {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_x\tan L}{R_N+H}& -({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_N+H})\\ -({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_x\tan L}{R_N+H})& 0& -\frac{V_y}{R_M+H}\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_N+H}& \frac{V_y}{R_M+H}& 0\end{array}\right]$

$F_2=\left[\begin{array}{ccc}0& -\frac{1}{R_M+H}& 0\\ \frac{1}{R_N+H}& 0& 0\\ \frac{\tan L}{R_M+H}& 0& 0\end{array}\right]$   $F_3=\left[\begin{array}{ccc}0& 0& \frac{V_y}{{(R_M+H)}^2}\\ -{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L& 0& -\frac{V_x}{{(R_N+H)}^2}\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_x{\sec }^{2}L}{R_N+H}& 0& \frac{V_x\tan L}{{(R_N+H)}^2}\end{array}\right]$

$F_5=\left[\begin{array}{ccc}\frac{V_y\tan L}{R_N+H}-\frac{V_z}{R_N+H}& {2\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_x\tan L}{R_N+H}& -{2\mathrm{\&omega;}}_{\mathrm{ie}}\cos L-\frac{V_x}{R_N+H}\\ -2({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_x\tan L}{R_N+H})& -\frac{V_z}{R_M+H}& -\frac{V_y}{R_M+H}\\ 2({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_N+H})& \frac{{2V}_y}{R_M+H}& 0\end{array}\right]$

$F_4=\left[\begin{array}{ccc}0& {-f}_z& f_y\\ f_z& 0& {-f}_x\\ {-f}_y& f_x& 0\end{array}\right]$   $F_6=\left[\begin{array}{ccc}{2\mathrm{\&omega;}}_{\mathrm{ie}}{V}_y\cos L+\frac{V_x{V}_y{\sec }^{2}L}{R_N+H}+{2\mathrm{\&omega;}}_{\mathrm{ie}}{V}_z\sin L& 0& \frac{V_x{V}_z-V_x{V}_y\tan L}{{(R_N+H)}^2}\\ -({2\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_x{\sec }^2}{R_N+H})V_E& 0& \frac{V_y{V}_z+V_x^2\tan L}{{(R_N+H)}^2}\\ {-2\mathrm{\&omega;}}_{\mathrm{ie}}{V}_x\sin L& 0& -\frac{V_x^2+V_y^2}{{(R_N+H)}^2}\end{array}\right]$

$F_7=\left[\begin{array}{ccc}0& \frac{1}{R_M+H}& 0\\ \frac{\sec L}{R_N+H}& 0& 0\\ 0& 0& 1\end{array}\right]$   $F_8=\left[\begin{array}{ccc}0& 0& -\frac{V_y}{{(R_M+H)}^2}\\ \frac{V_x\sec L\tan L}{R_N+H}& 0& -\frac{V_x\sec L}{{(R_N+H)}^2}\\ 0& 0& 0\end{array}\right]$

式中R_M和R_N分别为沿子午圈和卯酉圈的主曲率半径，H为载体高度，ω_ie为地球自转角速度，f_E、f_N和f_U分别为东向比力、北向比力和天向比力。

(b)量测方程的建立

量测量Z为经过杆臂补偿后的速度误差和位置误差，量测矩阵H为：

H＝[H_V H_P]^T

$H_V=\left[\begin{array}{ccccccc}& 1& 0& 0& & & \\ 0_{3\&times;3},& 0& 1& 0,& 0_{3\&times;9},& C_b^n& {\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;\\ & 0& 0& 1& & & \end{array}\right]$   $H_P=\left[\begin{array}{cccc}& R_M+H& 0& 0\\ 0_{3\&times;6},& 0& R_M+H& 0,0_{3\&times;9}\\ & 0& 0& 1\end{array}\right]$

(d)卡尔曼滤波器设计

利用标准卡尔曼滤波的五个公式进行滤波，可以得到估计的18个系统误差变量，利用卡尔曼滤波估计的结果对系统状态变量进行反馈校正，得到主/子IMU之间准确的基线长度和高精度的子IMU实时导航结果。

本发明与现有技术相比的优点在于：

针对分布式POS实际应用中子IMU处于长基线环境下的摇摆基座对准问题，采取基于惯性矢量二次积分的解析对准方法完成子IMU初始对准，克服了采用传统的解析式对准而导致的子IMU对准精度低的不足，相比惯性矢量一次积分的对准方法进一步减小了扰动引起的误差，提高了子IMU的初始对准精度；子IMU的实时导航不采用传统惯性/卫星组合导航方法，而采用基于多级杆臂补偿的卡尔曼滤波方法将主IMU实时导航信息与子IMU惯性导航信息融合，得到高精度的子IMU实时导航结果；将杆臂长度误差作为状态量引入系统模型，可以更加准确的估计出主/子IMU之间的基线长度，提高杆臂补偿精度；采用测量参数“速度+位置”的匹配方法，容易实现杆臂误差修正，对挠曲运动噪声以及惯性器件的测量噪声具有较好的积分平滑作用；针对分布式POS系统，在其数据采集阶段进行时间同步处理，保了各IMU数据采集的时间一致性。

附图说明

图1为本发明的流程图；

图2为本发明步骤一中的时间同步原理图；

图3为本发明步骤一及步骤三中的捷联解算流程图；

图4为本发明步骤二中基于惯性矢量二次积分的解析对准方法原理图；

图5为本发明步骤三中的传递对准流程图。

具体实施方式

如图1所示，本发明的具体方法实施如下：

1、分布式POS数据同步采集；采用基于PPS实时标定与分频的时间同步方法实现分布式POS主/子IMU原始数据的同时刻采集，原理如图2所示：将分布式POS数据处理计算机接收到的PPS当作同步脉冲信号引入三个IMU的信号采集电路中，以PPS为基准，触发主/子IMU在同一时刻启动数据采集，然后根据每秒的PPS标定IMU内部时钟，实时调整数据采集周期分频数，修正IMU内部的时钟漂移，保证各IMU一直在相同时刻进行数据采集。

2、主IMU初始对准；采用传统解析式对准方法完成主IMU初始对准，其步骤为：

(1)载体坐标系下：重力加速度g和地球自转角速度ω_ie可通过IMU的加速度计和陀螺仪的输出获取。

(2)导航坐标系下：当地的经度λ和纬度L是根据GPS信息获得的，重力加速度g和地球自转角速度ω_ie在地理坐标系下的分量都是确定的，可表示为：

$g^n=\left[\begin{array}{c}0\\ 0\\ g\end{array}\right],{\mathrm{\&omega;}}_{\mathrm{ie}}^n=\left[\begin{array}{c}{\mathrm{\&omega;}}_{\mathrm{iex}}^n\\ {\mathrm{\&omega;}}_{\mathrm{iey}}^n\\ {\mathrm{\&omega;}}_{\mathrm{iez}}^n\end{array}\right]=\left[\begin{array}{c}0\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L\end{array}\right]$

(3)捷联矩阵可由下式求出：

$C_b^n={\left[\begin{array}{c}{\left(g^n\right)}^T\\ {\left({\mathrm{\&omega;}}_{\mathrm{ie}}^n\right)}^T\\ {\left(g^n{\&times;\mathrm{\&omega;}}_{\mathrm{ie}}^n\right)}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left(g^b\right)}^T\\ {\left({\mathrm{\&omega;}}_{\mathrm{ie}}^b\right)}^T\\ {\left(g^b{\&times;\mathrm{\&omega;}}_{\mathrm{ie}}^b\right)}^T\end{array}\right]$

3、主POS实时导航；主POS实时导航包括捷联解算和卡尔曼滤波两部分：

(1)在捷联解算阶段，以上一时刻的位置、速度和姿态信息作为当前捷联解算的初始值，结合当前时刻的主IMU数据，获得当前时刻的惯性导航结果。捷联解算流程如图3所示，包括初始化、姿态矩阵更新、姿态计算、速度计算、位置矩阵更新和位置计算等步骤，具体说明如下：

(a)姿态矩阵更新

捷联解算模块采用四元数法来更新姿态矩阵其表达式见下式:

$\begin{array}{c}{C}_n^b=T_{\mathrm{\&gamma;}}\&CenterDot;T_{\mathrm{\&theta;}}\&CenterDot;T_{\mathrm{\&psi;}}\\ =\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}& \cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}\\ -\cos \mathrm{\&theta;}\sin \mathrm{\&psi;}& \cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& \sin \mathrm{\&theta;}\\ \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}& \sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right]\end{array}$

初始四元数计算公式为：

$q=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\left[\begin{array}{c}\cos \frac{\mathrm{\&psi;}}{2}\cos \frac{\mathrm{\&theta;}}{2}\cos \frac{\mathrm{\&gamma;}}{2}-\sin \frac{\mathrm{\&psi;}}{2}\sin \frac{\mathrm{\&theta;}}{2}\sin \frac{\mathrm{\&gamma;}}{2}\\ \cos \frac{\mathrm{\&psi;}}{2}\sin \frac{\mathrm{\&theta;}}{2}\cos \frac{\mathrm{\&gamma;}}{2}-\sin \frac{\mathrm{\&psi;}}{2}\cos \frac{\mathrm{\&theta;}}{2}\sin \frac{\mathrm{\&gamma;}}{2}\\ \cos \frac{\mathrm{\&psi;}}{2}\cos \frac{\mathrm{\&theta;}}{2}\sin \frac{\mathrm{\&gamma;}}{2}+\sin \frac{\mathrm{\&psi;}}{2}\sin \frac{\mathrm{\&psi;}}{2}\cos \frac{\mathrm{\&gamma;}}{2}\\ \cos \frac{\mathrm{\&psi;}}{2}\sin \frac{\mathrm{\&theta;}}{2}\sin \frac{\mathrm{\&gamma;}}{2}+\sin \frac{\mathrm{\&psi;}}{2}\cos \frac{\mathrm{\&theta;}}{2}\cos \frac{\mathrm{\&gamma;}}{2}\end{array}\right]$

离散化四阶计算公式为：

$q(n+1)=\{(1-\frac{{\left({\mathrm{\&Delta;\&theta;}}_0\right)}^2}{8}+\frac{{\left({\mathrm{\&Delta;\&theta;}}_0\right)}^4}{384})I+(\frac{1}{2}-\frac{{\left({\mathrm{\&Delta;\&theta;}}_0\right)}^2}{48})\left(\mathrm{\&Delta;\&theta;}\right)\}q\left(n\right),\mathrm{\&Delta;\&theta;}=\left[\begin{array}{cccc}0& -{\mathrm{\&Delta;\&theta;}}_x& -{\mathrm{\&Delta;\&theta;}}_y& -{\mathrm{\&Delta;\&theta;}}_z\\ {\mathrm{\&Delta;\&theta;}}_x& 0& {\mathrm{\&Delta;\&theta;}}_z& -{\mathrm{\&Delta;\&theta;}}_y\\ {\mathrm{\&Delta;\&theta;}}_y& -{\mathrm{\&Delta;\&theta;}}_z& 0& {\mathrm{\&Delta;\&theta;}}_x\\ {\mathrm{\&Delta;\&theta;}}_z& {\mathrm{\&Delta;\&theta;}}_y& -{\mathrm{\&Delta;\&theta;}}_x& 0\end{array}\right]$

式中 ${\mathrm{\&Delta;\&theta;}}_0^2={\mathrm{\&Delta;\&theta;}}_x^2+{\mathrm{\&Delta;\&theta;}}_y^2+{\mathrm{\&Delta;\&theta;}}_z^2,{\mathrm{\&Delta;\&theta;}}_x,{\mathrm{\&Delta;\&theta;}}_y,{\mathrm{\&Delta;\&theta;}}_z$ 为姿态速率在载体系的三个分量；表示载体系相对导航系的旋转角速度在b系上的投影。上式中的的计算公式为：

${\mathrm{\&omega;}}_{\mathrm{nb}}^b={\mathrm{\&omega;}}_{\mathrm{ib}}^b-{\mathrm{\&omega;}}_{\mathrm{in}}^b$

式中为陀螺输出角速率；表示载体系相对惯性系的旋转角速度在载体系上的投影表示地球自转角速率ω_ie在导航坐标系中的投影，表示导航坐标系相对于地球坐标系的角速率，二者的计算式分别为：

${\mathrm{\&omega;}}_{\mathrm{ie}}^n=\left[\begin{array}{c}0\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L\end{array}\right]$     ${\mathrm{\&omega;}}_{\mathrm{en}}^n=\left[\begin{array}{c}-\frac{V_N}{R_M+H}\\ \frac{V_E}{R_N+H}\\ \frac{V_E}{R_N+H}\tan L\end{array}\right].$

式中L为当地纬度；H为高度；V_E、V_N、V_U为导航坐标系东向、北向和天向的三个速度分量；R_M和R_N分别为沿子午圈和卯酉圈的主曲率半径。

得到四元数更新值q(n+1)后，由下式进行姿态矩阵更新：

$C_n^b=\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[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]$

(b)姿态计算

载体的姿态计量方式和有效范围定义如下：航向角ψ为载体坐标系y轴在导航坐标系的投影与导航坐标系y轴的夹角，从导航坐标系y轴起算，逆时针为正，有效范围为[0°，360°]；俯仰角θ为载体坐标系y轴与其在导航坐标系的投影间的夹角，以载体抬头为正，即向上为正，向下为负，有效范围为[-90°，90°]；横滚角γ为载体坐标系x轴与其在导航坐标系的投影间的夹角，以载体右倾为正(从载体正前朝后看)，左倾为负，有效范围为[-180°，180°]。在姿态矩阵更新以后，可由以下公式计算更新后的姿态角：

(c)速度计算

由下列速度方程进行速度的更新：

$\left[\begin{array}{c}{{\stackrel{\&CenterDot;}{V}}_x}^n\\ {{\stackrel{\&CenterDot;}{V}}_y}^n\\ {{\stackrel{\&CenterDot;}{V}}_z}^n\end{array}\right]=\left[\begin{array}{c}{a}_{\mathrm{ibx}}^n\\ a_{\mathrm{iby}}^n\\ a_{\mathrm{ibz}}^n\end{array}\right]+\left[\begin{array}{ccc}0& 2{\mathrm{\&omega;}}_{\mathrm{iez}}^n+{\mathrm{\&omega;}}_{\mathrm{enz}}^n& -(2{\mathrm{\&omega;}}_{\mathrm{iey}}^n+{\mathrm{\&omega;}}_{\mathrm{eny}}^n)\\ -(2{\mathrm{\&omega;}}_{\mathrm{iez}}^n++{\mathrm{\&omega;}}_{\mathrm{enz}}^n)& 0& 2{\mathrm{\&omega;}}_{\mathrm{iex}}^n+{\mathrm{\&omega;}}_{\mathrm{enx}}^n\\ 2{\mathrm{\&omega;}}_{\mathrm{iny}}^n+{\mathrm{\&omega;}}_{\mathrm{eny}}^n& -(2{\mathrm{\&omega;}}_{\mathrm{iex}}^n+{\mathrm{\&omega;}}_{\mathrm{enx}}^n)& 0\end{array}\right]\left[\begin{array}{c}{V_x}^n\\ {V_y}^n\\ {V_z}^n\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$

式中分别为导航坐标系下载体沿x轴、y轴、z轴三个方向上的速度增量，分别为导航坐标系下载体相对惯性空间的加速度沿x轴、y轴、z轴三个方向上的投影，分别为导航坐标系下地球自转角速度沿x轴、y轴、z轴三个方向上的投影，由上式求出加速度则

(d)位置矩阵更新与位置计算

由下列微分方程进行位置矩阵更新：

${\stackrel{\&CenterDot;}{C}}_n^e=C_n^e{\mathrm{\&Omega;}}_{\mathrm{en}}^n,{\mathrm{\&Omega;}}_{\mathrm{en}}^n=\left[\begin{array}{ccc}0& -{\mathrm{\&omega;}}_{\mathrm{enz}}^n& {\mathrm{\&omega;}}_{\mathrm{eny}}^n\\ {\mathrm{\&omega;}}_{\mathrm{enz}}^n& 0& -{\mathrm{\&omega;}}_{\mathrm{enx}}^n\\ -{\mathrm{\&omega;}}_{\mathrm{eny}}^n& {\mathrm{\&omega;}}_{\mathrm{enx}}^n& 0\end{array}\right]$

式中分别为导航坐标系下导航坐标系相对于地球坐标系的转动角速率沿x轴、y轴、z轴三个方向上的投影，采用一阶欧拉法进行位置矩阵更新，算法表达式为：

$C_n^e(t+T)=C_n^e\left(t\right)+T{C}_n^e\left(t\right){\mathrm{\&Omega;}}_{\mathrm{en}}^n\left(t\right)$

式中T为惯性导航系统采样周期。完成位置矩阵更新后，即可计算出导航位置参数。记 $C=C_n^e={\left(C_e^n\right)}^T,$ 有：

$\left\{\begin{array}{c}L={\sin }^{-1}\left(C_{33}\right)\\ \mathrm{\&lambda;}={\tan }^{-1}\left(\frac{C_{32}}{C_{31}}\right)\end{array}\right.$

关于高度H的计算，由于单纯惯性导航系统的高度计算通道是发散的，要使用外界高度信息对捷联算法的高度通道进行阻尼。

(2)卡尔曼滤波模块对捷联解算模块输出的位置、速度和姿态以及惯性器件的误差进行估计，并将估计出的误差进行校正。该模块具体步骤为：

(a)杆臂误差补偿

分布式POS与任务载荷固联，任务载荷与飞机固联，存在主IMU与GPS天线之间的刚性杆臂，需要对GPS数据进行位置杆臂补偿和速度杆臂补偿，刚性杆臂速度和加速度的计算公式分别如下：

$\begin{array}{c}{V}_r^n={\mathrm{\&omega;}}_{i{b}_m}^n\&times;l_s-{\mathrm{\&omega;}}_{{\mathrm{in}}_m}^n\&times;l_m={\mathrm{\&omega;}}_{{\mathrm{ib}}_m}^n\&times;r=C_b^n({\mathrm{\&omega;}}_{{\mathrm{ib}}_m}^{b_m}\&times;r)\\ a_r^{b_m}={\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_{{\mathrm{ib}}_m}^{b_m}\&times;r+{\mathrm{\&omega;}}_{{\mathrm{ib}}_m}^{b_m}\&times;({\mathrm{\&omega;}}_{{\mathrm{ib}}_m}^{b_m}\&times;r)\end{array}$

位置杆臂和速度杆臂补偿公式如下：

$\begin{array}{c}{P}_{\mathrm{IMU}}^n=P_{\mathrm{GNSS}}^n{C}_b^n{l}^b\\ V_{\mathrm{IMU}}^n=V_{\mathrm{GNSS}}^n-C_b^n({\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;l^b)\end{array}$

$C_b^n=\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}& \cos \mathrm{\&theta;}\sin \mathrm{\&psi;}& \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}\\ -\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin +\mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& \cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& -\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}\\ -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right]$

式中是载体相对惯性空间的角速度矢量，是载体相对惯性空间的角速度矢量在导航坐标系下的投影，l^b是载体坐标系下的固定杆臂向量，为载体系到当地地理系的姿态转换矩阵，为载体系相对惯性坐标系在载体系下的角速度，γ、θ、ψ分别是载体坐标系相对当地地理坐标系的横滚角、俯仰角和航向角。

(b)卡尔曼滤波

采用卡尔曼滤波的方法，以位置误差、速度误差作为量测信息，估计主IMU捷联解算的误差，主POS的系统误差模型采用15维，状态变量包括3个姿态误差、3个速度误差、3个位置误差、3个陀螺漂移误差和3个加速度计漂移误差，系统误差公式如下所示，分别为姿态误差方程、速度误差方程、位置误差方程和惯性器件偏置误差方程。

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_E=-\frac{\mathrm{\&delta;}{V}_N}{R_M+H}+({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_N+H}){\mathrm{\&phi;}}_N-({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+H}){\mathrm{\&phi;}}_U+\frac{V_N}{{(R_M+H)}^2}\mathrm{\&delta;H}+{\mathrm{\&epsiv;}}_E\\ {\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_N=\frac{\mathrm{\&delta;}{V}_E}{R_N+H}-{\mathrm{\&omega;}}_{\mathrm{ie}}\sin \mathrm{L\&delta;L}-({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_N+H}){\mathrm{\&phi;}}_E-\frac{V_N}{R_M+H}{\mathrm{\&phi;}}_U-\frac{V_E}{{(R_N+H)}^2}\mathrm{\&delta;H}+{\mathrm{\&epsiv;}}_N\\ {\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_U=\frac{\tan \mathrm{L\&delta;}{V}_E}{R_N+H}+({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E{\sec }^{2}L}{R_N+H})\mathrm{\&delta;L}+({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+H}){\mathrm{\&phi;}}_E+\frac{V_N{\mathrm{\&phi;}}_N}{R_M+H}-\frac{V_E\tan \mathrm{LH}}{{(R_N+H)}^2}+{\mathrm{\&epsiv;}}_U\end{array}\right.$

$\left\{\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{V}}_E=f_N{\mathrm{\&phi;}}_U-f_U{\mathrm{\&phi;}}_N+\left(\frac{V_N\tan L-V_U}{R_N+H}\right)\mathrm{\&delta;}{V}_E+(2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_N+H})\mathrm{\&delta;}{V}_N+(2{\mathrm{\&omega;}}_{\mathrm{ie}}{V}_N\cos L+)\\ \frac{V_E{V}_N{\sec }^{2}L}{R_N+H}+2{\mathrm{\&omega;}}_{\mathrm{ie}}{V}_U\sin L)\mathrm{\&delta;L}-(2{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+H})\mathrm{\&delta;}{V}_U+\frac{V_E{V}_U-V_E{V}_N\tan L}{{(R_N+H)}^2}\mathrm{\&delta;H}+{\&dtri;}_E\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{V}}_N=f_U{\mathrm{\&phi;}}_E-f_E{\mathrm{\&phi;}}_U-2({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_N+H})\mathrm{\&delta;}{V}_E-\frac{V_U\mathrm{\&delta;}{V}_N}{R_M+H}-\frac{V_N\mathrm{\&delta;}{V}_U}{R_M+H}-\\ (2{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E{\sec }^2}{R_N+H})V_E\mathrm{\&delta;L}+\frac{V_N{V}_U+V_E{V}_E\tan L}{{(R_N+H)}^2}\mathrm{\&delta;H}+{\&dtri;}_N\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{V}}_U=-f_N{\mathrm{\&phi;}}_E+f_E{\mathrm{\&phi;}}_N+2({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+H})\mathrm{\&delta;}{V}_E+2\frac{V_N\mathrm{\&delta;}{V}_N}{R_M+H}-2{\mathrm{\&omega;}}_{\mathrm{ie}}{V}_E\sin \mathrm{L\&delta;L}-\\ \frac{V_E{V}_E+V_N{V}_N}{{(R_N+H)}^2}\mathrm{\&delta;H}+{\&dtri;}_U\end{array}\right.$

$\left\{\begin{array}{c}\mathrm{\&delta;}\stackrel{\&CenterDot;}{L}=\frac{\mathrm{\&delta;}{V}_N}{R_M+H}-\frac{V_N}{{(R_M+H)}^2}\mathrm{\&delta;H}\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}=\frac{\sec L}{R_N+H}\mathrm{\&delta;}{V}_E+\frac{V_E\sec L\tan L}{R_N+H}\mathrm{\&delta;L}-\frac{V_E\sec L}{{(R_N+H)}^2}\mathrm{\&delta;H}\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{H}=\mathrm{\&delta;}{V}_U\end{array}\right.$

$\left\{\begin{array}{ccc}{\stackrel{\&CenterDot;}{\mathrm{\&epsiv;}}}_x=0& {\stackrel{\&CenterDot;}{\mathrm{\&epsiv;}}}_y=0& {\stackrel{\&CenterDot;}{\mathrm{\&epsiv;}}}_z=0\\ {\stackrel{\&CenterDot;}{\&dtri;}}_x=0& {\stackrel{\&CenterDot;}{\&dtri;}}_y=0& {\stackrel{\&CenterDot;}{\&dtri;}}_z=0\end{array}\right.$

式中分别为东向失准角、北向失准角和天向失准角，δV_E、δV_N和δV_U分别为东向速度误差、北向速度误差和天向速度误差，δL、δλ和δH分别为纬度误差、经度误差和高度误差，ε_x、ε_y和ε_z分别为x轴向陀螺仪常值漂移误差、y轴向陀螺仪常值漂移误差和z轴向陀螺仪常值漂移误差，和分别为x轴向加速度计常值漂移误差、y轴向加速度计常值漂移误差和z轴向加速度计常值漂移误差，R_M和R_N分别为沿子午圈和卯酉圈的主曲率半径，H为载体高度，ω_ie为地球自转角速度，f_E、f_N和f_U分别为东向比力、北向比力和天向比力。

利用标准卡尔曼滤波的五个公式进行滤波，可以得到估计的15个系统误差变量，利用卡尔曼滤波估计的结果对系统状态变量进行反馈校正，提高系统精度。

4、子IMU初始对准；利用子IMU数据和主POS的位置信息，采用基于惯性矢量二次积分的解析对准方法完成摇摆基座下子IMU初始对准，得到子IMU初始位置、速度和姿态，该方法通过下式确定子IMU姿态矩阵

$C_b^n\left(t\right)=C_{e_0}^n{C}_{i_0}^{e_0}\left(t\right)C_{i_{b_0}}^{i_0}{C}_b^{i_{b_0}}\left(t\right)$

式中是地心地球坐标系到导航坐标系的转换矩阵，是地心惯性坐标系到地心地球坐标系的转换矩阵，是基座惯性坐标系到地心惯性坐标系的转换矩阵，是载体坐标系到基座惯性坐标系的转换矩阵。

基于惯性矢量二次积分的解析对准方法原理如图4所示：根据陀螺仪的输出信息求取根据加速度计的输出信息和重力信息，通过二次积分的方式求取减小了环境扰动引起的加速度误差；根据初始对准地点的经纬度信息以及对准的时间信息求出和通过以上4个矩阵确定IMU初始姿态，具体步骤如下：

(1)设对准开始时刻t₀载体所在纬度为L，L可由主POS输出的实时导航结果获取，地球自转角速度ω_ie已知，上式中

$C_{e_0}^n=\left[\begin{array}{ccc}0& 1& 0\\ -\sin L& 0& \cos L\\ \cos L& 0& \sin L\end{array}\right]$    $C_{i_0}^{e_0}\left(t\right)=\left[\begin{array}{ccc}\cos {\mathrm{\&omega;}}_{\mathrm{ie}}(t-t_0)& \sin {\mathrm{\&omega;}}_{\mathrm{ie}}(t-t_0)& 0\\ -\sin {\mathrm{\&omega;}}_{\mathrm{ie}}(t-t_0)& \cos {\mathrm{\&omega;}}_{\mathrm{ie}}(t-t_0)& 0\\ 0& 0& 1\end{array}\right]$

(2)可根据陀螺输出进行实时姿态更新求得，姿态更新的微分方程为如下形式，即 ${\stackrel{\&CenterDot;}{C}}_b^{i_{b_0}}=C_b^{i_{b_0}}({\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;).$

(3)实质上是载体惯性坐标系到经线地心惯性坐标系的变换矩阵，利用重力信息实现的求取，将子IMU敏感到的加速度矢量通过两次积分转化为位移矢量，求取在摇摆过程中，IMU中的加速度计感测到的比力中包含重力加速度和杆臂加速度，经过杆臂加速度补偿后的比力在系中的投影可以表示为如下的形式：

${\hat{f}}^{i_{b_0}}={\hat{C}}_b^{i_{b_0}}{\hat{f}}^b$

式中为经过杆臂加速度补偿后的比力，为根据陀螺输出计算得到的b系与系间的变换矩阵。对上式两边积分，即可得到有：

${\hat{V}}_{(t_1,t_m)}^{i_{b_0}}=-{\&Integral;}_{t_l}^{t_m}{\hat{C}}_b^{i_{b_0}}{\hat{f}}^b\mathrm{dt}$

为t_l时刻到t_m时刻的速度矢量，对上式两边积分，即可得到有:

${\hat{S}}_{(t_1,t_m)}^{i_{b_0}}=-{\&Integral;}_{t_l}^{t_m}{\hat{V}}^{i_{b_0}}\mathrm{dt}$

为t_l时刻到t_m时刻的位移矢量，又由于：

$g^{i_0}=C_{e_0}^{i_0}{C}_n^{e_0}{g}^n$

对上式两边积分，得：

$V_{(t_r,t_k)}^{i_0}=\left[\begin{array}{c}\frac{g\cos L\sin {\mathrm{\&omega;}}_{\mathrm{ie}}\mathrm{\&Delta;}{t}_k}{{\mathrm{\&omega;}}_{\mathrm{ie}}}\\ -\frac{g\cos L(\cos {\mathrm{\&omega;}}_{\mathrm{ie}}\mathrm{\&Delta;}{t}_k-1)}{{\mathrm{\&omega;}}_{\mathrm{ie}}}\\ g\sin \mathrm{L\&Delta;}{t}_k\end{array}\right]$

对上式两边积分，得：

$S_{(t_r,t_k)}^{i_0}=\left[\begin{array}{c}\frac{g\cos L(1-\cos {\mathrm{\&omega;}}_{\mathrm{ie}}\mathrm{\&Delta;}{t}_k)}{{{\mathrm{\&omega;}}_{\mathrm{ie}}}^2}\\ -\frac{g\cos L({\mathrm{\&omega;}}_{\mathrm{ie}}\mathrm{\&Delta;}{t}_k-\sin {\mathrm{\&omega;}}_{\mathrm{ie}}\mathrm{\&Delta;}{t}_k)}{{{\mathrm{\&omega;}}_{\mathrm{ie}}}^2}\\ \frac{g\sin \mathrm{L\&Delta;}{t_k}^2}{2}\end{array}\right]$

式中△t_k＝t_k-t_r。取t₀<t_u≤t_v<t_d，其中t₀为对准开始时刻、t_d为对准结束时刻，得：

$\left\{\begin{array}{c}{\hat{S}}_{(t_0,t_u)}^{i_{b_0}}=C_{i_0}^{i_{b_0}}{S}_{(t_0,t_u)}^{i_0}\\ {\hat{S}}_{(t_v,t_d)}^{i_{b_0}}=C_{i_0}^{i_{b_0}}{S}_{(t_v,t_d)}^{i_0}\end{array}\right.$

由上式可得：

$C_{i_{b_0}}^{i_0}={\left[\begin{array}{c}{\left[S_{(t_0,t_u)}^{i_0}\right]}^T\\ {\left[S_{(t_v,t_d)}^{i_0}\right]}^T\\ {[S_{(t_0,t_u)}^{i_0}\&times;S_{(t_v,t_d)}^{i_0}]}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left[{\hat{S}}_{(t_0,t_u)}^{i_{b_0}}\right]}^T\\ {\left[{\hat{S}}_{(t_v,t_d)}^{i_{b_0}}\right]}^T\\ {[{\hat{S}}_{(t_0,t_u)}^{i_{b_0}}\&times;{\hat{S}}_{(t_v,t_d)}^{i_{b_0}}]}^T\end{array}\right]$

将和代入 $C_b^n\left(t\right)=C_{e_0}^n{C}_{i_0}^{e_0}\left(t\right)C_{i_{b_0}}^{i_0}{C}_b^{i_{b_0}}\left(t\right)$ 即可计算出初始对准确定的姿态阵。

5、子IMU实时导航；子IMU实时导航包括捷联解算和传递对准两部分：

(1)子IMU捷联解算与主IMU捷联解算步骤相同，功能是以上一时刻的位置、速度和姿态信息作为当前捷联解算的初始值，结合当前时刻的子IMU数据，获得当前时刻的惯性导航结果；

(2)传递对准流程如图5所示，将主POS实时导航结果进行杆臂补偿，将杆臂补偿后的位置误差和速度误差作为观测量，将主POS实时导航结果和子IMU捷联解算结果进行卡尔曼滤波，估计主/子IMU之间的基线长度误差、子IMU位置、速度、姿态和惯性器件误差，并将估计出的误差进行反馈校正，得到高精度的子IMU实时导航结果和主/子IMU之间的相对空间关系，具体步骤如下：

(a)系统状态方程的建立

系统状态方程为：

$\stackrel{\&CenterDot;}{X}=\mathrm{FX}+W$

式中 $X={\left[\begin{array}{cccccccccccccccccc}{\mathrm{\&phi;}}_E& {\mathrm{\&phi;}}_N& {\mathrm{\&phi;}}_U& \mathrm{\&delta;}{V}_E& \mathrm{\&delta;}{V}_N& \mathrm{\&delta;}{V}_U& \mathrm{\&delta;L}& \mathrm{\&delta;\&lambda;}& \mathrm{\&delta;H}& {\mathrm{\&epsiv;}}_E& {\mathrm{\&epsiv;}}_N& {\mathrm{\&epsiv;}}_U& {\&dtri;}_E& {\&dtri;}_N& {\&dtri;}_U& \mathrm{\&Delta;}{r}_x& \mathrm{\&Delta;}{r}_y& \mathrm{\&Delta;}{r}_z\end{array}\right]}^T,$ X为18维状态变量，包括3个姿态误差、3个速度误差、3个位置误差、3个陀螺仪偏置、3个加速度计偏置和3个杆臂长度误差；F为状态转移矩阵；W为系统噪声，并假设其为零均值高斯白噪声；F的表达式：

$F=\left[\begin{array}{cccccc}{F}_1& F_2& F_3& C_b^n& 0_{3\&times;3}& \begin{array}{cc}{C}_b^n& {\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;\end{array}\\ F_4& F_5& F_6& 0_{3\&times;3}& C_b^n& 0_{3\&times;3}\\ 0_{3\&times;3}& F_7& F_8& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right]$

式中：

$F_1=\left[\begin{array}{ccc}0& {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_x\tan L}{R_N+H}& -({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_N+H})\\ -({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_x\tan L}{R_N+H})& 0& -\frac{V_y}{R_M+H}\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_N+H}& \frac{V_y}{R_M+H}& 0\end{array}\right]$

$F_2=\left[\begin{array}{ccc}0& -\frac{1}{R_M+H}& 0\\ \frac{1}{R_N+H}& 0& 0\\ \frac{\tan L}{R_M+H}& 0& 0\end{array}\right]$   $F_3=\left[\begin{array}{ccc}0& 0& \frac{V_y}{{(R_M+H)}^2}\\ -{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L& 0& -\frac{V_x}{{(R_N+H)}^2}\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_x{\sec }^{2}L}{R_N+H}& 0& \frac{V_x\tan L}{{(R_N+H)}^2}\end{array}\right]$

$F_5=\left[\begin{array}{ccc}\frac{V_y\tan L}{R_N+H}-\frac{V_z}{R_N+H}& {2\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_x\tan L}{R_N+H}& -{2\mathrm{\&omega;}}_{\mathrm{ie}}\cos L-\frac{V_x}{R_N+H}\\ -2({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_x\tan L}{R_N+H})& -\frac{V_z}{R_M+H}& -\frac{V_y}{R_M+H}\\ 2({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_N+H})& \frac{{2V}_y}{R_M+H}& 0\end{array}\right]$

$F_4=\left[\begin{array}{ccc}0& {-f}_z& f_y\\ f_z& 0& {-f}_x\\ {-f}_y& f_x& 0\end{array}\right]$   $F_6=\left[\begin{array}{ccc}{2\mathrm{\&omega;}}_{\mathrm{ie}}{V}_y\cos L+\frac{V_x{V}_y{\sec }^{2}L}{R_N+H}+{2\mathrm{\&omega;}}_{\mathrm{ie}}{V}_z\sin L& 0& \frac{V_x{V}_z-V_x{V}_y\tan L}{{(R_N+H)}^2}\\ -({2\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_x{\sec }^2}{R_N+H})V_E& 0& \frac{V_y{V}_z+V_x^2\tan L}{{(R_N+H)}^2}\\ {-2\mathrm{\&omega;}}_{\mathrm{ie}}{V}_x\sin L& 0& -\frac{V_x^2+V_y^2}{{(R_N+H)}^2}\end{array}\right]$

$F_7=\left[\begin{array}{ccc}0& \frac{1}{R_M+H}& 0\\ \frac{\sec L}{R_N+H}& 0& 0\\ 0& 0& 1\end{array}\right]$   $F_8=\left[\begin{array}{ccc}0& 0& -\frac{V_y}{{(R_M+H)}^2}\\ \frac{V_x\sec L\tan L}{R_N+H}& 0& -\frac{V_x\sec L}{{(R_N+H)}^2}\\ 0& 0& 0\end{array}\right]$

式中R_M和R_N分别为沿子午圈和卯酉圈的主曲率半径，H为载体高度，L为当地纬度，ω_ie为地球自转角速度，V_x、V_y和V_z分别为x向速度、y向速度和z向速度。f_x、f_y和f_z分别为x向比力、y向比力和z向比力，为载体系到当地地理系的姿态转换矩阵。

(b)量测方程的建立

补偿主/子IMU之间的柔性杆臂效应，量测量Z为经过杆臂补偿后的速度误差和位置误差，量测矩阵H为：

H＝[H_V H_P]^T

$H_V=\left[\begin{array}{ccccccc}& 1& 0& 0& & & \\ 0_{3\&times;3},& 0& 1& 0,& 0_{3\&times;9},& C_b^n& {\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;\\ & 0& 0& 1& & & \end{array}\right]$   $H_P=\left[\begin{array}{cccc}& R_M+H& 0& 0\\ 0_{3\&times;6},& 0& R_M+H& 0,0_{3\&times;9}\\ & 0& 0& 1\end{array}\right]$

(c)卡尔曼滤波器设计

利用标准卡尔曼滤波的五个公式进行滤波，可以得到估计的18个系统误差变量，利用卡尔曼滤波估计的结果对系统状态变量进行反馈校正，得到更加准确的主/子IMU之间的基线长度和子IMU实时导航结果。

Claims (4)

1.一种分布式POS用数据处理计算机系统实时导航方法，其特征在于包括下列步骤：

(1)主POS实时导航；首先，采用基于PPS实时标定与分频的时间同步方法实现分布式POS主/子IMU原始数据的同时刻采集，其次，将GPS数据进行杆臂补偿，获得杆臂补偿后的位置和速度信息，利用主IMU数据进行初始对准以及捷联解算，获得惯性导航结果，采用卡尔曼滤波的方法估计主IMU位置、速度、姿态和惯性器件误差，得到高精度的主POS实时导航结果；

(2)子IMU初始对准；根据子IMU数据和主POS的位置信息，采用基于惯性矢量二次积分的解析对准方法完成摇摆基座下子IMU初始对准，得到子IMU初始位置、速度和姿态信息；

(3)子IMU实时导航；初始对准完成后，利用子IMU数据进行捷联解算，获得惯性导航结果；采用基于多级杆臂补偿的卡尔曼滤波方法完成分布式POS传递对准，估计主/子IMU之间的基线长度误差以及子IMU位置、速度、姿态和惯性器件误差，得到主/子IMU之间准确的基线长度和高精度的子IMU实时导航结果。

2.根据权利要求1所述的一种分布式POS用数据处理计算机系统实时导航方法，其特征在于：步骤(1)所述的基于PPS实时标定与分频的时间同步方法中，以PPS作为同步脉冲信号，触发IMU数据采集脉冲的产生，之后根据每秒的PPS标定IMU内部时钟，实时调整数据采集周期分频数，修正IMU内部的时钟漂移，保证各IMU一直在相同时刻进行数据采集。

3.根据权利要求1所述的一种分布式POS用数据处理计算机系统实时导航方法，其特征在于：步骤(2)所述的基于惯性矢量二次积分的解析对准方法中，二次积分是针对子IMU中加速度计敏感到的重力加速度进行的，第一次积分将加速度矢量转化为速度矢量，第二次积分将速度矢量转化为位移矢量。

4.根据权利要求1所述的一种分布式POS用数据处理计算机系统实时导航方法，其特征在于：步骤(3)所述的卡尔曼滤波方法中，状态变量X共有18维，包括东、北、天向的失准角东、北、天向的速度误差δV_E、δV_N、δV_U，纬度、经度、高度误差δL、δλ、δH，x、y、z轴向的陀螺仪常值漂移误差ε_x、ε_y、ε_z，x、y、z轴向加速度计常值漂 移误差x、y、z轴向杆臂长度误差△r_x、△r_y、△r_z；量测量Z是经过多级杆臂补偿的速度误差和位置误差，多级杆臂分别为GPS与主IMU之间的刚性杆臂和主/子IMU之间的柔性杆臂。