CN104501809A - 一种基于姿态耦合的捷联惯导/星敏感器组合导航方法 - Google Patents

Abstract

本发明涉及一种组合导航领域，特别是一种基于姿态耦合的捷联惯导/星敏感器组合导航方法。本发明包括：采集捷联惯导系统输出带有位置误差的地球坐标系相对地理坐标系的转换矩阵；星敏感器直接输出相对于惯性空间的姿态矩阵，得到载体坐标系相对地球坐标系的转换矩阵；利用转换矩阵得到星敏感器捷联矩阵；捷联惯导系统提供捷联矩阵；星敏感器捷联矩阵和捷联惯导捷联矩阵相乘；由捷联惯导捷联矩阵得到姿态角；由星敏感器捷联矩阵得到姿态角；星敏感器和捷联惯导解算两组姿态作差；耦合计算，得到各导航系统失准角；校正捷联惯导姿态信息；校正捷联惯导位置信息；校正捷联惯导速度信息。本发明不受惯性组件测量误差影响，计算量小，可靠性较强。

Description

一种基于姿态耦合的捷联惯导/星敏感器组合导航方法

技术领域

本发明涉及一种组合导航领域，特别是一种基于姿态耦合的捷联惯导/星敏感器组合导航方法。

背景技术

捷联惯导系统(SINS)是一种能够实时输出载体速度、姿态、位置信息的全自主导航系统，由于SINS导航过程不需要对外发射任何无线电信号，又不受外界环境条件等影响，因此受到各领域的青睐，广泛应用于航空、航天、航海等领域。SINS主要是利用陀螺仪和加速度计实时测量载体运动的角速度和线速度信息，经导航解算后得到导航信息。然而，惯性组件测量误差、初始对准误差等因素导致系统解算导航误差随导航时间增长而逐渐发散，影响系统导航精度，严重制约了捷联惯导系统的长时间导航能力。

为解决上述问题，常采用其它导航设备与惯性导航系统进行信息融合，组成以惯性导航系统为主体的组合导航系统。目前，SINS和全球定位系统(GPS)的组合技术相对成熟，但是由于美国国家政策限制各国对GPS的使用，使得战时需求受限。虽然我国已自主研发了北斗导航系统，但基于惯性/北斗的组合导航仍然存在导航过程中需要收发无线电信号的问题，导致该类组合技术应用受限。因此，对自主性强的天文导航技术，尤其是与天文导航有关的组合导航技术研究成为现代军事导航领域的主要趋势和热点。星敏感器作为目前精度最高的姿态敏感仪器，测量精度可达到角秒级，具有自主性强、无姿态积累误差、视场不受限制、抗干扰性强，隐蔽性好等优点。本发明提出星敏感器和捷联惯导的组合导航方案，使导航系统的精度和实时性得到大幅度提高。

目前，基于捷联惯导/星敏感器的相对成熟的组合方法是，星敏感器利用捷联惯导提供的水平姿态信息解算出载体相对地理系的位置与航向，然后对惯导的输出进行校正。这种组合方法能很好地抑制惯导误差随时间的发散，但其导航精度却受到惯导提供的水平姿态精度的制约，也就是说，这种组合方法的导航精度要受到捷联惯导导航误差的影响，限制了该组合方法的应用范围。除此之外，星敏感器直接测量输出相对于惯性系的姿态，当系统导航系选取当地地理系时，该信息无法直接使用。

《测绘科学技术学报》2009年26卷第2期由杨生等人撰写的《基于惯导/天文高精度定姿方法》，利用惯性导航系统与天文导航系统进行组合定姿，采用卡尔曼滤波技术设计了惯导/天文组合定姿算法，但是该文献主要针对飞行器提出了定姿方法，并没有定位方法，不满足低速航行的舰船的导航需求；公告号为103076015的中国发明专利在2013年5月1日公开的《一种基于全面最优校正的SINS/CNS组合导航系统及其导航方法》，该发明解决了天文导航系统高精度自主地平的问题，充分利用天文导航系统的位置和姿态信息，对SINS误差进行全面最优校正，但是该发明利用星光折射间接敏感地平的解析天文定位方法得到位置信息和地平信息，再利用惯性姿态信息得到载体的姿态信息，计算过程复杂繁琐，适用范围小；《测绘工程》2013年22卷第4期由黄志远等人撰写的《CNS/SINS组合导航系统仿真分析》，以弹道导弹为研究对象建立了CNS/SINS组合导航的数学模型，并设计滤波算法验证组合导航系统的可靠性，但是该文献并没有对CNS/SINS组合导航系统的组合方式进行阐述。以上文献都提出了天文和捷联惯导组合的导航方案，但却都没有给出以当地地理坐标系为导航坐标系时，星敏感器直接输出相对惯性系信息与捷联惯导系统提供相对地理系导航信息的组合方案。

发明内容

本发明针对现有技术的不足，提出了一种提高组合导航系统定姿、定速、定位精度的基于姿态耦合的星敏感器/捷联惯导组合导航方法。

一种基于姿态耦合的捷联惯导/星敏感器组合导航方法，包括：

(1)采集捷联惯导系统输出带有位置误差的地球坐标系相对地理坐标系的转换矩阵

其中，e表示地球坐标系，原点位于地球质心，z轴指向地球自转方向，x指向春分点方向，y轴与其它两轴构成右手螺旋定则；n′表示捷联惯导系统计算导航坐标系；表示地球坐标系相对捷联惯导系统计算地理坐标系的转换矩阵；sin为三角函数中的正弦函数，cos为三角函数中的余弦函数；λ_s＝λ+δλ_s为捷联惯导系统解算经度，λ为载体所在位置地理经度，δλ_s为捷联惯导系统解算经度误差；为捷联惯导系统解算纬度，为载体所在位置地理纬度，为捷联惯导系统解算纬度误差；

(2)星敏感器直接输出相对于惯性空间的姿态矩阵得到载体坐标系相对地球坐标系的转换矩阵

$C_b^e=C_i^e{C}_b^i=\left[\begin{array}{ccc}{c}_{\mathrm{be}11}& c_{\mathrm{be}12}& c_{\mathrm{be}13}\\ c_{\mathrm{be}21}& c_{\mathrm{be}22}& c_{\mathrm{be}23}\\ c_{\mathrm{be}31}& c_{\mathrm{be}32}& c_{\mathrm{be}33}\end{array}\right],$

其中，b表示载体坐标系，原点位于载体质心，z轴垂直于载体甲板平面，y轴指向载体艏向；x轴与其余两轴构成右手螺旋定则；表示载体系到地球系的转换矩阵；表示载体系到惯性系的转换矩阵，由星敏感器提供，c_beij(i,j＝1,2,3)表示中第i行第j列矩阵元素；表示惯性系到地球系的转换矩阵，与地球转速和导航时间有关，借助外部时间信息可得到该矩阵：

$C_i^e=\left[\begin{array}{ccc}\cos (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& \sin (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& 0\\ -\sin (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& \cos (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& 0\\ 0& 0& 1\end{array}\right],$

其中，ω_ie为地球自转角速度，t是世界标准时间系统提供的具体时间，k是初始位置经度与春分点之间的夹角。

(3)利用转换矩阵和得到星敏感器捷联矩阵

$C_b^{n_C^{\′}}=\left[\begin{array}{ccc}{c}_{{\mathrm{bn}}_C^{\′}11}& c_{{\mathrm{bn}}_C^{\′}12}& c_{{\mathrm{bn}}_C^{\′}13}\\ c_{{\mathrm{bn}}_C^{\′}21}& c_{{\mathrm{bn}}_C^{\′}22}& c_{{\mathrm{bn}}_C^{\′}23}\\ c_{{\mathrm{bn}}_C^{\′}31}& c_{{\mathrm{bn}}_C^{\′}32}& c_{{\mathrm{bn}}_C^{\′}33}\end{array}\right],$

其中，(i,j＝1,2,3)表示中第i行第j列矩阵元素；

(4)捷联惯导系统提供捷联矩阵

$C_b^{n_S^{\′}}=\left[\begin{array}{ccc}{c}_{{\mathrm{bn}}_S^{\′}11}& c_{{\mathrm{bn}}_S^{\′}12}& c_{{\mathrm{bn}}_S^{\′}13}\\ c_{{\mathrm{bn}}_S^{\′}21}& c_{{\mathrm{bn}}_S^{\′}22}& c_{{\mathrm{bn}}_S^{\′}23}\\ c_{{\mathrm{bn}}_S^{\′}31}& c_{{\mathrm{bn}}_S^{\′}32}& c_{{\mathrm{bn}}_S^{\′}33}\end{array}\right],$

其中，(i,j＝1,2,3)表示中第i行第j列矩阵元素；

(5)星敏感器捷联矩阵和捷联惯导捷联矩阵相乘：

$C_b^{n_C^{\′}}{\left(C_b^{n_S^{\′}}\right)}^T=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right],$

其中，角标T表示矩阵转置；c_ij(i,j＝1,2,3)表示乘积中第i行第j列矩阵元素，

$\left\{\begin{array}{c}{a}_1=c_{23}\\ a_2=c_{13}\\ a_3=c_{12}\end{array}\right.;$

(6)由捷联惯导捷联矩阵得到姿态角，即

$\left\{\begin{array}{c}{\mathrm{\θ}}_{\mathrm{Sx}}={\sin }^{-1}\left(c_{{\mathrm{bn}}_S^{\′}32}\right)\\ {\mathrm{\θ}}_{\mathrm{Sy}}={\tan }^{-1}({-c}_{{\mathrm{bn}}_S^{\′}31}/c_{{\mathrm{bn}}_S^{\′}33})\\ {\mathrm{\θ}}_{\mathrm{Sz}}={\tan }^{-1}({-c}_{{\mathrm{bn}}_S^{\′}12}/c_{{\mathrm{bn}}_S^{\′}22})\end{array}\right.,$

其中，θ_Sx，θ_Sy，θ_Sz分别为捷联惯导解算载体纵摇角、横摇角、航向角；

(7)由星敏感器捷联矩阵得到姿态角，即

$\left\{\begin{array}{c}{\mathrm{\θ}}_{\mathrm{Cx}}={\sin }^{-1}\left(c_{{\mathrm{bn}}_C^{\′}32}\right)\\ {\mathrm{\θ}}_{\mathrm{Cy}}={\tan }^{-1}({-c}_{{\mathrm{bn}}_C^{\′}31}/c_{{\mathrm{bn}}_C^{\′}33})\\ {\mathrm{\θ}}_{\mathrm{Cz}}={\tan }^{-1}({-c}_{{\mathrm{bn}}_C^{\′}12}/c_{{\mathrm{bn}}_C^{\′}22})\end{array}\right.,$

其中，θ_Cx，θ_Cy，θ_Cz分别为星敏感器捷联矩阵解算载体纵摇角、横摇角、航向角；

(8)星敏感器和捷联惯导解算两组姿态作差，得到

$\left\{\begin{array}{c}{a}_4={\mathrm{\θ}}_{\mathrm{Sx}}-{\mathrm{\θ}}_{\mathrm{Cx}}\\ a_5={\mathrm{\θ}}_{\mathrm{Sy}}-{\mathrm{\θ}}_{\mathrm{Cy}}\\ a_6={\mathrm{\θ}}_{\mathrm{Sz}}-{\mathrm{\θ}}_{\mathrm{Cz}}\end{array}\right.$

(9)耦合计算，得到各导航系统失准角：

其中，φ_Sx，φ_Sy，φ_Sz为捷联惯导数学平台失准角；φ_Cx，φ_Cy，φ_Cz为星敏感器数学平台失准角；m_(S)ij(i,j＝1,2)为与捷联惯导捷联矩阵元素相关的变量；m_(C)ij(i,j＝1,2)为与星敏感器捷联矩阵元素相关的变量：

$m_{\left(S\right)11}=-\frac{c_{{\mathrm{bn}}_S^{\′}22}}{\sqrt{1-c_{{\mathrm{bn}}_S^{\′}32}^2}},m_{\left(S\right)12}=\frac{c_{{\mathrm{bn}}_S^{\′}12}}{\sqrt{1-c_{{\mathrm{bn}}_S^{\′}32}^2}},m_{\left(S\right)21}=\frac{c_{{\mathrm{bn}}_S^{\′}21}{c}_{{\mathrm{bn}}_S^{\′}33}-c_{b{n}_S^{\′}23}{c}_{{\mathrm{bn}}_S^{\′}31}}{\sqrt{{{c_{{\mathrm{bn}}_S^{\′}}}_{33}^2{+c}_{{\mathrm{bn}}_S^{\′}}}_{31}^2}},$ $m_{\left(S\right)22}=\frac{c_{{\mathrm{bn}}_S^{\′}13}{c}_{{\mathrm{bn}}_S^{\′}31}-c_{b{n}_S^{\′}11}{c}_{{\mathrm{bn}}_S^{\′}33}}{\sqrt{{{c_{{\mathrm{bn}}_S^{\′}}}_{33}^2{+c}_{{\mathrm{bn}}_S^{\′}}}_{31}^2}};m_{\left(C\right)11}=-\frac{c_{{\mathrm{bn}}_C^{\′}22}}{\sqrt{1-c_{{\mathrm{bn}}_C^{\′}32}^2}},m_{\left(C\right)12}=\frac{c_{{\mathrm{bn}}_C^{\′}12}}{\sqrt{1-c_{{\mathrm{bn}}_C^{\′}32}^2}},$ $m_{\left(C\right)21}=\frac{c_{{\mathrm{bn}}_C^{\′}21}{c}_{{\mathrm{bn}}_C^{\′}33}-c_{b{n}_C^{\′}23}{c}_{{\mathrm{bn}}_C^{\′}31}}{\sqrt{{{c_{{\mathrm{bn}}_C^{\′}}}_{33}^2{+c}_{{\mathrm{bn}}_C^{\′}}}_{31}^2}},m_{\left(C\right)22}=\frac{c_{{\mathrm{bn}}_C^{\′}21}{c}_{{\mathrm{bn}}_C^{\′}33}-c_{b{n}_C^{\′}23}{c}_{{\mathrm{bn}}_C^{\′}31}}{\sqrt{{{c_{{\mathrm{bn}}_C^{\′}}}_{33}^2+c_{{\mathrm{bn}}_C^{\′}}}_{31}^2}};$

(10)校正捷联惯导姿态信息：

$C_b^n=C_{n_S^{\′}}^n{C}_b^{n_S^{\′}},$

其中， $C_{n_S^{\′}}^n=\left[\begin{array}{ccc}1& {-\mathrm{\φ}}_{\mathrm{Sz}}& {\mathrm{\φ}}_{\mathrm{Sy}}\\ {\mathrm{\φ}}_{\mathrm{Sz}}& 1& -{\mathrm{\φ}}_{\mathrm{Sx}}\\ {-\mathrm{\φ}}_{\mathrm{Sy}}& {\mathrm{\φ}}_{\mathrm{Sx}}& 1\end{array}\right],$

更新载体姿态信息，

$\left\{\begin{array}{c}\mathrm{\θ}=\arcsin \left(c_{\mathrm{bn}33}\right)\\ \mathrm{\φ}=\arctan (c_{\mathrm{bn}32}/c_{\mathrm{bn}31})\\ \mathrm{\ψ}=\arctan (c_{\mathrm{bn}13}/c_{\mathrm{bn}23})\end{array}\right.$

其中，c_bnij(i,j＝1,2,3)为第i行第j列矩阵元素；θ，φ，ψ表示组合导航解算载体纵摇角、横摇角和航向角；

(11)校正捷联惯导位置信息：

其中，λ分别表示组合导航解算载体纬度和经度信息；

(12)校正捷联惯导速度信息：

其中，R表示地球半径；分别表示φ_Cx、φ_Cy的微分形式；v_xS、v_yS分别表示捷联惯导解算东向、北向速度信息；v_x、v_y分别表示组合导航计算载体东向、北向速度信息。

本发明与现有技术相比的优点在于：本发明提出了一种基于姿态耦合的星敏感器/捷联惯导组合导航方法。该方法利用星敏感器测量的惯性姿态信息与捷联惯导测量输出的位置信息耦合计算，估算出捷联惯导导航解算位置误差、速度误差和姿态误差，将估算结果补偿后，可以抑制捷联惯导导航误差，提高系统导航精度。本发明的优点在于：(1)不受惯性组件测量误差影响；(2)计算量小，简单易实现；(3)不依靠人为推导实现，可靠性较强。

附图说明

图1为本发明的组合导航方法流程图；

图2为利用本发明进行的仿真试验，将组合导航估算结果补偿前后的姿态误差比较曲线；

图3为利用本发明进行的仿真试验，将组合导航估算结果补偿前后的速度误差比较曲线；

图4为利用本发明进行的仿真试验，将组合导航估算结果补偿前后的位置误差比较曲线。

具体实施方式

下面结合附图对本发明的具体进行详细描述。

本发明方法依据星敏感器直接测量输出的惯性系姿态，在惯导位置信息辅助下进行星敏感器定姿，并得到星敏感器捷联矩阵，通过将星敏感器捷联矩阵与捷联惯导捷联矩阵相乘，再利用乘积矩阵元素与星敏感器和惯导姿态差值进一步耦合，得到捷联惯导导航误差估算值，从而校正捷联惯导导航输出，达到目的。

一种基于姿态耦合的捷联惯导/星敏感器组合导航方法，包括以下步骤：

步骤1：采集SINS输出带有位置误差的地球坐标系相对地理坐标系的转换矩阵其中，e表示地球坐标系；n′表示捷联惯导(SINS)计算导航坐标系；表示地球坐标系相对SINS计算地理坐标系的转换矩阵；

步骤2：星敏感器直接输出相对于惯性空间的姿态矩阵根据转换矩阵间的数学关系，能够得到载体坐标系相对地球坐标系的转换矩阵其中，b表示载体坐标系；表示在载体系到地球系的转换矩阵；表示载体系到惯性系的转换矩阵，由星敏感器提供；表示惯性系到地球系的转换矩阵，与地球转速和导航时间有关，借助外部时间信息可得到该矩阵；

步骤3：利用步骤1和步骤2得到的转换矩阵相乘得到星敏感器捷联矩阵其中，n′_C表示星敏感器计算导航系；

步骤4：采集SINS捷联矩阵

步骤5：星敏感器捷联矩阵和捷联惯导捷联矩阵相乘得到新矩阵；

步骤6：由捷联惯导捷联矩阵得到姿态角θ_Sx，θ_Sy，θ_Sz；

步骤7：由星敏感器捷联矩阵得到姿态角θ_Cx，θ_Cy，θ_Cz；

步骤8：利用步骤6和步骤7得到的两组姿态作差；

步骤9：利用上述计算结果耦合计算，得到各导航系统失准角；

步骤10：校正捷联惯导姿态信息；

步骤11：校正捷联惯导位置信息；

步骤12：校正捷联惯导速度信息。

步骤9中所述耦合计算，具体过程为：

利用步骤5和步骤8矩阵乘积和姿态之差两组数学关系，得到组合导航系统的失准角，如下：

其中，φ_Sx，φ_Sy，φ_Sz为捷联惯导数学平台失准角；φ_Cx，φ_Cy，φ_Cz为星敏感器数学平台失准角；m_(S)ij(i,j＝1,2)为与捷联惯导捷联矩阵元素相关的变量；m_(C)ij(i,j＝1,2)为与星敏感器捷联矩阵元素相关的变量，具体如下：

$m_{\left(S\right)11}=-\frac{c_{{\mathrm{bn}}_S^{\′}22}}{\sqrt{1-c_{{\mathrm{bn}}_S^{\′}32}^2}},m_{\left(S\right)12}=\frac{c_{{\mathrm{bn}}_S^{\′}12}}{\sqrt{1-c_{{\mathrm{bn}}_S^{\′}32}^2}},m_{\left(S\right)21}=\frac{c_{{\mathrm{bn}}_S^{\′}21}{c}_{{\mathrm{bn}}_S^{\′}33}-c_{b{n}_S^{\′}23}{c}_{{\mathrm{bn}}_S^{\′}31}}{\sqrt{{{c_{{\mathrm{bn}}_S^{\′}}}_{33}^2{+c}_{{\mathrm{bn}}_S^{\′}}}_{31}^2}},$ $m_{\left(S\right)22}=\frac{c_{{\mathrm{bn}}_S^{\′}13}{c}_{{\mathrm{bn}}_S^{\′}31}-c_{b{n}_S^{\′}11}{c}_{{\mathrm{bn}}_S^{\′}33}}{\sqrt{{{c_{{\mathrm{bn}}_S^{\′}}}_{33}^2{+c}_{{\mathrm{bn}}_S^{\′}}}_{31}^2}};m_{\left(C\right)11}=-\frac{c_{{\mathrm{bn}}_C^{\′}22}}{\sqrt{1-c_{{\mathrm{bn}}_C^{\′}32}^2}},m_{\left(C\right)12}=\frac{c_{{\mathrm{bn}}_C^{\′}12}}{\sqrt{1-c_{{\mathrm{bn}}_C^{\′}32}^2}},$ $m_{\left(C\right)21}=\frac{c_{{\mathrm{bn}}_C^{\′}21}{c}_{{\mathrm{bn}}_C^{\′}33}-c_{b{n}_C^{\′}23}{c}_{{\mathrm{bn}}_C^{\′}31}}{\sqrt{{{c_{{\mathrm{bn}}_C^{\′}}}_{33}^2{+c}_{{\mathrm{bn}}_C^{\′}}}_{31}^2}},m_{\left(C\right)22}=\frac{c_{{\mathrm{bn}}_C^{\′}21}{c}_{{\mathrm{bn}}_C^{\′}33}-c_{b{n}_C^{\′}23}{c}_{{\mathrm{bn}}_C^{\′}31}}{\sqrt{{{c_{{\mathrm{bn}}_C^{\′}}}_{33}^2+c_{{\mathrm{bn}}_C^{\′}}}_{31}^2}};$

其中，(i,j＝1,2,3)表示步骤4中的第i行第j列矩阵元素；(i,j＝1,2,3)表示步骤3中的第i行第j列矩阵元素；

$\left\{\begin{array}{c}{a}_1=c_{23}\\ a_2=c_{13}\\ a_3=c_{12}\\ a_4={\mathrm{\θ}}_{\mathrm{Sx}}-{\mathrm{\θ}}_{\mathrm{Cx}}={\sin }^{-1}\left(c_{{\mathrm{bn}}_S^{\′}32}\right)-{\sin }^{-1}\left(c_{{\mathrm{bn}}_C^{\′}32}\right)\\ a_5={\mathrm{\θ}}_{\mathrm{Sy}}-{\mathrm{\θ}}_{\mathrm{Cy}}={\tan }^{-1}({-c}_{{\mathrm{bn}}_S^{\′}31}/c_{{\mathrm{bn}}_S^{\′}33})-{\tan }^{-1}({-c}_{{\mathrm{bn}}_C^{\′}31}/c_{{\mathrm{bn}}_C^{\′}33})\\ a_6={\mathrm{\θ}}_{\mathrm{Sz}}-{\mathrm{\θ}}_{\mathrm{Cz}}={\tan }^{-1}({-c}_{{\mathrm{bn}}_S^{\′}12}/c_{{\mathrm{bn}}_S^{\′}22})-{\tan }^{-1}({-c}_{{\mathrm{bn}}_C^{\′}12}/c_{{\mathrm{bn}}_C^{\′}22})\end{array}\right.$

其中，c_ij(i,j＝1,2,3)表示步骤5中矩阵乘积的第i行第j列矩阵元素。

如图1所示，本发明提供一种针对星敏感器/捷联惯导的组合导航方法，具体包括如下步骤：

步骤1：采集SINS输出带有其导航解算位置误差的地球坐标系到地理坐标系的转换矩阵如下：

其中，e表示地球坐标系，原点位于地球质心，z轴指向地球自转方向，x指向春分点方向，y轴与其它两轴构成右手螺旋定则；n′表示SINS计算导航坐标系；表示地球坐标系相对SINS计算地理坐标系的转换矩阵；sin为三角函数中的正弦函数，cos为三角函数中的余弦函数；λ为载体所在位置地理经度，δλ_s为SINS解算经度误差，λ_s＝λ+δλ_s为SINS解算经度；为载体所在位置地理纬度，为SINS解算纬度误差，为SINS解算纬度。

步骤2：星敏感器直接输出相对于惯性空间的姿态矩阵根据转换矩阵间的数学关系，能够得到载体坐标系相对于地球坐标系的转换矩阵转换过程如下

$C_b^e=C_i^e{C}_b^i=\left[\begin{array}{ccc}{c}_{\mathrm{be}11}& c_{\mathrm{be}12}& c_{\mathrm{be}13}\\ c_{\mathrm{be}21}& c_{\mathrm{be}22}& c_{\mathrm{be}23}\\ c_{\mathrm{be}31}& c_{\mathrm{be}32}& c_{\mathrm{be}33}\end{array}\right]$

其中，b表示载体坐标系，原点位于载体质心，z轴垂直于载体甲板平面，y轴指向载体艏向；x轴与其余两轴构成右手螺旋定则；表示载体系到地球系的转换矩阵；表示载体系到惯性系的转换矩阵，由星敏感器提供，c_beij(i,j＝1,2,3)表示中第i行第j列矩阵元素；表示惯性系到地球系的转换矩阵，与地球转速和导航时间有关，借助外部时间信息可得到该矩阵，形式如下：

$C_i^e=\left[\begin{array}{ccc}\cos (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& \sin (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& 0\\ -\sin (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& \cos (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& 0\\ 0& 0& 1\end{array}\right]$

其中，ω_ie为地球自转角速度，t是世界标准时间系统提供的具体时间，k是初始位置(经度)与春分点之间的夹角。

步骤3：利用步骤1和步骤2得到的转换矩阵和相乘，得到星敏感器捷联矩阵计算过程如下，

$C_b^{n_C^{\′}}=C_e^{n^{\′}}{C}_b^e$

即

其中，(i,j＝1,2,3)表示中第i行第j列矩阵元素。

步骤4：采集SINS导航解算得到的捷联矩阵并定义形式如下：

$C_b^{n_S^{\′}}=\left[\begin{array}{ccc}{c}_{{\mathrm{bn}}_S^{\′}11}& c_{{\mathrm{bn}}_S^{\′}12}& c_{{\mathrm{bn}}_S^{\′}13}\\ c_{{\mathrm{bn}}_S^{\′}21}& c_{{\mathrm{bn}}_S^{\′}22}& c_{{\mathrm{bn}}_S^{\′}23}\\ c_{{\mathrm{bn}}_S^{\′}31}& c_{{\mathrm{bn}}_S^{\′}32}& c_{{\mathrm{bn}}_S^{\′}33}\end{array}\right]$

其中，(i,j＝1,2,3)表示中第i行第j列矩阵元素。

步骤5：将步骤3得到的星敏感器捷联矩阵和步骤4采集的捷联惯导捷联矩阵相乘，得到新矩阵，形式如下：

$C_b^{n_C^{\′}}{\left(C_b^{n_S^{\′}}\right)}^T=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right]$

其中，角标T表示矩阵转置；c_ij(i,j＝1,2,3)表示矩阵乘积中第i行第j列矩阵元素，并定义：

$\left\{\begin{array}{c}{a}_1=c_{23}\\ a_2=c_{13}\\ a_3=c_{12}\end{array}\right.$

步骤6：利用步骤4采集的捷联惯导捷联矩阵解算载体姿态信息，即

$\left\{\begin{array}{c}{\mathrm{\θ}}_{\mathrm{Sx}}={\sin }^{-1}\left(c_{{\mathrm{bn}}_S^{\′}32}\right)\\ {\mathrm{\θ}}_{\mathrm{Sy}}={\tan }^{-1}({-c}_{{\mathrm{bn}}_S^{\′}31}/c_{{\mathrm{bn}}_S^{\′}33})\\ {\mathrm{\θ}}_{\mathrm{Sz}}={\tan }^{-1}({-c}_{{\mathrm{bn}}_S^{\′}12}/c_{{\mathrm{bn}}_S^{\′}22})\end{array}\right.$

其中，θ_Sx，θ_Sy，θ_Sz分别为捷联惯导解算载体纵摇角、横摇角、航向角。

步骤7：利用步骤3计算得到的星敏感器捷联矩阵解算载体姿态角，即

$\left\{\begin{array}{c}{\mathrm{\θ}}_{\mathrm{Cx}}={\sin }^{-1}\left(c_{{\mathrm{bn}}_C^{\′}32}\right)\\ {\mathrm{\θ}}_{\mathrm{Cy}}={\tan }^{-1}({-c}_{{\mathrm{bn}}_C^{\′}31}/c_{{\mathrm{bn}}_C^{\′}33})\\ {\mathrm{\θ}}_{\mathrm{Cz}}={\tan }^{-1}({-c}_{{\mathrm{bn}}_C^{\′}12}/c_{{\mathrm{bn}}_C^{\′}22})\end{array}\right.$

其中，θ_Cx，θ_Cy，θ_Cz分别为星敏感器解算载体纵摇角、横摇角、航向角。

步骤8：利用步骤6和步骤7计算得到的两组姿态作差，得到：

$\left\{\begin{array}{c}{a}_4={\mathrm{\θ}}_{\mathrm{Sx}}-{\mathrm{\θ}}_{\mathrm{Cx}}={\sin }^{-1}\left(c_{{\mathrm{bn}}_S^{\′}32}\right)-{\sin }^{-1}\left(c_{{\mathrm{bn}}_C^{\′}32}\right)\\ a_5={\mathrm{\θ}}_{\mathrm{Sy}}-{\mathrm{\θ}}_{\mathrm{Cy}}={\sin }^{-1}(-c_{{\mathrm{bn}}_S^{\′}31}/c_{{\mathrm{bn}}_S^{\′}33})-{\tan }^{-1}(-c_{{\mathrm{bn}}_C^{\′}31}/c_{{\mathrm{bn}}_C^{\′}33})\\ a_6={\mathrm{\θ}}_{\mathrm{Sz}}-{\mathrm{\θ}}_{\mathrm{Cz}}={\tan }^{-1}(-c_{{\mathrm{bn}}_S^{\′}12}/c_{{\mathrm{bn}}_S^{\′}22})-{\sin }^{-1}({-c}_{{\mathrm{bn}}_C^{\′}12}/c_{{\mathrm{bn}}_C^{\′}22})\end{array}\right.$

其中，a₄、a₅、a₆为两组姿态差值定义变量。

步骤9：利用步骤5和步骤8矩阵乘积和姿态之差两组数学关系，得到组合导航系统的失准角，如下：

其中，φ_Sx，φ_Sy，φ_Sz为捷联惯导数学平台失准角；φ_Cx，φ_Cy，φ_Cz为星敏感器数学平台失准角；m_(S)ij(i,j＝1,2)为与捷联惯导捷联矩阵元素相关的变量；m_(C)ij(i,j＝1,2)为与星敏感器捷联矩阵元素相关的变量，具体如下：

$m_{\left(S\right)11}=-\frac{c_{{\mathrm{bn}}_S^{\′}22}}{\sqrt{1-c_{{\mathrm{bn}}_S^{\′}32}^2}},m_{\left(S\right)12}=\frac{c_{{\mathrm{bn}}_S^{\′}12}}{\sqrt{1-c_{{\mathrm{bn}}_S^{\′}32}^2}},m_{\left(S\right)21}=\frac{c_{{\mathrm{bn}}_S^{\′}21}{c}_{{\mathrm{bn}}_S^{\′}33}-c_{b{n}_S^{\′}23}{c}_{{\mathrm{bn}}_S^{\′}31}}{\sqrt{{{c_{{\mathrm{bn}}_S^{\′}}}_{33}^2{+c}_{{\mathrm{bn}}_S^{\′}}}_{31}^2}},$ $m_{\left(S\right)22}=\frac{c_{{\mathrm{bn}}_S^{\′}13}{c}_{{\mathrm{bn}}_S^{\′}31}-c_{b{n}_S^{\′}11}{c}_{{\mathrm{bn}}_S^{\′}33}}{\sqrt{{{c_{{\mathrm{bn}}_S^{\′}}}_{33}^2{+c}_{{\mathrm{bn}}_S^{\′}}}_{31}^2}};m_{\left(C\right)11}=-\frac{c_{{\mathrm{bn}}_C^{\′}22}}{\sqrt{1-c_{{\mathrm{bn}}_C^{\′}32}^2}},m_{\left(C\right)12}=\frac{c_{{\mathrm{bn}}_C^{\′}12}}{\sqrt{1-c_{{\mathrm{bn}}_C^{\′}32}^2}},$ $m_{\left(C\right)21}=\frac{c_{{\mathrm{bn}}_C^{\′}21}{c}_{{\mathrm{bn}}_C^{\′}33}-c_{b{n}_C^{\′}23}{c}_{{\mathrm{bn}}_C^{\′}31}}{\sqrt{{{c_{{\mathrm{bn}}_C^{\′}}}_{33}^2{+c}_{{\mathrm{bn}}_C^{\′}}}_{31}^2}},m_{\left(C\right)22}=\frac{c_{{\mathrm{bn}}_C^{\′}21}{c}_{{\mathrm{bn}}_C^{\′}33}-c_{b{n}_C^{\′}23}{c}_{{\mathrm{bn}}_C^{\′}31}}{\sqrt{{{c_{{\mathrm{bn}}_C^{\′}}}_{33}^2+c_{{\mathrm{bn}}_C^{\′}}}_{31}^2}};$

步骤10：利用步骤9得到的捷联惯导数学平台失准角估算结果，对捷联惯导姿态信息进行校正，校正过程利用如下数学转换关系

$C_b^n=C_{n_S^{\′}}^n{C}_b^{n_S^{\′}},$

其中， $C_{n_S^{\′}}^n=\left[\begin{array}{ccc}1& {-\mathrm{\φ}}_{\mathrm{Sz}}& {\mathrm{\φ}}_{\mathrm{Sy}}\\ {\mathrm{\φ}}_{\mathrm{Sz}}& 1& -{\mathrm{\φ}}_{\mathrm{Sx}}\\ {-\mathrm{\φ}}_{\mathrm{Sy}}& {\mathrm{\φ}}_{\mathrm{Sx}}& 1\end{array}\right].$

更新载体姿态信息，

$\left\{\begin{array}{c}\mathrm{\θ}=\arcsin \left(c_{\mathrm{bn}33}\right)\\ \mathrm{\φ}=\arctan (c_{\mathrm{bn}32}/c_{\mathrm{bn}31})\\ \mathrm{\ψ}=\arctan (c_{\mathrm{bn}13}/c_{\mathrm{bn}23})\end{array}\right.$

其中，c_bnij(i,j＝1,2,3)为第i行第j列矩阵元素；θ，φ，ψ表示组合导航解算载体纵摇角、横摇角和航向角。

步骤11：利用步骤9得到的星敏感器数学平台失准角估算结果，校正捷联惯导位置信息，校正过程如下：

其中，λ分别表示组合导航解算载体纬度和经度信息。

步骤12：利用步骤9得到的星敏感器数学平台失准角估算结果，校正捷联惯导速度信息，校正过程如下：

其中，R表示地球半径；分别表示φ_Cx、φ_Cy的微分形式；v_xS、v_yS分别表示捷联惯导解算东向、北向速度信息；v_x、v_y分别表示组合导航计算载体东向、北向速度信息。

对本发明的有益效果进行验证如下：

在Matlab仿真条件下，对该方法进行仿真实验：

赤道半径：R＝6378393.0m；

由万有引力可得的地球表面重力加速度：g＝9.78049m/s²；

地球自转角速度：Ω＝7.2921158×10^-5rad/s；

常数：π＝3.1415926535；

载体初始位置45.7796°N、126.6709°E，初始姿态0°、0°、45°；

载体以5m/s的速度做任意形式运动；

光纤陀螺常值漂移：0.01°/h；

光纤陀螺白噪声误差：0.005°/h；

光纤陀螺刻度因数误差：10ppm；

加速度计零偏：10^-4g；

加速度计白噪声误差：5×10^-5g；

加速度计刻度因数误差：10ppm；

星敏感器输出信息中包含均值为0、幅值为1×10^-5的白噪声；

仿真时间2小时，采样频率0.1Hz。

利用所述发明，得到基于星敏感器/捷联惯导组合导航结果与捷联惯导单独导航结果比较曲线如图2-图4。其中，图2为姿态误差比较曲线，图3为速度误差比较曲线，图4为位置误差比较曲线。

根据图2-图4结果可以看出，利用本发明提出的基于姿态耦合的星敏感器/捷联惯导组合导航方法可以较好的估计各导航误差，估计结果补偿后能够有效地提高系统导航精度。

Claims (1)

1.一种基于姿态耦合的捷联惯导/星敏感器组合导航方法，其特征在于：

(1)采集捷联惯导系统输出带有位置误差的地球坐标系相对地理坐标系的转换矩阵

其中，e表示地球坐标系，原点位于地球质心，z轴指向地球自转方向，x指向春分点方向，y轴与其它两轴构成右手螺旋定则；n′表示捷联惯导系统计算导航坐标系；表示地球坐标系相对捷联惯导系统计算地理坐标系的转换矩阵；sin为三角函数中的正弦函数，cos为三角函数中的余弦函数；λ_s＝λ+δλ_s为捷联惯导系统解算经度，λ为载体所在位置地理经度，δλ_s为捷联惯导系统解算经度误差；为捷联惯导系统解算纬度，为载体所在位置地理纬度，为捷联惯导系统解算纬度误差；

(2)星敏感器直接输出相对于惯性空间的姿态矩阵得到载体坐标系相对地球坐标系的转换矩阵

$C_b^e=C_i^e{C}_b^i=\left[\begin{array}{ccc}{c}_{\mathrm{be}11}& c_{\mathrm{be}12}& c_{\mathrm{be}13}\\ c_{\mathrm{be}21}& c_{\mathrm{be}22}& c_{\mathrm{be}23}\\ c_{\mathrm{be}31}& c_{\mathrm{be}32}& c_{\mathrm{be}33}\end{array}\right],$

其中，b表示载体坐标系，原点位于载体质心，z轴垂直于载体甲板平面，y轴指向载体艏向；x轴与其余两轴构成右手螺旋定则；表示载体系到地球系的转换矩阵；表示载体系到惯性系的转换矩阵，由星敏感器提供，c_beij(i,j＝1,2,3)表示中第i行第j列矩阵元素；表示惯性系到地球系的转换矩阵，与地球转速和导航时间有关，借助外部时间信息可得到该矩阵：

$C_i^e=\left[\begin{array}{ccc}\cos (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& \sin (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& 0\\ -\sin (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& \cos (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& 0\\ 0& 0& 1\end{array}\right],$

其中，ω_ie为地球自转角速度，t是世界标准时间系统提供的具体时间，k是初始位置经度与春分点之间的夹角；

(3)利用转换矩阵和得到星敏感器捷联矩阵

$C_b^{n_C^{\′}}=\left[\begin{array}{ccc}{c}_{{\mathrm{bn}}_C^{\′}11}& c_{{\mathrm{bn}}_C^{\′}12}& c_{{\mathrm{bn}}_C^{\′}13}\\ c_{{\mathrm{bn}}_C^{\′}21}& c_{{\mathrm{bn}}_C^{\′}22}& c_{{\mathrm{bn}}_C^{\′}23}\\ c_{{\mathrm{bn}}_C^{\′}31}& c_{{\mathrm{bn}}_C^{\′}32}& c_{{\mathrm{bn}}_C^{\′}33}\end{array}\right],$

其中，表示中第i行第j列矩阵元素；

(4)捷联惯导系统提供捷联矩阵

$C_b^{n_S^{\′}}=\left[\begin{array}{ccc}{c}_{{\mathrm{bn}}_S^{\′}11}& c_{{\mathrm{bn}}_S^{\′}12}& c_{{\mathrm{bn}}_S^{\′}13}\\ c_{{\mathrm{bn}}_S^{\′}21}& c_{{\mathrm{bn}}_S^{\′}22}& c_{{\mathrm{bn}}_S^{\′}23}\\ c_{{\mathrm{bn}}_S^{\′}31}& c_{{\mathrm{bn}}_S^{\′}32}& c_{{\mathrm{bn}}_S^{\′}33}\end{array}\right],$

其中，表示中第i行第j列矩阵元素；

(5)星敏感器捷联矩阵和捷联惯导捷联矩阵相乘：

$C_b^{n_C^{\′}}{\left(C_b^{n_S^{\′}}\right)}^T=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right],$

其中，角标T表示矩阵转置；c_ij(i,j＝1,2,3)表示乘积中第i行第j列矩阵元素，

$\left\{\begin{array}{c}{a}_1=c_{23}\\ a_2=c_{13}\\ a_3=c_{12}\end{array}\right.;$

(6)由捷联惯导捷联矩阵得到姿态角，即

$\left\{\begin{array}{c}{\mathrm{\θ}}_{\mathrm{Sx}}={\sin }^{-1}\left(c_{{\mathrm{bn}}_S^{\′}32}\right)\\ {\mathrm{\θ}}_{\mathrm{Sy}}={\tan }^{-1}({-c}_{{\mathrm{bn}}_S^{\′}31}/c_{{\mathrm{bn}}_S^{\′}33})\\ {\mathrm{\θ}}_{\mathrm{Sz}}={\tan }^{-1}({-c}_{{\mathrm{bn}}_S^{\′}12}/c_{{\mathrm{bn}}_S^{\′}22})\end{array}\right.,$

其中，θ_Sx，θ_Sy，θ_Sz分别为捷联惯导解算载体纵摇角、横摇角、航向角；

(7)由星敏感器捷联矩阵得到姿态角，即

$\left\{\begin{array}{c}{\mathrm{\θ}}_{\mathrm{Cx}}={\sin }^{-1}\left(c_{{\mathrm{bn}}_C^{\′}32}\right)\\ {\mathrm{\θ}}_{\mathrm{Cy}}={\tan }^{-1}({-c}_{{\mathrm{bn}}_C^{\′}31}/c_{{\mathrm{bn}}_C^{\′}33})\\ {\mathrm{\θ}}_{\mathrm{Cz}}={\tan }^{-1}({-c}_{{\mathrm{bn}}_C^{\′}12}/c_{{\mathrm{bn}}_C^{\′}22})\end{array}\right.,$

其中，θ_Cx，θ_Cy，θ_Cz分别为星敏感器捷联矩阵解算载体纵摇角、横摇角、航向角；

(8)星敏感器和捷联惯导解算两组姿态作差，得到

$\left\{\begin{array}{c}{a}_4={\mathrm{\θ}}_{\mathrm{Sx}}-{\mathrm{\θ}}_{\mathrm{Cx}}\\ a_5={\mathrm{\θ}}_{\mathrm{Sy}}-{\mathrm{\θ}}_{\mathrm{Cy}}\\ a_6={\mathrm{\θ}}_{\mathrm{Sz}}-{\mathrm{\θ}}_{\mathrm{Cz}}\end{array}\right.;$

(9)耦合计算，得到各导航系统失准角：

其中，φ_Sx，φ_Sy，φ_Sz为捷联惯导数学平台失准角；φ_Cx，φ_Cy，φ_Cz为星敏感器数学平台失准角；m_(S)ij(i,j＝1,2)为与捷联惯导捷联矩阵元素相关的变量；m_(C)ij(i,j＝1,2)为与星敏感器捷联矩阵元素相关的变量：

$m_{\left(S\right)11}=-\frac{c_{{\mathrm{bn}}_S^{\′}22}}{\sqrt{1-c_{{\mathrm{bn}}_S^{\′}32}^2}},m_{\left(S\right)12}=\frac{c_{{\mathrm{bn}}_S^{\′}12}}{\sqrt{1-c_{{\mathrm{bn}}_S^{\′}32}^2}},m_{\left(S\right)21}=\frac{c_{{\mathrm{bn}}_S^{\′}21}{c}_{{\mathrm{bn}}_S^{\′}33}-c_{{\mathrm{bn}}_S^{\′}23}{c}_{{\mathrm{bn}}_S^{\′}31}}{\sqrt{c_{{\mathrm{bn}}_S^{\′}33}^2+c_{{\mathrm{bn}}_S^{\′}31}^2}},$

$m_{\left(S\right)22}=\frac{c_{{\mathrm{bn}}_S^{\′}13}{c}_{{\mathrm{bn}}_S^{\′}31}-c_{{\mathrm{bn}}_S^{\′}11}{c}_{{\mathrm{bn}}_S^{\′}33}}{\sqrt{c_{{\mathrm{bn}}_S^{\′}33}^2+c_{{\mathrm{bn}}_S^{\′}31}^2}},m_{\left(C\right)11}=\frac{c_{{\mathrm{bn}}_C^{\′}22}}{\sqrt{1-c_{{\mathrm{bn}}_C^{\′}32}^2}},m_{\left(C\right)12}=\frac{c_{{\mathrm{bn}}_C^{\′}12}}{\sqrt{1-c_{{\mathrm{bn}}_C^{\′}32}^2}}$

$m_{\left(C\right)21}=\frac{c_{{\mathrm{bn}}_C^{\′}21}{c}_{{\mathrm{bn}}_C^{\′}33}-c_{{\mathrm{bn}}_C^{\′}23}{c}_{{\mathrm{bn}}_C^{\′}31}}{\sqrt{c_{{\mathrm{bn}}_C^{\′}33}^2+c_{{\mathrm{bn}}_C^{\′}31}^2}},m_{\left(C\right)22}=\frac{c_{{\mathrm{bn}}_C^{\′}21}{c}_{{\mathrm{bn}}_C^{\′}33}-c_{{\mathrm{bn}}_C^{\′}23}{c}_{{\mathrm{bn}}_C^{\′}31}}{\sqrt{c_{{\mathrm{bn}}_C^{\′}33}^2+c_{{\mathrm{bn}}_C^{\′}31}^2}}$

(10)校正捷联惯导姿态信息：

$C_b^n=C_{n_S^{\′}}^n{C}_b^{n_S^{\′}},$

其中， $C_{n_S^{\′}}^n=\left[\begin{array}{ccc}1& {-\mathrm{\φ}}_{\mathrm{Sz}}& {\mathrm{\φ}}_{\mathrm{Sy}}\\ {\mathrm{\φ}}_{\mathrm{Sz}}& 1& {-\mathrm{\φ}}_{\mathrm{Sx}}\\ {-\mathrm{\φ}}_{\mathrm{Sy}}& {\mathrm{\φ}}_{\mathrm{Sx}}& 1\end{array}\right],$

更新载体姿态信息，

$\left\{\begin{array}{c}\mathrm{\θ}=\arcsin \left(c_{\mathrm{bn}33}\right)\\ \mathrm{\φ}=\arctan (c_{\mathrm{bn}32}/c_{\mathrm{bn}31})\\ \mathrm{\ψ}=\arctan (c_{\mathrm{bn}13}/c_{\mathrm{bn}23})\end{array}\right.$

其中，c_bnij(i,j＝1,2,3)为第i行第j列矩阵元素；θ，φ，ψ表示组合导航解算载体纵摇角、横摇角和航向角；

(11)校正捷联惯导位置信息：

其中，λ分别表示组合导航解算载体纬度和经度信息；

(12)校正捷联惯导速度信息：

其中，R表示地球半径；分别表示φ_Cx、φ_Cy的微分形式；v_xS、v_yS分别表示捷联惯导解算东向、北向速度信息；v_x、v_y分别表示组合导航计算载体东向、北向速度信息。