CN105865488A - 一种基于自主量测信息的静基座动态快速精确对准方法 - Google Patents

Abstract

一种基于自主量测信息的静基座动态快速精确对准方法，包括如下技术步骤设计：子惯导初始化、动态起竖子惯导、自主获取位置和速度量测信息、子惯导进行精对准；基于子惯导捷联解算结果和获取的位置量测信息与速度量测信息利用卡尔曼滤波方法估计子惯导随机误差，通过反馈校正减小航向和水平姿态误差，获取高精度的子惯导对准结果，本发明针对多级动态杆臂连接的主/子惯导系统，通过静基座起竖环境下动态杆臂补偿方法自主获取位置和速度量测信息，获得高精度动态惯导系统对准结果，从而提高子惯导系统对准的快速性和准确性。

Description

一种基于自主量测信息的静基座动态快速精确对准方法

技术领域

本发明专利涉及一种测量校准方法的技术领域，尤其是一种基于自主量测信息的静基座动态快速精确对准方法，该方法可以应用于静基座动态惯导系统对准，设计优化，安全可靠，该发明可以应用于INS(Inertial Navigation System，惯性导航系统)/GPS(Global Position System,全球定位系统)等组合导航系统的数据处理与导航计算，大大提高了导航解算的精度。

背景技术

惯性导航系统是根据惯性测量单元的实时测量数据，通过捷联解算实时获取载体位置、速度以及姿态信息的导航系统，在进入正常工作状态之前惯性导航系统必须进行初始对准，初始对准主要是获取INS初始位置、速度及姿态等信息，这是导航系统进行高精度导航的重要保证；对准速度和对准精度是初始对准的两大指标要求，故快速精确对准是进行高精度导航的前提条件；在双惯导系统甚至多惯导系统中，传递对准是广泛采用的对准方法；所谓传递对准是指当载体航行时，将已对准好的主惯导系统信息进行杆臂补偿，并作为量测信息传递给需要对准的子惯导系统，结合子惯导系统信息，估计出子惯导的失准角并进行反馈校正来完成初始对准，此技术适用于刚性杆臂连接的主/子惯导系统传递对准；而在多级动态杆臂连接的主/子惯导系统机动情况下进行初始对准时，主惯导导航信息不能通过刚性杆臂补偿为子惯导组合滤波精对准提供量测信息；这就迫切需要一种在机动过程中不依靠外界信息的自对准方法；

发明内容

本发明的目的是，提供一种基于自主量测信息的静基座动态快速精确对准方法，克服现有技术中，在对多级动态杆臂连接的主/子惯导系统在机动情况下，无法进行精确对准的不足，提高了惯导对准的快速性和准确性，弥补了初始对准方法的不足；

一种基于自主量测信息的静基座动态快速精确对准方法，在子惯导系统机动过程中进行初始对准，提高了子惯导对准的快速性和准确性，利用主惯导的导航信息对子惯导进行初始化操作；使子惯导进行横滚轴向机动起竖，在起竖过程中，一方面根据子惯导实时数据以及一级、二级杆臂矢量信息，采用静基座起竖环境下动态杆臂补偿方法自主获取位置和速度量测信息；通过子惯导进行捷联解算；利用卡尔曼滤波将自主获取的位置、速度量测信息和捷联解算结果进行组合，估计子惯导的位置、速度、姿态以及惯性器件误差，将位置、速度以及姿态误差进行反馈校正，减小航向及水平姿态误差，快速获得高精度子惯导对准结果，从而提高子惯导系统对准的快速性和准确性；

为了更好的说明本发明一种基于自主量测信息的静基座动态快速精确对准方法的设计及工作原理，现介绍本发明具体包括如下技术步骤：

1、子惯导初始化：

主惯导系统一直处于工作状态，实时的进行捷联解算，以主惯导上一时刻的位置、速度和姿态信息作为当前捷联解算的初始值，结合当前时刻的主惯导数据，获得当前时刻的导航结果，并通过和GNSS导航结果进行组合导航，获得高精度主惯导导航结果；

在所述机动起竖前，根据静止状态下主惯导的位置和姿态主惯导和子惯导的初始姿态偏差、一级杆臂矢量和二级杆臂矢量采用姿态转移及静态杆臂补偿方法获取子惯导初始位置和初始姿态由于子惯导机动起竖前保持水平静止，故速度信息为零，从而获得子惯导系统初始信息；

所采用的姿态转移及静态杆臂补偿方法：根据主惯导和子惯导之间的初始姿态偏差求取姿态转移矩阵然后通过姿态转移矩阵叉乘主惯导姿态计算得出子惯导的初始姿态将主惯导姿态信息转化为子惯导姿态信息，具体计算公式如下：

$\left[\begin{array}{c}{\mathrm{\Ψ}}^b\\ {\mathrm{\θ}}^b\\ {\mathrm{\γ}}^b\end{array}\right]=C_{b^{\′}}^b\left[\begin{array}{c}{\mathrm{\Ψ}}^{b^{\′}}\\ {\mathrm{\θ}}^{b^{\′}}\\ {\mathrm{\γ}}^{b^{\′}}\end{array}\right]$

式中为主惯导姿态，(航向角Ψ、俯仰角θ、横滚角γ)，为子惯导初始姿态，

在子惯导初始位置求取过程中，首先根据主惯导姿态求取姿态转移矩阵通过姿态转移矩阵叉乘一级杆臂矢量可求得一级杆臂位置矢量差然后主惯导位置加上一级杆臂位置矢量差计算出旋转中心位置矢量具体求解公式如下：

$P_O^n=P_m^n+\Π{\mathrm{\ΔR}}_I^n=P_m^n+\ΠC_{b^{\′}}^n{R}_I^{b^{\′}}$

$\left[\begin{array}{c}{L}_o\\ {\mathrm{\λ}}_o\\ H_o\end{array}\right]=\left[\begin{array}{c}{L}_{b^{\′}}\\ {\mathrm{\λ}}_{b^{\′}}\\ H_{b^{\′}}\end{array}\right]+\left[\begin{array}{ccc}1/(R_m+H_p)& 0& 0\\ 0& \sec \left(L_p\right)/(R_m+H_p)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{T}_{11}& T_{21}& T_{31}\\ T_{12}& T_{22}& T_{32}\\ T_{13}& T_{23}& T_{33}\end{array}\right]\left[\begin{array}{c}{X}_1\\ Y_1\\ Z_1\end{array}\right]$

式中为主惯导位置， 为主惯导坐标系下一级杆臂矢量， 为旋转中心位置矢量，

在获取旋转中心位置矢量的基础上根据子惯导初始姿态求得的姿态转移矩阵通过姿态转移矩阵叉乘二级杆臂矢量求得二级杆臂位置矢量差最后利用旋转中心位置矢量加上二级杆臂位置矢量差计算求取子惯导初始位置具体求解公式如下：

$P_S^n\left(0\right)=P_O^n+\Π{\mathrm{\ΔR}}_{II}^n\left(0\right)=P_O^n+\ΠC_b^n{R}_{II}^b$

$\left[\begin{array}{c}{L}_b\\ {\mathrm{\λ}}_b\\ H_b\end{array}\right]=\left[\begin{array}{c}{L}_o\\ {\mathrm{\λ}}_o\\ H_o\end{array}\right]+\left[\begin{array}{ccc}1/(R_m+H_p)& 0& 0\\ 0& \sec \left(L_p\right)/(R_m+H_p)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{T}_{11}& T_{21}& T_{31}\\ T_{12}& T_{22}& T_{32}\\ T_{13}& T_{23}& T_{33}\end{array}\right]\left[\begin{array}{c}{X}_2\\ Y_2\\ Z_2\end{array}\right]$

式中为旋转中心位置矢量， 为求得的子惯导初始位置， 为子惯导坐标系到导航坐标系的姿态转移矩阵，为子惯导坐标系下二级杆臂矢量，初始时刻固定于起竖机构上的子惯导保持静止状态，故子惯导的初始速度为零。

2、动态起竖子惯导：

起竖机构通过旋转轴与运载体固联，并可以沿着旋转轴进行旋转；设置子惯导坐标系O_sX_sY_sZ_s中的Y_s轴与起竖机构的旋转轴重合，初始时刻固定于起竖机构上的子惯导保持水平静止状态，水平姿态误差角≤0.01°，起竖机构沿着旋转轴以0.5°/s-1.5°/s之间的角速率旋转，旋转角为90°，旋转角误差≤0.05°，使子惯导机动起竖至竖直状态。

3、自主获取位置和速度量测信息：

子惯导初始化完成后，根据子惯导捷联解算结果以及二级杆臂矢量采用静基座起竖环境下动态杆臂补偿方法实时自主获取子惯导的位置量测信息与速度量测信息具体求取方法如下：

位置量测信息的求取：首先利用子惯导捷联解算结果中实时姿态信息求取姿态转移矩阵然后姿态转移矩阵叉乘二级杆臂矢量求得动态二级杆臂位置矢量差最后由旋转中心位置矢量加上动态二级杆臂位置矢量差实时计算出子惯导位置量测信息具体求解公式如下：

$P_S^n\left(t\right)=P_O^n+\Π{\mathrm{\ΔR}}_{II}^n\left(t\right)=P_O^n+\ΠC_b^n\left(t\right)R_{II}^b$

$\left[\begin{array}{c}{L}_b\\ {\mathrm{\λ}}_b\\ H_b\end{array}\right]=\left[\begin{array}{c}{L}_o\\ {\mathrm{\λ}}_o\\ H_o\end{array}\right]+\left[\begin{array}{ccc}1/(R_M+H_P)& 0& 0\\ 0& \sec \left(L_P\right)/(R_N+H_P)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{T}_{11}& T_{21}& T_{31}\\ T_{12}& T_{22}& T_{32}\\ T_{13}& T_{23}& T_{33}\end{array}\right]\left[\begin{array}{c}{X}_2\\ Y_2\\ Z_2\end{array}\right]$

式中为子惯导位置量测信息， 为旋转中心位置矢量， 为子惯导坐标系到导航坐标系的姿态转移矩阵，为子惯导坐标系下二级杆臂矢量，

速度量测信息的求取：首先通过子惯导捷联解算求得载体系相对于导航系的角速率然后载体系相对于导航系的角速率叉乘二级杆臂矢量求得杆臂速度矢量差利用姿态转移矩阵叉乘杆臂速度矢量差求得导航系下杆臂速度矢量差最后旋转中心的速度加上杆臂速度矢量差实时计算出子惯导速度量测信息具体计算公式如下：

$V_S^n\left(t\right)=V_O^n+{\mathrm{\ΔV}}_{II}^n\left(t\right)=V_O^n+C_b^n({\mathrm{\ω}}_{nb}^b\×R_{II}^b)$

$\left[\begin{array}{c}{V}_{SU}^n\\ V_{SN}^n\\ V_{SU}^n\end{array}\right]=\left[\begin{array}{c}{V}_{OE}^n\\ V_{ON}^n\\ V_{OU}^n\end{array}\right]+\left[\begin{array}{ccc}{T}_{11}& T_{21}& T_{31}\\ T_{12}& T_{22}& T_{32}\\ T_{13}& T_{23}& T_{33}\end{array}\right]\left(\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_{nbz}^b& {\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nbz}^b& 0& -{\mathrm{\ω}}_{nbx}^b\\ -{\mathrm{\ω}}_{nby}^b& {\mathrm{\ω}}_{nbx}^b& 0\end{array}\right]\left[\begin{array}{c}{X}_2\\ Y_2\\ Z_2\end{array}\right]\right)$

式中为子惯导速度量测信息， 为子惯导坐标系下二级杆臂矢量， 为载体系相对于导航系在载体系下的角速率，为旋转中心的速度信息，由于在静基座情况下开始动态起竖的，起竖机构旋转中心的速度理想值为故计算公式可以变为：

$V_S^n\left(t\right)={\mathrm{\ΔV}}_{II}^n\left(t\right)=C_b^n({\mathrm{\ω}}_{nb}^b\×R_{II}^b)$

4、子惯导进行精对准：基于子惯导捷联解算结果和获取的位置量测信息与速度量测信息利用卡尔曼滤波方法估计子惯导位置、速度、姿态和惯性器件误差，并将估计出的误差进行反馈校正，减小航向和水平姿态误差，获取高精度的子惯导对准结果；

为了方便技术理解，现将本发明采用的姿态转移及静态杆臂补偿方法中，应用到的设计原理简要叙述如下：

首先，根据几何矢量坐标转换原理将主惯导姿态进行转移求取子惯导初始姿态；

其次，依据平行四边形法则，通过旋转中心位置矢量加上二级杆臂位置矢量差求取子惯导初始位置；

再次，静基座起竖环境下动态杆臂补偿方法中，根据角速率乘以半径得出切向速度的原理自主获取子惯导速度量测信息，通过旋转中心位置矢量加上动态二级杆臂位置矢量差获得子惯导位置量测信息；而卡尔曼滤波方法则是利用递推最小方差估计的原理，从量测信息中消除高斯白噪声，估计出子惯导系统随机误差，进而修正航向及水平姿态角；

最后，通过该发明之技术设计，最终提高了子惯导的对准精度；

本发明的有益效果：

1、本发明针对多级动态杆臂连接的主/子惯导系统，提出了一种基于自主量测信息的静基座动态快速精确对准方法不同于一级刚性杆臂连接的主/子惯导系统，通过多级杆臂连接的主/子惯导系统仅利用主惯导系统信息对子惯导进行初始化，机动起竖对准过程中不需要主惯导系统实时向子惯导系统传递导航信息，子惯导系统在机动起竖过程中进行自对准；

2、本发明在机动起竖对准过程中，通过静基座起竖环境下动态杆臂补偿方法自主获取位置和速度量测信息，利用卡尔曼滤波估计子惯导的位置、速度、姿态以及惯性器件误差，将位置、速度以及姿态误差进行反馈校正，逐步减小航向及水平姿态误差，获得高精度动态惯导系统对准结果，从而提高子惯导系统对准的快速性和准确性。

附图说明

图1是本发明一种基于自主量测信息的静基座动态快速精确对准方法之流程示意图

图2是本发明一种基于自主量测信息的静基座动态快速精确对准方法之主/子惯导安装示意图

图3本发明一种基于自主量测信息的静基座动态快速精确对准方法之捷联解算流程图

具体实施方式

下面结合附图对本发明的优选实施例进行详细说明；

参照图1至图3所示，一种基于自主量测信息的静基座动态快速精确对准方法，在子惯导系统机动过程中进行初始对准，提高了子惯导对准的快速性和准确性，利用主惯导的导航信息对子惯导进行初始化操作；使子惯导进行横滚轴向机动起竖，在起竖过程中，一方面根据子惯导实时数据以及一级、二级杆臂矢量信息，采用静基座起竖环境下动态杆臂补偿方法自主获取位置和速度量测信息；通过子惯导进行捷联解算；利用卡尔曼滤波将自主获取的位置、速度量测信息和捷联解算结果进行组合，估计子惯导的位置、速度、姿态以及惯性器件误差，将位置、速度以及姿态误差进行反馈校正，减小航向及水平姿态误差，快速获得高精度子惯导对准结果，从而提高子惯导系统对准的快速性和准确性；具体技术包括如下：

1、子惯导初始化：主惯导系统一直处于工作状态，实时的进行捷联解算，以主惯导上一时刻的位置、速度和姿态信息作为当前捷联解算的初始值，结合当前时刻的主惯导数据，获得当前时刻的导航结果，并通过和GNSS导航结果进行组合导航，获得高精度主惯导导航结果。在机动起竖前根据静止状态下主惯导的位置和姿态主惯导和子惯导的初始姿态偏差、一级杆臂矢量和二级杆臂矢量采用姿态转移及静态杆臂补偿方法获取子惯导初始位置和初始姿态由于子惯导机动起竖前保持水平静止，故速度信息为零，从而获得子惯导系统初始信息。

所采用的姿态转移及静态杆臂补偿方法：根据主惯导和子惯导之间的初始姿态偏差求取姿态转移矩阵然后通过姿态转移矩阵叉乘主惯导姿态计算得出子惯导的初始姿态将主惯导姿态信息转化为子惯导姿态信息，具体计算公式如下：

$\left[\begin{array}{c}{\mathrm{\Ψ}}^b\\ {\mathrm{\θ}}^b\\ {\mathrm{\γ}}^b\end{array}\right]=C_{b^{\′}}^b\left[\begin{array}{c}{\mathrm{\Ψ}}^{b^{\′}}\\ {\mathrm{\θ}}^{b^{\′}}\\ {\mathrm{\γ}}^{b^{\′}}\end{array}\right]$

式中为主惯导姿态，(航向角Ψ、俯仰角θ、横滚角γ)，为子惯导初始姿态，

在子惯导初始位置求取过程中，首先根据主惯导姿态求取姿态转移矩阵通过姿态转移矩阵叉乘一级杆臂矢量可求得一级杆臂位置矢量差然后主惯导位置加上一级杆臂位置矢量差计算出旋转中心位置矢量具体求解公式如下：

$P_O^n=P_m^n+{\mathrm{\Π\ΔR}}_I^n=P_m^n+\ΠC_{b^{\′}}^n{R}_I^{b^{\′}}$

$\left[\begin{array}{c}{L}_o\\ {\mathrm{\λ}}_o\\ H_o\end{array}\right]=\left[\begin{array}{c}{L}_{b^{\′}}\\ {\mathrm{\λ}}_{b^{\′}}\\ H_{b^{\′}}\end{array}\right]+\left[\begin{array}{ccc}1/(R_m+H_p)& 0& 0\\ 0& \sec \left(L_p\right)/(R_m+H_p)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{T}_{11}& T_{21}& T_{31}\\ T_{12}& T_{22}& T_{32}\\ T_{13}& T_{23}& T_{33}\end{array}\right]\left[\begin{array}{c}{X}_1\\ Y_1\\ Z_1\end{array}\right]$

式中为主惯导位置， 为主惯导坐标系下一级杆臂矢量， 为旋转中心位置矢量，

在获取旋转中心位置矢量的基础上根据子惯导初始姿态求得的姿态转移矩阵通过姿态转移矩阵叉乘二级杆臂矢量求得二级杆臂位置矢量差最后利用旋转中心位置矢量加上二级杆臂位置矢量差计算求取子惯导初始位置具体求解公式如下：

$P_S^n\left(t\right)=P_O^n+\Π{\mathrm{\ΔR}}_{II}^n\left(t\right)=P_O^n+\ΠC_{b_{II}}^n{R}_{II}^b$

$\left[\begin{array}{c}{L}_b\\ {\mathrm{\λ}}_b\\ H_b\end{array}\right]=\left[\begin{array}{c}{L}_o\\ {\mathrm{\λ}}_o\\ H_o\end{array}\right]+\left[\begin{array}{ccc}1/(R_m+H_p)& 0& 0\\ 0& \sec \left(L_p\right)/(R_m+H_p)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{T}_{11}& T_{21}& T_{31}\\ T_{12}& T_{22}& T_{32}\\ T_{13}& T_{23}& T_{33}\end{array}\right]\left[\begin{array}{c}{X}_2\\ Y_2\\ Z_2\end{array}\right]$

式中为旋转中心位置矢量， 为求得的子惯导初始位置， 为子惯导坐标系到导航坐标系的姿态转移矩阵，为子惯导坐标系下二级杆臂矢量，初始时刻固定于起竖机构上的子惯导保持静止状态，故子惯导的初始速度为零。

2、动态起竖子惯导；起竖机构通过旋转轴与运载体固联，并可以沿着旋转轴进行旋转，设置子惯导坐标系O_sX_sY_sZ_s中的Y_s轴与起竖机构的旋转轴重合，初始时刻固定于起竖机构上的子惯导保持水平静止状态，水平姿态误差角≤0.01°，起竖机构沿着旋转轴以0.5°/s-1.5°/s之间的角速率旋转，旋转角为90°，旋转角误差≤0.05°，使子惯导机动起竖至竖直状态。

3、自主获取位置和速度量测信息；子惯导初始化完成后，根据子惯导捷联解算结果以及二级杆臂矢量采用静基座起竖环境下动态杆臂补偿方法实时自主获取子惯导的位置量测信息与速度量测信息具体求取方法如下：

位置量测信息的求取：首先利用子惯导捷联解算结果中实时姿态信息求取姿态转移矩阵然后姿态转移矩阵叉乘二级杆臂矢量求得动态二级杆臂位置矢量差最后由旋转中心位置矢量加上动态二级杆臂位置矢量差实时计算出子惯导位置量测信息具体求解公式如下：

$P_S^n\left(t\right)=P_O^n+\Π{\mathrm{\ΔR}}_{II}^n\left(t\right)=P_O^n+\ΠC_b^n\left(t\right)R_{II}^b$

$\left[\begin{array}{c}{L}_b\\ {\mathrm{\λ}}_b\\ H_b\end{array}\right]=\left[\begin{array}{c}{L}_o\\ {\mathrm{\λ}}_o\\ H_o\end{array}\right]+\left[\begin{array}{ccc}1/(R_M+H_P)& 0& 0\\ 0& \sec \left(L_P\right)/(R_N+H_P)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{T}_{11}& T_{21}& T_{31}\\ T_{12}& T_{22}& T_{32}\\ T_{13}& T_{23}& T_{33}\end{array}\right]\left[\begin{array}{c}{X}_2\\ Y_2\\ Z_2\end{array}\right]$

式中为子惯导位置量测信息， 为旋转中心位置矢量， 为子惯导坐标系到导航坐标系的姿态转移矩阵，为子惯导坐标系下二级杆臂矢量，

速度量测信息的求取：首先通过子惯导捷联解算求得载体系相对于导航系的角速率然后载体系相对于导航系的角速率叉乘二级杆臂矢量求得杆臂速度矢量差利用姿态转移矩阵叉乘杆臂速度矢量差求得导航系下杆臂速度矢量差最后旋转中心的速度加上杆臂速度矢量差实时计算出子惯导速度量测信息具体计算公式如下：

$V_S^n\left(t\right)=V_O^n+{\mathrm{\ΔV}}_{II}^n\left(t\right)=V_O^n+C_b^n({\mathrm{\ω}}_{nb}^b\×R_{II}^b)$

$\left[\begin{array}{c}{V}_{SU}^n\\ V_{SN}^n\\ V_{SU}^n\end{array}\right]=\left[\begin{array}{c}{V}_{OE}^n\\ V_{ON}^n\\ V_{OU}^n\end{array}\right]+\left[\begin{array}{ccc}{T}_{11}& T_{21}& T_{31}\\ T_{12}& T_{22}& T_{32}\\ T_{13}& T_{23}& T_{33}\end{array}\right]\left(\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_{nbz}^b& {\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nbz}^b& 0& -{\mathrm{\ω}}_{nbx}^b\\ -{\mathrm{\ω}}_{nby}^b& {\mathrm{\ω}}_{nbx}^b& 0\end{array}\right]\left[\begin{array}{c}{X}_2\\ Y_2\\ Z_2\end{array}\right]\right)$

式中为子惯导速度量测信息， 为子惯导坐标系下二级杆臂矢量， 为载体系相对于导航系在载体系下的角速率，为旋转中心的速度信息，由于在静基座情况下开始动态起竖的，起竖机构旋转中心的速度理想值为故计算公式可以变为：

$V_S^n\left(t\right)={\mathrm{\ΔV}}_{II}^n\left(t\right)=C_b^n({\mathrm{\ω}}_{nb}^b\×R_{II}^b)$

4、子惯导进行精对准；子惯导精对准过程包括捷联解算和卡尔曼滤波误差估计两个部分：

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

(a)姿态矩阵更新

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

$\begin{array}{c}{C}_n^b=T_{\mathrm{\γ}}\·T_{\mathrm{\θ}}\·T_{\mathrm{\ψ}}\\ =\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\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{\ψ}}{2}\cos \frac{\mathrm{\θ}}{2}\cos \frac{\mathrm{\γ}}{2}-\sin \frac{\mathrm{\ψ}}{2}\sin \frac{\mathrm{\θ}}{2}\sin \frac{\mathrm{\γ}}{2}\\ \cos \frac{\mathrm{\ψ}}{2}\sin \frac{\mathrm{\θ}}{2}\cos \frac{\mathrm{\γ}}{2}-\sin \frac{\mathrm{\ψ}}{2}\cos \frac{\mathrm{\θ}}{2}\sin \frac{\mathrm{\γ}}{2}\\ \cos \frac{\mathrm{\ψ}}{2}\cos \frac{\mathrm{\θ}}{2}\sin \frac{\mathrm{\γ}}{2}+\sin \frac{\mathrm{\ψ}}{2}\sin \frac{\mathrm{\θ}}{2}\cos \frac{\mathrm{\γ}}{2}\\ \cos \frac{\mathrm{\ψ}}{2}\sin \frac{\mathrm{\θ}}{2}\sin \frac{\mathrm{\γ}}{2}+\sin \frac{\mathrm{\ψ}}{2}\cos \frac{\mathrm{\θ}}{2}\cos \frac{\mathrm{\γ}}{2}\end{array}\right]$

离散化四阶计算公式为：

$q(n+1)=\{(1-\frac{{\left({\mathrm{\Δ\θ}}_0\right)}^2}{8}+\frac{{\left({\mathrm{\Δ\θ}}_0\right)}^4}{384})I+(\frac{1}{2}-\frac{{\left({\mathrm{\Δ\θ}}_0\right)}^2}{48})\left(\mathrm{\Δ}\mathrm{\θ}\right)\}q\left(n\right),$

$\mathrm{\Δ}\mathrm{\θ}=\left[\begin{array}{cccc}0& -{\mathrm{\Δ\θ}}_x& -{\mathrm{\Δ\θ}}_y& -{\mathrm{\Δ\θ}}_z\\ {\mathrm{\Δ\θ}}_x& 0& {\mathrm{\Δ\θ}}_z& -{\mathrm{\Δ\θ}}_y\\ {\mathrm{\Δ\θ}}_y& -{\mathrm{\Δ\θ}}_z& 0& {\mathrm{\Δ\θ}}_x\\ {\mathrm{\Δ\θ}}_z& {\mathrm{\Δ\θ}}_y& -{\mathrm{\Δ\θ}}_x& 0\end{array}\right]$

式中Dq_x、Dq_y、Dq_z为姿态速率在载体系的三个分量；表示载体系相对导航系的旋转角速度在b系上的投影。上式中的的计算公式为：

${\mathrm{\ω}}_{nb}^b={\mathrm{\ω}}_{ib}^b-{\mathrm{\ω}}_{in}^b$

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

$\begin{array}{cc}{\mathrm{\ω}}_{ie}^n=\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{ie}\cos L\\ {\mathrm{\ω}}_{ie}\sin L\end{array}\right]& {\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]\end{array}.$

式中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°]；俯仰角q为载体坐标系y轴与其在导航坐标系的投影间的夹角，以载体抬头为正，即向上为正，向下为负，有效范围为[-90°，90°]；横滚角γ为载体坐标系x轴与其在导航坐标系的投影间的夹角，以载体右倾为正(从载体正前朝后看)，左倾为负，有效范围为[-180°，180°]。在姿态矩阵更新以后，可由以下公式计算更新后的姿态角：

(c)速度计算

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

$\left[\begin{array}{c}{\stackrel{\·}{V}}_x^n\\ {\stackrel{\·}{V}}_y^n\\ {\stackrel{\·}{V}}_z^n\end{array}\right]=\left[\begin{array}{c}{a}_{ibx}^n\\ a_{iby}^n\\ a_{ibz}^n\end{array}\right]+\left[\begin{array}{ccc}0& 2{\mathrm{\ω}}_{iez}^n+{\mathrm{\ω}}_{enz}^n& -(2{\mathrm{\ω}}_{iey}^n+{\mathrm{\ω}}_{eny}^n)\\ -(2{\mathrm{\ω}}_{iez}^n++{\mathrm{\ω}}_{enz}^n)& 0& 2{\mathrm{\ω}}_{iex}^n+{\mathrm{\ω}}_{enx}^n\\ 2{\mathrm{\ω}}_{iey}^n+{\mathrm{\ω}}_{eny}^n& -(2{\mathrm{\ω}}_{iex}^n+{\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{\·}{C}}_n^e=C_n^e{\mathrm{\Ω}}_{en}^n,{\mathrm{\Ω}}_{en}^n=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_{enz}^n& {\mathrm{\ω}}_{eny}^n\\ {\mathrm{\ω}}_{enz}^n& 0& -{\mathrm{\ω}}_{enx}^n\\ -{\mathrm{\ω}}_{eny}^n& {\mathrm{\ω}}_{enx}^n& 0\end{array}\right]$

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

$C_n^e(t+T)=C_n^e\left(t\right)+{\mathrm{TC}}_n^e\left(t\right){\mathrm{\Ω}}_{en}^n\left(t\right)$

式中T为惯性导航系统采样周期。完成位置矩阵更新后，即可计算出导航位置参数。记有：

$\left\{\begin{array}{c}L=si{n}^{-1}\left(C_{33}\right)\\ \mathrm{\λ}=ta{n}^{-1}\left(\frac{C_{32}}{C_{31}}\right)\end{array}\right.$

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

(2)采用基于自主量测信息的卡尔曼滤波方法完成子惯导系统起竖对准，通过估计子惯导位置、速度、姿态和惯性器件误差，并将估计出的误差进行反馈校正，得到高精度的子惯导对准结果；卡尔曼滤波模型中的状态变量X共有15维，包括东、北、天向的失准角东、北、天向的速度误差δV_E、δV_N、δV_U，纬度、经度、高度误差δL、δλ、δH，x、y、z轴向的陀螺仪常值漂移误差ε_x、ε_y、ε_z，x、y、z轴向加速度计常值漂移误差 卡尔曼滤波模型中的量测量Z是惯性导航结果和自主递推导航结果的速度误差和位置误差，起竖对准具体步骤如下：

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

系统状态方程为：

$\stackrel{\·}{X}=FX+W$

式中X为15维状态变量，包括3个姿态误差、3个速度误差、3个位置误差、3个陀螺仪偏置、3个加速度计偏置，F为状态转移矩阵；W为系统噪声，并假设其为零均值高斯白噪声；F的表达式：

$F=\left[\begin{array}{ccccc}{F}_1& F_2& F_3& C_b^n& 0_{3\×3}\\ F_4& F_5& F_6& 0_{3\×3}& C_b^n\\ 0_{3\×3}& F_7& F_8& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

式中：

$F_1=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{ie}\sin L+\frac{V_x\tan L}{R_N+H}& -({\mathrm{\ω}}_{ie}\cos L+\frac{V_x}{R_N+H})\\ -({\mathrm{\ω}}_{ie}\sin L+\frac{V_x\tan L}{R_N+H})& 0& -\frac{V_y}{R_M+H}\\ {\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{\ω}}_{ie}\sin L& 0& -\frac{V_x}{{(R_N+H)}^2}\\ {\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{\ω}}_{ie}\sin L+\frac{V_x\tan L}{R_N+H}& -2{\mathrm{\ω}}_{ie}\cos L-\frac{V_x}{R_N+H}\\ -2({\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{\ω}}_{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{\ω}}_{ie}{V}_y\cos L+\frac{V_x{V}_y{\sec }^{2}L}{R_N+H}+2{\mathrm{\ω}}_{ie}{V}_z\sin L& 0& \frac{V_x{V}_z-V_x{V}_y\tan L}{{(R_N+H)}^2}\\ -(2{\mathrm{\ω}}_{ie}\cos L+\frac{V_x{\sec }^{2}L}{R_N+H})V_E& 0& \frac{V_y{V}_z+V_x^2\tan L}{{(R_N+H)}^2}\\ -2{\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{secL}{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)量测方程的建立

量测量Z为是惯性导航结果和自主递推导航结果的速度误差和位置误差，量测矩阵H为：

H＝[H_V H_P]^T

H_P＝[0_3×6 diag(R_M+H,(R_N+H)cosL,1) 0_3×6]

H_V＝[0_3×3 diag(1,1,1) 0_3×9]

(c)卡尔曼滤波器设计

利用标准卡尔曼滤波的五个公式进行滤波，可以得到估计的15个系统误差变量，利用卡尔曼滤波估计的结果对系统状态变量进行反馈校正，得到更加准确的子惯导实时对准结果；

以上所述的仅为本发明的优选实施例，所应理解的是，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想，并不用于限定本发明的保护范围，凡在本发明的思想和原则之内所做的任何修改、等同替换等等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于自主量测信息的静基座动态快速精确对准方法，其特征在于，包括如下技术步骤设计：

(1)子惯导初始化：根据静止状态下主惯导的位置和姿态主惯导和子惯导的初始姿态偏差、一级杆臂矢量和二级杆臂矢量采用姿态转移及静态杆臂补偿方法获取子惯导初始位置和初始姿态并将速度置为零；

(2)动态起竖子惯导：起竖机构通过旋转轴与运载体固联，并可以沿着旋转轴进行旋转；固定于起竖机构的子惯导采用静基座横滚轴向机动起竖方法进行快速起竖，由水平状态旋转至竖直状态，提高组合滤波中系统状态量可观测度；

(3)自主获取位置和速度量测信息：子惯导初始化完成后，根据子惯导捷联解算结果以及二级杆臂矢量采用静基座起竖环境下动态杆臂补偿方法实时自主获取子惯导的位置量测信息与速度量测信息

(4)子惯导进行精对准：基于子惯导捷联解算结果和获取的位置量测信息与速度量测信息利用卡尔曼滤波方法估计子惯导随机误差，通过反馈校正减小航向和水平姿态误差，获取高精度的子惯导对准结果。

2.根据权利要求书1所述的一种基于自主量测信息的静基座动态快速精确对准方法，其特征在于所述步骤(1)所述的姿态转移及静态杆臂补偿方法中，根据主惯导和子惯导之间的初始姿态偏差求取姿态转移矩阵然后通过姿态转移矩阵叉乘主惯导姿态计算得出子惯导的初始姿态在静态杆臂补偿方法中，首先根据主惯导姿态求取姿态转移矩阵利用姿态转移矩阵叉乘一级杆臂矢量求得一级杆臂位置矢量差然后主惯导位置加上一级杆臂位置矢量差计算出旋转中心位置矢量在此基础上根据子惯导初始姿态求得的姿态转移矩阵通过姿态转移矩阵叉乘二级杆臂矢量求得二级杆臂位置矢量差最后利用旋转中心位置矢量加上二级杆臂位置矢量差计算出子惯导初始位置完成姿态转移和静态杆臂补偿。

3.根据权利要求书1所述的一种基于自主量测信息的静基座动态快速精确对准方法，其特征在于所述步骤(2)所述的静基座横滚轴向机动起竖方法中，设置子惯导坐标系O_sX_sY_sZ_s中的Y_s轴与起竖机构的旋转轴重合，初始时刻固定于起竖机构上的子惯导保持水平静止状态，水平姿态误差角≤0.01°，起竖机构沿着旋转轴以0.5°/s-1.5°/s之间的角速率旋转，旋转角为90°，旋转角误差≤0.05°，使子惯导机动起竖至竖直状态。

4.根据权利要求书1所述的一种基于自主量测信息的静基座动态快速精确对准方法，其特征在于：步骤(3)所述的静基座起竖环境下动态杆臂补偿方法中，首先利用子惯导捷联解算结果中实时姿态信息求取姿态转移矩阵然后姿态转移矩阵叉乘二级杆臂矢量求得动态二级杆臂位置矢量差最后由旋转中心位置矢量加上动态二级杆臂位置矢量差实时计算出子惯导位置量测信息在求取子惯导速度量测信息过程中，通过子惯导捷联解算求得载体系相对于导航系的角速率然后利用载体系相对于导航系的角速率叉乘二级杆臂矢量求取动态二级杆臂速度矢量差利用姿态转移矩阵叉乘动态二级杆臂速度矢量差求得导航系下动态二级杆臂速度矢量差最后旋转中心的速度加上动态二级杆臂速度矢量差实时计算出子惯导速度量测信息