CN102393201B - 航空遥感用位置和姿态测量***（pos）动态杆臂补偿方法 - Google Patents

Abstract

一种航空遥感用位置和姿态测量***(POS)动态杆臂补偿方法。该方法针对三轴惯性稳定平台框架转动导致惯性测量单元(IMU)量测中心与GPS天线相位中心之间的杆臂实时变化的问题，通过实时计算三轴惯性稳定平台中心与IMU量测中心之间的动态杆臂，得到IMU量测中心与GPS天线相位中心之间的实际杆臂，并实时计算三轴惯性稳定平台初始坐标系相对当地地理坐标系在三轴惯性稳定平台初始坐标系下的角速度，进行动态杆臂补偿。本发明具有精度高、操作简单、易于实现的特点，解决了航空遥感使用三轴惯性稳定平台时POS杆臂难以精确补偿的问题，提高了POS和航空遥感成像载荷的精度。

Description

航空遥感用位置和姿态测量***(POS)动态杆臂补偿方法

技术领域

本发明涉及一种航空遥感用位置和姿态测量***(POS)动态杆臂补偿方法，属于航空遥感领域，应用于采用三轴惯性稳定平台的航空遥感，提高了POS和航空遥感成像载荷的精度。

背景技术

航空遥感是一种以飞机为载体，利用遥感载荷获取地球表面各种空间地理信息的高技术。航空遥感***进行高分辨率成像时要求载荷在空间做匀速直线的理想运动，但飞机不可避免会受到阵风、湍流等外部扰动的影响，使载荷偏离理想运动轨迹，导致载荷成像质量下降。因此，要实现航空遥感***的高精度成像，必须对飞行扰动进行隔离和运动补偿。三轴惯性稳定平台由三个框架组成，通过伺服控制三个框架保持成像载荷水平并指向飞机飞行方向，从而在一定程度上隔离飞行过程中的外部扰动。由于三轴惯性稳定平台负载大，自重小，响应速度有限，不可能将飞行扰动完全隔离，因此，必须使用POS精确测量未能隔离的飞行扰动，得到成像载荷相位中心的位置、速度和姿态等运动参数，并在成像过程中进行运动补偿。

POS则由IMU、GPS接收机、POS导航计算机(PCS)和后处理软件等组成。其中IMU用于测量与其固联载体的三维角速度和三维线加速度，经捷联惯性导航解算，可得到载体的位置、速度和姿态信息，具有短时精度高，输出连续和完全自主等优点，但其导航误差随时间积累。GPS则可长时间提供高精度的位置和速度信息，但输出不连续，不能提供姿态信息，且GPS信号受到遮挡时不能实现定位。POS利用惯性导航和GPS导航天然的互补性，应用信息融合技术，将惯性量测信息同GPS量测信息进行融合，能够连续、实时地获取载体的位置、速度和姿态等全面的运动参数，且误差不随时间积累。

GPS量测信息同惯性量测信息进行融合时，必须进行杆臂补偿。因为GPS天线不可能同IMU安装在一起，为了接收GPS卫星信号，GPS天线一般安装在飞机顶部，而IMU则安装在机舱内部，两者之间的距离一般都在1米以上，但GPS的位置精度和速度精度可分别达到0.05m和0.005m/s，因此必须将GPS得到的位置和速度信息通过IMU量测中心与GPS天线相位中心之间的杆臂补偿到IMU量测中心。但在飞行过程中，惯性稳定平台会实时转动三个框架以保持成像载荷水平并指向飞机飞行方向，从而导致IMU量测中心与GPS天线相位中心之间的相对方位不断发生变化，使得IMU量测中心与GPS天线相位中心之间的杆臂实时变化。传统的杆臂补偿方法假设IMU和GPS天线之间没有相对方位变化，即IMU量测中心与GPS天线相位中心之间的杆臂恒定不变，利用捷联惯性导航算法解算得到的

阵、和杆臂测量值l^b进行杆臂补偿。因此，传统的杆臂补偿方法应用于采用三轴惯性稳定平台的航空遥感会产生很大的误差甚至不能使用。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提出一种航空遥感用POS动态杆臂补偿方法。该方法针对三轴惯性稳定平台框架转动导致IMU量测中心与GPS天线相位中心之间的杆臂实时变化的问题，通过实时计算三轴惯性稳定平台中心与IMU量测中心之间的动态杆臂，得到IMU量测中心与GPS天线相位中心之间的实际杆臂，并实时计算三轴惯性稳定平台初始坐标系相对当地地理坐标系在三轴惯性稳定平台初始坐标系下的角速度，进行动态杆臂补偿。本发明具有精度高、操作简单、易于实现的特点，解决了航空遥感使用三轴惯性稳定平台时POS杆臂难以精确补偿的问题，提高了POS和航空遥感成像载荷的精度。

本发明的技术解决方案为：一种航空遥感用POS动态杆臂补偿方法，具体步骤如下：

(1)在三轴惯性稳定平台初始坐标系下，分别测量得到三轴惯性稳定平台中心与GPS天线相位中心之间的杆臂

和三轴惯性稳定平台中心与惯性测量单元(IMU)量测中心之间的杆臂

(2)计算GPS数据时刻三轴惯性稳定平台初始坐标系到当地地理坐标系的方向余弦阵；

(3)利用步骤(1)得到的杆臂和步骤(2)得到的方向余弦阵，计算GPS数据时刻IMU量测中心与GPS天线相位中心之间的动态杆臂；

(4)利用步骤(2)得到的方向余弦阵，计算三轴惯性稳定平台初始坐标系相对当地地理坐标系在三轴惯性稳定平台初始坐标系下的角速度；

(5)基于步骤(2)得到的方向余弦阵、步骤(3)得到的动态杆臂和步骤(4)得到的角速度，对GPS位置数据和速度数据进行动态杆臂补偿，并将补偿后的GPS数据与惯性数据进行融合，得到最优的运动参数；

(6)重复步骤(2)至步骤(5)，直至POS***数据处理结束。

本发明的原理：针对三轴惯性稳定平台框架转动导致IMU量测中心与GPS天线相位中心之间的杆臂实时变化的问题，本发明在三轴惯性稳定平台初始坐标系下，将IMU量测中心与GPS天线相位中心之间的动态杆臂l分解为两个杆臂的差，如说明书附图2所示，计算杆臂l₁与杆臂l₂的差，即可得到动态杆臂l。其中杆臂l₁为三轴惯性稳定平台中心与GPS天线相位中心之间的杆臂，杆臂l₂为三轴惯性稳定平台中心与IMU量测中心之间的动态杆臂。通过对比说明书附图2a和说明书附图2b可知，l₁为固定杆臂，不随着惯性稳定平台的框架转动而变化；l₂为动态杆臂，随着惯性稳定平台的框架转动而改变。通过测量得到三轴惯性稳定平台中心与GPS天线相位中心之间的固定杆臂l₁，并实时计算三轴惯性稳定平台中心与IMU量测中心之间的动态杆臂l₂，从而可准确获得由于三轴惯性稳定平台框架转动导致的IMU量测中心与GPS天线相位中心之间实时变化的杆臂l，再利用捷联惯性导航算法和三轴惯性稳定平台三轴转动关系解算出的

阵、

和杆臂计算值l^b′进行实时动态杆臂补偿。

本发明与现有技术相比的优点在于：本发明通过实时计算IMU量测中心与GPS天线相位中心之间的动态杆臂l^b′和

进行精确的动态杆臂补偿，相对现有技术具有精度高的特点，解决了航空遥感使用三轴惯性稳定平台时POS杆臂难以精确补偿的问题，提高了POS和航空遥感成像载荷的精度。

附图说明

图1为本发明的POS动态杆臂补偿方法流程图；

图2为本发明的航空遥感各子***相对方位示意图，图中，ox_b′y_b′z_b′坐标系为三轴惯性稳定平台初始坐标系，ox_by_bz_b坐标系为三轴惯性稳定平台内框架坐标系，O_P为三轴惯性稳定平台中心，O_I为IMU量测中心，O_G为GPS天线相位中心。其中，图2a为三轴惯性稳定平台内框架坐标系ox_by_bz_b和三轴惯性稳定平台初始坐标系ox_b′y_b′z_b′重合时的相对方位示意图；图2b为三轴惯性稳定平台内框架坐标系ox_by_bz_b和三轴惯性稳定平台初始坐标系ox_b′y_b′z_b′不重合时的相对方位示意图。

具体实施方式

如说明书附图1所示，本发明的具体实施包括以下步骤：

1、将三轴惯性稳定平台设置为调平模式，三轴惯性稳定平台控制三轴框架，使惯性稳定平台三轴的电编码器输出为零，设此时三轴惯性稳定平台内框架坐标系为三轴惯性稳定平台初始坐标系ox_b′y_b′z_b′，以b′表示；设三轴惯性稳定平台实时的内框架坐标系ox_by_bz_b以b表示；设当地地理坐标系ox_ny_nz_n以n表示。在三轴惯性稳定平台初始坐标系ox_b′y_b′z_b′下，使用经纬仪测量三轴惯性稳定平台中心O_P与GPS天线相位中心O_G之间的杆臂和三轴惯性稳定平台中心O_P与IMU量测中心之间O_I的杆臂，得到三轴惯性稳定平台中心O_P与GPS天线相位中心O_G之间的杆臂为

三轴惯性稳定平台中心O_P与IMU量测中心O_I之间的杆臂为

然后将三轴惯性稳定平台设置为遥控模式，跟踪POS输出，进行实时控制。成像载荷、三轴惯性稳定平台和IMU间的方位关系见说明书附图2，其中三轴惯性稳定平台安装在飞机机体上，成像载荷则安装在三轴惯性稳定平台上，与三轴惯性稳定平台的内框架固联，IMU则与成像载荷固联，测量成像载荷实时的运动参数。

2、计算GPS数据时刻三轴惯性稳定平台初始坐标系到当地地理坐标系的方向余弦阵

(1)计算GPS数据时刻三轴惯性稳定平台初始坐标系ox_b′y_b′z_b′到三轴惯性稳定平台内框架坐标系ox_by_bz_b的方向余弦阵

由于三轴惯性稳定平台最外框为横滚框，中框为俯仰框，内框为航向框，故可得

$C_{b^{\′}}^b=\left[\begin{array}{ccc}\cos \left({\mathrm{\ψ}}_1\right)& \sin \left({\mathrm{\ψ}}_1\right)& 0\\ -\sin \left({\mathrm{\ψ}}_1\right)& \cos \left({\mathrm{\ψ}}_1\right)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \left({\mathrm{\θ}}_1\right)& \sin \left({\mathrm{\θ}}_1\right)\\ 0& -\sin \left({\mathrm{\θ}}_1\right)& \cos \left({\mathrm{\θ}}_1\right)\end{array}\right]\left[\begin{array}{ccc}\cos \left({\mathrm{\γ}}_1\right)& 0& -\sin \left({\mathrm{\γ}}_1\right)\\ 0& 1& 0\\ \sin \left({\mathrm{\γ}}_1\right)& 0& \cos \left({\mathrm{\γ}}_1\right)\end{array}\right]$

其中，γ₁角为GPS数据时刻三轴惯性稳定平台外框相对于惯性稳定平台初始外框旋转的角度，θ₁角为GPS数据时刻三轴惯性稳定平台中框相对于三轴惯性稳定平台初始中框旋转的角度，ψ₁角为GPS数据时刻三轴惯性稳定平台内框相对于三轴惯性稳定平台初始内框旋转的角度。

由于三轴惯性稳定平台与GPS各自具有独立的时钟***，三轴惯性稳定平台输出的电编码器数据与GPS数据难以完全同步，设在GPS数据时刻前采样得到的惯性稳定平台三轴的电编码器数据分别为

和

在GPS数据时刻后采样得到的惯性稳定平台三轴的电编码器数据分别为

和由于三轴惯性稳定平台各框架惯量都较大，可假设在每个采样间隔下三轴惯性稳定平台框架角速度不变，可得

$\left[\begin{array}{c}{\mathrm{\γ}}_1\\ {\mathrm{\θ}}_1\\ {\mathrm{\ψ}}_1\end{array}\right]=\left[\begin{array}{c}(P_{\mathrm{Ay}}^{+}-P_{\mathrm{Ay}}^{-})(T_{\mathrm{GPS}}-T_A^{-})/(T_A^{+}-T_A^{-})+P_{\mathrm{Ay}}^{-}\\ (P_{\mathrm{Ax}}^{+}-P_{\mathrm{Ax}}^{-})(T_{\mathrm{GPS}}-T_A^{-})/(T_A^{+}-T_A^{-})+P_{\mathrm{Ax}}^{-}\\ (P_{\mathrm{Az}}^{+}-P_{\mathrm{Az}}^{-})(T_{\mathrm{GPS}}-T_A^{-})/(T_A^{+}-T_A^{-})+P_{\mathrm{Az}}^{-}\end{array}\right]$

其中，T_GPS为GPS数据时刻的UTC时间，

为

和

数据采样时刻的UTC时间，

为

和

数据采样时刻的UTC时间。

(2)计算GPS数据时刻三轴惯性稳定平台内框架坐标系ox_by_bz_b到当地地理坐标系ox_ny_nz_n的方向余弦阵

$C_b^n=\left[\begin{array}{ccc}\cos {\mathrm{\γ}}_2\cos {\mathrm{\ψ}}_2+\sin {\mathrm{\γ}}_2\sin {\mathrm{\θ}}_2\sin {\mathrm{\ψ}}_2& \cos {\mathrm{\θ}}_2\sin {\mathrm{\ψ}}_2& \sin {\mathrm{\γ}}_2\cos {\mathrm{\ψ}}_2-\cos {\mathrm{\γ}}_2\sin {\mathrm{\θ}}_2\sin {\mathrm{\ψ}}_2\\ -\cos {\mathrm{\γ}}_2\sin {\mathrm{\ψ}}_2+\sin {\mathrm{\γ}}_2\sin {\mathrm{\θ}}_2\cos {\mathrm{\ψ}}_2& \cos {\mathrm{\θ}}_2\cos {\mathrm{\ψ}}_2& -\sin {\mathrm{\γ}}_2\sin {\mathrm{\ψ}}_2-\cos {\mathrm{\γ}}_2\sin {\mathrm{\θ}}_2\cos {\mathrm{\ψ}}_2\\ -\sin {\mathrm{\γ}}_2\cos {\mathrm{\θ}}_2& \sin {\mathrm{\θ}}_2& \cos {\mathrm{\γ}}_2\cos {\mathrm{\θ}}_2\end{array}\right]$

其中，γ₂、θ₂、ψ₂分别是三轴惯性稳定平台内框架坐标系ox_by_bz_b相对当地地理坐标系ox_ny_nz_n的横滚角、俯仰角和航向角。

(3)由上述分析可进一步求得：GPS数据时刻三轴惯性稳定平台初始坐标系ox_b′y_b′z_b′到当地地理坐标系ox_ny_nz_n的方向余弦阵可以由GPS数据时刻三轴惯性稳定平台内框架坐标系ox_by_bz_b到当地地理坐标系ox_ny_nz_n的方向余弦阵和GPS数据时刻三轴惯性稳定平台初始坐标系ox_b′y_b′z_b′到三轴惯性稳定平台内框架坐标系ox_by_bz_b的方向余弦阵

点积得到，即 $C_{b^{\′}}^n=C_b^n{C}_{b^{\′}}^b$

3、计算GPS数据时刻IMU量测中心与GPS天线相位中心之间的动态杆臂l^b′

(1)计算GPS数据时刻三轴惯性稳定平台中心O_P与GPS天线相位中心O_G之间的杆臂

在三轴惯性稳定平台初始坐标系ox_b′y_b′z_b′下，惯性稳定平台在控制三轴框架转动过程中，三轴惯性稳定平台中心O_P与飞机之间没有相对方位变化，因此三轴惯性稳定平台中心O_P与GPS天线相位中心O_G之间的相对方位关系不变，由步骤1得到的三轴惯性稳定平台中心O_P与GPS天线相位中心O_G之间的杆臂与相等，即

$l_1^{b^{\′}}=l_{01}^{b^{\′}}$

(2)计算GPS数据时刻三轴惯性稳定平台中心O_P与IMU量测中心O_I之间的杆臂

由于三轴惯性稳定平台实时控制框架转动以保持成像载荷水平并指向飞机飞行方向，使得

实时变化，但由于IMU与三轴惯性稳定平台内框架固联，因此IMU量测中心O_I在三轴惯性稳定平台内框架坐标系ox_by_bz_b的位置坐标与IMU量测中心在三轴惯性稳定平台初始坐标系ox_b′y_b′z_b′的位置坐标相同，可得

$l_2^b=l_{02}^{b^{\′}}$

由步骤2中得到的GPS数据时刻三轴惯性稳定平台初始坐标系ox_b′y_b′z_b′到三轴惯性稳定平台内框架坐标系ox_by_bz_b的方向余弦阵通过转置可得GPS数据时刻三轴惯性稳定平台内框架坐标系ox_by_bz_b到三轴惯性稳定平台初始坐标系ox_b′y_b′z_b′的方向余弦阵

$C_b^{b^{\′}}=C_{b^{\′}}^{\mathrm{bT}}$

从而可得GPS数据时刻三轴惯性稳定平台中心O_P与IMU量测中心O_I之间的杆臂

$l_2^{b^{\′}}=C_b^{b^{\′}}{l}_2^b$

(3)由上述分析可进一步求得：GPS数据时刻IMU量测中心O_I与GPS天线相位中心O_G之间的动态杆臂l^b′可由GPS数据时刻三轴惯性稳定平台中心O_P与GPS天线相位中心O_G之间的杆臂

和GPS数据时刻三轴惯性稳定平台中心O_P与IMU量测中心O_I之间的杆臂

之差得到，即

$l^{b^{\′}}=l_1^{b^{\′}}-l_2^{b^{\′}}$

4、计算三轴惯性稳定平台初始坐标系相对当地地理坐标系在三轴惯性稳定平台初始坐标系下的角速度

(1)计算三轴惯性稳定平台内框架坐标系ox_by_bz_b相对三轴惯性稳定平台初始坐标系ox_b′y_b′z_b′在三轴惯性稳定平台内框架坐标系ox_by_bz_b下的角速度

由步骤2可知，GPS数据时刻前采样得到的惯性稳定平台三轴的电编码器数据分别为

GPS数据时刻后采样得到的惯性稳定平台三轴的电编码器数据分别为

和

三轴惯性稳定平台输出的电编码器数据周期为0.01秒。由于三轴惯性稳定平台各框架惯量较大，可假设相邻两个采样时刻之间，三轴惯性稳定平台框架角速度不变，可得三个框架角速度为：

$w_{b^{\′}b}^b=\left[\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right]=\left[\begin{array}{c}(P_{\mathrm{Ax}}^{+}-P_{\mathrm{Ax}}^{-})/0.01\\ (P_{\mathrm{Ay}}^{+}-P_{\mathrm{Ay}}^{-})/0.01\\ (P_{\mathrm{Az}}^{+}-P_{\mathrm{Az}}^{-})/0.01\end{array}\right]$

当三轴惯性稳定平台输出的电编码器数据噪声较大时，需要先对电编码器数据进行滤波，然后采用插值的方法得到惯性稳定平台三轴角位置随时间变化的函数，通过在GPS数据时刻对函数进行微分运算得到惯性稳定平台三轴的角速度。

(2)计算三轴惯性稳定平台初始坐标系ox_b′y_b′z_b′相对当地地理坐标系ox_ny_nz_n在三轴惯性稳定平台初始坐标系ox_b′y_b′z_b′下的角速度

由于

$w_{\mathrm{in}}^n=w_{\mathrm{ie}}^n+w_{\mathrm{en}}^n$

其中，为当地地理坐标系ox_ny_nz_n相对惯性坐标系在当地地理坐标系ox_ny_nz_n下的角速度，为地球坐标系相对惯性坐标系在当地地理坐标系ox_ny_nz_n下的角速度，

为当地地理坐标系ox_ny_nz_n相对地球坐标系在当地地理坐标系ox_ny_nz_n下的角速度，且

$w_{\mathrm{ie}}^n=\left[\begin{array}{c}0\\ \cos \left(\mathrm{Lat}\right)\\ \sin \left(\mathrm{Lat}\right)\end{array}\right]$

$w_{\mathrm{en}}^n=\left[\begin{array}{c}-V_N^n/R_M\\ V_E^n/R_N\\ V_E^n\tan \left(\mathrm{Lat}\right)/R_N\end{array}\right]$

其中，Lat为当地纬度，为飞行载体在当地地理坐标系ox_ny_nz_n下的东向速度，

为飞行载体在当地地理坐标系ox_ny_nz_n下的北向速度，R_M和R_N分别为当地子午圈主曲率半径和当地卯酉圈主曲率半径。

可得

$w_{\mathrm{in}}^b=C_n^b{w}_{\mathrm{in}}^n$

由于

$w_{{\mathrm{ib}}^{\′}}^b=w_{\mathrm{ib}}^b-w_{b^{\′}b}^b$

其中，

为陀螺仪输出的三轴惯性稳定平台内框架坐标系ox_by_bz_b相对惯性坐标系在三轴惯性稳定平台内框架坐标系ox_by_bz_b下的角速度，

为三轴惯性稳定平台初始坐标系ox_b′y_b′z_b′相对惯性坐标系在三轴惯性稳定平台内框架坐标系ox_by_bz_b下的角速度。

因此可得

$w_{{\mathrm{nb}}^{\′}}^{b^{\′}}=C_b^{b^{\′}}(w_{{\mathrm{ib}}^{\′}}^b-w_{\mathrm{in}}^b)$

5、对GPS位置数据和速度数据进行动态杆臂补偿

(1)对GPS位置数据进行动态杆臂补偿

GPS数据时刻IMU量测中心O_I与GPS天线相位中心O_G之间的杆臂lⁿ

$l^n=C_{b^{\′}}^n{l}^{b^{\′}}=\left[\begin{array}{c}{l}_E^n\\ l_N^n\\ l_U^n\end{array}\right]$

其中，

为杆臂lⁿ在当地地理坐标系ox_ny_nz_n下的东向分量，

为杆臂lⁿ在当地地理坐标系ox_ny_nz_n下的北向分量，

为杆臂lⁿ在当地地理坐标系ox_ny_nz_n下的天向分量。

GPS天线相位中心O_G的位置数据

通过杆臂补偿得到的IMU量测中心O_I的位置数据

为

$P_{\mathrm{IMU}}^n=P_{\mathrm{GPS}}^n-l^n=\left[\begin{array}{c}\mathrm{Lat}-l_N^n/R_M\\ \mathrm{Lon}-l_E^n/R_N/\cos \left(\mathrm{Lat}\right)\\ H-l_U^n\end{array}\right]$

其中，Lat、Lon、H分别为GPS输出的纬度、经度和高度。

(2)对GPS速度数据进行动态杆臂补偿

GPS天线相位中心O_G的速度数据

通过杆臂补偿得到的IMU量测中心O_I的速度数据

为

$V_{\mathrm{IMU}}^n=V_{\mathrm{GPS}}^n-C_{b^{\′}}^n{V}_l^{b^{\′}}$

其中

为GPS速度数据在三轴惯性稳定平台初始坐标系ox_b′y_b′z_b′下的速度杆臂误差，由下式计算得到

$V_l^{b^{\′}}=w_{{\mathrm{nb}}^{\′}}^{b^{\′}}\×l^{b^{\′}}$

将补偿后的GPS位置数据和速度数据与惯性数据进行融合，得到最优的运动参数。

6、重复步骤2至步骤5，直至POS***数据处理结束。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (1)

1.一种航空遥感用位置和姿态测量***(POS)动态杆臂补偿方法，其特征在于包括以下步骤：

步骤(1)、在三轴惯性稳定平台初始坐标系下，分别测量得到三轴惯性稳定平台中心与GPS天线相位中心之间的杆臂

和三轴惯性稳定平台中心与惯性测量单元(IMU)量测中心之间的杆臂所述三轴惯性稳定平台初始坐标系ox_b'y_b′z_b'为三轴惯性稳定平台的电编码器输出为零时的内框架坐标系，ox_b'y_b′z_b'坐标系以b'表示；

步骤(2)、计算GPS数据时刻三轴惯性稳定平台初始坐标系到当地地理坐标系的方向余弦阵；所述GPS数据时刻三轴惯性稳定平台初始坐标系到当地地理坐标系的方向余弦阵的计算过程为：

1)计算GPS数据时刻三轴惯性稳定平台初始坐标系ox_b'y_b′z_b′到三轴惯性稳定平台内框架坐标系ox_by_bz_b的方向余弦阵

$\left[\begin{array}{c}{\mathrm{\γ}}_1\\ {\mathrm{\θ}}_1\\ {\mathrm{\ψ}}_1\end{array}\right]=\left[\begin{array}{c}(P_{\mathrm{Ay}}^{+}-P_{\mathrm{Ay}}^{-})(T_{\mathrm{GPS}}-T_A^{-})/(T_A^{+}-T_A^{-})+P_{\mathrm{Ay}}^{-}\\ (P_{\mathrm{Ax}}^{+}-P_{\mathrm{Ax}}^{-})(T_{\mathrm{GPS}}-T_A^{-})/(T_A^{+}-T_A^{-})+P_{\mathrm{Ax}}^{-}\\ (P_{\mathrm{Az}}^{+}-P_{\mathrm{Az}}^{-})(T_{\mathrm{GPS}}-T_A^{-})/(T_A^{+}-T_A^{-})+P_{\mathrm{Az}}^{-}\end{array}\right]$

$C_{b^{\′}}^b=\left[\begin{array}{ccc}\cos \left({\mathrm{\ψ}}_1\right)& \sin \left({\mathrm{\ψ}}_1\right)& 0\\ -\sin \left({\mathrm{\ψ}}_1\right)& \cos \left({\mathrm{\ψ}}_1\right)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \left({\mathrm{\θ}}_1\right)& \sin \left({\mathrm{\θ}}_1\right)\\ 0& -\sin \left({\mathrm{\θ}}_1\right)& \cos \left({\mathrm{\θ}}_1\right)\end{array}\right]\left[\begin{array}{ccc}\cos \left({\mathrm{\γ}}_1\right)& 0& -\sin \left({\mathrm{\γ}}_1\right)\\ 0& 1& 0\\ \sin \left({\mathrm{\γ}}_1\right)& 0& \cos \left({\mathrm{\γ}}_1\right)\end{array}\right]$

其中，γ₁角为GPS数据时刻三轴惯性稳定平台外框相对于三轴惯性稳定平台初始外框旋转的角度，θ₁角为GPS数据时刻三轴惯性稳定平台中框相对于三轴惯性稳定平台初始中框旋转的角度，ψ₁角为GPS数据时刻三轴惯性稳定平台内框相对于三轴惯性稳定平台初始内框旋转的角度，

和

为GPS数据时刻前采样得到的惯性稳定平台三轴的电编码器数据，

和

为GPS数据时刻后采样得到的惯性稳定平台三轴的电编码器数据，T_GPS为GPS数据时刻的UTC时间，

为

和

数据采样时刻的UTC时间，

为

和

数据采样时刻的UTC时间；

2)计算GPS数据时刻三轴惯性稳定平台内框架坐标系ox_by_bz_b到当地地理坐标系ox_ny_nz_n的方向余弦阵

$C_b^n=\left[\begin{array}{ccc}{\cos \mathrm{\γ}}_2\cos {\mathrm{\ψ}}_2+{\sin \mathrm{\γ}}_2\sin {\mathrm{\θ}}_2{\sin \mathrm{\ψ}}_2& \cos {\mathrm{\θ}}_2{\sin \mathrm{\ψ}}_2& {\sin \mathrm{\γ}}_2{\cos \mathrm{\ψ}}_2-{\cos \mathrm{\γ}}_2{\sin \mathrm{\θ}}_2{\sin \mathrm{\ψ}}_2\\ {-\cos \mathrm{\γ}}_2{\sin \mathrm{\ψ}}_2+{\sin \mathrm{\γ}}_2{\sin \mathrm{\θ}}_2{\cos \mathrm{\ψ}}_2& {\cos \mathrm{\θ}}_2{\cos \mathrm{\ψ}}_2& -{\sin \mathrm{\γ}}_2{\sin \mathrm{\ψ}}_2-{\cos \mathrm{\γ}}_2{\sin \mathrm{\θ}}_2{\cos \mathrm{\ψ}}_2\\ -{\sin \mathrm{\γ}}_2{\cos \mathrm{\θ}}_2& {\sin \mathrm{\θ}}_2& {\cos \mathrm{\γ}}_2{\cos \mathrm{\θ}}_2\end{array}\right]$

其中，γ₂、θ₂、ψ₂分别是三轴惯性稳定平台内框架坐标系ox_by_bz_b相对当地地理坐标系ox_ny_nz_n的横滚角、俯仰角和航向角；

3)计算GPS数据时刻三轴惯性稳定平台初始坐标系ox_b'y_b′z_b′到当地地理坐标系ox_ny_nz_n的方向余弦阵

$C_{b^{\′}}^n=C_b^n{C}_{b^{\′}}^b;$

步骤(3)、利用步骤(1)得到的杆臂

和

以及步骤(2)得到的方向余弦阵，计算GPS数据时刻IMU量测中心与GPS天线相位中心之间的动态杆臂，计算过程为：

1)计算GPS数据时刻三轴惯性稳定平台中心O_P与IMU量测中心O_I之间的杆臂

$l_2^b=l_{02}^{b^{\′}}$

$C_b^{b^{\′}}=C_{b^{\′}}^{\mathrm{bT}}$

$l_2^{b^{\′}}=C_b^{b^{\′}}{l}_2^b$

其中为在三轴惯性稳定平台内框架坐标系ox_by_bz_b下三轴惯性稳定平台中心O_P与IMU量测中心O_I之间的杆臂，为

的转置；

2)计算GPS数据时刻IMU量测中心O_I与GPS天线相位中心O_G之间的动态杆臂l^b'

$l_1^{b^{\′}}=l_{01}^{b^{\′}}$

$l^{b^{\′}}=l_1^{b^{\′}}-l_2^{b^{\′}};$

步骤(4)、利用步骤(2)得到的方向余弦阵，计算三轴惯性稳定平台初始坐标系相对当地地理坐标系在三轴惯性稳定平台初始坐标系下的角速度，计算过程为：

1)计算三轴惯性稳定平台内框架坐标系ox_by_bz_b相对三轴惯性稳定平台初始坐标系ox_b'y_b′z_b'在三轴惯性稳定平台内框架坐标系ox_by_bz_b下的角速度

$w_{b^{\′}b}^b=\left[\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right]=\left[\begin{array}{c}(P_{\mathrm{Ax}}^{+}-P_{\mathrm{Ax}}^{-})/T_s\\ (P_{\mathrm{Ay}}^{+}-P_{\mathrm{Ay}}^{-})/T_s\\ (P_{\mathrm{Az}}^{+}-P_{\mathrm{Az}}^{-})/T_s\end{array}\right]$

其中T_s为三轴惯性稳定平台的电编码器采样周期；

2)计算三轴惯性稳定平台初始坐标系ox_b'y_b′z_b'相对当地地理坐标系ox_ny_nz_n在三轴惯性稳定平台初始坐标系ox_b'y_b′z_b'下的角速度

$w_{{\mathrm{ib}}^{\′}}^b=w_{\mathrm{ib}}^b-w_{b^{\′}b}^b$

$w_{\mathrm{ie}}^n=\left[\begin{array}{c}0\\ \cos \left(\mathrm{Lat}\right)\\ \sin \left(\mathrm{Lat}\right)\end{array}\right]$

$w_{\mathrm{en}}^n=\left[\begin{array}{c}-V_N^n/R_M\\ V_E^n/R_N\\ V_E^n\tan \left(\mathrm{Lat}\right)/R_N\end{array}\right]$

$w_{\mathrm{in}}^n=w_{\mathrm{ie}}^n+w_{\mathrm{en}}^n$

$w_{\mathrm{in}}^b=C_n^b{w}_{\mathrm{in}}^n$

$w_{{\mathrm{nb}}^{\′}}^{b^{\′}}=C_b^{b^{\′}}(w_{{\mathrm{ib}}^{\′}}^b-w_{\mathrm{in}}^b)$

其中，为陀螺仪输出的三轴惯性稳定平台内框架坐标系ox_by_bz_b相对惯性坐标系在三轴惯性稳定平台内框架坐标系ox_by_bz_b下的角速度，

为三轴惯性稳定平台初始坐标系ox_b'y_b′z_b'相对惯性坐标系在三轴惯性稳定平台内框架坐标系ox_by_bz_b下的角速度，

为地球坐标系相对惯性坐标系在当地地理坐标系ox_ny_nz_n下的角速度，

为当地地理坐标系ox_ny_nz_n相对地球坐标系在当地地理坐标系ox_ny_nz_n下的角速度，Lat为当地纬度，

为飞行载体在当地地理坐标系ox_ny_nz_n下的东向速度，

为飞行载体在当地地理坐标系ox_ny_nz_n下的北向速度，R_M和R_N分别为当地子午圈主曲率半径和当地卯酉圈主曲率半径，

为当地地理坐标系ox_ny_nz_n相对惯性坐标系在当地地理坐标系ox_ny_nz_n下的角速度，

为当地地理坐标系ox_ny_nz_n相对惯性坐标系在三轴惯性稳定平台内框架坐标系ox_by_bz_b下的角速度；

步骤(5)、基于步骤(2)得到的方向余弦阵、步骤(3)得到的动态杆臂和步骤(4)得到的角速度，对GPS位置数据和速度数据进行动态杆臂补偿，并将补偿后的GPS数据与惯性数据进行融合，得到最优的运动参数；所述对GPS位置数据和速度数据进行动态杆臂补偿的计算过程为：

1)对GPS位置数据进行动态杆臂补偿

$l^n=C_{b^{\′}}^n{l}^{b^{\′}}=\left[\begin{array}{c}{l}_E^n\\ l_N^n\\ l_U^n\end{array}\right]$

$P_{\mathrm{IMU}}^n=P_{\mathrm{GPS}}^n-l^n=\left[\begin{array}{c}\mathrm{Lat}-l_N^n/R_M\\ \mathrm{Lon}-l_E^n/R_N/\cos \left(\mathrm{Lat}\right)\\ H-l_U^n\end{array}\right]$

其中，

为杆臂lⁿ在当地地理坐标系ox_ny_nz_n下的东向分量，

为杆臂lⁿ在当地地理坐标系ox_ny_nz_n下的北向分量，

为杆臂lⁿ在当地地理坐标系ox_ny_nz_n下的天向分量，lⁿ为GPS数据时刻IMU量测中心O_I与GPS天线相位中心O_G之间的杆臂，Lat、Lon、H分别为GPS输出的纬度、经度和高度，

为GPS输出的GPS天线相位中心O_G的位置数据，

为通过杆臂补偿得到的IMU量测中心O_I的位置数据

2)对GPS速度数据进行动态杆臂补偿

$V_l^{b^{\′}}=w_{n{b}^{\′}}^{b^{\′}}\×l^{b^{\′}}$

$V_{\mathrm{IMU}}^n=V_{\mathrm{GPS}}^n-C_{b^{\′}}^n{V}_l^{b^{\′}}$

其中，

为GPS速度数据在三轴惯性稳定平台初始坐标系ox_b′y_b′z_b′下的速度杆臂误差，×为叉乘，

为GPS输出的GPS天线相位中心O_G的速度数据，

为通过杆臂补偿得到的IMU量测中心O_I的速度数据；

步骤(6)、重复步骤(2)至步骤(5)，直至POS***数据处理结束。

Images