[发明专利]GNSS辅助的加速度计和陀螺仪常值漂移估算的导航方法及系统

权利要求书

1.一种基于GNSS辅助的加速度计和陀螺仪常值漂移估算的导航方法，其特征在于，包括：

(1)惯导器件输出的测量值包含误差，根据对加速度计与陀螺仪的误差分析，给出其误差的数学模型，所述误差包含加速度计和陀螺仪的常值漂移值；

(2)根据惯导基本方程通过积分、移项、合并同类项和离散化处理，在考虑加速度计和陀螺仪常值漂移的情况下，结合四元数的性质，得到线运动机动条件下的惯性系对准的数学模型；

(3)根据线运动机动条件下的惯性系对准的数学模型结合四元数的约束条件，构造新的存在极值点的多元函数，通过求解该函数的Jacobian矩阵和Hessian矩阵，逐次迭代，逐步靠近多元函数的极值点的方法，来求解姿态初始值、陀螺仪与加速度计常值漂移值；

(4)利用得到的姿态初始值根据姿态矩阵的链式分解求得当前时刻载体的姿态。

2.根据权利要求1所述的基于GNSS辅助的加速度计和陀螺仪常值漂移估算的导航方法，其特征在于，所述步骤1中加速度计与陀螺仪的误差模型为：

式中，为陀螺仪的输出，即载体坐标系(b系)相对于惯性坐标系(i系)的角速度矢量在载体坐标系的投影，其中包含了常值漂移值b_g与随机误差ξ_g；为加速度计的输出，即载体的加速度信息，其中包含了常值漂移值b_a与随机误差ξ_a；与表示理论真实值，ξ_g与ξ_a服从高斯分布。

3.根据权利要求1所述的基于GNSS辅助的加速度计和陀螺仪常值漂移估算的导航方法，其特征在于，所述步骤2中线运动机动条件下的惯性系对准的数学模型为：

式中，未知向量q、b_a、b_g分别是载体运动姿态及加速度计和陀螺仪的常值漂移值，α、β、χ、λ均可通过加速度计、陀螺仪和卫星接收机输出的数据计算得到：

式中Δv表示惯导采样周期内速度的增加量，是加速度计输出量加速度与采样周期的乘积；Δθ表示惯导采样周期内角度的增加量，是陀螺仪输出量角速度与采样周期的乘积；T表示惯导采样周期；M表示当前时间所对应的时间是惯导采样周期的M倍，即当前时刻的惯导采样次数；K依次取0,1,2，…，M-1；表示丛0～t_k时刻载体坐标系的姿态转移矩阵。

4.根据权利要求3所述的基于GNSS辅助的加速度计和陀螺仪常值漂移估算的导航方法，其特征在于，所述步骤3中的多元函数为包含q、b_a、b_g未知参数的牛顿-拉格朗日迭代函数，其形式如下：

其中，u为拉格朗日乘数。

5.根据权利要求4所述的基于GNSS辅助的加速度计和陀螺仪常值漂移估算的导航方法，其特征在于，在姿态初始值的牛顿-拉格朗日函数迭代求解过程中，迭代终止条件为达到设置的最大迭代次数或迭代增量小于某一设定值。

6.一种基于GNSS辅助的加速度计和陀螺仪常值漂移估算的导航解算系统，其特征在于，包括：

加速度计和陀螺仪误差建模模块：根据加速度计与陀螺仪的误差分析，给出其误差的数学模型，惯导器件输出的测量值包含这部分误差，即包含加速度计和陀螺仪的常值漂移值；

惯性系对准的数学建模模块：根据惯导基本方程通过积分，移项、合并同类项和离散化处理，在考虑加速度计和陀螺仪常值漂移的情况下，结合四元数的性质，得到线运动机动条件下的惯性系对准的数学模型；

导航参数辨识模块：根据线运动机动条件下的惯性系对准的数学模型结合四元数的约束条件，构造新的存在极值点的多元函数，通过求解该函数的Jacobian矩阵和Hessian矩阵，逐次迭代，逐步靠近多元函数的极值点的方法，来求解姿态初始值、陀螺仪与加速度计常值漂移值；

载体姿态获取模块：利用得到的姿态初始值根据姿态矩阵的链式分解求得当前时刻载体的姿态。