CN109443378A - 速度辅助行进间回溯初始对准方法 - Google Patents

Abstract

本发明涉及一种速度辅助行进间回溯初始对准方法，解决了捷联惯导***行进间快速初始对准的问题。本发明的主要步骤为：步骤一：获取传感器实时数据；步骤二：进行基于速度辅助的矢量观测行进间粗对准并存储中间变量；步骤三：利用存储的中间变量，构建基于初始导航系的行进间正向‑正向回溯初始对准；步骤四：将行进间正向‑正向回溯初始对准估计的初始姿态反馈，估计当前姿态角；步骤五：行进间正向‑正向回溯过程次数为M，若k＝M，则输出估计的姿态结果，完成初始对准过程，若k<M，表示回溯过程未完成，则重复上述步骤一至步骤五，直至行进间初始对准过程结束。

Description

速度辅助行进间回溯初始对准方法

技术领域

本发明涉及联惯性导航***行进间初始对准领域，特别是涉及速度辅助行 进间回溯初始对准方法。

背景技术

捷联惯性导航***行进间初始对准技术是捷联惯性导航定位的关键技术之 一，采用外部参考速度辅助惯性测量单元进行行进间初始对准具有测姿精度高、 可靠性好、自主性强等优点。当前，行进间初始对准可以分为粗对准和精对准 两个过程，其中粗对准主要实现粗略的姿态估计；精对准则是在粗对准的基础 上进行姿态精估计。众多学者都对捷联惯导***行进间初始对准技术进行了深 入的研究，但是研究均有将粗对准和精对准分别研究的缺点，精对准过程没有 充分利用粗对准过程的有效信息，导致对准时间较长，进而延长了捷联惯导准 备时间，不利于***应用。

传统技术存在以下技术问题：

发明内容

基于此，有必要针对上述技术问题，提供一种速度辅助行进间回溯初始对 准方法，发明采用基于速度辅助的矢量观测器行进间粗对准方法，并保存对准 过程中的有效数据，采用基于初始导航系的***误差模型实现行进间正向-正向 初始对准过程，减小了初始对准时间，实现了快速对准。

一种速度辅助行进间回溯初始对准方法，包括：

获取传感器实时数据，所述传感器实时数据包括陀螺仪数据、加速度计数 据以及多普勒测速仪数据；

进行基于速度辅助的矢量观测行进间粗对准并存储中间变量；

利用存储的中间变量，构建基于初始导航系的行进间正向-正向回溯初始 对准；

将行进间正向-正向回溯初始对准估计的初始姿态反馈，估计当前姿态角；

行进间正向-正向回溯过程次数为M，若k＝M，则输出估计的姿态结果，完 成初始对准过程，若k<M，表示回溯过程未完成，则重复上述步骤，直至行进 间初始对准过程结束。

上述速度辅助行进间回溯初始对准方法，采用基于初始导航系的***误差 模型，具有计算简便的优点；估计姿态误差为初始时刻姿态，为实现行进间正 向-正向回溯过程提供了理论支撑；本发明采用行进间正向-正向回溯计算，充分 利用了计算机的计算效能，极大的加快了初始对准的时间。

在另外的一个实施例中，“进行基于速度辅助的矢量观测行进间粗对准并 存储中间变量；”具体包括：

定义解算所需的参考坐标系如下：

b—载体坐标系，表示捷联惯性导航***三轴正交坐标系，其x轴、y轴和z 轴分别指向载体的右-前-上；

n—导航坐标系，表示载体所在位置的地理坐标系，其三轴分别指向当地 东向、北向和天向；

e—地球坐标系，表示原点在地心，x轴为地心指向本初子午线与赤道交点， z轴为地心指向北极点，y轴与x轴和z轴构成右手坐标系；

i—惯性坐标系，表示惯性空间非旋转坐标系；

b0—初始载体坐标系，表示惯导***开机运行时刻的载体坐标系，并在整 个对准过程中相对于惯性空间保持静止；

n0—初始导航坐标系，表示惯导***开机运行时刻的导航坐标系，并在整 个对准过程中相对于惯性空间保持静止；

e0—初始地球坐标系，表示惯导***开机运行时刻的地球坐标系，并在整 个对准过程中相对于惯性空间保持静止；

由坐标变换可知：

式中：vⁿ表示导航系速度；v^b表示载体系速度；表示b系到n系转换的方 向余弦矩阵；

对等式两边进行微分，可得：

式中：表示n系速度微分；表示b系到n系转换的方向余弦矩阵微分；v^b表示载体系速度；表示b系到n系转换的方向余弦矩阵；表示b系速度微分；

其中，

式中：表示n系速度微分；表示b系到n系转换的方向余弦矩阵；f^b表 示比力；表示e系相对于i系的转动角速度在n系的投影；表示n系相对于e 系的转动角速度在n系的投影；vⁿ表示n系速度；gⁿ表示重力加速度在n系的投 影；

由上面推导可知：

式中：表示b系到n系转换的方向余弦矩阵微分；v^b表示载体系速度；表示b系到n系转换的方向余弦矩阵；表示b系速度微分；表示b系相对 于n系的转动角速度在b系的投影所对应的叉乘矩阵；表示n0系相对于n系 的方向余弦矩阵；表示b0系相对于n0系的方向余弦矩阵；表示b系相对 于b0系的方向余弦矩阵；f^b表示比力；表示e系相对于i系的转动角速度在n系 的投影；表示n系相对于e系的转动角速度在n系的投影；vⁿ表示n系速度；gⁿ表示重力加速度在n系的投影；

对等式两边同时乘以可得：

式中：表示b0系相对于n0系的方向余弦矩阵；表示b系相对于b0系 的方向余弦矩阵；表示b系相对于n系的转动角速度在b系的投影所对应 的叉乘矩阵；v^b表示载体系速度；表示b系速度微分；f^b表示比力；表示 n0系相对于n系的方向余弦矩阵；表示e系相对于i系的转动角速度在n系的投 影；表示n系相对于e系的转动角速度在n系的投影；vⁿ表示n系速度；gⁿ表 示重力加速度在n系的投影；

对等式两边积分并进行运算可得：

式中：表示b0系相对于n0系的方向余弦矩阵；表示b系相对于b0系 的方向余弦矩阵；v^b表示载体系速度；f^b表示比力；表示n0系相对于n系的 方向余弦矩阵；表示e系相对于i系的转动角速度在n系的投影；vⁿ表示n系速 度；gⁿ表示重力加速度在n系的投影；

因此可得矢量观测模型：

式中：β表示观测矢量；表示b0系相对于n0系的方向余弦矩阵；α表示 参考矢量；

观测矢量和参考矢量可以表示为：

式中：表示b系相对于b0系的方向余弦矩阵；v^b表示载体系速度；f^b表 示比力；表示n0系相对于n系的方向余弦矩阵；表示e系相对于i系的转 动角速度在n系的投影；vⁿ表示n系速度；gⁿ表示重力加速度在n系的投影；

由最优基姿态确定算法可知：

式中：K表示构造矩阵；β表示观测矢量；α表示参考矢量；和[α⊙]分 别表示为：

式中：β表示观测矢量；α表示参考矢量；[·×]表示矢量转换成斜对称矩阵；

通过提取K矩阵最小特征值对应的特征矢量可以计算方向余弦从而实 现速度辅助行进间粗对准；

存储中间变量为：量测比力在初始载体系上的映射载体系相对于初 始载体系变化的方向余弦矩阵

在另外的一个实施例中，陀螺仪量测常值漂移误差为 ε^b＝[0.02 0.02 0.02]To/h，陀螺仪量测随机游走误差为 输出频率为200Hz；加速度计量测常值漂移误差 为陀螺仪量测随机游走误差为输出频率为200Hz。

在另外的一个实施例中，“利用存储的中间变量，构建基于初始导航系的行 进间正向-正向回溯初始对准；”具体包括：

(1)姿态误差方程

由粗对准可知：

式中，表示粗对准结束时刻方向余弦矩阵；表示初始时刻方向余弦 矩阵；表示b系相对于b0系变化的方向余弦矩阵；表示n0系与n系之间的 方向余弦矩阵；

由方向余弦误差方程可知：

式中，表示初始时刻方向余弦矩阵；表示初始时刻方向余弦矩阵真 值；表示初始姿态误差在n0系上的映射；I₃表示3维单位矩阵；[·×]表示矢 量转换成斜对称矩阵；表示b系相对于b0系变化的方向余弦矩阵；表示载 体系相对于b0系变化的方向余弦矩阵真值；表示陀螺量测造成的姿态更新 误差；

由上面推导可知：

式中，表示粗对准结束时刻方向余弦矩阵；表示初始时刻方向余弦 矩阵真值；表示b系相对于b0系变化的方向余弦矩阵真值；表示初始姿 态误差在n0系上的映射；表示陀螺量测造成的姿态更新误差；I₃表示3维 单位矩阵；[·×]表示矢量转换成斜对称矩阵；表示n0系与n系之间的方向余 弦矩阵；

简化计算得到：

式中，表示粗对准结束时刻方向余弦矩阵；表示初始时刻方向余弦 矩阵真值；表示b系相对于初始载体系变化的方向余弦矩阵真值；表示 初始姿态误差在n0系上的映射；表示陀螺量测造成的姿态更新误差；I₃表 示3维单位矩阵；[·×]表示矢量转换成斜对称矩阵；表示n系与n0系之间的 方向余弦矩阵；

由姿态误差方程可知：

式中，表示初始姿态误差的微分在n0系上的映射；表示陀螺量测造 成的姿态更新误差的微分；ε^b表示陀螺仪量测常值漂移误差；表示陀螺仪量 测随机游走误差；表示初始时刻方向余弦矩阵；表示b系相对于b0系变化 的方向余弦矩阵；

有上面计算可知：

式中，表示姿态误差的微分在n0系上的映射；ε^b表示陀螺仪量测常值 漂移误差；表示陀螺仪量测随机游走误差；表示初始时刻方向余弦矩阵； 表示b系相对于b0系变化的方向余弦矩阵；

(2)速度误差方程

由惯性导航比力方程可知：

式中，表示n0系速度微分；表示b系与n0系之间的方向余弦矩阵；f^b表示比力真值；表示地球自转角速度在n0系的映射；表示n0系相对于e 系的转动角速度在n0系上的映射；vⁿ⁰表示载体运动速度在n0系的投影；gⁿ⁰表 示重力矢量在n0系上的映射；

根据角速度映射关系：

式中，表示n0系相对于i系的转动角速度在n0系上的映射；表示e系 相对于i系的转动角速度在n0系上的映射；表示n0系相对于e系的转动角速 度在n0系上的映射；

因此速度误差微分方程可以表示为：

式中，表示n0系速度微分在初始导航系的映射；表示b0系与n0系之 间的方向余弦矩阵；表示b系相对于b0系变化的方向余弦矩阵；表示加速 度计测量值；φⁿ⁰表示姿态误差；表示地球自转角速度在n0系的映射；δvⁿ⁰表示速度误差在n0系的投影；表示加速度零偏；表示加速度随机游走噪 声；

(3)量测方程

当测量速度为DVL速度时，此时量测方程可以表示为：

式中，表示量测量；表示b0系与n0系之间的方向余弦矩阵；表示 b系相对于b0系变化的方向余弦矩阵；表示DVL测量速度；φⁿ⁰表示安装误差 角；δvⁿ⁰表示速度误差；δv^b表示测量速度随机噪声；

(4)***模型

由速度误差方程和姿态误差方程可得***模型为：

式中，表示***状态；表示***过程噪声；z表示量测速度误差；表示量测噪声；***矩阵F和量测矩阵H分别表示为：

***模型离散化之后表示为：

式中，k表示离散化时刻；Φ_k表示离散化***矩阵；x_k+1和x_k表示k+1时刻 和k时刻***状态；z_k表示k时刻量测；H_k表示量测矩阵；W_k表示k时刻过程噪 声；V_k表示k时刻量测噪声；

(4)Kalman滤波

利用离散化之后的***模型得到Kalman滤波如下：

式中，表示k-1时刻的最优状态估计；表示k时刻状态一步预 测；Φ_k表示离散化***矩阵；P_k-1|k-1表示k-1时刻的状态误差协方差；Q_k-1表 示k-1时刻的过程噪声协方差；P_k|k-1表示k时刻一步预测状态误差协方差；H_k表示量测矩阵；R_k表示k时刻量测噪声协方差矩阵；K_k表示k时刻滤波增益矩 阵；z_k表示k时刻量测；表示k时刻的最优状态估计；P_k|k表示k时刻的状态 误差协方差。

在另外的一个实施例中，Kalman滤波参数初值为 P_0|0＝diag([0.1 0.11 0.1 0.1 0.1 0.02e₃ 500e₃])²e₃＝[1 1 1]。

在另外的一个实施例中，M＝4。

一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上 运行的计算机程序，所述处理器执行所述程序时实现任一项所述方法的步骤。

一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行 时实现任一项所述方法的步骤。

一种处理器，所述处理器用于运行程序，其中，所述程序运行时执行任一 项所述的方法。

附图说明

图1为本申请实施例提供的一种速度辅助行进间回溯初始对准方法的流程 图。

图2为本申请实施例提供的一种速度辅助行进间回溯初始对准方法中的回 溯对准纵摇角误差图。

图3为本申请实施例提供的一种速度辅助行进间回溯初始对准方法中的回 溯对准横摇角误差图。

图4为本申请实施例提供的一种速度辅助行进间回溯初始对准方法中的回 溯对准航向角误差图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实 施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅 仅用以解释本发明，并不用于限定本发明。

本实施例将本发明提出的一种速度辅助行进间回溯初始对准方法通过 Matlab仿真软件进行仿真验证，从而证明行进间正向-正向回溯过程的快速性。 仿真硬件环境均为Intel(R)Core(TM)T9600 CPU 2.80GHz，4G RAM，Windows 7操作***。如图2和图3所示，为行进间正向-正向回溯对准纵摇角误差图和 横摇角误差图，一共进行四次回溯对准，对准过程中单次运行时间为1s，总对 准时间为4s(不包括行进间粗对准时间)。图4为行进间正向-正向回溯对准航 向角误差图，从图中可以看出，经过两次回溯之后，航向角对准误差小于0.1°， 达到了极限对准精度。综合行进间粗对准采集数据过程，仿真中为300s，整个 对准过程持续时间为304s左右，相较于传统对准方法，在同样***指标的要求 下大大提升了初始对准过程的速度。

本发明是一种速度辅助行进间回溯初始对准方法，算法结构如图1所示， 包括以下几个步骤：

步骤1：获取传感器实时数据，所述传感器实时数据包括陀螺仪数据、加 速度计数据以及多普勒测速仪数据；

步骤2：进行基于速度辅助的矢量观测行进间粗对准并存储中间变量；

定义解算所需的参考坐标系如下：

b—载体坐标系，表示捷联惯性导航***三轴正交坐标系，其x轴、y轴和z 轴分别指向载体的右-前-上；

n—导航坐标系，表示载体所在位置的地理坐标系，其三轴分别指向当地 东向、北向和天向；

e—地球坐标系，表示原点在地心，x轴为地心指向本初子午线与赤道交点， z轴为地心指向北极点，y轴与x轴和z轴构成右手坐标系；

i—惯性坐标系，表示惯性空间非旋转坐标系；

b0—初始载体坐标系，表示惯导***开机运行时刻的载体坐标系，并在整 个对准过程中相对于惯性空间保持静止；

n0—初始导航坐标系，表示惯导***开机运行时刻的导航坐标系，并在整 个对准过程中相对于惯性空间保持静止；

e0—初始地球坐标系，表示惯导***开机运行时刻的地球坐标系，并在整 个对准过程中相对于惯性空间保持静止；

由坐标变换可知：

式中：vⁿ表示导航系速度；v^b表示载体系速度；表示b系到n系转换的方 向余弦矩阵；

对等式两边进行微分，可得：

式中：表示n系速度微分；表示b系到n系转换的方向余弦矩阵微分；v^b表示载体系速度；表示b系到n系转换的方向余弦矩阵；表示b系速度微分；

其中，

式中：表示n系速度微分；表示b系到n系转换的方向余弦矩阵；f^b表 示比力；表示e系相对于i系的转动角速度在n系的投影；表示n系相对于e 系的转动角速度在n系的投影；vⁿ表示n系速度；gⁿ表示重力加速度在n系的投 影；

由上面推导可知：

式中：表示b系到n系转换的方向余弦矩阵微分；v^b表示载体系速度；表示b系到n系转换的方向余弦矩阵；表示b系速度微分；表示b系相对 于n系的转动角速度在b系的投影所对应的叉乘矩阵；表示n0系相对于n系 的方向余弦矩阵；表示b0系相对于n0系的方向余弦矩阵；表示b系相对 于b0系的方向余弦矩阵；f^b表示比力；表示e系相对于i系的转动角速度在n系 的投影；表示n系相对于e系的转动角速度在n系的投影；vⁿ表示n系速度；gⁿ表示重力加速度在n系的投影；

对等式两边同时乘以可得：

式中：表示b0系相对于n0系的方向余弦矩阵；表示b系相对于b0系 的方向余弦矩阵；表示b系相对于n系的转动角速度在b系的投影所对应 的叉乘矩阵；v^b表示载体系速度；表示b系速度微分；f^b表示比力；表示 n0系相对于n系的方向余弦矩阵；表示e系相对于i系的转动角速度在n系的投 影；表示n系相对于e系的转动角速度在n系的投影；vⁿ表示n系速度；gⁿ表 示重力加速度在n系的投影；

对等式两边积分并进行运算可得：

式中：表示b0系相对于n0系的方向余弦矩阵；表示b系相对于b0系 的方向余弦矩阵；v^b表示载体系速度；f^b表示比力；表示n0系相对于n系的 方向余弦矩阵；表示e系相对于i系的转动角速度在n系的投影；vⁿ表示n系速 度；gⁿ表示重力加速度在n系的投影；

因此可得矢量观测模型：

式中：β表示观测矢量；表示b0系相对于n0系的方向余弦矩阵；α表示 参考矢量；

观测矢量和参考矢量可以表示为：

式中：表示b系相对于b0系的方向余弦矩阵；v^b表示载体系速度；f^b表 示比力；表示n0系相对于n系的方向余弦矩阵；表示e系相对于i系的转 动角速度在n系的投影；vⁿ表示n系速度；gⁿ表示重力加速度在n系的投影；

由最优基姿态确定算法可知：

式中：K表示构造矩阵；β表示观测矢量；α表示参考矢量；和[α⊙]分 别表示为：

式中：β表示观测矢量；α表示参考矢量；[·×]表示矢量转换成斜对称矩阵；

通过提取K矩阵最小特征值对应的特征矢量可以计算方向余弦从而实 现速度辅助行进间粗对准；

存储中间变量为：量测比力在初始载体系上的映射载体系相对于初 始载体系变化的方向余弦矩阵

步骤3：利用存储的中间变量，构建基于初始导航系的行进间正向-正向 回溯初始对准；

(1)姿态误差方程

由粗对准可知：

式中，表示粗对准结束时刻方向余弦矩阵；表示初始时刻方向余弦 矩阵；表示b系相对于b0系变化的方向余弦矩阵；表示n0系与n系之间的 方向余弦矩阵；

由方向余弦误差方程可知：

式中，表示初始时刻方向余弦矩阵；表示初始时刻方向余弦矩阵真 值；表示初始姿态误差在n0系上的映射；I₃表示3维单位矩阵；[·×]表示矢 量转换成斜对称矩阵；表示b系相对于b0系变化的方向余弦矩阵；表示载 体系相对于b0系变化的方向余弦矩阵真值；表示陀螺量测造成的姿态更新 误差；

由上面推导可知：

式中，表示粗对准结束时刻方向余弦矩阵；表示初始时刻方向余弦 矩阵真值；表示b系相对于b0系变化的方向余弦矩阵真值；表示初始姿 态误差在n0系上的映射；表示陀螺量测造成的姿态更新误差；I₃表示3维 单位矩阵；[·×]表示矢量转换成斜对称矩阵；表示n0系与n系之间的方向余 弦矩阵；

简化计算得到：

式中，表示粗对准结束时刻方向余弦矩阵；表示初始时刻方向余弦 矩阵真值；表示b系相对于初始载体系变化的方向余弦矩阵真值；表示 初始姿态误差在n0系上的映射；表示陀螺量测造成的姿态更新误差；I₃表 示3维单位矩阵；[·×]表示矢量转换成斜对称矩阵；表示n系与n0系之间的 方向余弦矩阵；

由姿态误差方程可知：

式中，表示初始姿态误差的微分在n0系上的映射；表示陀螺量测造 成的姿态更新误差的微分；ε^b表示陀螺仪量测常值漂移误差；表示陀螺仪量 测随机游走误差；表示初始时刻方向余弦矩阵；表示b系相对于b0系变化 的方向余弦矩阵；

有上面计算可知：

式中，表示姿态误差的微分在n0系上的映射；ε^b表示陀螺仪量测常值 漂移误差；表示陀螺仪量测随机游走误差；表示初始时刻方向余弦矩阵； 表示b系相对于b0系变化的方向余弦矩阵；

(2)速度误差方程

由惯性导航比力方程可知：

式中，表示n0系速度微分；表示b系与n0系之间的方向余弦矩阵；f^b表示比力真值；表示地球自转角速度在n0系的映射；表示n0系相对于e 系的转动角速度在n0系上的映射；vⁿ⁰表示载体运动速度在n0系的投影；gⁿ⁰表 示重力矢量在n0系上的映射；

根据角速度映射关系：

式中，表示n0系相对于i系的转动角速度在n0系上的映射；表示e系 相对于i系的转动角速度在n0系上的映射；表示n0系相对于e系的转动角速 度在n0系上的映射；

因此速度误差微分方程可以表示为：

式中，表示n0系速度微分在初始导航系的映射；表示b0系与n0系之 间的方向余弦矩阵；表示b系相对于b0系变化的方向余弦矩阵；表示加速 度计测量值；φⁿ⁰表示姿态误差；表示地球自转角速度在n0系的映射；δvⁿ⁰表示速度误差在n0系的投影；表示加速度零偏；表示加速度随机游走噪 声；

(3)量测方程

当测量速度为DVL速度时，此时量测方程可以表示为：

式中，表示量测量；表示b0系与n0系之间的方向余弦矩阵；表示 b系相对于b0系变化的方向余弦矩阵；表示DVL测量速度；φⁿ⁰表示安装误差 角；δvⁿ⁰表示速度误差；δv^b表示测量速度随机噪声；

(4)***模型

由速度误差方程和姿态误差方程可得***模型为：

式中，表示***状态；表示***过程噪声；z表示量测速度误差；表示量测噪声；***矩阵F和量测矩阵H分别表示为：

***模型离散化之后表示为：

式中，k表示离散化时刻；φ_k表示离散化***矩阵；x_k+1和x_k表示k+1时刻 和k时刻***状态；z_k表示k时刻量测；H_k表示量测矩阵；W_k表示k时刻过程噪 声；V_k表示k时刻量测噪声；

(4)Kalman滤波

利用离散化之后的***模型得到Kalman滤波如下：

P_k|k＝P_k|k-1-K_kH_kP_k|k-1

式中，表示k-1时刻的最优状态估计；表示k时刻状态一步预 测；φ_k表示离散化***矩阵；P_k-1|k-1表示k-1时刻的状态误差协方差；Q_k-1表 示k-1时刻的过程噪声协方差；P_k|k-1表示k时刻一步预测状态误差协方差；H_k表示量测矩阵；R_k表示k时刻量测噪声协方差矩阵；K_k表示k时刻滤波增益矩 阵；z_k表示k时刻量测；表示k时刻的最优状态估计；P_k|k表示k时刻的状态 误差协方差。

步骤4：将行进间正向-正向回溯初始对准估计的初始姿态反馈，估计当前 姿态角。

步骤5：行进间正向-正向回溯过程次数为M，若k＝M，则输出估计的姿态 结果，完成初始对准过程，若k<M，表示回溯过程未完成，则重复上述步骤， 直至行进间初始对准过程结束。

一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上 运行的计算机程序，所述处理器执行所述程序时实现任一项所述方法的步骤。

一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行 时实现任一项所述方法的步骤。

一种处理器，所述处理器用于运行程序，其中，所述程序运行时执行任一 项所述的方法。

对本发明的有益效果说明如下：

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

陀螺仪量测常值漂移误差为ε^b＝[0.02 0.02 0.02]To/h，陀螺仪量测随机游 走误差为输出频率为200Hz；加速度计量测常值 漂移误差为陀螺仪量测随机游走误差为 输出频率为200Hz。Kalman滤波参数初值为 P_0|0＝diag([0.1 0.1 1 0.1 0.1 0.10.02e₃ 500e₃])²，e₃＝[1 1 1]。步骤5中， M＝4。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对 上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技 术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细， 但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的 普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改 进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权 利要求为准。

Claims (9)

1.一种速度辅助行进间回溯初始对准方法，其特征在于，包括：

获取所述传感器实时数据，所述传感器实时数据包括陀螺仪数据、加速度计数据以及多普勒测速仪数据；

进行基于速度辅助的矢量观测行进间粗对准并存储中间变量；

利用存储的中间变量，构建基于初始导航系的行进间正向-正向回溯初始对准；

将行进间正向-正向回溯初始对准估计的初始姿态反馈，估计当前姿态角；

行进间正向-正向回溯过程次数为M，若k＝M，则输出估计的姿态结果，完成初始对准过程，若k<M，表示回溯过程未完成，则重复上述步骤，直至行进间初始对准过程结束。

2.根据权利要求1所述的速度辅助行进间回溯初始对准方法，其特征在于，“进行基于速度辅助的矢量观测行进间粗对准并存储中间变量；”具体包括：

定义解算所需的参考坐标系如下：

b—载体坐标系，表示捷联惯性导航***三轴正交坐标系，其x轴、y轴和z轴分别指向载体的右-前-上；

n—导航坐标系，表示载体所在位置的地理坐标系，其三轴分别指向当地东向、北向和天向；

e—地球坐标系，表示原点在地心，x轴为地心指向本初子午线与赤道交点，z轴为地心指向北极点，y轴与x轴和z轴构成右手坐标系；

i—惯性坐标系，表示惯性空间非旋转坐标系；

b0—初始载体坐标系，表示惯导***开机运行时刻的载体坐标系，并在整个对准过程中相对于惯性空间保持静止；

n0—初始导航坐标系，表示惯导***开机运行时刻的导航坐标系，并在整个对准过程中相对于惯性空间保持静止；

e0—初始地球坐标系，表示惯导***开机运行时刻的地球坐标系，并在整个对准过程中相对于惯性空间保持静止；

由坐标变换可知：

式中：vⁿ表示导航系速度；v^b表示载体系速度；表示b系到n系转换的方向余弦矩阵；

对等式两边进行微分，可得：

式中：表示n系速度微分；表示b系到n系转换的方向余弦矩阵微分；v^b表示载体系速度；表示b系到n系转换的方向余弦矩阵；表示b系速度微分；

其中，

式中：表示n系速度微分；表示b系到n系转换的方向余弦矩阵；f^b表示比力；表示e系相对于i系的转动角速度在n系的投影；表示n系相对于e系的转动角速度在n系的投影；vⁿ表示n系速度；gⁿ表示重力加速度在n系的投影；

由上面推导可知：

式中：表示b系到n系转换的方向余弦矩阵微分；v^b表示载体系速度；表示b系到n系转换的方向余弦矩阵；表示b系速度微分；表示b系相对于n系的转动角速度在b系的投影所对应的叉乘矩阵；表示n0系相对于n系的方向余弦矩阵；表示b0系相对于n0系的方向余弦矩阵；表示b系相对于b0系的方向余弦矩阵；f^b表示比力；表示e系相对于i系的转动角速度在n系的投影；表示n系相对于e系的转动角速度在n系的投影；vⁿ表示n系速度；gⁿ表示重力加速度在n系的投影；

对等式两边同时乘以可得：

式中：表示b0系相对于n0系的方向余弦矩阵；表示b系相对于b0系的方向余弦矩阵；表示b系相对于n系的转动角速度在b系的投影所对应的叉乘矩阵；v^b表示载体系速度；表示b系速度微分；f^b表示比力；表示n0系相对于n系的方向余弦矩阵；表示e系相对于i系的转动角速度在n系的投影；表示n系相对于e系的转动角速度在n系的投影；vⁿ表示n系速度；gⁿ表示重力加速度在n系的投影；

对等式两边积分并进行运算可得：

式中：表示b0系相对于n0系的方向余弦矩阵；表示b系相对于b0系的方向余弦矩阵；v^b表示载体系速度；f^b表示比力；表示n0系相对于n系的方向余弦矩阵；表示e系相对于i系的转动角速度在n系的投影；vⁿ表示n系速度；gⁿ表示重力加速度在n系的投影；

因此可得矢量观测模型：

式中：β表示观测矢量；表示b0系相对于n0系的方向余弦矩阵；α表示参考矢量；

观测矢量和参考矢量可以表示为：

式中：表示b系相对于b0系的方向余弦矩阵；v^b表示载体系速度；f^b表示比力；表示n0系相对于n系的方向余弦矩阵；表示e系相对于i系的转动角速度在n系的投影；vⁿ表示n系速度；gⁿ表示重力加速度在n系的投影；

由最优基姿态确定算法可知：

式中：K表示构造矩阵；β表示观测矢量；α表示参考矢量；和[α⊙]分别表示为：

式中：β表示观测矢量；α表示参考矢量；[·×]表示矢量转换成斜对称矩阵；

通过提取K矩阵最小特征值对应的特征矢量可以计算方向余弦从而实现速度辅助行进间粗对准；

存储中间变量为：量测比力在初始载体系上的映射载体系相对于初始载体系变化的方向余弦矩阵

3.根据权利要求2所述的速度辅助行进间回溯初始对准方法，其特征在于，陀螺仪量测常值漂移误差为ε^b＝[0.02 0.02 0.02]^To/h，陀螺仪量测随机游走误差为输出频率为200Hz；加速度计量测常值漂移误差为陀螺仪量测随机游走误差为输出频率为200Hz。

4.根据权利要求1所述的速度辅助行进间回溯初始对准方法，其特征在于，“利用存储的中间变量，构建基于初始导航系的行进间正向-正向回溯初始对准；”具体包括：

(1)姿态误差方程

由粗对准可知：

式中，表示粗对准结束时刻方向余弦矩阵；表示初始时刻方向余弦矩阵；表示b系相对于b0系变化的方向余弦矩阵；表示n0系与n系之间的方向余弦矩阵；

由方向余弦误差方程可知：

式中，表示初始时刻方向余弦矩阵；表示初始时刻方向余弦矩阵真值；表示初始姿态误差在n0系上的映射；I₃表示3维单位矩阵；[·×]表示矢量转换成斜对称矩阵；表示b系相对于b0系变化的方向余弦矩阵；表示载体系相对于b0系变化的方向余弦矩阵真值；表示陀螺量测造成的姿态更新误差；

由上面推导可知：

式中，表示粗对准结束时刻方向余弦矩阵；表示初始时刻方向余弦矩阵真值；表示b系相对于b0系变化的方向余弦矩阵真值；表示初始姿态误差在n0系上的映射；表示陀螺量测造成的姿态更新误差；I₃表示3维单位矩阵；[·×]表示矢量转换成斜对称矩阵；表示n0系与n系之间的方向余弦矩阵；

简化计算得到：

式中，表示粗对准结束时刻方向余弦矩阵；表示初始时刻方向余弦矩阵真值；表示b系相对于初始载体系变化的方向余弦矩阵真值；表示初始姿态误差在n0系上的映射；表示陀螺量测造成的姿态更新误差；I₃表示3维单位矩阵；[·×]表示矢量转换成斜对称矩阵；表示n系与n0系之间的方向余弦矩阵；

由姿态误差方程可知：

式中，表示初始姿态误差的微分在n0系上的映射；表示陀螺量测造成的姿态更新误差的微分；ε^b表示陀螺仪量测常值漂移误差；表示陀螺仪量测随机游走误差；表示初始时刻方向余弦矩阵；表示b系相对于b0系变化的方向余弦矩阵；

有上面计算可知：

式中，表示姿态误差的微分在n0系上的映射；ε^b表示陀螺仪量测常值漂移误差；表示陀螺仪量测随机游走误差；表示初始时刻方向余弦矩阵；表示b系相对于b0系变化的方向余弦矩阵；

(2)速度误差方程

由惯性导航比力方程可知：

式中，表示n0系速度微分；表示b系与n0系之间的方向余弦矩阵；f^b表示比力真值；表示地球自转角速度在n0系的映射；表示n0系相对于e系的转动角速度在n0系上的映射；vⁿ⁰表示载体运动速度在n0系的投影；gⁿ⁰表示重力矢量在n0系上的映射；

根据角速度映射关系：

式中，表示n0系相对于i系的转动角速度在n0系上的映射；表示e系相对于i系的转动角速度在n0系上的映射；表示n0系相对于e系的转动角速度在n0系上的映射；

因此速度误差微分方程可以表示为：

式中，表示n0系速度微分在初始导航系的映射；表示b0系与n0系之间的方向余弦矩阵；表示b系相对于b0系变化的方向余弦矩阵；表示加速度计测量值；φⁿ⁰表示姿态误差；表示地球自转角速度在n0系的映射；δvⁿ⁰表示速度误差在n0系的投影；表示加速度零偏；表示加速度随机游走噪声；

(3)量测方程

当测量速度为DVL速度时，此时量测方程可以表示为：

式中，表示量测量；表示b0系与n0系之间的方向余弦矩阵；表示b系相对于b0系变化的方向余弦矩阵；表示DVL测量速度；φⁿ⁰表示安装误差角；δvⁿ⁰表示速度误差；δv^b表示测量速度随机噪声；

(4)***模型

由速度误差方程和姿态误差方程可得***模型为：

式中，表示***状态；表示***过程噪声；z表示量测速度误差；表示量测噪声；***矩阵F和量测矩阵H分别表示为：

***模型离散化之后表示为：

式中，k表示离散化时刻；Φ_k表示离散化***矩阵；x_k+1和x_k表示k+1时刻和k时刻***状态；z_k表示k时刻量测；H_k表示量测矩阵；W_k表示k时刻过程噪声；V_k表示k时刻量测噪声；

(4)Kalman滤波

利用离散化之后的***模型得到Kalman滤波如下：

p_k|k＝P_k|k-1-K_kH_kP_k|k-1

式中，表示k-1时刻的最优状态估计；表示k时刻状态一步预测；Φ_k表示离散化***矩阵；P_k-1|k-1表示k-1时刻的状态误差协方差；Q_k-1表示k-1时刻的过程噪声协方差；P_k|k-1表示k时刻一步预测状态误差协方差；H_k表示量测矩阵；R_k表示k时刻量测噪声协方差矩阵；K_k表示k时刻滤波增益矩阵；z_k表示k时刻量测；表示k时刻的最优状态估计；P_k|k表示k时刻的状态误差协方差。

5.根据权利要求4所述的速度辅助行进间回溯初始对准方法，其特征在于，Kalman滤波参数初值为P_0|0＝diag([0.1 0.1 1 0.1 0.1 0.1 0.02e₃ 500e₃])²，e₃＝[1 11]。

6.根据权利要求1所述的速度辅助行进间回溯初始对准方法，其特征在于，M＝4。

7.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现权利要求1到6任一项所述方法的步骤。

8.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现权利要求1到6任一项所述方法的步骤。

9.一种处理器，其特征在于，所述处理器用于运行程序，其中，所述程序运行时执行权利要求1到6任一项所述的方法。