CN108592943A - 一种基于opreq方法的惯性系粗对准计算方法 - Google Patents

Abstract

本发明公开了一种基于OPREQ方法的惯性系粗对准计算方法，将初始姿态矩阵的计算转换为常值姿态矩阵的确定，采用OPREQ方法来求解姿态矩阵所对应的最优四元数。本发明能够通过自适应调节增益大小，能有效地对观测矢量中的噪声进行滤波处理，从而达到提高粗对准收敛性能的效果，作为一种静基座及摇摆基座粗对准方法，具有很好的工程参考和应用价值。

Description

一种基于OPREQ方法的惯性系粗对准计算方法

技术领域

本发明属于捷联惯性导航系统初始对准技术，具体涉及一种基于OPREQ方法的惯性系粗对准计算方法。

背景技术

捷联惯性导航系统(SINS)是利用惯性传感器的测量来计算载体相对于初始点的位置和方位的一种自主系统。因此，用于求解初始姿态的初始对准技术的发展对实现高精度导航具有重要意义。初始对准的有两个重要的性能指标，一个是对准精度，另一个是收敛速度。如何在较短的时间内获得较高精度的初始姿态是惯性导航领域的一个重要的研究热点。

根据定位过程中，初始对准一般分为两个阶段。第一阶段称为粗对准过程，粗对准主要利用地球引力和地球自转角速度来确定一个粗略的初始姿态矩阵，粗对准的贡献主要体现在对准速度上。因此，一种有效的粗对准方法可以减少对准时间，从而使系统快速进入导航状态。第二阶段是精对准过程，精对准是在粗对准的基础上更准确地确定初始姿态。在粗对准过程中，失准角可以收敛到一个小角度范围，从而使捷联惯性导航系统的非线性误差模型近似简化为线性误差模型。然后，在精对准阶段就可以采用线性卡尔曼滤波器来获取精确的初始姿态。另外在采用线性卡尔曼滤波器进行精对准过程中，可以估计出惯性传感器的偏置，从而进一步减小失调角。因此，粗对准过程是精对准的前提和基础，且粗对准的性能将直接影响精对准的结果。在实际应用中，设计具有收敛速度快、对准精度高的粗对准算法具有重要意义。

惯性系粗对准方法将初始对准问题总结为姿态确定问题，姿态确定问题一般有两种解决方案，一种是直接计算姿态矩阵，例如双矢量定姿方法，但该方法精度差，且更新率较低；另一种是求解姿态矩阵对应的姿态四元数，例如REQUEST算法，该方法通过迭代构造K矩阵的方式简化了算法计算量，且K矩阵最大特征值对应的特征向量即为姿态四元数；但该方法的权值是固定的，使得该方法在整个姿态确定过程中无法取得最优的效果。

发明内容

发明目的：本发明的目的在于解决现有技术中存在的由于惯性传感器获取的观测向量通常含有各种噪声，影响了粗对准的收敛速度和精度的不足，提供一种基于OPREQ方法的惯性系粗对准计算方法。

技术方案：本发明提供一种基于OPREQ方法的惯性系粗对准计算方法，通过将初始姿态矩阵的计算转化为常值姿态矩阵的确定，其包括如下步骤：

(1)获取传感器实时数据；

(2)根据地球自转角速度及载体纬度数据，计算出坐标变换矩阵及

(3)根据步骤(1)得到的陀螺仪输出数据更新计算坐标变换矩阵

(4)根据步骤(2)、(3)得到的坐标变换矩阵、步骤(1)得到的加速度计输出数据、及地球重力矢量，计算出参考矢量及观测矢量；

(5)根据步骤(4)中得到的参考矢量及观测矢量，利用OPREQ方法求解量测更新矩阵K_k+1/k+1；

(6)根据步骤(5)中得到的量测更新矩阵，求解常值姿态矩阵对应的最优姿态四元数；

(7)根据步骤(2)、(3)、(6)中得到的坐标变换矩阵，利用矩阵链式法则计算初始姿态矩阵

(8)重复步骤(1)到(7)，实时更新计算初始姿态矩阵直至对准时间结束。

进一步地，初始姿态矩阵的计算转化为常值姿态矩阵的确定，其具体的计算步骤如下：

(1.1)将初始姿态矩阵分解为四个坐标变换矩阵：

其中，e0系是e坐标系在初始时刻相对地球自转保持静止的惯性系，b0系是b坐标系在初始时刻相对地球自转保持静止的惯性系；

(1.2)由步骤(1.1)得，导航坐标系n系与地心地球坐标系e系之间的坐标转换矩阵为：

其中，L表示载体所在位置的纬度。

(1.3)由步骤(1.1)得，地心地球坐标系e系与地心惯性坐标系e0系之间的坐标变换矩阵为：

其中，ω_ie表示地球自转角速度。

(1.4)由步骤(1.1)得，载体坐标系b系与b0系之间的姿态矩阵可以利用陀螺仪的输出实时计算，即求解如下的姿态矩阵微分方程：

其中，表示载体系相对惯性系的旋转角速度在载体系下的投影，可用陀螺仪测量得到；I₃表示3阶单位矩阵。

(1.5)由步骤(1.1)、(1.2)、(1.3)、(1.4)得，计算初始姿态矩阵就转化为求解b0系和e0系之间的常值姿态矩阵

进一步地根据所述步骤(4)构造惯性系粗对准观测模型，其具体步骤如下：

(2.1)当载体没有线运动的情况下，加速度计的输出f^b可以看作由三部分信息组成：重力加速度矢量在b系的投影g^b，加速度计偏置和外界干扰加速度a^b，即：

(2.2)由步骤(2.1)得，f^b在b0系中的投影可表示为：

(2.3)步骤(2.2)中参数g^e0表示地球重力矢量在e0系下的投影，具体计算如下：

(2.4)由步骤(2.2)得，为抑制干扰加速度的影响，对上式两端积分：

(2.5)由步骤(2.4)得，记并忽略上式右侧第二项可得：

其中，

(2.6)由步骤(2.5)得，将矢量V^b0(t)和V^e0(t)单位化处理，分别记为b和r，

(2.7)由步骤(2.6)得，惯性系粗对准观测模型可表示为：

进一步地，所述步骤(5)和步骤(6)中采用OPREQ算法求解常值姿态矩阵所对应的姿态四元数，其具体步骤如下：

(3.1)设一组t_k时刻获得的矢量表示为b_i,r_i,i＝1,2,…,n，对应的权值为a_i，其中，

定义4×4对称矩阵K如下：

其中3×3矩阵S_k，列向量z_k，以及标量σ_k分别定义如下：

其中tr(·)表示欧几里得范数；

(3.2)设t_k+1时刻获得的矢量记为b_k+1,r_k+1，相应的权值记为a_k+1。则对应t_k+1时刻的矩阵K定义如下，并用δK_k+1表示：

其中3阶矩阵S_k+1，列向量z_k+1，及标量σ_k+1分别定义如下：

(3.3)由步骤(3.1)得，定义4阶矩阵为：

其中，是3阶子矩阵，表达式和分别计算如下：

其中，μ_k是观测矢量b_k中所包含误差的标准差；

(3.4)初始化设置参数矩阵K_0/0、方差矩阵P、及比例系数m₀分别为：

K_0/0＝δK₀

m₀＝δm₀＝1

(3.5)由步骤(3.3)、(3.4)得，量测更新方程的权值计算如下：

(3.6)由步骤(3.5)得，比例系数更新方程计算如下：

(3.7)由步骤(3.1)、(3.2)、(3.5)、(3.6)得，量测更新方程计算如下：

(3.8)由步骤(3.3)、(3.4)、(3.5)、(3.6)得，方程矩阵更新方程计算如下：

(3.9)由步骤(3.5)、(3.6)、(3.7)、(3.8)得，可以迭代计算得到t_k+1时刻的K_k+1/k+1矩阵，而K_k+1/k+1矩阵最大特征值对应的特征向量即为常值姿态矩阵对应的姿态四元数。

进一步地，利用计算得到的常值姿态矩阵所对应的姿态四元数求解初始姿态矩阵及初始姿态角，其具体步骤如下：

(4.1)由计算得到的姿态四元数，对应的常值姿态矩阵计算如下：

其中，q＝[q₀ q₁ q₂ q₃]^T表示姿态四元数。

(4.2)初始姿态矩阵可计算如下：

其中，θ,γ,ψ分别表示载体的纵摇角、横摇角及航向角。

(4.3)由步骤(4.2)得，可以根据初始姿态矩阵计算得到欧拉角：

有益效果：本发明与现有技术相比，具备如下优点：

(1)本发明采用OPREQ算法来计算惯性系粗对准中的常值姿态矩阵，能够根据观测噪声自适应调节滤波增益，使得该粗对准方法收敛速度更快且收敛结果更加稳定。

(2)本发明在求解滤波器增益及量测更新时，采用迭代计算的方式，减小了对准算法的计算量，提高了对准算法的实时性。

附图说明

图1为本发明的算法整体流程图；

图2为本发明粗对准纵摇角姿态误差曲线图；

图3为本发明粗对准横摇角姿态误差曲线图；

图4为本发明粗对准航向角姿态误差曲线图。

具体实施方式

本发明提供一种基于OPREQ方法的惯性系粗对准计算方法，其包括如下步骤：

(1)获取传感器实时数据；

(2)根据地球自转角速度及载体纬度数据，计算出坐标变换矩阵及

(3)根据步骤(1)得到的陀螺仪输出数据更新计算坐标变换矩阵

(4)根据步骤(2)、(3)得到的坐标变换矩阵、步骤(1)得到的加速度计输出数据、及地球重力矢量，计算出参考矢量及观测矢量；

(5)根据步骤(4)中得到的参考矢量及观测矢量，利用OPREQ方法求解量测更新矩阵K_k+1/k+1；

(6)根据步骤(5)中得到的量测更新矩阵，求解常值姿态矩阵对应的最优姿态四元数；

(7)根据步骤(2)、(3)、(6)中得到的坐标变换矩阵，利用矩阵链式法则计算初始姿态矩阵

(8)重复步骤(1)到(7)，实时更新计算初始姿态矩阵直至对准时间结束。

如图1所示，初始姿态矩阵的计算转化为常值姿态矩阵的确定，其具体的算法步骤如下：

(1.1)将初始姿态矩阵分解为四个坐标变换矩阵：

其中，e0系是e坐标系在初始时刻相对地球自转保持静止的惯性系，b0系是b坐标系在初始时刻相对地球自转保持静止的惯性系；

(1.2)由步骤(1.1)得，导航坐标系n系与地心地球坐标系e系之间的坐标转换矩阵为：

其中，L表示载体所在位置的纬度。

(1.3)由步骤(1.1)得，地心地球坐标系e系与地心惯性坐标系e0系之间的坐标变换矩阵为：

(1.4)由步骤(1.1)得，载体坐标系b系与b0系之间的姿态矩阵可以利用陀螺仪的输出实时计算，即求解如下的姿态矩阵微分方程：

其中，表示载体系相对惯性系的旋转角速度在载体系下的投影，可用陀螺仪测量得到；I₃表示3阶单位矩阵。

(1.5)由步骤(1.1)、(1.2)、(1.3)、(1.4)得，计算初始姿态矩阵就转化为求解b0系和e0系之间的常值姿态矩阵

进一步地根据所述步骤(4)构造惯性系粗对准观测模型，其具体步骤如下：

(2.1)当载体没有线运动的情况下，加速度计的输出f^b可以看作由三部分信息组成：重力加速度矢量在b系的投影g^b，加速度计偏置和外界干扰加速度a^b，即：

(2.2)由步骤(2.1)得，f^b在b0系中的投影可表示为：

(2.3)步骤(2.2)中参数g^e0表示地球重力矢量在e0系下的投影，具体计算如下：

(2.4)由步骤(2.2)得，为抑制干扰加速度的影响，对上式两端积分：

(2.5)由步骤(2.4)得，记并忽略上式右侧第二项可得：

其中，

(2.6)由步骤(2.5)得，将矢量V^b0(t)和V^e0(t)单位化处理，分别记为b和r，

(2.7)由步骤(2.6)得，惯性系粗对准观测模型可表示为：

所述步骤(5)和步骤(6)中采用OPREQ算法求解常值姿态矩阵所对应的姿态四元数，其具体步骤如下：

(3.1)设一组t_k时刻获得的矢量表示为b_i,r_i,i＝1,2,…,n，对应的权值为a_i，其中，定义4×4对称矩阵K如下：

其中3×3矩阵S_k，列向量z_k，以及标量σ_k分别定义如下：

其中tr(·)表示欧几里得范数；

(3.2)设t_k+1时刻获得的矢量记为b_k+1,r_k+1，相应的权值记为a_k+1。则对应t_k+1时刻的矩阵K定义如下，并用δK_k+1表示：

其中3阶矩阵S_k+1，列向量z_k+1，及标量σ_k+1分别定义如下：

(3.3)由步骤(3.1)得，定义4阶矩阵为：

其中，是3阶子矩阵，表达式和分别计算如下：

其中，μ_k是观测矢量b_k中所包含误差的标准差；

(3.4)初始化设置参数矩阵K_0/0、方差矩阵P、及比例系数m₀分别为：

K_0/0＝δK₀

m₀＝δm₀＝1

(3.5)由步骤(3.3)、(3.4)得，量测更新方程的权值计算如下：

(3.6)由步骤(3.5)得，比例系数更新方程计算如下：

(3.7)由步骤(3.1)、(3.2)、(3.5)、(3.6)得，量测更新方程计算如下：

(3.8)由步骤(3.3)、(3.4)、(3.5)、(3.6)得，方程矩阵更新方程计算如下：

(3.9)由步骤(3.5)、(3.6)、(3.7)、(3.8)得，可以迭代计算得到t_k+1时刻的K_k+1/k+1矩阵，而K_k+1/k+1矩阵最大特征值对应的特征向量即为常值姿态矩阵对应的姿态四元数。

利用计算得到的常值姿态矩阵所对应的姿态四元数求解初始姿态矩阵及初始姿态角，其具体步骤如下：

(4.1)由计算得到的姿态四元数，对应的常值姿态矩阵计算如下：

其中，q＝[q₀ q₁ q₂ q₃]^T表示姿态四元数。

(4.2)初始姿态矩阵可计算如下：

其中，θ,γ,ψ分别表示载体的纵摇角、横摇角及航向角。

(4.3)由步骤(4.2)得，可以根据初始姿态矩阵计算得到欧拉角：

本实施例将本发明提出的一种基于OPREQ方法的惯性系粗对准计算方法通过MATLAB仿真软件进行仿真实验效果,从而证明观测矢量存在噪声时，本发明在粗对准快速性方面的优势。

仿真实验中惯性传感器的性能指标设置如下：陀螺常值漂移：0.01°/h；陀螺随机漂移：0.01°/h；加速度计常值偏置：50μg；加速度计随机偏置：50μg。经纬度设置为：纬度32°(N)，经度118°(E)。

仿真实验在摇摆基座下进行，三轴摇摆的运动方式设置为：内框摆幅为3°，频率为0.15Hz；中框摆幅为3°，频率为0.2Hz；外框摆幅为2°，频率为0.125Hz。三轴同时进行摇摆运动，以此运动状态模拟舰船实际的应用环境，仿真实验进行200s。

图2-4显示了上述实施例基于OPREQ方法的惯性系粗对准计算方法的三个姿态角的误差曲线，

图2-图4中纵轴分别表示纵摇角误差、横摇角误差及航向角误差，单位为度；横轴均为时间，单位为秒。

从图2-图3中可知，粗对准水平角误差保持在极限对准精度范围内；且从图4中航向角误差曲线中可以看出，对准时间20s以后，航向角误差保持在0.05°范围以内，并在对准时间40s以后，航向角误差一直稳定在0.03度左右；相比于传统粗对准方法，该粗对准方法收敛速度明显加快。实验结果表明本发明能够有效提高粗对准收敛速度，并使得收敛结果更加稳定。

Claims (5)

1.一种基于OPREQ方法的惯性系粗对准计算方法，其特征在于：通过将初始姿态矩阵的计算转化为常值姿态矩阵的确定，其包括如下步骤：

(1)获取传感器实时数据；

(2)根据地球自转角速度及载体纬度数据，计算出坐标变换矩阵及

(3)根据步骤(1)得到的陀螺仪输出数据更新计算坐标变换矩阵

(4)根据步骤(2)、(3)得到的坐标变换矩阵、步骤(1)得到的加速度计输出数据、及地球重力矢量，计算出参考矢量及观测矢量；

(5)根据步骤(4)中得到的参考矢量及观测矢量，利用OPREQ方法求解量测更新矩阵K_k+1/k+1；

(6)根据步骤(5)中得到的量测更新矩阵，求解常值姿态矩阵对应的最优姿态四元数；

(7)根据步骤(2)、(3)、(6)中得到的坐标变换矩阵，利用矩阵链式法则计算初始姿态矩阵

(8)重复步骤(1)到(7)，实时更新计算初始姿态矩阵直至对准时间结束。

2.根据权利要求1所述的一种基于OPREQ方法的惯性系粗对准计算方法，其特征在于：初始姿态矩阵的计算转化为常值姿态矩阵的确定，其具体的计算步骤如下：

(1.1)将初始姿态矩阵分解为四个坐标变换矩阵：

其中，e0系是e坐标系在初始时刻相对地球自转保持静止的惯性系，b0系是b坐标系在初始时刻相对地球自转保持静止的惯性系；

(1.2)由步骤(1.1)得，导航坐标系n系与地心地球坐标系e系之间的坐标转换矩阵为：

其中，L表示载体所在位置的纬度。

(1.3)由步骤(1.1)得，地心地球坐标系e系与地心惯性坐标系e0系之间的坐标变换矩阵为：

其中，ω_ie表示地球自转角速度。

(1.4)由步骤(1.1)得，载体坐标系b系与b0系之间的姿态矩阵可以利用陀螺仪的输出实时计算，即求解如下的姿态矩阵微分方程：

其中，表示载体系相对惯性系的旋转角速度在载体系下的投影；I₃表示3阶单位矩阵。

(1.5)由步骤(1.1)、(1.2)、(1.3)、(1.4)得，计算初始姿态矩阵就转化为求解b0系和e0系之间的常值姿态矩阵

3.根据权利要求1所述的一种基于OPREQ方法的惯性系粗对准计算方法，其特征在于：根据所述步骤(4)构造惯性系粗对准观测模型，其具体步骤如下：

(2.1)当载体没有线运动的情况下，加速度计的输出f^b可以看作由三部分信息组成：重力加速度矢量在b系的投影g^b，加速度计偏置和外界干扰加速度a^b，即：

(2.2)由步骤(2.1)得，f^b在b0系中的投影可表示为：

(2.3)步骤(2.2)中参数g^e0表示地球重力矢量在e0系下的投影，具体计算如下：

(2.4)由步骤(2.2)得，为抑制干扰加速度的影响，对上式两端积分：

(2.5)由步骤(2.4)得，记并忽略上式右侧第二项可得：

其中，

(2.6)由步骤(2.5)得，将矢量V^b0(t)和V^e0(t)单位化处理，分别记为b和r，

(2.7)由步骤(2.6)得，惯性系粗对准观测模型可表示为：

4.根据权利要求1或2所述的一种基于OPREQ方法的惯性系粗对准计算方法，其特征在于：所述步骤(5)和步骤(6)中采用OPREQ算法求解常值姿态矩阵所对应的姿态四元数，其具体步骤如下：

(3.1)设一组t_k时刻获得的矢量表示为b_i,r_i,i＝1,2,…,n，对应的权值为a_i，

其中，定义4×4对称矩阵K如下：

其中3×3矩阵S_k，列向量z_k，以及标量σ_k分别定义如下：

其中tr(·)表示欧几里得范数；

(3.2)设t_k+1时刻获得的矢量记为b_k+1,r_k+1，相应的权值记为a_k+1。则对应t_k+1时刻的矩阵K定义如下，并用δK_k+1表示：

其中3阶矩阵S_k+1，列向量z_k+1，及标量σ_k+1分别定义如下：

(3.3)由步骤(3.1)得，定义4阶矩阵为：

其中，是3阶子矩阵，表达式和分别计算如下：

其中，μ_k是观测矢量b_k中所包含误差的标准差；

(3.4)初始化设置参数矩阵K_0/0、方差矩阵P、及比例系数m₀分别为：

K_0/0＝δK₀

m₀＝δm₀＝1

(3.5)由步骤(3.3)、(3.4)得，量测更新方程的权值计算如下：

(3.6)由步骤(3.5)得，比例系数更新方程计算如下：

(3.7)由步骤(3.1)、(3.2)、(3.5)、(3.6)得，量测更新方程计算如下：

(3.8)由步骤(3.3)、(3.4)、(3.5)、(3.6)得，方程矩阵更新方程计算如下：

(3.9)由步骤(3.5)、(3.6)、(3.7)、(3.8)得，可以迭代计算得到t_k+1时刻的K_k+1/k+1矩阵，而K_k+1/k+1矩阵最大特征值对应的特征向量即为常值姿态矩阵对应的姿态四元数。

5.根据权利要求4所述的一种基于OPREQ方法的惯性系粗对准计算方法，其特征在于：利用计算得到的常值姿态矩阵所对应的姿态四元数求解初始姿态矩阵及初始姿态角，其具体步骤如下：

(4.1)由计算得到的姿态四元数，对应的常值姿态矩阵计算如下：

其中，q＝[q₀ q₁ q₂ q₃]^T表示姿态四元数。

(4.2)初始姿态矩阵可计算如下：

其中，θ,γ,ψ分别表示载体的纵摇角、横摇角及航向角。

(4.3)由步骤(4.2)得，可以根据初始姿态矩阵计算得到欧拉角：