CN110398257B - Gps辅助的sins系统快速动基座初始对准方法 - Google Patents

Abstract

本发明涉及一种GPS辅助的SINS系统快速动基座初始对准方法，属于组合导航的技术领域。包括使用GPS数据和SINS数据计算导航系姿态变化和载体系姿态变化；构建SINS/GPS系统的状态方程和量测方程；利用简化的自适应无迹卡尔曼滤波算法对失准角和陀螺仪常值漂移进行估计；将估计的失准角和陀螺仪常值漂移对SINS系统对准的姿态和陀螺仪的输出进行矫正；实现SINS系统的快速动基座初始对准。本发明通过对算法中失准角和陀螺仪误差进行估计，提高了对准的精度和收敛性能，使用自适应算法对量测噪声进行估计，避免了以往算法中量测噪声协方差矩阵设置不准确的问题，提高了对准算法的稳定性，对无迹卡尔曼滤波进行化简，减少了对准算法的计算量；应用前景广阔。

Description

GPS辅助的SINS系统快速动基座初始对准方法

技术领域

本发明涉及一种GPS辅助的SINS系统快速动基座初始对准方法，属于组合导航的技术领域。

背景技术

捷联式惯性导航系统(SINS)根据陀螺仪输出的角速率和加速度计输出的比力可以计算出载体的姿态、速度和位置，能够为许多军事和民用设备提供丰富的导航信息。SINS系统在导航前需要知道准确的初始导航信息，初始导航信息的准确与否决定了接下来的导航是否可靠。因此，精确的初始对准是SINS系统准确导航的关键。对于高精度SINS系统，系统可以在静基座环境下通过感应地球自转和重力加速度确定出初始姿态，系统的位置和速度可以通过GPS系统得到。但是在一些紧急情况下，SINS系统需要在运动过程中进行初始对准，不能满足静基座对准的条件。对与低精度的SINS系统，例如MEMS-IMU，系统本身的噪声大于地球自转角速率，同样无法进行静基座对准，需要在外部传感器辅助的情况下通过设备的运动完成初始对准。

在动基座对准过程中，由于载体在实时的运动，无法像静基座对准一样获取载体系和导航系之间的捷联矩阵，从而导致传统的惯性导航误差方程不再适用。现阶段，动基座对准算法的解决方法可大致分为两类：一种是导航系下使用非线性卡尔曼滤波进行对准的算法；另一种是惯性系下基于Wahba's problem的姿态矩阵求解算法。传统的惯性导航误差方程是建立在静态对准完成后姿态误差是小角的前提下，而导航系下非线性滤波算法则是假设航向角误差是大失准角或是所有姿态角误差均为大失准角，这种情况下建立的系统误差模型为非线性，需要使用非线性滤波算法进行估计，且对准的速度与初始姿态误差有很大的关系。导航系下非线性滤波算法没有了传统算法的小失准角假设，误差模型精确，即使有大失准角也能完成动基座对准，但是算法的计算量大且收敛速度慢。与导航系下非线性对准算法不同，惯性系下初始对准算法是将载体系与导航系之间的姿态计算问题转化为惯性系下两惯性坐标系之间的常值姿态矩阵的计算问题，并利用矢量观测对其进行估计，所以收敛速度快，但是该方法原理上是一个开环的算法，不具备对惯性器件误差估计和补偿的能力，因此该算法的误差会随着时间增大，当这种方法用在低精度的惯性导航系统中时，对准的精度会降低，且会随着对准时间的延长而逐渐发散。

发明内容

本发明的目的是为了提高基座对准的精度以及收敛性能而提供一种GPS辅助的SINS系统快速动基座初始对准方法，本发明对基于优化的对准算法中的误差进行了分析，建立了新的SINS/GPS系统误差模型，并利用简化的自适应无迹卡尔曼滤波算法进行滤波估计，将估计结果对惯导解算结果进行反馈校正，从而在保证对准速度的同时提高初始对准的精度和收敛性能，以达到在任意失准角条件下快速动基座对准的目的。

本发明的目的是这样实现的，GPS辅助的SINS系统快速动基座初始对准方法，具体包括以下步骤：

步骤1、采集SINS和GPS系统输出数据；

步骤2、SINS系统对准参数初始化；

步骤3、使用GPS数据和SINS数据分别计算出导航系姿态变化和载体系姿态变化；

步骤4、确定初始载体系和初始导航系下的α_v和β_v矢量，并根据最小二乘原理估计初始姿态矩阵；

步骤5、选取状态变量，构建SINS/GPS系统的状态方程和量测方程；

步骤6、使用简化的自适应无迹卡尔曼滤波算法估计失准角和陀螺仪常值漂移；

步骤7、用估计的失准角对载体系姿态变化矩阵进行反馈，用估计的陀螺仪常值漂移对陀螺仪输出进行校正；

步骤8、使用反馈后的导航系和载体系变化矩阵以及估计的初始姿态矩阵计算姿态矩阵；

步骤9、根据对准时间判断动基座对准是否完成，若没完成则返回步骤1继续对准，若对准完成，则结束动基座对准。

本发明还包括这样一些结构特征：

1、所述步骤3计算出导航系姿态变化和载体系姿态变化具体包括以下步骤：

3.1、导航系姿态变化矩阵更新：

式中，n表示导航系，t_k＝kT，k是采样次数，T是采样时间，

为当前时刻t_k到初始0时刻的导航系姿态变化矩阵，

为时刻t_k-1到初始0时刻的导航系姿态变化矩阵，矩阵

为时刻t_k到时刻t_k-1的导航系姿态变化矩阵，其计算公式为：

式中，I是三维单位矩阵，φ_n是由n(t_k-1)至n(t_k)的等效旋转矢量，

是导航系相对于惯性系的角速率，φ_n×是φ_n的反对称矩阵，||φ_n||是φ_n的模，其计算公式为：

式中，v_E和v_N是东向和北向速度，ω_ie是地球自转角速率，L和h是纬度和高度，R_M和R_N是子午圈和卯酉圈主曲率半径；

3.2、载体系姿态变化矩阵更新：

式中，b表示载体系，

为当前时刻t_k到初始0时刻的载体系姿态变化矩阵，

为时刻t_k-1到初始0时刻的载体系姿态变化矩阵，矩阵

为时刻t_k到时刻t_k-1的载体系姿态变化矩阵，其计算公式为：

式中，φ_b是由b(t_k-1)至b(t_k)的等效旋转矢量，Δθ₁和Δθ₂是陀螺仪两个相邻时刻的角增量输出。

2、所述步骤4确定初始载体系和初始导航系下的α_v和β_v矢量，并根据最小二乘原理估计初始姿态矩阵

估计具体包括以下步骤：

4.1、矢量计算：

式中，α_v(t)是初始载体系下的矢量，β_v(t)是初始导航系下的矢量，f^b是加速度计输出，gⁿ是重力加速度；

4.2、初始姿态矩阵

估计：

设四元数

其中q₀为标量部分，上标T表示对矩阵求转置，q_v＝[q₁ q₂q₃]^T为矢量部分，定义四元数乘法的两种反对称矩阵：

其中I为单位矩阵，矢量α_v和β_v可以看成标量部分为0的四元数，定义代价函数：

式中，

为初始姿态矩阵

对应的姿态四元数，根据最小二乘原理，矩阵K的最小特征值对应的特征向量即为最优的

从而便得到最优初始姿态矩阵

其中，

满足约束条件：

3、所述步骤5中构建SINS/GPS系统的状态方程和量测方程具体包括以下步骤：

5.1、选择系统状态变量X：

式中，

是载体系姿态矩阵对应的失准角，ε^b是陀螺仪的常值漂移；

5.2、建立SINS/GPS对准系统的状态方程：

式中，

是状态变量

的导数，

是陀螺仪实际的角速率输出，0_3×1代表三行一列的全零矩阵，上述状态方程可表示为：

式中，f(·)表示非线性状态方程；W_k是均值为零、协方差为Q_k的系统白噪声，其大小由陀螺仪的噪声决定；

5.3、建立SINS/GPS对准系统的量测方程：

Z_k＝HX_k+V_k

式中，量测方程中的V_k是均值为零的量测白噪声，其协方差记为R_k，矩阵Z_k和H_k分别为：

式中，

表示计算出的载体坐标系b系，矩阵

是实际中计算出的带误差的矩阵

矩阵H_k中的矢量

为当前载体系b(t_k)下的矢量，其计算公式为：

4、所述步骤6中简化的自适应无迹卡尔曼滤波算法具体包括以下步骤：

6.1、选择滤波初值和权值W_i ^(c)以及W_i ^(m)计算：

式中，

是状态变量X在第0时刻估计值即所选状态变量滤波初始值，P₀是状态变量误差方差矩阵在0时刻的滤波初始值，n是状态变量的维数，滤波参数α可取值为10^-4≤α≤1，滤波参数κ＝3-n，当X服从正太分布时，滤波参数β取值为2，λ＝α²(n+κ)-n；

6.2、Sigma样本点

计算：

式中，

6.3、计算Sigma采样点一步预测

6.4、状态变量的一步预测

及一步预测均方误差矩阵P_k/k-1的计算：

6.5、根据式系统状态方程和量测方程可看出，系统的状态方程为非线性，量测方程为线性，所以在使用UKF算法时可对量测更新部分做进一步简化，减少采样次数和计算量，简化后的量测更新公式为：

式中，

为量测Z_k的一步预测，P_ZZ表示量测一步预测误差方差矩阵，P_XZ是状态变量一步预测误差与量测一步预测误差间的协方差矩阵；

6.6、滤波增益K_k计算：

式中，

表示矩阵P_ZZ的逆；

6.7、系统的状态估计

更新和状态估计的均方误差P_k更新：

6.8、在进行UKF算法滤波时，量测噪声方差矩阵R设置的不准确会导致系统滤波的不稳定，在系统建模过程中，量测方程由速度、比力等的积分构成，很难通过手动调试将量测噪声方差矩阵设置为准确值，针对这一问题，需使用Sage-Husa自适应滤波算法对量测噪声方差矩阵R进行实时估计，从而提高系统的滤波精度和稳定性，对量测噪声方差矩阵R的估计公式为：

式中，

是量测一步预测误差，权值β_k的初始值取0，b为渐消因子，且0＜b＜1，一般情况下，b取0.9～0.999。

5、所述步骤7用估计的失准角对载体系姿态变化矩阵进行反馈，用估计的陀螺仪常值漂移对陀螺仪输出进行校正具体包括以下步骤：

7.1、姿态失准角反馈：

因为所估计的姿态失准角是小角，所以将反馈公式近似为：

式中，矩阵

是由估计的失准角构成的误差反馈矩阵，

为未经反馈补偿的载体系变化矩阵，执行反馈后，对相应的状态变量置零；

7.2、陀螺仪误差反馈：

ε^b＝ε^b+[X_k(4:6)]^T

式中，[X_k(4:6)]为状态变量中的第4至第6行，

为补偿后的陀螺仪角速率，陀螺仪常值漂移对应的状态量在反馈后需置零。

6、所述步骤8中姿态矩阵为：

即为初始对准所得姿态矩阵。

与现有技术相比，本发明的有益效果是：本发明推导了一种新的SINS/GPS系统状态方程和量程方程，对算法中的失准角和陀螺仪误差进行估计，进而提高对准的精度以及收敛性能。并且，本发明根据量测方程是积分形式的特点，使用自适应算法对量测噪声进行估计，避免了以往算法中量测噪声协方差矩阵设置不准确的问题，提高了对准算法的稳定性。因为建立的量测方程为线性，本发明对无迹卡尔曼滤波进行化简，减少了对准算法的计算量。

附图说明

图1是本发明GPS辅助的SINS系统快速动基座初始对准方法原理图；

图2是仿真运动轨迹；

图3是仿真航向角变化；

图4是本发明FIMA对准算法与OBA算法姿态仿真误差对比；

图5是本发明FIMA对准算法与EKF算法姿态仿真误差对比；

图6是本发明FIMA对准算法和EKF算法估计的常值漂移与理论常值漂移的仿真对比；

图7是跑车实验过程中的运动轨迹；

图8是参考姿态实验变化；

图9是本发明FIMA对准算法与OBA算法姿态实验误差对比；

图10是FIMA对准算法对准姿态实验误差放大图；

图11是FIMA对准算法对陀螺仪常值漂移的实验估计；

图12是纯惯导算法姿态实验误差对比；

图13是本发明FIMA对准算法与EKF算法姿态实验误差对比。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

本发明为GPS辅助的SINS系统快速动基座初始对准方法，图1是本发明流程图，具体步骤包括：

(1)采集SINS和GPS系统输出数据；

(2)SINS系统对准参数初始化；

(3)使用GPS数据和SINS数据分别计算出导航系姿态变化和载体系姿态变化；

(4)确定初始载体系和初始导航系下的和矢量，并根据最小二乘原理估计初始姿态矩阵；

(5)选取状态变量，构建SINS/GPS系统的状态方程和量测方程；

(6)使用简化的自适应无迹卡尔曼滤波算法估计失准角和陀螺仪常值漂移；

(7)用估计的失准角对载体系姿态变化矩阵进行反馈，用估计的陀螺仪常值漂移对陀螺仪输出进行校正；

(8)使用反馈后的相应参数计算姿态矩阵，实现动基座对准。

本发明GPS辅助的SINS系统快速动基座初始对准方法还包括：

1.计算导航系和载体系姿态变化

(1)导航系姿态变化矩阵更新：

其中，n表示导航系，t_k＝kT，k是采样次数，T是采样时间，

为当前时刻t_k到初始0时刻的导航系姿态变化矩阵，

为时刻t_k-1到初始0时刻的导航系姿态变化矩阵，矩阵

为时刻t_k到时刻t_k-1的导航系姿态变化矩阵，其计算公式为：

其中，I是三维单位矩阵，φ_n是由n(t_k-1)至n(t_k)的等效旋转矢量，

是导航系相对于惯性系

的角速率，φ_n×是φ_n的反对称矩阵，||φ_n||是φ_n的模，其计算公式为：

其中，v_E和v_N是东向和北向速度，ω_ie是地球自转角速率，L和h是纬度和高度，R_M和R_N是子午圈和卯酉圈主曲率半径。

(2)载体系姿态变化矩阵更新：

其中，b表示载体系，

为当前时刻t_k到初始0时刻的载体系姿态变化矩阵，

为时刻t_k-1到初始0时刻的载体系姿态变化矩阵，矩阵

为时刻t_k到时刻t_k-1的载体系姿态变化矩阵，其计算公式为：

其中，φ_b是由b(t_k-1)至b(t_k)的等效旋转矢量，Δθ₁和Δθ₂是陀螺仪两个相邻时刻的角增量输出。2.初始载体系和初始导航系下的α_v和β_v矢量计算及初始姿态矩阵

估计

(1)矢量计算：

其中，α_v(t)是初始载体系下的矢量，β_v(t)是初始导航系下的矢量，f^b是加速度计输出，gⁿ是重力加速度。

(2)初始姿态矩阵

估计：

设四元数

其中q₀为标量部分，上标T表示对矩阵求转置，q_v＝[q₁ q₂q₃]^T为矢量部分，定义四元数乘法的两种反对称矩阵：

其中I为单位矩阵，矢量α_v和β_v可以看成标量部分为0的四元数，定义代价函数：

其中，

为初始姿态矩阵

对应的姿态四元数，根据最小二乘原理，矩阵K的最小特征值对应的特征向量即为最优的

从而便得到最优初始姿态矩阵

其中，

需要满足约束条件：

3.构建SINS/GPS系统状态方程和量测方程

(1)选择系统状态变量X：

其中，

是载体系姿态矩阵对应的失准角，ε^b是陀螺仪的常值漂移。

(2)建立SINS/GPS对准系统的状态方程：

其中，

是状态变量

的导数，

是陀螺仪实际的角速率输出，0_3×1代表三行一列的全零矩阵，上述状态方程可表示为：

其中，f(·)表示非线性状态方程；W_k是均值为零、协方差为Q_k的系统白噪声，其大小由陀螺仪的噪声决定。

(3)建立SINS/GPS对准系统的量测方程：

Z_k＝HX_k+V_k

其中，量测方程中的V_k是均值为零的量测白噪声，其协方差记为R_k，矩阵Z_k和H_k分别为：

其中，

表示计算出的载体坐标系b系，矩阵

是实际中计算出的带误差的矩阵

矩阵H_k中的矢量

为当前载体系b(t_k)下的矢量，其计算公式为：

4.简化的自适应无迹卡尔曼滤波算法

(1)选择滤波初值和权值W_i ^(c)以及W_i ^(m)计算：

其中，

是状态变量X在第0时刻估计值即所选状态变量滤波初始值，P₀是状态变量误差方差矩阵在0时刻的滤波初始值，n是状态变量的维数，滤波参数α可取值为10^-4≤α≤1，滤波参数κ＝3-n，当X服从正太分布时，滤波参数β取值为2，λ＝α²(n+κ)-n。

(2)Sigma样本点

计算：

其中，

(3)计算Sigma采样点一步预测

(4)状态变量的一步预测

及一步预测均方误差矩阵P_k/k-1的计算：

(5)根据式系统状态方程和量测方程可以看出，系统的状态方程为非线性，量测方程为线性，所以在使用UKF算法时可以对量测更新部分做进一步简化，减少采样次数和计算量。简化后的量测更新公式为：

其中，

为量测Z_k的一步预测，P_ZZ表示量测一步预测误差方差矩阵，P_XZ是状态变量一步预测误差与量测一步预测误差间的协方差矩阵。

(6)滤波增益K_k计算：

其中，

表示矩阵P_ZZ的逆。

(7)系统的状态估计

更新和状态估计的均方误差P_k更新：

(8)在进行UKF算法滤波时，量测噪声方差矩阵R设置的不准确会导致系统滤波的不稳定。在系统建模过程中，量测方程是由速度、比力等的积分构成的，很难通过手动调试将量测噪声方差矩阵设置为准确值。针对这一问题，需使用Sage-Husa自适应滤波算法对量测噪声方差矩阵R进行实时估计，从而提高系统的滤波精度和稳定性。对量测噪声方差矩阵R的估计公式为：

其中，

是量测一步预测误差，权值β_k的初始值取0，b为渐消因子，且0＜b＜1，一般情况下，b取0.9～0.999。

5.状态估计的反馈和初始对准姿态的输出

(1)姿态失准角反馈：

因为所估计的姿态失准角是小角，所以可以将反馈公式近似为：

其中，矩阵

是由估计的失准角构成的误差反馈矩阵，

为未经反馈补偿的载体系变化矩阵。执行反馈后，需对相应的状态变量置零。

(2)陀螺仪误差反馈：

ε^b＝ε^b+[X_k(4:6)]^T

其中，[X_k(4:6)]为状态变量中的第4至第6行，

为补偿后的陀螺仪角速率，陀螺仪常值漂移对应的状态量在反馈后需置零。

(3)初始对准姿态输出：

即为初始对准所得姿态矩阵。

本发明是在分析了现有的OBA算法中存在的误差后进行系统建模，然后结合无迹卡尔曼滤波进行误差的估计和补偿，从而提高OBA算法在低精度SINS系统中应用时的精度和收敛性能，同时本发明是利用无迹卡尔曼滤波对误差的估计，不会影响姿态的收敛速度。所以本发明在利用MATLAB软件仿真和实验设备实验时，将本发明与现有的OBA算法和EKF动态对准算法进行了比较。

本发明实施例用来解释本发明，而不是对本发明进行限制，在发明的精神和权利要求的保护范围内，对本发明做出的任何修改和改变，都落入本发明的保护范围。

一、用MATLAB仿真对本发明进行验证：

仿真参数设置为：陀螺仪的常值漂移和随机噪声分别为1°/h和0.1°/h，加速度计的常值漂移和随机噪声分别为100ug和50ug，陀螺仪和加速度计的输出频率均为100Hz；GPS系统的速度误差为0.05m/s，位置误差为5m，GPS系统的输出频率为10Hz。SINS系统的初始状态设置为：SINS系统的初始位置为北纬45°和东经126°，初始姿态为航向角45°、俯仰角0°和横滚角0°，初始速度为东向速度5m/s、北向速度5m/s和天向速度0m/s。SINS系统的状态变化为：载体匀速运动50s→左转弯90°，用时5s→匀速运动50s→右转弯90°，用时5s→匀速运动50s→左转弯90°，用时5s→匀速运动50s→右转弯90°，用时5s→匀速运动50s，总的运动时长为270s。图2为载体运动轨迹变化，图3为载体的航向角变化。

图4为仿真过程中OBA算法与所提出的FIMA算法的姿态误差对比图，由图中效果可以看出，使用所提出的FIMA算法航向角误差在80s可以收敛到0.2°，并且航向角误差能够始终保持在0.2°以内，而OBA算法虽然也可以快速收敛，但是其航向角误差会随着对准时间的延长而变大。图中两算法的俯仰角误差和横滚角误差均能快速收敛，且收敛的精度相差不大，但是FIMA算法的水平姿态误差更稳定。

图5为FIMA算法与EKF动基座对准算法的姿态误差对比图，因为FIMA算法针对的是任意初始姿态，所以为了对比算法的效果，在仿真时EKF算法的姿态初始误差设置为航向角误差50°、俯仰角误差15°和横滚角误差15°的大失准角误差。由图中效果可以看出，与非线性EKF动态对准算法相比，当姿态初始值存在大失准角时，FIMA算法的航向角对准速度更快，水平姿态的对准效果更稳定。

图6是FIMA算法与EKF算法关于陀螺仪零偏估计的对比图，EKF算法的仿真条件与姿态误差对比的仿真条件相同。图5中，黑色虚线是仿真轨迹中加入的陀螺仪常值漂移，大小为1°/h，图中的零偏估计效果显示，FIMA算法对陀螺仪x轴和y轴的估计精度高于EKF算法，而两种算法对z轴陀螺仪零偏的估计都不太准确，这一现象与仿真轨迹运动方式的设计过于单一有关。图5的仿真效果说明，FIMA算法可以实现对陀螺仪零偏的估计。

二、对本发明进行跑车实验验证：

为验证本发明的动基座对准性能，跑车实验使用ADIS16488生产的MEMS惯性器件作为SINS系统进行动基座对准，实验中以高精度的光纤惯导系统作为姿态参考系统，图7为跑车实验过程中汽车的运动轨迹，图8为运动过程中参考姿态的变化，整个实验过程大约1300s，汽车在180s开始运动，算法在汽车运动后13s开始运行。

由图9中姿态误差可以看出，对于低精度的SINS系统，OBA算法虽然能够快速的确定出姿态角，但是姿态角并没有随着时间的延长而趋于稳定，且航向角误差会越来越大；本发明中的FIMA则克服了OBA算法的这一缺点，能够保证算法在低精度的SINS系统中依然能够稳定的收敛。图10为FIMA算法姿态误差放大图，由图中航向角误差可以知道，在没有任何初始姿态信息的条件下，算法用15s的时间便可将航向角误差收敛到3°以内，俯仰角误差和横滚角误差收敛到0.4°以内。在FIMA对准算法运行190s以后，航向角误差可收敛到1°以内。

图11为FIMA算法对陀螺仪零偏的估计结果，估计结果表明，FIMA算法能够实现对陀螺仪零偏的估计。为证明零偏估计的有效性，将FIMA算法估计的陀螺仪零偏写入纯惯导算法中进行补偿，补偿后的效果如图12中所示。经过陀螺仪误差补偿后的纯惯导算法的航向角误差明显好于补偿前的效果，陀螺仪误差的补偿对水平姿态误差影响不大，由此可以看出，FIMA算法对陀螺仪零偏的估计是有效的。

为了体现本文中算法在任意失准角条件下快速对准的优越性，本文对FIMA算法与EKF对准算法用跑车实测数据进行比较。在EKF对准算法运行前，设置航向角误差70°、俯仰角误差和横滚角误差20°的大失准角误差。两种算法的姿态误差对比如图13所示，由于大失准角的原因，EKF算法的航向误差收敛速度明显比FIMA算法慢，EKF算法的水平姿态误差在对准的前期也不如FIMA算法的稳定。

综上，本发明提供了一种全球定位系统(GPS)辅助的捷联式惯导系统(SINS)快速动基座对准(Fast In-motion Alignment.FIMA)方法，主要解决SINS系统运动条件下的初始对准问题。对于低精度的微机电系统(MEMS)，基于优化的对准算法(OBA)无法估计陀螺仪误差，会导致初始对准精度降低。本发明对陀螺仪误差的影响进行分析，建立了OBA算法对准过程中失准角误差方程和量测方程，同时利用简化的自适应无迹卡尔曼滤波算法(AUKF)对失准角和陀螺仪常值漂移进行估计，将估计的失准角和陀螺仪常值漂移对SINS系统对准的姿态和陀螺仪的输出进行矫正，从而实现SINS系统的快速动基座初始对准。

Claims (2)

1.GPS辅助的SINS系统快速动基座初始对准方法，其特征在于，具体包括以下步骤：

步骤1、采集SINS和GPS系统输出数据；

步骤2、SINS系统对准参数初始化；

步骤3、使用GPS数据和SINS数据分别计算出导航系姿态变化和载体系姿态变化；

3.1、导航系姿态变化矩阵更新：

式中，n表示导航系，t_k＝kT，k是采样次数，T是采样时间，

为当前时刻t_k到初始0时刻的导航系姿态变化矩阵，

为时刻t_k-1到初始0时刻的导航系姿态变化矩阵，矩阵

为时刻t_k到时刻t_k-1的导航系姿态变化矩阵，其计算公式为：

式中，I是三维单位矩阵，φ_n是由n(t_k-1)至n(t_k)的等效旋转矢量，

是导航系相对于惯性系的角速率，φ_n×是φ_n的反对称矩阵，||φ_n||是φ_n的模，其计算公式为：

式中，v_E和v_N是东向和北向速度，ω_ie是地球自转角速率，L和h是纬度和高度，R_M和R_N是子午圈和卯酉圈主曲率半径；

3.2、载体系姿态变化矩阵更新：

式中，b表示载体系，

为当前时刻t_k到初始0时刻的载体系姿态变化矩阵，

为时刻t_k-1到初始0时刻的载体系姿态变化矩阵，矩阵

为时刻t_k到时刻t_k-1的载体系姿态变化矩阵，其计算公式为：

式中，φ_b是由b(t_k-1)至b(t_k)的等效旋转矢量，Δθ₁和Δθ₂是陀螺仪两个相邻时刻的角增量输出；

步骤4、确定初始载体系和初始导航系下的α_v和β_v矢量，并根据最小二乘原理估计初始姿态矩阵；

4.1、矢量计算：

式中，α_v(t)是初始载体系下的矢量，β_v(t)是初始导航系下的矢量，f^b是加速度计输出，gⁿ是重力加速度；

4.2、初始姿态矩阵

估计：

设四元数

其中q₀为标量部分，上标T表示对矩阵求转置，q_v＝[q₁ q₂ q₃]^T为矢量部分，定义四元数乘法的两种反对称矩阵：

其中I为单位矩阵，矢量α_v和β_v看成标量部分为0的四元数，定义代价函数：

式中，

为初始姿态矩阵

对应的姿态四元数，根据最小二乘原理，矩阵K的最小特征值对应的特征向量即为最优的

从而便得到最优初始姿态矩阵

其中，

满足约束条件：

步骤5、选取状态变量，构建SINS/GPS系统的状态方程和量测方程；

5.1、选择系统状态变量X：

式中，

是载体系姿态矩阵对应的失准角，ε^b是陀螺仪的常值漂移；

5.2、建立SINS/GPS对准系统的状态方程：

式中，

是状态变量

的导数，

是陀螺仪实际的角速率输出，0_3×1代表三行一列的全零矩阵，上述状态方程表示为：

式中，f(·)表示非线性状态方程；W_k是均值为零、协方差为Q_k的系统白噪声，其大小由陀螺仪的噪声决定；

5.3、建立SINS/GPS对准系统的量测方程：

Z_k＝H_kX_k+V_k

式中，量测方程中的V_k是均值为零的量测白噪声，其协方差记为R_k，矩阵Z_k和H_k分别为：

式中，

表示计算出的载体坐标系b系，矩阵

是实际中计算出的带误差的矩阵

矩阵H_k中的矢量

为当前载体系b(t_k)下的矢量，其计算公式为：

步骤6、使用简化的自适应无迹卡尔曼滤波算法估计失准角和陀螺仪常值漂移；

6.1、选择滤波初值和权值W_i ^(c)以及W_i ^(m)计算：

式中，

是状态变量X在第0时刻估计值即所选状态变量滤波初始值，P₀是状态变量误差方差矩阵在0时刻的滤波初始值，n是状态变量的维数，滤波参数α取值为10^-4≤α≤1，滤波参数κ＝3-n，当X服从正太分布时，滤波参数β取值为2，λ＝α²(n+κ)-n；

6.2、Sigma样本点

计算：

式中，

6.3、计算Sigma采样点一步预测

6.4、状态变量的一步预测

及一步预测均方误差矩阵P_k/k-1的计算：

6.5、根据式系统状态方程和量测方程，系统的状态方程为非线性，量测方程为线性，使用UKF算法时对量测更新部分做进一步简化，减少采样次数和计算量，简化后的量测更新公式为：

式中，

为量测Z_k的一步预测，P_ZZ表示量测一步预测误差方差矩阵，P_XZ是状态变量一步预测误差与量测一步预测误差间的协方差矩阵；

6.6、滤波增益K_k计算：

式中，

表示矩阵P_ZZ的逆；

6.7、系统的状态估计

更新和状态估计的均方误差P_k更新：

6.8、进行UKF算法滤波时，量测噪声方差矩阵R设置的不准确会导致系统滤波的不稳定，在系统建模过程中，量测方程由速度、比力的积分构成，很难通过手动调试将量测噪声方差矩阵设置为准确值，针对这一问题，需使用Sage-Husa自适应滤波算法对量测噪声方差矩阵R进行实时估计，从而提高系统的滤波精度和稳定性，对量测噪声方差矩阵R的估计公式为：

式中，

是量测一步预测误差，权值β_k的初始值取0，b为渐消因子，且0＜b＜1，b取0.9～0.999；

步骤7、用估计的失准角对载体系姿态变化矩阵进行反馈，用估计的陀螺仪常值漂移对陀螺仪输出进行校正；

7.1、姿态失准角反馈：

因为所估计的姿态失准角是小角，所以将反馈公式近似为：

式中，矩阵

是由估计的失准角构成的误差反馈矩阵，

为未经反馈补偿的载体系变化矩阵，执行反馈后，对相应的状态变量置零；

7.2、陀螺仪误差反馈：

ε^b＝ε^b+[X_k(4:6)]^T

式中，[X_k(4:6)]为状态变量中的第4至第6行，

为补偿后的陀螺仪角速率，陀螺仪常值漂移对应的状态量在反馈后需置零；

步骤8、使用反馈后的导航系和载体系变化矩阵以及估计的初始姿态矩阵计算姿态矩阵；

步骤9、根据对准时间判断动基座对准是否完成，若没完成则返回步骤1继续对准，若对准完成，则结束动基座对准。

2.根据权利要求1所述GPS辅助的SINS系统快速动基座初始对准方法，其特征在于，所述步骤8中姿态矩阵为：

即为初始对准所得姿态矩阵。

Images