GNSS位置辅助SINS行进间初始对准方法与流程

本发明涉及捷联惯性导航系统初始对准领域，具体涉及一种gnss位置辅助sins行进间初始对准方法。

背景技术：

捷联惯性导航系统初始对准技术是系统正常导航定位的关键技术之一，采用gnss辅助惯性导航系统进行行进间初始对准具有对准精度高、可靠性好等优点。当前，初始对准可以分为粗对准和精对准两个过程，其中粗对准主要实现粗略的姿态估计；精对准则是在粗对准的基础上进行姿态精估计。众多学者都对捷联惯导系统初始对准技术进行了深入的研究。在精对准方面，通过引入鲁棒卡尔曼滤波技术可以实现对准过程的鲁棒化，提高系统的稳定性。

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

但在gnss辅助惯导粗对准方面，当前的研究热点均是采用速度辅助，利用速度积分方程实现行进间初始粗对准，但是由于gnss易受外部干扰，导致速度测量存在异常现象，这也导致对准结果出现波动或者不收敛，造成对准结果稳定性差的缺点。

技术实现要素：

本发明要解决的技术问题是提供一种gnss位置辅助sins行进间初始对准方法，为克服传统gnss辅助sins粗对准方法无法进行鲁棒化粗对准的问题，采用gnss位置矢量建模方法，利用位置矢量弱化gnss量测异常问题，可以实现粗对准过程的鲁棒化，提高系统对准的稳定性。

为了解决上述技术问题，本发明提供了一种gnss位置辅助sins行进间初始对准方法，包括：

获取惯性传感器实时数据并进行姿态更新，定义解算所需的参考坐标系；

获取gnss实时位置数据：

利用获取的gnss位置信息，建立位置法矢量观测器；

设计基于迭代原理的矢量观测器；

利用最优基四元数姿态确定方法，并计算确定姿态与真实姿态之间的误差角。

在其中一个实施例中，其中，定义解算所需的参考坐标系具体如下：

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

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

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

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

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

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

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

由姿态更新算法可知：

式中：表示导航系姿态矩阵微分；表示载体系姿态矩阵微分；表示导航系相对于初始导航系的方向余弦矩阵；表示载体系相对于初始载体系的方向余弦矩阵；表示导航系相对于惯性系的旋转角速度在导航系的映射；表示载体系相对于惯性系的旋转角速度在载体系的映射；

利用方向余弦链式法则，可得：

式中：表示载体系相对于导航系的实时方向余弦矩阵；表示初始导航系相对于导航系的方向余弦矩阵；表示载体系相对于初始载体系的方向余弦矩阵。

在其中一个实施例中，“利用获取的gnss位置信息，建立位置法矢量观测器；”具体如下：

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

式中：表示导航系速度的微分；表示载体系相对于导航系变化的方向余弦矩阵；f^b表示比力；表示地球系相对于惯性系的旋转角速度在导航系的投影；表示导航系相对于地球系的转动角速度在导航系的投影；×表示矢量叉乘运算；vⁿ表示导航系速度；gⁿ表示重力加速度在导航系下的投影；

由上面推导可知：

式中：表示导航系速度的微分；表示初始导航系相对于导航系的方向余弦矩阵；表示初始载体系相对于初始导航系的方向余弦矩阵；表示载体系相对于初始载体系的方向余弦矩阵；f^b表示比力；

表示地球系相对于惯性系的旋转角速度在导航系的投影；表示导航系相对于地球系的转动角速度在导航系的投影；×表示矢量叉乘运算；

vⁿ表示导航系速度；gⁿ表示重力加速度在导航系下的投影；

对上式进行计算可得：

式中：βa表示加速度观测矢量；表示初始载体系相对于初始导航系的方向余弦矩阵；αa表示加速度参考矢量；表示导航系相对于初始导航系的方向余弦矩阵；表示导航系速度微分；表示地球系相对于惯性系的旋转角速度在导航系的投影；表示导航系相对于地球系的转动角速度在导航系的投影；×表示矢量叉乘运算；vⁿ表示导航系速度；gⁿ表示重力加速度在导航系下的投影；表示载体系相对于初始载体系的方向余弦矩阵；f^b表示比力；

根据gnss位置信息与导航系速度关系可知：

式中：表示gnss位置微分；rc表示当地曲率矩阵；vⁿ表示导航系速度；表示位置矢量微分；l表示载体当前纬度；rm表示表示子午线曲率半径；rn表示横向曲率半径；h表示载体水平高度；

对加速度观测矢量和参考矢量进行两次积分可得：

式中：βp表示gnss位置观测矢量；αp表示gnss位置参考矢量；

表示初始载体系相对于初始导航系的方向余弦矩阵；表示位置矢量微分；表示τ时刻导航系相对于初始导航系的方向余弦矩阵；

t表示当前时刻；vⁿ(0)表示初始时刻导航系速度；表示σ时刻导航系相对于初始导航系的方向余弦矩阵；表示地球系相对于惯性系的旋转角速度在导航系的投影；gⁿ表示重力加速度在导航系下的投影；

表示σ时刻载体系相对于初始载体系的方向余弦矩阵；f^b表示比力。

在其中一个实施例中，“设计基于迭代原理的矢量观测器；”具体如下：

对上面基于gnss位置的观测矢量和参考矢量进行积分运算，其中参考矢量可以计算为：

式中，αp(m)表示离散化m时刻的参考矢量；δtg表示gnss采样时间；表示tm时刻载体系相对于初始载体系的方向余弦矩阵；表示σ时刻载体系相对于tm时刻载体系的方向余弦矩阵；

f^b表示比力；

其中，积分项可以计算为：

式中，表示tm时刻载体系相对于初始载体系的方向余弦矩阵；表示σ时刻载体系相对于tm时刻载体系的方向余弦矩阵；

f^b表示比力；δv1表示加速度计输出的一子样；δv2表示加速度计输出的二子样；δθ1表示陀螺仪量测角增量的一子样；δθ2表示陀螺仪量测角增量的二子样；

对参考矢量各积分项进行计算：

式中：表示位置矢量微分；表示τ时刻导航系相对于初始导航系的方向余弦矩阵；rⁿ表示位置矢量；表示tm时刻导航系相对于初始导航系的方向余弦矩阵；表示tk时刻导航系相对于初始导航系的方向余弦矩阵；表示τ时刻导航系相对于tk时刻导航系的方向余弦矩阵；表示导航系相对于惯性系的旋转角速度在导航系的投影；表示σ时刻导航系相对于初始导航系的方向余弦矩阵；表示地球系相对于惯性系的旋转角速度在导航系的投影；gⁿ表示重力加速度在导航系下的投影；δtg表示gnss采样时间；表示tm时刻导航系相对于初始导航系的方向余弦矩阵；表示σ时刻导航系相对于tm时刻导航系的方向余弦矩阵；表示σ时刻导航系相对于tk时刻导航系的方向余弦矩阵；

因此观测矢量可以迭代计算为：

式中，βp(m)表示离散化m时刻的观测矢量；表示

tm时刻导航系相对于初始导航系的方向余弦矩阵；t表示当前时刻；

vⁿ(0)表示初始时刻导航系速度；δtg表示gnss采样时间；表示tm时刻导航系相对于初始导航系的方向余弦矩阵；表示导航系相对于惯性系的旋转角速度在导航系的投影；表示tk时刻导航系相对于初始导航系的方向余弦矩阵；gⁿ表示重力加速度在导航系下的投影；

位置矢量可以通过gnss位置观测进行迭代计算：

式中，rⁿ表示位置矢量；表示k时刻当地曲率矩阵的逆；

表示k时刻gnss位置观测；表示k-1时刻gnss位置观测。

在其中一个实施例中，“利用最优基四元数姿态确定方法，并计算确定姿态与真实姿态之间的误差角。”具体如下：

由最优基四元数姿态确定方法可知，姿态k矩阵可以表示为：

式中：kk表示k时刻的k矩阵；kk-1表示k-1时刻的k矩阵；βp(m)表示m时刻观测矢量；αp(m)表示m时刻参考矢量。

在其中一个实施例中，“获取惯性传感器实时数据并进行姿态更新，定义解算所需的参考坐标系；”中，陀螺仪量测常值漂移误差为ε^b＝[0.020.020.02]^t°/h，陀螺仪量测随机游走误差为

输出频率为200hz；加速度计量测常值漂移误差为陀螺仪量测随机游走误差为

输出频率为200hz。

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

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

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

本发明的有益效果：

本发明采用gnss位置信息进行适量构造，提高了算法的适用范围，可以在较为低端的gnss接收机上进行对准过程；

本发明采用位置信息迭代算法，减小了算法的复杂度，易于在系统上集成；

本发明采用gnss位置矢量进行观测矢量构造，有效的弱化了异常噪声，提高了对准结果的稳定性。

附图说明

图1是本发明gnss位置辅助sins行进间初始对准方法的流程图。

图2是本发明gnss位置辅助sins行进间初始对准方法中的载体运动轨迹及gnss位置量测。

图3是本发明gnss位置辅助sins行进间初始对准方法中的载体运动姿态及速度曲线图。

图4是本发明gnss位置辅助sins行进间初始对准方法中的纵摇角误差曲线图。

图5是本发明gnss位置辅助sins行进间初始对准方法中的横摇角误差曲线图。

图6是本发明gnss位置辅助sins行进间初始对准方法中的航向角误差曲线图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。

本实施例将本发明提出的一种gnss位置辅助sins行进间初始对准方法通过matlab仿真软件进行仿真验证，从而证明对准过程的鲁棒化。仿真硬件环境均为intel(r)core(tm)t9600cpu2.80ghz，4gram，windows7操作系统。如图2和图3所示，为行进间对准过程gnss位置信息曲线及载体运动曲线图，从图中可以看出，gnss信息容易受到外部干扰，造成输出的位置及速度出现较大的波动，从而影响对准结果。图4、图5和图6为gnss辅助sins行进间初始对准误差图，从图中可以看出，采用gnss位置矢量构造观测矢量，对准结果有效的抑制了gnss量测异常噪声的干扰，航向角对准误差可以在50s左右达到1°的对准精度，而传统方法则受到外部异常噪声的干扰造成对准不稳定现象。

本发明是一种gnss位置辅助sins行进间初始对准方法，算法流程如图1所示，包括以下几个步骤：

步骤1：获取惯性传感器实时数据并进行姿态更新，定义解算所需的参考坐标系如下：

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

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

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

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

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

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

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

由姿态更新算法可知：

式中：表示导航系姿态矩阵微分；表示载体系姿态矩阵微分；表示导航系相对于初始导航系的方向余弦矩阵；表示载体系相对于初始载体系的方向余弦矩阵；表示导航系相对于惯性系的旋转角速度在导航系的映射；表示载体系相对于惯性系的旋转角速度在载体系的映射；

利用方向余弦链式法则，可得：

式中：表示载体系相对于导航系的实时方向余弦矩阵；表示初始导航系相对于导航系的方向余弦矩阵；表示载体系相对于初始载体系的方向余弦矩阵；

步骤2：获取gnss实时位置数据：

步骤3：利用获取的gnss位置信息，建立位置法矢量观测器；

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

式中：表示导航系速度的微分；表示载体系相对于导航系变化的方向余弦矩阵；f^b表示比力；表示地球系相对于惯性系的旋转角速度在导航系的投影；表示导航系相对于地球系的转动角速度在导航系的投影；×表示矢量叉乘运算；vⁿ表示导航系速度；gⁿ表示重力加速度在导航系下的投影；

由上面推导可知：

式中：表示导航系速度的微分；表示初始导航系相对于导航系的方向余弦矩阵；表示初始载体系相对于初始导航系的方向余弦矩阵；表示载体系相对于初始载体系的方向余弦矩阵；f^b表示比力；

表示地球系相对于惯性系的旋转角速度在导航系的投影；表示导航系相对于地球系的转动角速度在导航系的投影；×表示矢量叉乘运算；

vⁿ表示导航系速度；gⁿ表示重力加速度在导航系下的投影；

对上式进行计算可得：

式中：βa表示加速度观测矢量；表示初始载体系相对于初始导航系的方向余弦矩阵；αa表示加速度参考矢量；表示导航系相对于初始导航系的方向余弦矩阵；表示导航系速度微分；表示地球系相对于惯性系的旋转角速度在导航系的投影；表示导航系相对于地球系的转动角速度在导航系的投影；×表示矢量叉乘运算；vⁿ表示导航系速度；gⁿ表示重力加速度在导航系下的投影；表示载体系相对于初始载体系的方向余弦矩阵；f^b表示比力；

根据gnss位置信息与导航系速度关系可知：

式中：表示gnss位置微分；rc表示当地曲率矩阵；vⁿ表示导航系速度；表示位置矢量微分；l表示载体当前纬度；rm表示表示子午线曲率半径；rn表示横向曲率半径；h表示载体水平高度；

对加速度观测矢量和参考矢量进行两次积分可得：

式中：βp表示gnss位置观测矢量；αp表示gnss位置参考矢量；

表示初始载体系相对于初始导航系的方向余弦矩阵；表示位置矢量微分；表示τ时刻导航系相对于初始导航系的方向余弦矩阵；

t表示当前时刻；vⁿ(0)表示初始时刻导航系速度；表示σ时刻导航系相对于初始导航系的方向余弦矩阵；表示地球系相对于惯性系的旋转角速度在导航系的投影；gⁿ表示重力加速度在导航系下的投影；

表示σ时刻载体系相对于初始载体系的方向余弦矩阵；f^b表示比力；

步骤4：设计基于迭代原理的矢量观测器；

对上面基于gnss位置的观测矢量和参考矢量进行积分运算，其中参考矢量可以计算为：

式中，αp(m)表示离散化m时刻的参考矢量；δtg表示gnss采样时间；表示tm时刻载体系相对于初始载体系的方向余弦矩阵；表示σ时刻载体系相对于tm时刻载体系的方向余弦矩阵；

f^b表示比力；

其中，积分项可以计算为：

式中，表示tm时刻载体系相对于初始载体系的方向余弦矩阵；表示σ时刻载体系相对于tm时刻载体系的方向余弦矩阵；

f^b表示比力；δv1表示加速度计输出的一子样；δv2表示加速度计输出的二子样；δθ1表示陀螺仪量测角增量的一子样；δθ2表示陀螺仪量测角增量的二子样；

对参考矢量各积分项进行计算：

式中：表示位置矢量微分；表示τ时刻导航系相对于初始导航系的方向余弦矩阵；rⁿ表示位置矢量；表示tm时刻导航系相对于初始导航系的方向余弦矩阵；表示tk时刻导航系相对于初始导航系的方向余弦矩阵；表示τ时刻导航系相对于tk时刻导航系的方向余弦矩阵；表示导航系相对于惯性系的旋转角速度在导航系的投影；表示σ时刻导航系相对于初始导航系的方向余弦矩阵；表示地球系相对于惯性系的旋转角速度在导航系的投影；gⁿ表示重力加速度在导航系下的投影；δtg表示gnss采样时间；表示tm时刻导航系相对于初始导航系的方向余弦矩阵；表示σ时刻导航系相对于tm时刻导航系的方向余弦矩阵；表示σ时刻导航系相对于tk时刻导航系的方向余弦矩阵；

因此观测矢量可以迭代计算为：

式中，βp(m)表示离散化m时刻的观测矢量；表示

tm时刻导航系相对于初始导航系的方向余弦矩阵；t表示当前时刻；

vⁿ(0)表示初始时刻导航系速度；δtg表示gnss采样时间；表示tm时刻导航系相对于初始导航系的方向余弦矩阵；表示导航系相对于惯性系的旋转角速度在导航系的投影；表示tk时刻导航系相对于初始导航系的方向余弦矩阵；gⁿ表示重力加速度在导航系下的投影；

位置矢量可以通过gnss位置观测进行迭代计算：

式中，rⁿ表示位置矢量；表示k时刻当地曲率矩阵的逆；

表示k时刻gnss位置观测；表示k-1时刻gnss位置观测；

步骤5：利用最优基四元数姿态确定方法，并计算确定姿态与真实姿态之间的误差角；

由最优基四元数姿态确定方法可知，姿态k矩阵可以表示为：

式中：kk表示k时刻的k矩阵；kk-1表示k-1时刻的k矩阵；βp(m)表示m时刻观测矢量；αp(m)表示m时刻参考矢量；

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

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

陀螺仪量测常值漂移误差为ε^b＝[0.020.020.02]^t°/h，陀螺仪量测随机游走误差为输出频率为200hz；加速度计量测常值漂移误差为陀螺仪量测随机游走误差为输出频率为200hz。gnss采样周期为1s。步骤6中，m＝600。

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

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

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

以上所述实施例仅是为充分说明本发明而所举的较佳的实施例，本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或变换，均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。