CN103994763B - 一种火星车的sins/cns深组合导航系统及其实现方法 - Google Patents

Abstract

一种火星车的SINS/CNS深组合导航系统，它包括捷联惯导、天文导航、惯导位置单元、惯导姿态单元及滤波器；捷联惯导将数学平台和位置矩阵提供给惯导姿态单元，同时还将经纬度信息提供给惯导位置单元，天文导航将惯性姿态矩阵传送给滤波器，惯导姿态单元将其构造的惯性姿态矩阵输入到滤波器，惯导位置单元将其构造的位置矢量信息输入到滤波器，滤波器将分别为天文导航子系统和捷联惯导子系统提供姿态和位置估计误差；其实现方法包括以下步骤：一：大视场星敏感器辅助捷联惯导系统获得高精度水平基准；二：基于高精度水平基准的天文位置矢量确定；三：建立组合导航系统的状态模型和量测模型；四：组合导航系统信息融合；五：通过信息反馈校正各子系统误差。

Description

一种火星车的SINS/CNS深组合导航系统及其实现方法

一、技术领域

本发明提供一种火星车的SINS/CNS深组合导航系统及其实现方法,它涉及一种适用于火星车的捷联惯导/天文导航(即：SINS/CNS)深组合导航系统及其实现方法，属于组合导航技术领域。

二、背景技术

火星车是火星探测的重要组成部分，它可以代替人类在复杂的非结构化环境中执行探测任务，大大节省探测成本，是目前对火星进行近距离探测的最直接有效的工具。火星车应该具有自主漫游和探测功能，能够在火星表面自动行驶几百米甚至是几十公里，通过自身携带的科学仪器可实现对火星表面环境的简单直接勘测。火星车要实现自主漫游和完成科学实验，必须具备高精度的自主导航能力。火星车的自主导航系统是探测过程中实现自动障碍规避和路径规划的基础，是火星车安全有效完成任务的重要保证。

火星车通常借助地面站通过无线电测控进行导航控制，但由于火星和地球的自转和公转运动以及所带来的地面站和火星车相对地理位置的变化等，有时火星车会处于地面站无法测控的区域内。同时由于火星距离地球非常遥远，无线电测控在实时性和可靠性上都无法满足火星车的自主导航要求。因此，火星车利用自身携带的仪器设备进行自主导航已经成为了一个亟待解决的重要问题。

现有的自主导航方法很多，有天文导航、惯性导航、地磁导航、车轮里程计等等。由于未知火星的磁场信息，地磁导航方法在火星表面不适用；考虑到火星表面的地质松软，车轮容易出现打滑等现象，导致里程计误差较大，也不适合用于火星车自主导航；而天文导航和惯性导航同属环境敏感导航技术，具有自主性强，隐蔽性好，而且这两种导航方式还具有优势互补的特点。因此，天文/惯性组合导航系统是火星车自主导航的首选。目前，相对成熟的天文导航和惯导的组合方式有简单组合模式、基于陀螺漂移校正的组合模式、全面最优校正模式以及深组合模式。

简单组合模式中捷联惯导(即：SINS)独立工作，提供位置、姿态、速度等导航信息，天文导航系统与SINS提供的基准信息相结合，可获得载体的位置、姿态信息，并对SINS的位置、航向数据进行校正。但由于该组合模式使用过程中并未对惯性器件(陀螺仪和加速度计)误差进行校正，因此，随着时间的增加，该组合模式的位置、速度和姿态误差均发散。

基于陀螺仪漂移校正的组合模式又称最优组合模式，将大视场星敏感器输出的高精度惯性姿态信息，与SINS构造的惯性姿态信息相结合，通过估计并补偿SINS中陀螺仪漂移误差，可以通过有效修正SINS的姿态误差，来达到提高导航精度的目的。但该组合模式由于无法对加速度计的误差进行估计和补偿，因此，随着时间的延长，其位置误差仍然发散。

全面最优校正模式必须解决高精度自主水平基准问题，目前常用星光折射间接敏感地平的方法得到水平基准，而火星大气模型未知，因此该模式不适用于火星车自主导航系统的设计。

深组合模式中，SINS与天文导航系统相互辅助，星敏感器三轴定姿原理得到惯性姿态辅助SINS可输出高精度的地平信息，SINS为天文提供地平信息，使得天文导航系统能够输出高精度的位置信息。并将捷联惯导系统和天文导航系统的位置、姿态输出的差值作为量测量，估计并校正SINS的导航解算误差。组合滤波器能够估计并补偿平台角误差和陀螺仪漂移，从而提高导航精度。由于不能估计和补偿加速度计零偏引起的误差，导航信息依然发散。传统的深组合不能满足长运行时间的导航精度。

为了满足火星车长时间、高精度自主导航的要求，以及克服天文导航水平基准的限制，本发明提出了一种适用于火星车的SINS/CNS深组合导航方案及其实现方法。

三、发明内容

针对现有技术中存在的问题，本发明提出一种火星车的SINS/CNS深组合导航系统及其实现方法，通过利用高精度水平基准，结合大视场星敏感器提供的高精度惯性姿态，获得精确的天文位置矢量，再结合SINS提供的位置、姿态信息，实现了对惯性器件误差的有效估计并校正，从而达到了大幅度提高导航精度的目的。

本发明一种火星车的SINS/CNS深组合导航系统，包括捷联惯导子系统、天文导航子系统、惯导位置量测信息构造单元、惯导姿态量测信息构造单元以及组合导航滤波器；它们之间的关系是：捷联惯导子系统将数学平台和位置矩阵提供给惯导姿态量测信息构造单元，同时还将经纬度信息提供给惯导位置量测信息构造单元，天文导航子系统将惯性姿态矩阵传送给组合导航滤波器，惯导姿态量测信息构造单元将其构造的惯导的惯性姿态矩阵输入到组合导航滤波器，惯导位置量测信息构造单元将其构造的位置矢量信息输入到组合导航滤波器，组合导航滤波器将分别为天文导航子系统和捷联惯导子系统提供姿态和位置估计误差。

所述捷联惯导子系统包括惯性测量元件和惯导解算单元，它们之间的关系是惯性测量元件将相对于惯性空间的角速度和比力提供给惯导解算单元，作为惯导解算单元的一个输入：惯性测量元件测量火星车相对于惯性空间的角速度和比力，将所得的角速度和比力信息传送到惯导解算单元；惯导解算单元根据载体的初始位置信息以及惯性测量组件传输的信息，通过力学编排实时解算火星车的位置和姿态矩阵；

该惯性测量元件即为一个惯组，包括三个加速度计和三个陀螺仪(在现有惯组中，选取满足要求的元件，要求陀螺仪常值漂移为0.01°/h，随机漂移为0.01°/h，加速度计常值漂移为50μg，随机偏置为10μg，惯性器件数据输出周期为0.01s)，通过正交安装，能够敏感相对于惯性空间的角速度和比力。(h—小时，s—秒)

该惯导解算单元是一个将惯性测量元件输出的比力和角速度信息经过一系列的计算和更新，解算出载体的实时位置、速度、姿态等导航信息的算法，该算法流程图如图3，首先输入火星车的初始位置、速度和姿态等信息，经过初始对准，并计算火星自转速率、位置速率，再根据惯性测量单元输出的角速度和比力，计算姿态速率，从而更新姿态矩阵，然后使用惯导基本方程计算速度增量，并二次积分得到经纬度信息，最后计算姿态角，如此循环，直到系统停止工作，即可实时解算出惯导提供的导航信息，为后续的组合导航提供数据支持。

所述天文导航子系统包括大视场星敏感器、多矢量定姿模块和天文位置矢量构造单元，它们之间的关系是：大视场星敏感器将其观测到的多个恒星星光矢量提供给多矢量定姿模块，作为多矢量定姿模块的输入，多矢量定姿模块将其输出的惯性姿态矩阵作为天文位置矢量构造单元的一个输入，天文位置矢量构造单元再结合高精度的数学水平基准，构造出天文位置矢量：该大视场星敏感器主要由CCD器件、外围采样电路、信号处理电路和光学镜头四部分组成，在现有的大视场星敏感器中，按要求选取：其视场为20°×20°，量测噪声为2″，数据输出周期为1s。它在同一时刻能够观测到三颗及以上的恒星星光矢量，并将其观测信息送入多矢量定姿模块；多矢量定姿模块处理接收到的星光矢量信息，得到火星车车体坐标系相对于惯性空间的姿态矩阵；天文位置矢量构造单元结合多矢量定姿模块得到的惯性姿态矩阵以及高精度的数学水平基准，构造出天文位置矢量；

该大视场星敏感器是一种常用的星敏感器，它具有较大的视场，可以同时观测多颗恒星的星光矢量。恒星星光通过透镜投影到CCD面阵上，根据光学成像原理以及几何知识，可以得到恒星星光在星敏感器坐标系下的表示，而假设星敏感器坐标系与车体坐标系完全一致，因此，可以通过星敏感器所获得的信息，得到多颗恒星矢量在车体坐标系下的表示，为多矢量定姿模块提供了信息。

该多矢量定姿模块根据星敏感器的测姿原理，可以得到等式S＝G·A，其中G为大视场星敏感器输出的n个星光矢量组成的维数为n×3的矩阵，S为对应于G中n个恒星在惯性空间中的坐标组成的维数为n×3的矩阵，通过采用最小二乘法求解星敏感器的惯性姿态矩阵A＝(G^TG)^-1(G^TS)。

该天文位置矢量构造单元是一个计算单元，它根据姿态四元数与刚体旋转的关系，将机体系下的轴旋转到与地理坐标系下的Z轴重合，得到矢量由于姿态四元数与姿态转换矩阵之间在该转换情况下存在一定的等价关系，上述的姿态四元数可由高精度的数学平台代替，然后，通过惯性姿态矩阵将矢量投影到火星惯性坐标系，得到位置矢量从而构造出了天文位置矢量。

所述惯导位置量测信息构造单元根据捷联惯导子系统确定的经纬度等位置信息，确定载体的位置矢量，并将其作为量测信息输入组合导航滤波器中；该惯导位置量测信息构造单元是一个计算单元，通过经纬度信息以及其与位置矢量的关系，计算出惯导位置量测信息

所述惯导姿态量测信息构造单元根据捷联惯导子系统输出的位置和姿态矩阵等导航信息，确定从火星赤道惯性坐标系到载体坐标系的惯性姿态矩阵；该惯导姿态量测信息构造单元也是一个计算单元，它计算惯导捷联矩阵的转置、位置矩阵以及从火星惯性坐标系I转换到火星星固坐标系m的方向余弦矩阵的连乘积，并将该计算结果作为惯导构造的姿态量测信息；

所述组合导航滤波器是卡尔曼滤波器，它是以SINS误差方程作为状态方程，以惯导姿态量测信息构造单元与多矢量定姿模块分别提供的惯性姿态矩阵之差作为一个观测量，同时，以惯导位置量测信息构造单元与天文位置矢量构造单元分别提供的载体位置矢量信息之差作为另一个观测量，通过组合导航滤波器得到火星车位置、姿态以及惯性测量组件误差的估计值；

本发明一种火星车的SINS/CNS深组合导航方案的实现方法，具体包括以下步骤：

步骤一：大视场星敏感器辅助捷联惯导系统获得高精度水平基准；

为了方便以下讨论，首先定义几个常用的坐标系：导航坐标系OX_nY_nZ_n(东北天)、车体坐标系OX_bY_bZ_b(右前上)、火星惯性坐标系OX_IY_IZ_I、火星星固坐标系OX_mY_mZ_m。

从车体坐标系b到导航坐标系n的坐标转换矩阵即为水平基准，由于SINS对准误差以及数学水平基准核心部件陀螺仪漂移导致的系统导航误差是随工作时间增加而积累的，惯性数学水平基准误差是振荡和发散的，其姿态误差随时间增大，因此未经修正的SINS姿态矩阵不能用作数学水平基准。根据姿态矩阵与姿态角的关系，SINS的捷联矩阵与理想转换矩阵之间的姿态误差可由平台失准角(φ_E φ_N φ_U)表示为：

采用大视场星敏感器辅助修正SINS姿态矩阵的方法通过估计平台失准角，修正捷联矩阵从而建立了数学水平基准而把地理坐标系的转动角速率与水平基准相结合，使得水平基准可实时跟踪车体所在点的水平面。如式(2)所示：

${\hat{C}}_b^n=C_n^{n1}\·C_b^n=\left[\begin{array}{ccc}1& {\mathrm{\φ}}_U& {-\mathrm{\φ}}_N\\ {-\mathrm{\φ}}_U& 1& {\mathrm{\φ}}_E\\ {\mathrm{\φ}}_N& {-\mathrm{\φ}}_E& 1\end{array}\right]\·C_b^n---\left(2\right)$

由捷联惯导解算的位置、姿态等信息，可得到火星惯性坐标系到火星车车体坐标系的方向余弦矩阵结合多矢量定姿模块获得的惯性姿态方向余弦矩阵可估计出惯导的平台失准角及陀螺漂移，并进行校正以得到高精度的数学水平基准。示意图如图4，具体过程如下：

根据当前导航时间t与火星自转角速度ω_im能够得到从火星惯性坐标系I转换到火星星固坐标系m的方向余弦矩阵结合SINS的捷联矩阵和位置矩阵有

${\hat{C}}_I^b={\hat{C}}_n^b{\hat{C}}_m^n{C}_I^m={\left({\hat{C}}_b^n\right)}^T{\hat{C}}_m^n{C}_I^m---\left(3\right)$

由惯性器件加速度计输出的比力以及SINS解算所获得的捷联矩阵，通过二次积分，可解算出车体的位置λ_I、L_I，再根据位置矩阵与经纬度的关系，可得到位置矩阵

$\begin{array}{c}{\hat{C}}_m^n=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos (\mathrm{\π}/2-L_I)& \sin (\mathrm{\π}/2-L_I)\\ 0& -\sin (\mathrm{\π}/2-L_I)& \cos (\mathrm{\π}/2-L_I)\end{array}\right]\·\left[\begin{array}{ccc}\cos (\mathrm{\π}/2+{\mathrm{\λ}}_I)& \sin (\mathrm{\π}/2+{\mathrm{\λ}}_I)& 0\\ \sin (\mathrm{\π}/2+{\mathrm{\λ}}_i)& \cos (\mathrm{\π}/2+{\mathrm{\λ}}_I)& 0\\ 0& 0& 1\end{array}\right]\\ =\left[\begin{array}{ccc}1& 0& 0\\ 0& \sin L_i& \cos L_I\\ 0& -\cos L_I& \sin L_I\end{array}\right]\·\left[\begin{array}{ccc}-\sin {\mathrm{\λ}}_I& \cos {\mathrm{\λ}}_I& 0\\ -\cos {\mathrm{\λ}}_I& -\sin {\mathrm{\λ}}_I& 0\\ 0& 0& 1\end{array}\right]\end{array}---\left(4\right)$

设SINS解算存在的位置误差为δλ、δL，因此SINS的位置矩阵与真实的位置矩阵的关系如下：

${\hat{C}}_m^n=(I-[\mathrm{\δP}\left]\right)\·C_m^n=\left[\begin{array}{ccc}1& \mathrm{\δ\λ}\·\sin L& \mathrm{\δ\λ}\·\cos L\\ -\mathrm{\δ\λ}\·\sin L& 1& -\mathrm{\δL}\\ -\mathrm{\δ\λ}\·\cos L& \mathrm{\δL}& 1\end{array}\right]\·C_m^n---\left(5\right)$

最后，通过将SINS计算得到的变换矩阵与星敏感器输出的变换矩阵相结合，通过组合导航滤波器，获得失准角及陀螺漂移的估计值然后通过反馈该估计值到捷联惯导子系统，对SINS数学平台失准角和陀螺仪漂移进行修正，即可获得高精度的数学水平基准 ${\tilde{C}}_b^n.$

步骤二：基于高精度水平基准的天文位置矢量确定；

由于导航坐标系OX_nY_nZ_n(东北天)的Z_n轴指示当地地垂线，可将Z_n轴在火星惯性坐标系I的位置矢量作为当地位置矢量。并且约定，表示由坐标系a转换到坐标系b的方向余弦矩阵；为坐标系a的Z轴的单位矢量在坐标系b下的投影。

车体坐标系b中Z_b轴的单位矢量在车体系b下的坐标为根据四元数表示的向量旋转：向量绕着车体系b中Y_b轴的负方向旋转γ(滚转角)，得到再绕着b₁系的X_b1轴的负方向旋转θ(俯仰角)，以及四元数向量旋转与方向余弦矩阵的关系可知，方向余弦矩阵也可实现向量的旋转。经过方向余弦矩阵将矢量旋转为地理系的Z轴单位向量在车体系下的投影后，即可得到地理系n中铅垂线方向Z_n的单位矢量在车体系b中的坐标为：

新型大视场星敏感器的出现，使得同时观测三颗及三颗以上的导航星成为可能，在不需要任何外部基准信息的前提下，提供精确的车体相对惯性空间的姿态信息，且精度在全过程保持稳定。如果星敏感器同时观测三颗及以上的导航星，并假设被观测的导航星数目为n，惯性空间中的单位星光矢量为参考矢量G₁,G₂,…,G_n，星敏感器坐标系下的单位星光矢量为观测矢量S₁,S₂,…,S_n。将参考矢量和观测矢量写为两个向量矩阵：

S＝[S₁ S₂ … S_n] G＝[G₁ G₂ … G_n]

方向余弦为本体坐标系到地心惯性系的转换，因此，满足以下关系：

$S={\mathrm{GC}}_b^I$

并通过最小二乘法得到最优解

根据惯性空间姿态矩阵，即可得到位置矢量在火星惯性系下的投影为

${\tilde{Z}}_n^I={\left({\tilde{C}}_I^b\right)}^T\·{\tilde{Z}}_n^b---\left(7\right)$

由于构造量测量所使用的是SINS提供的水平基准，其中包含平台失准角，即：

因此，令车体的真实地理位置为(λ L)，则可以得到真实的火星惯性坐标系下位置矢量与天文导航系统(即CNS)解算的同一坐标系下的位置矢量的关系为：

其中t_GHA为火星的格林尼治时角，以弧度为单位；λ_c,L_c分别为天文解算出的火星车经纬度；α_c,δ_c分别为天文导航系统解算的火星车的赤经和赤纬。而SINS解算的地理位置为(λ_I L_I)，则将其转换为火星惯性坐标系下车体的位置矢量为：

${\hat{Z}}_n^I=\left[\begin{array}{c}\cos {\mathrm{\α}}_I\·\cos {\mathrm{\δ}}_I\\ \sin {\mathrm{\α}}_I\·\cos {\mathrm{\δ}}_I\\ \sin {\mathrm{\δ}}_I\end{array}\right]=\left[\begin{array}{c}\cos L_I\·\cos ({\mathrm{\λ}}_I+t_{\mathrm{GHA}})\\ \cos L_I\·\sin ({\mathrm{\λ}}_I+t_{\mathrm{GHA}})\\ \sin L_I\end{array}\right]---\left(10\right)$

其中α_I,δ_I为惯导解算的火星车的赤经和赤纬。因此，SINS解算的地理位置(λ_IL_I)与车体真实的地理位置(λ L)之间的关系可以表示为：

$\begin{array}{c}{\mathrm{\λ}}_I=\mathrm{\λ}+\mathrm{\δ\λ}\\ L_I=L+\mathrm{\δL}\end{array}---\left(11\right)$

将上述位置误差作为小量，并将其表达式带入中，忽略高阶小量可得：

$\begin{array}{c}{\hat{Z}}_n^I=\left[\begin{array}{c}\cos (L+\mathrm{\δL})\·\cos (\mathrm{\λ}+\mathrm{\δ\λ}+t_{\mathrm{GHA}})\\ \cos (L+\mathrm{\δL})\·\sin (\mathrm{\λ}+\mathrm{\δ\λ}+t_{\mathrm{GHA}})\\ \sin (L+\mathrm{\δL})\end{array}\right]\\ =\left[\begin{array}{c}\cos L\·\cos (\mathrm{\λ}+t_{\mathrm{GHA}})\\ \cos L\·\sin (\mathrm{\λ}+t_{\mathrm{GHA}})\\ \sin L\end{array}\right]+\left[\begin{array}{c}\sin L\·\cos (\mathrm{\λ}+t_{\mathrm{GHA}})\\ \sin L\·\sin (\mathrm{\λ}+t_{\mathrm{GHA}})\\ \cos L\end{array}\right]\·\mathrm{\δL}+\left[\begin{array}{c}\cos L\·\sin (\mathrm{\λ}+t_{\mathrm{GHA}})\\ \cos L\·\cos (\mathrm{\λ}+t_{\mathrm{GHA}})\\ 0\end{array}\right]\·\mathrm{\δ\λ}\end{array}---\left(12\right)$

综上，根据高精度的水平基准以及星敏感器量测信息，可获得天文位置矢量。

步骤三：建立组合导航系统的状态模型和量测模型；

SINS的误差状态包括姿态误差角(φ_E,φ_N,φ_U)、位置误差(δL,δλ,δh)、速度误差(δV_E,δV_N,δV_U)、陀螺仪常值漂移(ε_bx,ε_by,ε_bz)、加速度计零偏SINS系统误差状态方程为

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

其中， $X={[{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\δV}}_E,{\mathrm{\δV}}_N,{\mathrm{\δV}}_U,\mathrm{\δL},\mathrm{\δ\λ},\mathrm{\δh},{\mathrm{\ϵ}}_{\mathrm{by}},{\mathrm{\ϵ}}_{\mathrm{by}},{\▿}_{\mathrm{bx}},{\▿}_{\mathrm{by}},{\▿}_{\mathrm{bz}}]}^T$ 为状态变量；F为系统状态矩阵，W＝[ω_gx,ω_gy,ω_gz,ω_dx,ω_dy,ω_dz]^T为系统噪声序列，ω_gi(i＝x,y,z)、ω_di(i＝x,y,z)分别为陀螺仪、加速度计随机白噪声，为捷联矩阵，G为噪声矩阵。F、F_S和G分别为

其中，F_N为对应于SINS的9个参数(3个姿态误差、3个速度误差、3个位置误差)的系统动态矩阵。

由式(1)到式(3)，可得到由惯导解算单元的输出构造出的火星惯性坐标系到车体坐标系的方向余弦变换矩阵进而，将平台失准角及位置误差看作小量，且忽略高阶项，则由惯导姿态量测信息构造单元4得到的惯性姿态矩阵经整理得：

其中位置误差矩阵为：

$\mathrm{\δP}=\left[\begin{array}{ccc}1& \mathrm{\δ\λ}\·\sin L& \mathrm{\δ\λ}\·\cos L\\ -\mathrm{\δ\λ}\·\sin L& 1& -\mathrm{\δL}\\ -\mathrm{\δ\λ}\·\cos L& \mathrm{\δL}& 1\end{array}\right]$

星敏感器的误差主要有量测误差和安装误差，其中量测误差为角秒级的，而安装误差为角分级的，但安装误差可以通过标定进行严格校正，由于经过安装误差标定后的星敏感器测量的姿态精度较高，且其测量误差可以考虑为零均值的白噪声，所以星敏感器输出的变换矩阵可以认为是真实的变换矩阵与星敏感器的量测白噪声阵V_3×3之和，即可表示如式(16)。

${\tilde{C}}_I^b=C_I^b+V_{3\×3}---\left(16\right)$

因此，将惯导姿态量测构造单元5、多矢量定姿模块202的输出分别确定的惯性姿态矩阵的差值记作姿态量测量Z_s，有

并将Z_s(3×3)展开成列向量Z_1(9×1)，结合组合系统的状态向量X，可列写出量测方程，

Z₁＝Η₁X+V₁ (18)

其中，H₁＝[H_M 0_9×3 H_N 0_9×6]为姿态量测矩阵，V₁为星敏感器的量测白噪声序列。

$M=C_n^b{C}_n^e{C}_e^I=\left[\begin{array}{ccc}{M}_1& M_2& M_3\\ M_4& M_4& M_6\\ M_7& M_8& M_9\end{array}\right]$

$H_M={\left[\begin{array}{ccccccccc}0& M_3& -M_2& 0& M_6& -M_5& 0& M_9& -M_8\\ -M_3& 0& M_1& -M_6& 0& M_4& -M_9& 0& M_7\\ M_2& -M_1& 0& M_5& -M_4& 0& M_8& -M_7& 0\end{array}\right]}^T$

$H_N=H_M\·\left[\begin{array}{ccc}-1& 0& 0\\ 0& \cos L& 0\\ 0& \sin L& 0\end{array}\right]$

将惯性位置量测构造单元4、天文位置矢量构造单元203输出的垂线矢量信息的差值作为位置量测量，并结合状态向量X，可得量测方程为：

Z₂＝H₂X-V₂ (19)

式中，H₂＝[H₂₁ 0_3×3 H₂₂ 0_3×7]为垂线矢量量测矩阵，V₂为CNS垂线矢量误差，

$H_{21}=\left[\begin{array}{ccc}0& \sin L& \cos L\·\sin (\mathrm{\λ}+t_{\mathrm{GHA}})\\ \sin L& 0& -\cos L\·\cos (\mathrm{\λ}+t_{\mathrm{GHA}})\\ -\cos L\·\sin (\mathrm{\λ}+t_{\mathrm{GHA}})& \cos L\·\cos (\mathrm{\λ}+t_{\mathrm{GHA}})& 0\end{array}\right]---\left(20\right)$

$H_{22}=\left[\begin{array}{cc}\cos (\mathrm{\λ}+t_{\mathrm{GHA}})\·\sin L& \cos L\·\sin (\mathrm{\λ}+t_{\mathrm{GHA}})\\ \sin (\mathrm{\λ}+t_{\mathrm{GHA}})\·\sin L& \cos L\·\cos (\mathrm{\λ}+t_{\mathrm{GHA}})\\ \cos L& 0\end{array}\right]---\left(21\right)$

步骤四：组合导航系统信息融合；

由惯导位置量测信息构造单元3和天文位置矢量构造单元203提供的垂线矢量和惯导姿态量测信息构造单元4和多矢量定姿模块202提供的姿态信息分别作差后作为量测信息，通过组合导航滤波器5进行信息融合、可以估计出平台失准角、惯性测量元件101中的陀螺仪的漂移、加速度计零偏以及捷联惯导子系统1的位置误差、速度误差。

步骤五：通过信息反馈校正各子系统误差；

根据姿态矩阵与姿态角的关系，组合导航系统的姿态角误差直接体现了大视场星敏感器201辅助捷联惯导子系统1所得到的水平基准的精度。由于传统深组合模式仅能估计并校正陀螺仪漂移和平台失准角等姿态相关的误差，而未能校正加速度计零偏，导致位置误差发散，并且位置误差的发散会导致姿态误差的发散，从而导致由SINS提供的数学地平存在较大的误差，通过基于水平基准的天文位置矢量构造单元203校正惯性测量元件中的加速度计零偏以及捷联惯导子系统1的位置误差，消除了定位误差随时间发散的趋势，从而抑制了姿态误差的发散。

然后通过高精度大视场星敏感器201观测，并由多矢量定姿模块202输出的惯性姿态信息连续辅助并修正捷联惯导子系统1，使捷联惯导子系统1的姿态误差收敛到了一定范围内，提高了其姿态精度，从而为天文导航子系统2提供了高精度的水平基准；与此同时，结合高精度水平基准以及大视场星敏感器201观测信息，天文位置矢量构造单元可获得更为精确的当地位置矢量，从而引入天文导航子系统2位置矢量量测信息以估计捷联惯导子系统1的加速度计误差、位置误差，并进行反馈校正，进一步提高数学水平基准的精度及基于数学水平基准的CNS位置矢量的精度。

最终，通过基于数学水平基准的CNS位置矢量校正SINS的加速度计零偏以及位置误差，消除了定姿和定位误差随时间发散的趋势，使系统取得长时间定姿和定位精度，并且姿态和位置误差始终保持稳定。

其中，在步骤一中所述的“采用大视场星敏感器辅助修正SINS姿态矩阵的方法通过估计平台失准角，修正捷联矩阵从而建立了数学水平基准是利用大视场星敏感器提供的多个星光矢量信息，并由多矢量定姿模块处理得到高精度的惯性姿态矩阵，结合惯导姿态量测信息构造单元提供的惯导惯性姿态信息，两者作差即可构造姿态量测量，通过组合导航滤波器进行信息融合，估计出平台失准角，对捷联矩阵进行修正，最后建立数学水平基准。

其中，在步骤一中所述的“由捷联惯导解算的位置、姿态等信息，可得到火星惯性坐标系到火星车车体坐标系的方向余弦矩阵结合多矢量定姿模块获得的惯性姿态方向余弦矩阵可估计出惯导的平台失准角及陀螺漂移，并进行校正以得到高精度的数学水平基准”，即利用多矢量定姿模块提供的天文惯性姿态矩阵以及惯导姿态量测信息构造单元提供的捷联惯导惯性姿态矩阵分别将矩阵与矩阵的每个元素对应相减，得到九个观测量，建立这些观测量与状态量的关系，得到量测方程，通过组合导航滤波器进行信息融合，可估计出平台失准角与陀螺漂移，在捷联矩阵的基础上，经过平台失准角修正，即可得到高精度的数学水平基准并且通过修正陀螺仪漂移抑制了姿态误差的发散。

其中，在步骤一中所述的“由惯性器件加速度计输出的比力以及SINS解算所获得的捷联矩阵”，由于惯性器件加速度计输出的比力是在车体系下的，而惯导基本方程中需要的比力是在导航坐标系下的，因此需要车体系到导航系的方向余弦矩阵，此处用SINS解算的捷联矩阵代替，从而可根据惯导基本方程计算速度的增量。

其中，在步骤一中所述的“通过二次积分，可解算出车体的位置λ_I、L_I”，对前述所得到的速度的增量积分，并与上一时刻的速度矢量相加即为这一时刻的速度矢量，对这一时刻的速度矢量进行积分并与上一时刻的位置信息相加，即可得到这一时刻的位置信息λ_I、L_I。

其中，在步骤一中所述的“再根据位置矩阵与经纬度的关系，可得到位置矩阵所谓位置矩阵，就是火星星固坐标系到导航坐标系之间的方向余弦矩阵，由于导航坐标系与每一时刻的火星车位置有关，因此，位置矩阵也与经纬度有关，它们之间的关系由公式(4)给出。

其中，在步骤一中所述的“通过将SINS计算得到的变换矩阵与星敏感器输出的变换矩阵相结合，通过组合导航滤波器，获得失准角及陀螺漂移的估计值然后通过反馈该估计值到捷联惯导子系统，对SINS数学平台失准角和陀螺仪漂移进行修正，即可获得高精度的数学水平基准通过将惯导姿态量测信息构造单元输出的惯导惯性姿态与多矢量定姿模块提供的天文惯性姿态相结合，每个对应位置的元素作差，差值作为一个姿态量测量，并建立每个量测量与状态量的关系，建立量测方程，经过卡尔曼滤波进行信息融合，滤波结果能够估计出平台失准角和陀螺漂移，然后分别将估计信息反馈到惯导解算单元和惯性测量元件，修正SINS的捷联矩阵的误差和惯性测量元件陀螺仪漂移，实时修正了捷联矩阵的姿态误差，并从根本上抑制了误差的产生，因此可获得高精度的数学水平基准。

其中，在步骤二中所述的“经过方向余弦矩阵将矢量旋转为地理系的Z轴单位向量在车体系下的投影后，即可得到地理系n中铅垂线方向Z_n的单位矢量在车体系b中的坐标这是通过四元数对刚体的转动进行描述的，需要两次转动：向量绕着车体系b中Y_b轴的负方向旋转γ(滚转角)，得到再绕着b₁系的X_b1轴的负方向旋转θ(俯仰角)，得到即可达到要求。水平基准方向余弦矩阵将一个矢量的投影从车体系转到导航系至少需要转动三次，最后一次转动是绕Z_n轴旋转ψ(偏航角)。因此，对于轴来说，通过四元数转动二次与通过水平基准方向余弦矩阵转动三次的结果是相同的，它们都能得到在实际计算时，由于该方向余弦矩阵是已知的，而姿态四元数未知，因此，是用方向余弦矩阵代替姿态四元数进行矢量的转动的。

其中，在步骤二中所述的“令车体的真实地理位置为(λ L)，则可以得到真实的火星惯性坐标系下位置矢量与天文导航系统即CNS解算的同一坐标系下的位置矢量I的关系”，在CNS解算位置矢量的过程当中，由于引入了一定的姿态误差，因此，CNS解算的位置矢量与真实的火星惯性坐标系下的位置矢量之间有差别，这种差别可以通过平台误差角表示出来，如公式(9)所示，为后续组合导航滤波建立姿态量测方程提供依据。

其中，在步骤三中所述的“将平台失准角及位置误差看作小量，且忽略高阶项，则由惯导姿态量测信息构造单元4得到的惯性姿态矩阵惯导姿态量测信息构造单元是通过将三个方向余弦矩阵连乘得到的，在计算姿态矩阵时引入了姿态误差，同样由于惯导的导航信息存在误差，在构造位置矩阵时，又引入了位置误差，这些误差可以分别由与δP表示，而与δP均属于小量，经过连乘后，会出现高阶误差项，在此处将该高阶项忽略。因此通过考虑误差后，将三个方向余弦矩阵连乘，就能够得到惯导姿态量测信息构造的惯性姿态矩阵。

其中，在步骤三中所述的“将惯导姿态量测构造单元5、多矢量定姿模块202的输出分别确定的惯性姿态矩阵的差值记作姿态量测量Z_s”，由于公式(15)与(16)分别表示了天文导航和惯导提供的惯性姿态矩阵与状态量之间的关系，因此很容易建立姿态量测量与状态量的关系，即确定姿态量测方程。

其中，在步骤三中所述的“将惯性位置量测构造单元4、天文位置矢量构造单元203输出的垂线矢量信息的差值作为位置量测量，并结合状态向量X，可得量测方程”，状态向量X中包括位置误差、平台角误差等，在公式(9)与(12)中说明了两个位置矢量 分别与X中的元素之间的关系，由此可以建立位置量测量与状态量的关系，即建立量测方程。

其中，在步骤四中所述的“通过组合导航滤波器5进行信息融合、可以估计出平台失准角、惯性测量元件101中的陀螺仪的漂移、加速度计零偏以及捷联惯导子系统1的位置误差、速度误差”，该组合导航滤波器为卡尔曼滤波器，在经过前面部分对系统状态方程和量测方程的建立以及相关滤波参数、滤波初值的设定后，即可进行信息融合并对相关状态量进行估计，当滤波达到稳定时，所估计的状态量的值将趋近于其真实值，用于反馈校正惯性器件加速度计的零偏和陀螺仪的漂移以及姿态、位置、速度误差。

其中，在步骤五中所述的“通过基于水平基准的天文位置矢量构造单元203校正惯性测量元件中的加速度计零偏以及捷联惯导子系统1的位置误差，消除了定位误差随时间发散的趋势，从而抑制了姿态误差的发散”，由于车体的位置误差与姿态误差是相互影响的，因此，通过引入天文位置矢量并结合惯导提供的位置矢量，可以估计并校正车体的位置误差以及惯性测量元件加速度计的零偏，从根本上解决位置误差的发散导致姿态误差发散的位置，因此，提高定位精度，在另一方面也为抑制姿态误差的发散做出了一定的贡献。

其中，在步骤五中所述的“通过基于数学水平基准的CNS位置矢量校正SINS的加速度计零偏以及位置误差，消除了定姿和定位误差随时间发散的趋势，使系统取得长时间定姿和定位精度，并且姿态和位置误差始终保持稳定”，惯导和天文提供的惯性姿态矩阵可以解决估计并校正姿态误差和陀螺仪漂移，从而达到高精度姿态，但由于在惯导解算中位置和姿态的交联关系，如果位置误差发散，必将导致姿态误差的发散，因此，通过基于数学水平基准的CAN位置矢量与惯导位置矢量的信息融合，去校正SINS的加速度计零偏以及位置误差，在消除定位误差随时间发散的趋势的基础上，也消除了定姿误差随时间的发散，因此，位姿误差始终保持稳定。

本发明一种火星车的SINS/CNS深组合导航系统及其实现方法，其优点在于：

(1)本发明通过利用大视场星敏感器201的高精度观测信息以及经过多矢量定姿模块202处理后所输出的高精度惯性姿态信息，可以辅助捷联惯导子系统1，从而得到高精度的数学水平基准信息。

(2)本发明通过利用高精度的数学水平基准信息，构造天文位置矢量信息，从而得到高精度的天文位置信息，进一步提高了天文定位精度。

(3)本发明利用卡尔曼滤波器对天文导航子系统2和捷联惯导子系统1的姿态和位置矢量信息进行信息融合，对平台失准角、惯性测量元件101的零偏以及载体位置误差进行最优估计，通过对导航误差的反馈校正，提高了数学水平基准和载体的定位精度，能够满足高精度的自主导航的要求。

(4)本发明通过在组合导航的过程中捷联惯导子系统与天文导航子系统的相互辅助、取长补短，充分发挥了捷联惯导子系统和天文导航子系统各自的优势，并且该方法还可以应用于其他巡视探测器，具有广阔的应用前景。

四、附图说明

图1为本发明所述的导航系统结构示意图；

图2为本发明所述的导航系统实现方法流程图；

图3为本发明所述的捷联惯导解算流程图；

图4为本发明高精度数学平台确定示意图；

图1中：

1—捷联惯导子系统 2—天文导航子系统 3—惯导位置量测信息构造单元

4—惯导姿态量测信息构造单元 5—组合导航滤波器

101—惯性测量元件 102—惯导解算单元

201—大视场星敏感器 202—多矢量定姿模块

203—天文位置矢量构造单元

—天文导航子系统所确定的惯性姿态矩阵 —高精度数学水平基准

—捷联惯导子系统所确定的惯性姿态矩阵

λ_I L_I—惯导解算的载体经纬度

—捷联惯导子系统所确定的位置矢量

—天文导航子系统所确定的位置矢量

图4中：

—天文导航子系统所确定的惯性姿态矩阵 —惯导的姿态矩阵

—捷联惯导子系统所确定的惯性姿态矩阵 —惯导的位置矩阵

—平台失准角误差 —高精度数学水平基准

—比力信息 —角速度信息 —陀螺仪漂移

—比力在导航系下的投影 —修正后的角速度信息 —姿态速率

—位置速率和地球速率之和在载体坐标系下的投影

五、具体实施方式

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

本发明一种火星车的SINS/CNS深组合导航系统及其实现方法。通过利用大视场星敏感器的量测信息，辅助捷联惯导系统的修正其姿态矩阵，得到高精度的数学水平基准在此基础上，利用高精度的数学水平基准以及天文导航提供的惯性姿态信息通过确定天文位置矢量实现天文定位。再通过惯导姿态量测信息构造单元和惯导位置量测信息构造单元分别得到的惯导惯性姿态矩阵和惯导位置矢量最后，结合捷联惯导系统的误差模型，利用组合导航滤波器估计并反馈校正系统误差，从而实现高精度的惯性/天文组合导航。

见图1，本发明一种适用于火星车的SINS/CNS深组合导航系统，包括捷联惯导子系统1、天文导航子系统2、惯导位置量测信息构造单元3、惯导姿态量测信息构造单元4和组合导航滤波器5；

捷联惯导子系统1包括惯性测量元件101和惯导解算单元102；惯性测量元件101测得火星车相对于惯性空间的角速度和比力，并将其输出到惯导解算单元102；惯导解算单元102根据惯性测量元件101的信息，结合惯导的力学编排算法实时计算出火星车的位置(λ_IL_I)和姿态信息；

天文导航子系统2包括大视场星敏感器201、多矢量定姿模块202和天文位置矢量构造单元203；大视场星敏感器201同时观测三颗及三颗以上恒星的星光得到星光矢量信息，并将该信息提供给多矢量定姿模块202；多矢量定姿模块202对接收到的星光矢量信息进行处理，得到火星车相对于惯性空间的姿态矩阵天文位置矢量构造单元203结合高精度数学水平基准和多矢量定姿模块202提供的惯性姿态，得到天文位置矢量；

惯导位置量测信息构造单元3根据惯导解算单元102提供的火星车经纬度信息，能够构造出惯导提供的位置矢量；

惯导姿态量测信息构造单元4根据惯导阶段单元102提供的火星车姿态矩阵和位置矩阵，能够构造出惯导提供的火星车相对于惯性空间的惯性姿态矩阵；

组合导航滤波器5结合捷联惯导系统的误差模型，以多矢量定姿模块202输出的天文惯性姿态信息惯导姿态量测信息构造单元4构造的惯导惯性姿态信息以及天文位置矢量构造单元203构造的天文位置矢量惯导位置量测信息构造单元3构造的惯导位置矢量两两作差作为量测量，建立量测方程，进行组合导航信息融合，得到系统误差信息并通过反馈校正进行补偿；

见图2，本发明一种火星车的SINS/CNS深组合导航系统的实现方法，具体包括以下步骤：

步骤一：大视场星敏感器辅助捷联惯导系统获得高精度水平基准；

为了方便以下讨论，首先定义几个常用的坐标系：导航坐标系OX_nY_nZ_n(东北天)、车体坐标系OX_bY_bZ_b(右前上)、火星惯性坐标系OX_IY_IZ_I、火星星固坐标系OX_mY_mZ_m。

从车体坐标系b到导航坐标系n的坐标转换矩阵即为水平基准，由于SINS对准误差以及数学水平基准核心部件陀螺仪漂移导致的系统导航误差是随工作时间增加而积累的，惯性数学水平基准误差是振荡和发散的，其姿态误差随时间增大，因此未经修正的SINS姿态矩阵不能用作数学水平基准。根据姿态矩阵与姿态角的关系，SINS的捷联矩阵与理想转换矩阵之间的姿态误差可由平台失准角(φ_E φ_N φ_U)表示为：

采用大视场星敏感器辅助修正SINS姿态矩阵的方法通过估计平台失准角，修正捷联矩阵从而建立了数学水平基准而把地理坐标系的转动角速率与水平基准相结合，使得水平基准可实时跟踪车体所在点的水平面。如式(2)所示：

${\hat{C}}_b^n=C_n^{n1}\·C_b^n=\left[\begin{array}{ccc}1& {\mathrm{\φ}}_U& {-\mathrm{\φ}}_N\\ {-\mathrm{\φ}}_U& 1& {\mathrm{\φ}}_E\\ {\mathrm{\φ}}_N& {-\mathrm{\φ}}_E& 1\end{array}\right]\·C_b^n---\left(2\right)$

由捷联惯导解算的位置、姿态等信息，可得到火星惯性坐标系到火星车车体坐标系的方向余弦矩阵结合多矢量定姿模块获得的惯性姿态方向余弦矩阵可估计出惯导的平台失准角及陀螺漂移，并进行校正以得到高精度的数学水平基准。示意图如图4，具体过程如下：

根据当前导航时间t与火星自转角速度ω_im能够得到从火星惯性坐标系I转换到火星星固坐标系m的方向余弦矩阵结合SINS的捷联矩阵和位置矩阵有

${\hat{C}}_I^b={\hat{C}}_n^b{\hat{C}}_m^n{C}_I^m={\left({\hat{C}}_b^n\right)}^T{\hat{C}}_m^n{C}_I^m---\left(3\right)$

由惯性器件加速度计输出的比力以及SINS解算所获得的捷联矩阵，通过二次积分，可解算出车体的位置λ_I、L_I，再根据位置矩阵与经纬度的关系，可得到位置矩阵

$\begin{array}{c}{\hat{C}}_m^n=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos (\mathrm{\π}/2-L_I)& \sin (\mathrm{\π}/2-L_I)\\ 0& -\sin (\mathrm{\π}/2-L_I)& \cos (\mathrm{\π}/2-L_I)\end{array}\right]\·\left[\begin{array}{ccc}\cos (\mathrm{\π}/2+{\mathrm{\λ}}_I)& \sin (\mathrm{\π}/2+{\mathrm{\λ}}_I)& 0\\ \sin (\mathrm{\π}/2+{\mathrm{\λ}}_i)& \cos (\mathrm{\π}/2+{\mathrm{\λ}}_I)& 0\\ 0& 0& 1\end{array}\right]\\ =\left[\begin{array}{ccc}1& 0& 0\\ 0& \sin L_i& \cos L_I\\ 0& -\cos L_I& \sin L_I\end{array}\right]\·\left[\begin{array}{ccc}-\sin {\mathrm{\λ}}_I& \cos {\mathrm{\λ}}_I& 0\\ -\cos {\mathrm{\λ}}_I& -\sin {\mathrm{\λ}}_I& 0\\ 0& 0& 1\end{array}\right]\end{array}---\left(4\right)$

设SINS解算存在的位置误差为δλ、δL，因此SINS的位置矩阵与真实的位置矩阵的关系如下：

${\hat{C}}_m^n=(I-[\mathrm{\δP}\left]\right)\·C_m^n=\left[\begin{array}{ccc}1& \mathrm{\δ\λ}\·\sin L& \mathrm{\δ\λ}\·\cos L\\ -\mathrm{\δ\λ}\·\sin L& 1& -\mathrm{\δL}\\ -\mathrm{\δ\λ}\·\cos L& \mathrm{\δL}& 1\end{array}\right]\·C_m^n---\left(5\right)$

最后，通过将SINS计算得到的变换矩阵与星敏感器输出的变换矩阵相结合，通过组合导航滤波器，获得失准角及陀螺漂移的估计值然后通过反馈该估计值到捷联惯导子系统，对SINS数学平台失准角和陀螺仪漂移进行修正，即可获得高精度的数学水平基准 ${\tilde{C}}_b^n.$

步骤二：基于高精度水平基准的天文位置矢量确定；

由于导航坐标系OX_nY_nZ_n(东北天)的Z_n轴指示当地地垂线，可将Z_n轴在火星惯性坐标系I的位置矢量作为当地位置矢量。并且约定，表示由坐标系a转换到坐标系b的方向余弦矩阵；为坐标系a的Z轴的单位矢量在坐标系b下的投影。

车体坐标系b中Z_b轴的单位矢量在车体系b下的坐标为 $Z_b^b={\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T,$ 根据四元数表示的向量旋转：向量绕着车体系b中Y_b轴的负方向旋转γ(滚转角)，得到再绕着b₁系的X_b1轴的负方向旋转θ(俯仰角)，以及四元数向量旋转与方向余弦矩阵的关系可知，方向余弦矩阵也可实现向量的旋转。经过方向余弦矩阵将矢量旋转为地理系的Z轴单位向量在车体系下的投影后，即可得到地理系n中铅垂线方向Z_n的单位矢量在车体系b中的坐标为：

新型大视场星敏感器的出现，使得同时观测三颗及三颗以上的导航星成为可能，在不需要任何外部基准信息的前提下，提供精确的车体相对惯性空间的姿态信息，且精度在全过程保持稳定。如果星敏感器同时观测三颗及以上的导航星，并假设被观测的导航星数目为n，惯性空间中的单位星光矢量为参考矢量G₁,G₂,…,G_n，星敏感器坐标系下的单位星光矢量为观测矢量S₁,S₂,…,S_n。将参考矢量和观测矢量写为两个向量矩阵：

S＝[S₁ S₂ … S_n] G＝[G₁ G₂ … G_n]

方向余弦为本体坐标系到地心惯性系的转换，因此，满足以下关系：

$S={\mathrm{GC}}_b^I$

并通过最小二乘法得到最优解

根据惯性空间姿态矩阵，即可得到位置矢量在火星惯性系下的投影为

${\tilde{Z}}_n^I={\left({\tilde{C}}_I^b\right)}^T\·{\tilde{Z}}_n^b---\left(7\right)$

由于构造量测量所使用的是SINS提供的水平基准，其中包含平台失准角，即：

因此，令车体的真实地理位置为(λ L)，则可以得到真实的火星惯性坐标系下位置矢量与天文导航系统(CNS)解算的同一坐标系下的位置矢量的关系为：

其中t_GHA为火星的格林尼治时角，以弧度为单位；λ_c,L_c分别为天文解算出的火星车经纬度；α_c,δ_c分别为天文导航系统解算的火星车的赤经和赤纬。而SINS解算的地理位置为(λ_IL_I)，则将其转换为火星惯性坐标系下车体的位置矢量为：

${\hat{Z}}_n^I=\left[\begin{array}{c}\cos {\mathrm{\α}}_I\·\cos {\mathrm{\δ}}_I\\ \sin {\mathrm{\α}}_I\·\cos {\mathrm{\δ}}_I\\ \sin {\mathrm{\δ}}_I\end{array}\right]=\left[\begin{array}{c}\cos L_I\·\cos ({\mathrm{\λ}}_I+t_{\mathrm{GHA}})\\ \cos L_I\·\sin ({\mathrm{\λ}}_I+t_{\mathrm{GHA}})\\ \sin L_I\end{array}\right]---\left(10\right)$

其中α_I,δ_I为惯导解算的火星车的赤经和赤纬。因此，SINS解算的地理位置(λ_I L_I)与车体真实的地理位置(λ L)之间的关系可以表示为：

$\begin{array}{c}{\mathrm{\λ}}_I=\mathrm{\λ}+\mathrm{\δ\λ}\\ L_I=L+\mathrm{\δL}\end{array}---\left(11\right)$

将上述位置误差作为小量，并将其表达式带入中，忽略高阶小量可得：

$\begin{array}{c}{\hat{Z}}_n^I=\left[\begin{array}{c}\cos (L+\mathrm{\δL})\·\cos (\mathrm{\λ}+\mathrm{\δ\λ}+t_{\mathrm{GHA}})\\ \cos (L+\mathrm{\δL})\·\sin (\mathrm{\λ}+\mathrm{\δ\λ}+t_{\mathrm{GHA}})\\ \sin (L+\mathrm{\δL})\end{array}\right]\\ =\left[\begin{array}{c}\cos L\·\cos (\mathrm{\λ}+t_{\mathrm{GHA}})\\ \cos L\·\sin (\mathrm{\λ}+t_{\mathrm{GHA}})\\ \sin L\end{array}\right]+\left[\begin{array}{c}\sin L\·\cos (\mathrm{\λ}+t_{\mathrm{GHA}})\\ \sin L\·\sin (\mathrm{\λ}+t_{\mathrm{GHA}})\\ \cos L\end{array}\right]\·\mathrm{\δL}+\left[\begin{array}{c}\cos L\·\sin (\mathrm{\λ}+t_{\mathrm{GHA}})\\ \cos L\·\cos (\mathrm{\λ}+t_{\mathrm{GHA}})\\ 0\end{array}\right]\·\mathrm{\δ\λ}\end{array}---\left(12\right)$

综上，根据高精度的水平基准以及星敏感器量测信息，可获得天文位置矢量。

步骤三：建立组合导航系统的状态模型和量测模型；

SINS的误差状态包括姿态误差角(φ_E,φ_N,φ_U)、位置误差(δL,δλ,δh)、速度误差(δV_E,δV_N,δV_U)、陀螺仪常值漂移(ε_bx,ε_by,ε_bz)、加速度计零偏SINS系统误差状态方程为

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

其中， $X={[{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\δV}}_E,{\mathrm{\δV}}_N,{\mathrm{\δV}}_U,\mathrm{\δL},\mathrm{\δ\λ},\mathrm{\δh},{\mathrm{\ϵ}}_{\mathrm{by}},{\mathrm{\ϵ}}_{\mathrm{by}},{\▿}_{\mathrm{bx}},{\▿}_{\mathrm{by}},{\▿}_{\mathrm{bz}}]}^T$ 为状态变量；F为系统状态矩阵，W＝[ω_gx,ω_gy,ω_gz,ω_dx,ω_dy,ω_dz]^T为系统噪声序列，ω_gi(i＝x,y,z)、ω_di(i＝x,y,z)分别为陀螺仪、加速度计随机白噪声，为捷联矩阵，G为噪声矩阵。F、F_S和G分别为

其中，F_N为对应于SINS的9个参数(3个姿态误差、3个速度误差、3个位置误差)的系统动态矩阵。

由式(1)到式(3)，可得到由惯导解算单元的输出构造出的火星惯性坐标系到车体坐标系的方向余弦变换矩阵进而，将平台失准角及位置误差看作小量，且忽略高阶项，则由惯导姿态量测信息构造单元4得到的惯性姿态矩阵经整理得：

其中位置误差矩阵为：

$\mathrm{\δP}=\left[\begin{array}{ccc}1& \mathrm{\δ\λ}\·\sin L& \mathrm{\δ\λ}\·\cos L\\ -\mathrm{\δ\λ}\·\sin L& 1& -\mathrm{\δL}\\ -\mathrm{\δ\λ}\·\cos L& \mathrm{\δL}& 1\end{array}\right]$

星敏感器的误差主要有量测误差和安装误差，其中量测误差为角秒级的，而安装误差为角分级的，但安装误差可以通过标定进行严格校正，由于经过安装误差标定后的星敏感器测量的姿态精度较高，且其测量误差可以考虑为零均值的白噪声，所以星敏感器输出的变换矩阵可以认为是真实的变换矩阵与星敏感器的量测白噪声阵V_3×3之和，即可表示如式(16)。

${\tilde{C}}_I^b=C_I^b+V_{3\×3}---\left(16\right)$

因此，将惯导姿态量测构造单元5、多矢量定姿模块202的输出分别确定的惯性姿态矩阵的差值记作姿态量测量Z_s，有

并将Z_s(3×3)展开成列向量Z_1(9×1)，结合组合系统的状态向量X，可列写出量测方程，

Z₁＝Η₁X+V₁ (18)

其中，H₁＝[H_M 0_9×3 H_N 0_9×6]为姿态量测矩阵，V₁为星敏感器的量测白噪声序列。

$M=C_n^b{C}_n^e{C}_e^I=\left[\begin{array}{ccc}{M}_1& M_2& M_3\\ M_4& M_4& M_6\\ M_7& M_8& M_9\end{array}\right]$

$H_M={\left[\begin{array}{ccccccccc}0& M_3& -M_2& 0& M_6& -M_5& 0& M_9& -M_8\\ -M_3& 0& M_1& -M_6& 0& M_4& -M_9& 0& M_7\\ M_2& -M_1& 0& M_5& -M_4& 0& M_8& -M_7& 0\end{array}\right]}^T$

$H_N=H_M\·\left[\begin{array}{ccc}-1& 0& 0\\ 0& \cos L& 0\\ 0& \sin L& 0\end{array}\right]$

将惯性位置量测构造单元4、天文位置矢量构造单元203输出的垂线矢量信息的差值作为位置量测量，并结合状态向量X，可得量测方程为：

Z₂＝H₂X-V₂ (19)

式中，H₂＝[H₂₁ 0_3×3 H₂₂ 0_3×7]为垂线矢量量测矩阵，V₂为CNS垂线矢量误差，

$H_{21}=\left[\begin{array}{ccc}0& \sin L& \cos L\·\sin (\mathrm{\λ}+t_{\mathrm{GHA}})\\ \sin L& 0& -\cos L\·\cos (\mathrm{\λ}+t_{\mathrm{GHA}})\\ -\cos L\·\sin (\mathrm{\λ}+t_{\mathrm{GHA}})& \cos L\·\cos (\mathrm{\λ}+t_{\mathrm{GHA}})& 0\end{array}\right]---\left(20\right)$

$H_{22}=\left[\begin{array}{cc}\cos (\mathrm{\λ}+t_{\mathrm{GHA}})\·\sin L& \cos L\·\sin (\mathrm{\λ}+t_{\mathrm{GHA}})\\ \sin (\mathrm{\λ}+t_{\mathrm{GHA}})\·\sin L& \cos L\·\cos (\mathrm{\λ}+t_{\mathrm{GHA}})\\ \cos L& 0\end{array}\right]---\left(21\right)$

步骤四：组合导航系统信息融合；

由惯导位置量测信息构造单元3和天文位置矢量构造单元203提供的垂线矢量和惯导姿态量测信息构造单元4和多矢量定姿模块202提供的姿态信息分别作差后作为量测信息，通过组合导航滤波器5进行信息融合、可以估计出平台失准角、惯性测量元件101中的陀螺仪的漂移、加速度计零偏以及捷联惯导子系统1的位置误差、速度误差。

步骤五：通过信息反馈校正各子系统误差；

根据姿态矩阵与姿态角的关系，组合导航系统的姿态角误差直接体现了大视场星敏感器201辅助捷联惯导子系统1所得到的水平基准的精度。由于传统深组合模式仅能估计并校正陀螺仪漂移和平台失准角等姿态相关的误差，而未能校正加速度计零偏，导致位置误差发散，并且位置误差的发散会导致姿态误差的发散，从而导致由SINS提供的数学地平存在较大的误差，通过基于水平基准的天文位置矢量构造单元203校正惯性测量元件中的加速度计零偏以及捷联惯导子系统1的位置误差，消除了定位误差随时间发散的趋势，从而抑制了姿态误差的发散。

然后通过高精度大视场星敏感器201观测，并由多矢量定姿模块202输出的惯性姿态信息连续辅助并修正捷联惯导子系统1，使捷联惯导子系统1的姿态误差收敛到了一定范围内，提高了其姿态精度，从而为天文导航子系统2提供了高精度的水平基准；与此同时，结合高精度水平基准以及大视场星敏感器201观测信息，天文位置矢量构造单元可获得更为精确的当地位置矢量，从而引入天文导航子系统2位置矢量量测信息以估计捷联惯导子系统1的加速度计误差、位置误差，并进行反馈校正，进一步提高数学水平基准的精度及基于数学水平基准的CNS位置矢量的精度。

最终，通过基于数学水平基准的CNS位置矢量校正SINS的加速度计零偏以及位置误差，消除了定姿和定位误差随时间发散的趋势，使系统取得长时间定姿和定位精度，并且姿态和位置误差始终保持稳定。

其中，在步骤一中所述的“采用大视场星敏感器辅助修正SINS姿态矩阵的方法通过估计平台失准角，修正捷联矩阵从而建立了数学水平基准是利用大视场星敏感器提供的多个星光矢量信息，并由多矢量定姿模块处理得到高精度的惯性姿态矩阵，结合惯导姿态量测信息构造单元提供的惯导惯性姿态信息，两者作差即可构造姿态量测量，通过组合导航滤波器进行信息融合，估计出平台失准角，对捷联矩阵进行修正，最后建立数学水平基准。

其中，在步骤一中所述的“由捷联惯导解算的位置、姿态等信息，可得到火星惯性坐标系到火星车车体坐标系的方向余弦矩阵结合多矢量定姿模块获得的惯性姿态方向余弦矩阵可估计出惯导的平台失准角及陀螺漂移，并进行校正以得到高精度的数学水平基准”，即利用多矢量定姿模块提供的天文惯性姿态矩阵以及惯导姿态量测信息构造单元提供的捷联惯导惯性姿态矩阵分别将矩阵与矩阵的每个元素对应相减，得到九个观测量，建立这些观测量与状态量的关系，得到量测方程，通过组合导航滤波器进行信息融合，可估计出平台失准角与陀螺漂移，在捷联矩阵的基础上，经过平台失准角修正，即可得到高精度的数学水平基准并且通过修正陀螺仪漂移抑制了姿态误差的发散。

其中，在步骤一中所述的“由惯性器件加速度计输出的比力以及SINS解算所获得的捷联矩阵”，由于惯性器件加速度计输出的比力是在车体系下的，而惯导基本方程中需要的比力是在导航坐标系下的，因此需要车体系到导航系的方向余弦矩阵，此处用SINS解算的捷联矩阵代替，从而可根据惯导基本方程计算速度的增量。

其中，在步骤一中所述的“通过二次积分，可解算出车体的位置λ_I、L_I”，对前述所得到的速度的增量积分，并与上一时刻的速度矢量相加即为这一时刻的速度矢量，对这一时刻的速度矢量进行积分并与上一时刻的位置信息相加，即可得到这一时刻的位置信息λ_I、L_I。

其中，在步骤一中所述的“再根据位置矩阵与经纬度的关系，可得到位置矩阵所谓位置矩阵，就是火星星固坐标系到导航坐标系之间的方向余弦矩阵，由于导航坐标系与每一时刻的火星车位置有关，因此，位置矩阵也与经纬度有关，它们之间的关系由公式(4)给出。

其中，在步骤一中所述的“通过将SINS计算得到的变换矩阵与星敏感器输出的变换矩阵相结合，通过组合导航滤波器，获得失准角及陀螺漂移的估计值然后通过反馈该估计值到捷联惯导子系统，对SINS数学平台失准角和陀螺仪漂移进行修正，即可获得高精度的数学水平基准通过将惯导姿态量测信息构造单元输出的惯导惯性姿态与多矢量定姿模块提供的天文惯性姿态相结合，每个对应位置的元素作差，差值作为一个姿态量测量，并建立每个量测量与状态量的关系，建立量测方程，经过卡尔曼滤波进行信息融合，滤波结果能够估计出平台失准角和陀螺漂移，然后分别将估计信息反馈到惯导解算单元和惯性测量元件，修正SINS的捷联矩阵的误差和惯性测量元件陀螺仪漂移，实时修正了捷联矩阵的姿态误差，并从根本上抑制了误差的产生，因此可获得高精度的数学水平基准。

其中，在步骤二中所述的“经过方向余弦矩阵将矢量旋转为地理系的Z轴单位向量在车体系下的投影后，即可得到地理系n中铅垂线方向Z_n的单位矢量在车体系b中的坐标这是通过四元数对刚体的转动进行描述的，需要两次转动：向量绕着车体系b中Y_b轴的负方向旋转γ(滚转角)，得到再绕着b₁系的X_b1轴的负方向旋转θ(俯仰角)，得到即可达到要求。水平基准方向余弦矩阵将一个矢量的投影从车体系转到导航系至少需要转动三次，最后一次转动是绕Z_n轴旋转ψ(偏航角)。因此，对于轴来说，通过四元数转动二次与通过水平基准方向余弦矩阵转动三次的结果是相同的，它们都能得到在实际计算时，由于该方向余弦矩阵是已知的，而姿态四元数未知，因此，是用方向余弦矩阵代替姿态四元数进行矢量的转动的。

其中，在步骤二中所述的“令车体的真实地理位置为(λ L)，则可以得到真实的火星惯性坐标系下位置矢量与CNS(天文导航系统)解算的同一坐标系下的位置矢量的关系”，在CNS解算位置矢量的过程当中，由于引入了一定的姿态误差，因此，CNS解算的位置矢量与真实的火星惯性坐标系下的位置矢量之间有差别，这种差别可以通过平台误差角表示出来，如公式(9)所示，为后续组合导航滤波建立姿态量测方程提供依据。

其中，在步骤三中所述的“将平台失准角及位置误差看作小量，且忽略高阶项，则由惯导姿态量测信息构造单元4得到的惯性姿态矩阵惯导姿态量测信息构造单元是通过将三个方向余弦矩阵连乘得到的，在计算姿态矩阵时引入了姿态误差，同样由于惯导的导航信息存在误差，在构造位置矩阵时，又引入了位置误差，这些误差可以分别由与δP表示，而与δP均属于小量，经过连乘后，会出现高阶误差项，在此处将该高阶项忽略。因此通过考虑误差后，将三个方向余弦矩阵连乘，就能够得到惯导姿态量测信息构造的惯性姿态矩阵。

其中，在步骤三中所述的“将惯导姿态量测构造单元5、多矢量定姿模块202的输出分别确定的惯性姿态矩阵的差值记作姿态量测量Z_s”，由于公式(15)与(16)分别表示了天文导航和惯导提供的惯性姿态矩阵与状态量之间的关系，因此很容易建立姿态量测量与状态量的关系，即确定姿态量测方程。

其中，在步骤三中所述的“将惯性位置量测构造单元4、天文位置矢量构造单元203输出的垂线矢量信息的差值作为位置量测量，并结合状态向量X，可得量测方程”，状态向量X中包括位置误差、平台角误差等，在公式(9)与(12)中说明了两个位置矢量 分别与X中的元素之间的关系，由此可以建立位置量测量与状态量的关系，即建立量测方程。

其中，在步骤四中所述的“通过组合导航滤波器5进行信息融合、可以估计出平台失准角、惯性测量元件101中的陀螺仪的漂移、加速度计零偏以及捷联惯导子系统1的位置误差、速度误差”，该组合导航滤波器为卡尔曼滤波器，在经过前面部分对系统状态方程和量测方程的建立以及相关滤波参数、滤波初值的设定后，即可进行信息融合并对相关状态量进行估计，当滤波达到稳定时，所估计的状态量的值将趋近于其真实值，用于反馈校正惯性器件加速度计的零偏和陀螺仪的漂移以及姿态、位置、速度误差。

其中，在步骤五中所述的“通过基于水平基准的天文位置矢量构造单元203校正惯性测量元件中的加速度计零偏以及捷联惯导子系统1的位置误差，消除了定位误差随时间发散的趋势，从而抑制了姿态误差的发散”，由于车体的位置误差与姿态误差是相互影响的，因此，通过引入天文位置矢量并结合惯导提供的位置矢量，可以估计并校正车体的位置误差以及惯性测量元件加速度计的零偏，从根本上解决位置误差的发散导致姿态误差发散的位置，因此，提高定位精度，在另一方面也为抑制姿态误差的发散做出了一定的贡献。

其中，在步骤五中所述的“通过基于数学水平基准的CNS位置矢量校正SINS的加速度计零偏以及位置误差，消除了定姿和定位误差随时间发散的趋势，使系统取得长时间定姿和定位精度，并且姿态和位置误差始终保持稳定”，惯导和天文提供的惯性姿态矩阵可以解决估计并校正姿态误差和陀螺仪漂移，从而达到高精度姿态，但由于在惯导解算中位置和姿态的交联关系，如果位置误差发散，必将导致姿态误差的发散，因此，通过基于数学水平基准的CAN位置矢量与惯导位置矢量的信息融合，去校正SINS的加速度计零偏以及位置误差，在消除定位误差随时间发散的趋势的基础上，也消除了定姿误差随时间的发散，因此，位姿误差始终保持稳定。

Claims (10)

1.一种火星车的SINS/CNS深组合导航系统，其特征在于：包括捷联惯导子系统、天文导航子系统、惯导位置量测信息构造单元、惯导姿态量测信息构造单元以及组合导航滤波器；它们之间的关系是：捷联惯导子系统将数学平台和位置矩阵提供给惯导姿态量测信息构造单元，同时还将经纬度信息提供给惯导位置量测信息构造单元，天文导航子系统将惯性姿态矩阵传送给组合导航滤波器，惯导姿态量测信息构造单元将其构造的惯导的惯性姿态矩阵输入到组合导航滤波器，惯导位置量测信息构造单元将其构造的位置矢量信息输入到组合导航滤波器，组合导航滤波器将分别为天文导航子系统和捷联惯导子系统提供姿态和位置估计误差；

所述捷联惯导子系统包括惯性测量元件和惯导解算单元，它们之间的关系是惯性测量元件将相对于惯性空间的角速度和比力提供给惯导解算单元，作为惯导解算单元的一个输入：惯性测量元件测量火星车相对于惯性空间的角速度和比力，将所得的角速度和比力信息传送到惯导解算单元；惯导解算单元根据载体的初始位置信息以及惯性测量组件传输的信息，通过力学编排实时解算火星车的位置和姿态矩阵；

该惯性测量元件即为一个惯组，包括三个加速度计和三个陀螺仪；在现有惯组中，选取满足要求的元件，要求陀螺仪常值漂移为0.01^°/h，随机漂移为0.01^°/h，加速度计常值漂移为50μg，随机偏置为10μg，惯性器件数据输出周期为0.01s，通过正交安装，能够敏感相对于惯性空间的角速度和比力；

该惯导解算单元是一个将惯性测量元件输出的比力和角速度信息经过一系列的计算和更新，解算出载体的实时位置、速度和姿态导航信息的算法，首先输入火星车的初始位置、速度和姿态信息，经过初始对准，并计算火星自转速率、位置速率，再根据惯性测量单元输出的角速度和比力，计算姿态速率，从而更新姿态矩阵，然后使用惯导基本方程计算速度增量，并二次积分得到经纬度信息，最后计算姿态角，如此循环，直到系统停止工作，即能实时解算出惯导提供的导航信息，为后续的组合导航提供数据支持；

所述天文导航子系统包括大视场星敏感器、多矢量定姿模块和天文位置矢量构造单元，它们之间的关系是：大视场星敏感器将其观测到的多个恒星星光矢量提供给多矢量定姿模块，作为多矢量定姿模块的输入，多矢量定姿模块将其输出的惯性姿态矩阵作为天文位置矢量构造单元的一个输入，天文位置矢量构造单元再结合高精度的数学水平基准，构造出天文位置矢量：该大视场星敏感器由CCD器件、外围采样电路、信号处理电路和光学镜头四部分组成，在现有的大视场星敏感器中，按要求选取：其视场为20°×20°，量测噪声为2″，数据输出周期为1s；它在同一时刻能够观测到三颗及以上的恒星星光矢量，并将其观测信息送入多矢量定姿模块；多矢量定姿模块处理接收到的星光矢量信息，得到火星车车体坐标系相对于惯性空间的姿态矩阵；天文位置矢量构造单元结合多矢量定姿模块得到的惯性姿态矩阵以及高精度的数学水平基准，构造出天文位置矢量；

该大视场星敏感器是一种常用的星敏感器，它具有较大的视场，能同时观测多颗恒星的星光矢量；恒星星光通过透镜投影到CCD面阵上，根据光学成像原理以及几何知识，能得到恒星星光在星敏感器坐标系下的表示，而假设星敏感器坐标系与车体坐标系完全一致，因此，能通过星敏感器所获得的信息，得到多颗恒星矢量在车体坐标系下的表示，为多矢量定姿模块提供了信息；

该多矢量定姿模块根据星敏感器的测姿原理，能得到等式S＝G·A，其中G为大视场星敏感器输出的n个星光矢量组成的维数为n×3的矩阵，S为对应于G中n个恒星在惯性空间中的坐标组成的维数为n×3的矩阵，通过采用最小二乘法求解星敏感器的惯性姿态矩阵A＝(G^TG)^-1(G^TS)；

该天文位置矢量构造单元是一个计算单元，它根据姿态四元数与刚体旋转的关系，将机体系下的轴旋转到与地理坐标系下的Z轴重合，得到矢量由于姿态四元数与姿态转换矩阵之间在该转换情况下存在预定的等价关系，上述的姿态四元数可由高精度的数学平台代替，然后，通过惯性姿态矩阵将矢量投影到火星惯性坐标系，得到位置矢量从而构造出了天文位置矢量；

所述惯导位置量测信息构造单元根据捷联惯导子系统确定的经纬度的位置信息，确定载体的位置矢量，并将其作为量测信息输入组合导航滤波器中；该惯导位置量测信息构造单元是一个计算单元，通过经纬度信息以及其与位置矢量的关系，计算出惯导位置量测信息

所述惯导姿态量测信息构造单元根据捷联惯导子系统输出的位置和姿态矩阵的导航信息，确定从火星赤道惯性坐标系到载体坐标系的惯性姿态矩阵；该惯导姿态量测信息构造单元也是一个计算单元，它计算惯导捷联矩阵的转置、位置矩阵以及从火星惯性坐标系I转换到火星星固坐标系m的方向余弦矩阵的连乘积，并将该计算结果作为惯导构造的姿态量测信息；

所述组合导航滤波器是卡尔曼滤波器，它是以SINS误差方程作为状态方程，以惯导姿态量测信息构造单元与多矢量定姿模块分别提供的惯性姿态矩阵之差作为一个观测量，同时，以惯导位置量测信息构造单元与天文位置矢量构造单元分别提供的载体位置矢量信息之差作为另一个观测量，通过组合导航滤波器得到火星车位置、姿态以及惯性测量组件误差的估计值。

2.根据权利要求1所述的一种火星车的SINS/CNS深组合导航系统的实现方法，其特征在于：具体包括以下步骤：

步骤一：大视场星敏感器辅助捷联惯导系统获得高精度水平基准；

为了方便以下讨论，首先定义几个常用的坐标系：导航坐标系n即“东北天”、车体坐标系b即“右前上”、火星惯性坐标系I、火星星固坐标系m；

从车体坐标系b到导航坐标系n的坐标转换矩阵即为水平基准，由于SINS对准误差以及数学水平基准核心部件陀螺仪漂移导致的系统导航误差是随工作时间增加而积累的，惯性数学水平基准误差是振荡和发散的，其姿态误差随时间增大，因此未经修正的SINS姿态矩阵不能用作数学水平基准；根据姿态矩阵与姿态角的关系，SINS的捷联矩阵与理想转换矩阵之间的姿态误差由平台失准角(φ_E φ_N φ_U)表示为：

采用大视场星敏感器辅助修正SINS姿态矩阵的方法通过估计平台失准角，修正捷联矩阵从而建立了数学水平基准而把地理坐标系的转动角速率与水平基准相结合，使得水平基准可实时跟踪车体所在点的水平面，如式(2)所示：

${\hat{C}}_b^n=C_n^{n1}\·C_b^n=\left[\begin{array}{ccc}1& {\mathrm{\φ}}_U& -{\mathrm{\φ}}_N\\ -{\mathrm{\φ}}_U& 1& {\mathrm{\φ}}_E\\ {\mathrm{\φ}}_N& -{\mathrm{\φ}}_E& 1\end{array}\right]\·C_b^n---\left(2\right)$

由捷联惯导解算的位置和姿态信息，得到火星惯性坐标系到火星车车体坐标系的方向余弦矩阵结合多矢量定姿模块获得的惯性姿态方向余弦矩阵能估计出惯导的平台失准角及陀螺漂移，并进行校正以得到高精度的数学水平基准,具体过程如下：

根据当前导航时间t与火星自转角速度ω_im能够得到从火星惯性坐标系I转换到火星星固坐标系m的方向余弦矩阵结合SINS的捷联矩阵和位置矩阵有

${\hat{C}}_I^b={\hat{C}}_n^b{\hat{C}}_m^n{C}_I^m={\left({\hat{C}}_b^n\right)}^T{\hat{C}}_m^n{C}_I^m---\left(3\right)$

由惯性器件加速度计输出的比力以及SINS解算所获得的捷联矩阵，通过二次积分，解算出车体的位置λ_I、L_I，再根据位置矩阵与经纬度的关系，得到位置矩阵

$\begin{array}{c}{\hat{C}}_m^n=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(\mathrm{\π}/2-L_I\right)& \sin \left(\mathrm{\π}/2-L_I\right)\\ 0& -\sin \left(\mathrm{\π}/2-L_I\right)& \cos \left(\mathrm{\π}/2-L_I\right)\end{array}\right]\·\left[\begin{array}{ccc}\cos \left(\mathrm{\π}/2+{\mathrm{\λ}}_I\right)& \sin \left(\mathrm{\π}/2+{\mathrm{\λ}}_I\right)& 0\\ \sin \left(\mathrm{\π}/2+{\mathrm{\λ}}_I\right)& \cos \left(\mathrm{\π}/2+{\mathrm{\λ}}_I\right)& 0\\ 0& 0& 1\end{array}\right]\\ =\left[\begin{array}{ccc}1& 0& 0\\ 0& \sin L_I& \cos L_I\\ 0& -\cos L_I& \sin L_I\end{array}\right]\·\left[\begin{array}{ccc}-{\mathrm{sin\λ}}_I& {\mathrm{cos\λ}}_I& 0\\ -{\mathrm{cos\λ}}_I& -{\mathrm{sin\λ}}_I& 0\\ 0& 0& 1\end{array}\right]\end{array}---\left(4\right)$

设SINS解算存在的位置误差为δλ、δL，因此SINS的位置矩阵与真实的位置矩阵的关系如下：

${\hat{C}}_m^n=(I-\[\mathrm{\δ}P\])\·C_m^n=\left[\begin{array}{ccc}1& \mathrm{\δ}\mathrm{\λ}\·\sin L& \mathrm{\δ}\mathrm{\λ}\·\cos L\\ -\mathrm{\δ}\mathrm{\λ}\·\sin L& 1& -\mathrm{\δ}L\\ -\mathrm{\δ}\mathrm{\λ}\·\cos L& \mathrm{\δ}L& 1\end{array}\right]\·C_m^n---\left(5\right)$

最后，通过将SINS计算得到的变换矩阵与星敏感器输出的变换矩阵相结合，通过组合导航滤波器，获得失准角及陀螺漂移的估计值然后通过反馈该估计值到捷联惯导子系统，对SINS数学平台失准角和陀螺仪漂移进行修正，即能获得高精度的数学水平基准

步骤二：基于高精度水平基准的天文位置矢量确定；

由于导航坐标系n即“东北天”的Z_n轴指示当地地垂线，将Z_n轴在火星惯性坐标系I的位置矢量作为当地位置矢量，并且约定，表示由坐标系a转换到坐标系b的方向余弦矩阵；为坐标系a的Z轴的单位矢量在坐标系b下的投影；

车体坐标系b中Z_b轴的单位矢量在车体坐标系b下的坐标为根据四元数表示的向量旋转：向量绕着车体坐标系b中Y_b轴的负方向旋转γ即滚转角，得到再绕着b₁系的X_b1轴的负方向旋转θ即俯仰角，以及四元数向量旋转与方向余弦矩阵的关系可知，方向余弦矩阵也能实现向量的旋转，经过方向余弦矩阵将矢量旋转为导航坐标系的Z轴单位向量在车体坐标系下的投影后，即得到导航坐标系n中铅垂线方向Z_n的单位矢量在车体坐标系b中的坐标为：

新型大视场星敏感器的出现，使得同时观测三颗及三颗以上的导航星成为可能，在不需要任何外部基准信息的前提下，提供精确的车体相对惯性空间的姿态信息，且精度在全过程保持稳定；如果星敏感器同时观测三颗及以上的导航星，并假设被观测的导航星数目为n，惯性空间中的单位星光矢量为参考矢量G₁,G₂,…,G_n，星敏感器坐标系下的单位星光矢量为观测矢量S₁,S₂,…,S_n；将参考矢量和观测矢量写为两个向量矩阵：

S＝[S₁ S₂ … S_n] G＝[G₁ G₂ … G_n]

方向余弦为车体坐标系到火星惯性坐标系的转换，因此，满足以下关系：

$S={\mathrm{GC}}_b^I$

并通过最小二乘法得到最优解

根据惯性空间姿态矩阵，即可得到位置矢量在火星惯性系下的投影为

${\tilde{Z}}_n^I={\left({\tilde{C}}_I^b\right)}^T\·{\tilde{Z}}_n^b---\left(7\right)$

由于构造量测量所使用的是SINS提供的水平基准，其中包含平台失准角，即：

因此，令车体的真实地理位置为(λL)，则得到真实的火星惯性坐标系下位置矢量与天文导航系统即CNS解算的同一坐标系下的位置矢量的关系为：

其中t_GHA为火星的格林尼治时角，以弧度为单位；λ_c,L_c分别为天文解算出的火星车经纬度；α_c,δ_c分别为天文导航系统解算的火星车的赤经和赤纬；而SINS解算的地理位置为(λ_IL_I)，则将其转换为火星惯性坐标系下车体的位置矢量为：

${\hat{Z}}_n^I=\left[\begin{array}{c}{\mathrm{cos\α}}_I\·{\mathrm{cos\δ}}_I\\ {\mathrm{sin\α}}_I\·{\mathrm{cos\δ}}_I\\ {\mathrm{sin\δ}}_I\end{array}\right]=\left[\begin{array}{c}\cos L_I\·\cos \left({\mathrm{\λ}}_I+t_{GHA}\right)\\ \cos L_I\·\sin \left({\mathrm{\λ}}_I+t_{GHA}\right)\\ \sin L_I\end{array}\right]---\left(10\right)$

其中α_I,δ_I为惯导解算的火星车的赤经和赤纬；因此，SINS解算的地理位置(λ_I L_I)与车体真实的地理位置(λ L)之间的关系可以表示为：

$\begin{array}{c}{\mathrm{\λ}}_1=\mathrm{\λ}+\mathrm{\δ}\mathrm{\λ}\\ L_1=L+\mathrm{\δ}L\end{array}---\left(11\right)$

将上述位置误差作为小量，并将其表达式带入中，忽略高阶小量即得：

$\begin{array}{c}{\hat{Z}}_n^I=\left[\begin{array}{c}\cos \left(L+\mathrm{\δ}L\right)\·\cos \left(\mathrm{\λ}+\mathrm{\δ}\mathrm{\λ}+t_{GHA}\right)\\ \cos \left(L+\mathrm{\δ}L\right)\·\sin \left(\mathrm{\λ}+\mathrm{\δ}\mathrm{\λ}+t_{GHA}\right)\\ \sin \left(L+\mathrm{\δ}L\right)\end{array}\right]\\ =\left[\begin{array}{c}\cos L\·\cos \left(\mathrm{\λ}+t_{GHA}\right)\\ \cos L\·\sin \left(\mathrm{\λ}+t_{GHA}\right)\\ \sin L\end{array}\right]+\left[\begin{array}{c}\sin L\·\cos \left(\mathrm{\λ}+t_{GHA}\right)\\ \sin L\·\sin \left(\mathrm{\λ}+t_{GHA}\right)\\ \cos L\end{array}\right]\·\mathrm{\δ}L+\left[\begin{array}{c}\cos L\·\sin \left(\mathrm{\λ}+t_{GHA}\right)\\ \cos L\·\cos \left(\mathrm{\λ}+t_{GHA}\right)\\ 0\end{array}\right]\·\mathrm{\δ}\mathrm{\λ}\end{array}---\left(12\right)$

综上，根据高精度的水平基准以及星敏感器量测信息，能获得天文位置矢量；

步骤三：建立组合导航系统的状态模型和量测模型；

SINS的误差状态包括姿态误差角(φ_E,φ_N,φ_U)、位置误差(δL,δλ,δh)、速度误差(δV_E,δV_N,δV_U)、陀螺仪常值漂移(ε_bx,ε_by,ε_bz)、加速度计零偏(▽_bx,▽_by,▽_bz)；SINS系统误差状态方程为

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

其中，X＝[φ_E,φ_N,φ_U,δV_E,δV_N,δV_U,δL,δλ,δh,ε_bx,ε_by,ε_bz,▽_bx,▽_by,▽_bz]^T为状态变量；F为系统状态矩阵，W＝[ω_gx,ω_gy,ω_gz,ω_dx,ω_dy,ω_dz]^T为系统噪声序列，ω_gi(i＝x,y,z)、ω_di(i＝x,y,z)分别为陀螺仪、加速度计随机白噪声，C_b ⁿ为捷联矩阵，G为噪声矩阵；F、F_S和G分别为

其中，F_N为对应于SINS的9个参数即3个姿态误差、3个速度误差和3个位置误差的系统动态矩阵；

由式(1)到式(3)，得到由惯导解算单元的输出构造出的火星惯性坐标系到车体坐标系的方向余弦变换矩阵进而，将平台失准角及位置误差看作小量，且忽略高阶项，则由惯导姿态量测信息构造单元得到的惯性姿态矩阵经整理得：

其中位置误差矩阵为：

$\mathrm{\δ}P=\left[\begin{array}{ccc}1& \mathrm{\δ}\mathrm{\λ}\·\sin L& \mathrm{\δ}\mathrm{\λ}\·\cos L\\ -\mathrm{\δ}\mathrm{\λ}\·\sin L& 1& -\mathrm{\δ}L\\ -\mathrm{\δ}\mathrm{\λ}\·\cos L& \mathrm{\δ}L& 1\end{array}\right]$

星敏感器的误差主要有量测误差和安装误差，其中量测误差为角秒级的，而安装误差为角分级的，但安装误差能通过标定进行严格校正，由于经过安装误差标定后的星敏感器测量的姿态精度较高，且其测量误差考虑为零均值的白噪声，所以星敏感器输出的变换矩阵认为是真实的变换矩阵与星敏感器的量测白噪声阵V_3×3之和，即表示如式(16):

${\tilde{C}}_I^b=C_I^b+V_{3\×3}---\left(16\right)$

因此，将惯导姿态量测构造单元、多矢量定姿模块的输出分别确定的惯性姿态矩阵的差值记作姿态量测量Z_s，有

并将Z_s(3×3)展开成列向量Z_1(9×1)，结合组合系统的状态向量X，列写出量测方程，

Z₁＝Η₁X+V₁ (18)

其中，H₁＝[H_M 0_9×3 H_N 0_9×6]为姿态量测矩阵，V₁为星敏感器的量测白噪声序列；

$H_N=H_M\·\left[\begin{array}{ccc}-1& 0& 0\\ 0& \cos L& 0\\ 0& \sin L& 0\end{array}\right]$

将惯性位置量测构造单元、天文位置矢量构造单元输出的垂线矢量信息的差值作为位置量测量，并结合状态向量X，得到量测方程为：

Z₂＝H₂X-V₂ (19)

式中，H₂＝[H₂₁ 0_3×3 H₂₂ 0_3×7]为垂线矢量量测矩阵，V₂为CNS垂线矢量误差，

$H_{21}=\left[\begin{array}{ccc}0& -\sin L& \cos L\·sin(\mathrm{\λ}+t_{GHA})\\ \sin L& 0& -\cos L\·\cos (\mathrm{\λ}+t_{GHA})\\ -\cos L\·sin(\mathrm{\λ}+t_{GHA})& \cos L\·\cos (\mathrm{\λ}+t_{GHA})& 0\end{array}\right]---\left(20\right)$

$H_{22}=\left[\begin{array}{cc}cos(\mathrm{\λ}+t_{GHA})\·\sin L& \cos L\·sin(\mathrm{\λ}+t_{GHA})\\ sin(\mathrm{\λ}+t_{GHA})\·\sin L& \cos L\·cos(\mathrm{\λ}+t_{GHA})\\ \cos L& 0\end{array}\right]---\left(21\right)$

步骤四：组合导航系统信息融合；

由惯导位置量测信息构造单元和天文位置矢量构造单元提供的垂线矢量和惯导姿态量测信息构造单元和多矢量定姿模块提供的姿态信息分别作差后作为量测信息，通过组合导航滤波器进行信息融合、能估计出平台失准角、惯性测量元件中的陀螺仪的漂移、加速度计零偏以及捷联惯导子系统的位置误差、速度误差；

步骤五：通过信息反馈校正各子系统误差；

根据姿态矩阵与姿态角的关系，组合导航系统的姿态角误差直接体现了大视场星敏感器辅助捷联惯导子系统所得到的水平基准的精度，由于传统深组合模式仅能估计并校正陀螺仪漂移和平台失准角的姿态相关的误差，而未能校正加速度计零偏，导致位置误差发散，并且位置误差的发散会导致姿态误差的发散，从而导致由SINS提供的数学地平存在较大的误差，通过基于水平基准的天文位置矢量构造单元校正惯性测量元件中的加速度计零偏以及捷联惯导子系统的位置误差，消除了定位误差随时间发散的趋势，从而抑制了姿态误差的发散；

然后通过高精度大视场星敏感器观测，并由多矢量定姿模块输出的惯性姿态信息连续辅助并修正捷联惯导子系统，使捷联惯导子系统的姿态误差收敛到了一定范围内，提高了其姿态精度，从而为天文导航子系统提供了高精度的水平基准；与此同时，结合高精度水平基准以及大视场星敏感器观测信息，天文位置矢量构造单元获得更为精确的当地位置矢量，从而引入天文导航子系统位置矢量量测信息以估计捷联惯导子系统的加速度计误差、位置误差，并进行反馈校正，进一步提高数学水平基准的精度及基于数学水平基准的CNS位置矢量的精度；

最终，通过基于数学水平基准的CNS位置矢量校正SINS的加速度计零偏以及位置误差，消除了定姿和定位误差随时间发散的趋势，使系统取得长时间定姿和定位精度，并且姿态和位置误差始终保持稳定。

3.根据权利要求2所述的一种火星车的SINS/CNS深组合导航系统的实现方法，其特征在于：在步骤一中所述的“采用大视场星敏感器辅助修正SINS姿态矩阵的方法通过估计平台失准角，修正捷联矩阵从而建立了数学水平基准”，是利用大视场星敏感器提供的多个星光矢量信息，并由多矢量定姿模块处理得到高精度的惯性姿态矩阵，结合惯导姿态量测信息构造单元提供的惯导惯性姿态信息，两者作差即能构造姿态量测量，通过组合导航滤波器进行信息融合，估计出平台失准角，对捷联矩阵进行修正，最后建立数学水平基准。

4.根据权利要求2所述的一种火星车的SINS/CNS深组合导航系统的实现方法，其特征在于：在步骤一中所述的“由捷联惯导解算的位置和姿态信息，得到火星惯性坐标系到火星车车体坐标系的方向余弦矩阵结合多矢量定姿模块获得的惯性姿态方向余弦矩阵能估计出惯导的平台失准角及陀螺漂移，并进行校正以得到高精度的数学水平基准”，即利用多矢量定姿模块提供的天文惯性姿态矩阵以及惯导姿态量测信息构造单元提供的捷联惯导惯性姿态矩阵分别将矩阵与矩阵的每个元素对应相减，得到九个观测量，建立这些观测量与状态量的关系，得到量测方程，通过组合导航滤波器进行信息融合，估计出平台失准角与陀螺漂移，在捷联矩阵的基础上，经过平台失准角修正，即得到高精度的数学水平基准并且通过修正陀螺仪漂移抑制了姿态误差的发散。

5.根据权利要求2所述的一种火星车的SINS/CNS深组合导航系统的实现方法，其特征在于：在步骤一中所述的“通过将SINS计算得到的变换矩阵与星敏感器输出的变换矩阵相结合，通过组合导航滤波器，获得失准角及陀螺漂移的估计值然后通过反馈该估计值到捷联惯导子系统，对SINS数学平台失准角和陀螺仪漂移进行修正，即能获得高精度的数学水平基准”，通过将惯导姿态量测信息构造单元输出的惯导惯性姿态与多矢量定姿模块提供的天文惯性姿态相结合，每个对应位置的元素作差，差值作为一个姿态量测量，并建立每个量测量与状态量的关系，建立量测方程，经过卡尔曼滤波进行信息融合，滤波结果能够估计出平台失准角和陀螺漂移，然后分别将估计信息反馈到惯导解算单元和惯性测量元件，修正SINS的捷联矩阵的误差和惯性测量元件陀螺仪漂移，实时修正了捷联矩阵的姿态误差，并从根本上抑制了误差的产生，因此能获得高精度的数学水平基准。

6.根据权利要求2所述的一种火星车的SINS/CNS深组合导航系统的实现方法，其特征在于：在步骤二中所述的“经过方向余弦矩阵将矢量旋转为导航坐标系的Z轴单位向量在车体坐标系下的投影后，即得到导航坐标系n中铅垂线方向Z_n的单位矢量在车体坐标系b中的坐标”，其含意和作法是：这是通过四元数对刚体的转动进行描述的，需要两次转动：向量绕车体坐标系b中Y_b轴的负方向旋转γ即滚转角，得到再绕着b₁系的X_b1轴的负方向旋转θ即俯仰角，得到即能达到要求；水平基准方向余弦矩阵将一个矢量的投影从车体坐标系转到导航系至少需要转动三次，最后一次转动是绕Z_n轴旋转ψ即偏航角；因此，对于轴来说，通过四元数转动二次与通过水平基准方向余弦矩阵转动三次的结果是相同的，它们都能得到在实际计算时，由于该方向余弦矩阵是已知的，而姿态四元数未知，因此，是用方向余弦矩阵代替姿态四元数进行矢量的转动的。

7.根据权利要求2所述的一种火星车的SINS/CNS深组合导航系统的实现方法，其特征在于：在步骤二中所述的“令车体的真实地理位置为(λL)，则得到真实的火星惯性坐标系下位置矢量与天文导航系统即CNS解算的同一坐标系下的位置矢量的关系”，其含意和作法是：在CNS解算位置矢量的过程当中，由于引入了预定的姿态误差，因此，CNS解算的位置矢量与真实的火星惯性坐标系下的位置矢量之间有差别，这种差别通过平台误差角表示出来，为后续组合导航滤波建立姿态量测方程提供依据。

8.根据权利要求2所述的一种火星车的SINS/CNS深组合导航系统的实现方法，其特征在于：在步骤三中所述的“将平台失准角及位置误差看作小量，且忽略高阶项，则由惯导姿态量测信息构造单元得到的惯性姿态矩阵”，其含意和作法是：惯导姿态量测信息构造单元是通过将三个方向余弦矩阵连乘得到的，在计算姿态矩阵时引入了姿态误差，同样由于惯导的导航信息存在误差，在构造位置矩阵时，又引入了位置误差，这些误差可以分别由与δP表示，而与δP均属于小量，经过连乘后，会出现高阶误差项，在此处将该高阶项忽略，因此通过考虑误差后，将三个方向余弦矩阵连乘，就能够得到惯导姿态量测信息构造的惯性姿态矩阵。

9.根据权利要求2所述的一种火星车的SINS/CNS深组合导航系统的实现方法，其特征在于：在步骤五中所述的“通过基于水平基准的天文位置矢量构造单元校正惯性测量元件中的加速度计零偏以及捷联惯导子系统的位置误差，消除了定位误差随时间发散的趋势，从而抑制了姿态误差的发散”，其含意和作法是：由于车体的位置误差与姿态误差是相互影响的，因此，通过引入天文位置矢量并结合惯导提供的位置矢量，能估计并校正车体的位置误差以及惯性测量元件加速度计的零偏，从根本上解决位置误差的发散导致姿态误差发散的位置，因此，提高定位精度，在另一方面也为抑制姿态误差的发散做出了贡献。

10.根据权利要求2所述的一种火星车的SINS/CNS深组合导航系统的实现方法，其特征在于：在步骤五中所述的“通过基于数学水平基准的CNS位置矢量校正SINS的加速度计零偏以及位置误差，消除了定姿和定位误差随时间发散的趋势，使系统取得长时间定姿和定位精度，并且姿态和位置误差始终保持稳定”，其含意和作法是：惯导和天文提供的惯性姿态矩阵能解决估计并校正姿态误差和陀螺仪漂移，从而达到高精度姿态，但由于在惯导解算中位置和姿态的交联关系，如果位置误差发散，必将导致姿态误差的发散，因此，通过基于数学水平基准的CNS位置矢量与惯导位置矢量的信息融合，去校正SINS的加速度计零偏以及位置误差，在消除定位误差随时间发散的趋势的基础上，也消除了定姿误差随时间的发散，因此，位姿误差始终保持稳定。