一种基于窗循环的加速度计零偏迭代估计方法及系统与流程

本发明属于导航技术领域，特别涉及一种基于窗循环的加速度计零偏迭代估计方法及系统。

背景技术：

随着惯性导航在军事和民用领域的广泛使用，包含惯性器件误差参数辨识的初始对准方法一直是惯性导航系统中的一个关键问题。申请号为cn201810635511.9(一种重力视运动参数辨识与加速度计常值漂移分离方法)与申请号为cn201810669045.6(一种基于重力视运动模型的加速度计零偏迭代寻优估计方法)的专利均提出晃动基座条件下，基于重力视运动重构的惯性系矢量计算方法能去除惯性器件随机噪声的影响，且同时完成姿态角和惯性器件加速度计的常值漂移的参数估计。但分析及仿真结果表明，已有方法未能估计陀螺仪常值漂移，且较长的时间内仪表的常值漂移会发生变化，会导致长时间的参数辨识精度下降甚至失效，这些问题可以通过缩短辨识时间来缓解。针对这些问题，本发明提出一种结合窗循环的牛顿迭代优化的参数辨识方法。

技术实现要素：

为解决上述问题，本发明提出了一种基于窗循环的加速度计零偏迭代估计方法及系统，目的是提供载体无线运动条件下的姿态、加速度计常值漂移估计方法，可以有效缓解包含加速度计常值漂移的初始对准过程中的长时间参数辨识精度下降甚至失效的问题，本专利提供一种基于窗循环的加速度计零偏迭代估计方法，包括：

(1)构建了包含加速度计常值漂移在内的重力视运动模型；

步骤(1)中所述的包含加速度计常值漂移在内的重力视运动模型为：

式中参数如下，x＝[a11a12a13a21a22a23a31a32a33▽x▽y▽z]^t，k＝[cos(ωiet)sin(ωiet)1]。其中，aij为重构参数，cij为姿态转换矩阵的元素，随着时间的变化而变化，▽为加速度计常值漂移值，为加速度计测量值；

(2)设计了重力视运动/加速度计常值漂移参数寻优目标函数；

步骤(2)中所述的构建的重力视运动/加速度计常值漂移参数寻优目标函数为

式中参数如下，理论上当a11～33与▽x～z取真值时l＝03×1，同时f(x)趋近于0，此时a11～33与▽x～z的求解问题可转化为参数寻优问题；

(3)设计了基于牛顿迭代寻优重力视运动参数、加速度计常值漂移估计算法；

步骤(3)中所述的基于牛顿迭代寻优重力视运动参数、加速度计常值漂移估计算法是指利用牛顿迭代算法去求解权利要求3中的参数寻优问题；

(4)设计了基于窗循环的加速度计零偏迭代估计算法。

作为本发明一种基于窗循环的加速度计零偏迭代估计方法进一步改进，步骤(4)中所述的基于窗循环的加速度计零偏迭代估计算法具体流程为：

在t0～tn时间内，sins仪表进行正常的仪表数据采集，获得加速度计测量值与陀螺测量值并存储；同时进行陀螺姿态矩阵更新获得每时每刻的将投影至ib0系下获得进行量测更新，按牛顿迭代算法重构流程进行参数辨识。tn+1时刻值是利用t1～tn+1时间段数据在tn时刻解算的基础上进行的窗循环解算，tn时刻的状态量x作为t1时刻初始迭代值，与时间相关的矩阵：姿态矩阵r阵需清零，运行过程按牛顿迭代算法求解。

本发明提供一种基于窗循环的加速度计零偏迭代估计的导航解算系统，包含加速度计常值漂移的重力视运动建模模块：构建了包含加速度计常值漂移在内的重力视运动模型；

所述的包含加速度计常值漂移在内的重力视运动模型为：

式中参数如下，x＝[a11a12a13a21a22a23a31a32a33▽x▽y▽z]^t，k＝[cos(ωiet)sin(ωiet)1]。其中，aij为重构参数，cij为姿态转换矩阵的元素，随着时间的变化而变化，▽为加速度计常值漂移值，为加速度计测量值；

目标函数数学建模模块：设计了重力视运动/加速度计常值漂移参数寻优目标函数；

所述的构建的重力视运动/加速度计常值漂移参数寻优目标函数的数学模型为：

式中参数如下，理论上当a11～33与▽x～z取真值时l＝03×1，同时此时a11～33与▽x～z的求解问题可转化为参数寻优问题；

导航参数辨识模块：设计了基于牛顿迭代寻优重力视运动参数、加速度计常值漂移估计算法；

所述的导航参数辨识是指利用牛顿迭代算法去求解重力视运动/加速度计常值漂移参数寻优目标函数的数学模型中的参数寻优问题

结合窗循环的导航参数辨识模块：设计了基于窗循环的加速度计零偏迭代估计算法。

所述的结合窗循环的导航参数辨识模块的具体流程为：在t0～tn时间内，sins仪表进行正常的仪表数据采集，获得加速度计测量值与陀螺测量值并存储；同时进行陀螺姿态矩阵更新获得每时每刻的将投影至ib0系下获得进行量测更新，按牛顿迭代算法重构流程进行参数辨识。tn+1时刻值是利用t1～tn+1时间段数据在tn时刻解算的基础上进行的窗循环解算，tn时刻的状态量x作为t1时刻初始迭代值，与时间相关的矩阵：姿态矩阵r阵需清零，运行过程按牛顿迭代算法求解。

技术原理：加速度计常值漂移的估计曲线收敛过程缓慢，延长系统辨识时间可以解决仪表误差辨识精度问题，但这会导致系统反应时间变长。虽然加速度计常值误差短时间可以视为常值，但长期看来并不稳定，若延长了系统的参数辨识时间，就不能保证仪表误差是常值的假设，这成为了一对矛盾。在晃动基座条件下，由于姿态角运动带来了不可交换性误差，因此陀螺仪的估计效果不佳，重力视运动与加速度计常值漂移耦合模型没有考虑陀螺的常值误差影响，经过分析表明，时间越长，陀螺常值误差对对准误差影响越严重。上述问题可以通过缩短参数辨识的时间来解决，基于此思路，本发明提出了一种结合窗循环的参数辨识算法。

有益效果：本方法基于窗循环的加速度计零偏迭代估计方法，通过对包含加速度计常值漂移的重力视运动模型进行重构，结合窗循环使用牛顿-拉格朗日迭代算法同时估计出载体姿态及加速度计常值漂移。本发明的优点是在无线运动条件下通过纯惯性仪表输出实现了自对准和加速度计常值漂移的估计，提高了观测数据的利用率，在提高参数估计精度的同时具有更好的时间敏感性，可以避免长时间辨识带来的系统误差。

附图说明

图1为本发明的导航方法总体流程图；

图2为本发明的窗循环解算示意图；

图3为本发明的结合窗循环的姿态角估计误差结果；

图4为本发明的结合窗循环的加速度计常值漂移估计结果。

具体实施方式

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

本发明提出了一种基于窗循环的加速度计零偏迭代估计方法及系统，目的是提供载体无线运动条件下的姿态、加速度计常值漂移估计方法，可以有效缓解包含加速度计常值漂移的初始对准过程中的长时间参数辨识精度下降甚至失效的问题。

如图1所示，本发明提出基于窗循环的加速度计零偏迭代估计方法，实现步骤如下：

(1)构建包含加速度计常值漂移在内的重力视运动模型；

(2)设计重力视运动/加速度计常值漂移参数寻优目标函数；

(3)设计基于牛顿迭代寻优重力视运动参数、加速度计常值漂移估计算法；

(4)设计基于窗循环的加速度计零偏迭代估计算法。

纯惯性解算不依赖任何外部条件，仅利用基本惯导单元的数据输出，结合地球自转与重力信息进行解算。比力方程为：

在无线运动条件下，vⁿ均为零，比力方程可以进行简化如下：

上式左右均乘以将重力信息投影到起始时刻载体坐标系固化的惯性坐标系ib0：

整理得：

根据姿态矩阵的链式分解，(4)式可进一步表达为：

(5)式左侧的在体系从0时刻到当前时刻的姿态转换矩阵可以由下式得到：

式中，表示陀螺仪的输出值，δt表示采样时间，表示的反对对称矩阵。考虑到加速度计的常值漂移与随机漂移，加速度计的输出模型可以表示为：

式中，表示加速度计的测量值，▽^b与η^b分别表示加速度计的常值与随机漂移。

(5)式右侧，为一未知常数阵，随地球自转而变化，与所处地理纬度相关的矩阵，gⁿ为常数阵，其具体表示分别如下：

gⁿ＝[00g]^t

结合式(5)～(10)，式(5)可以表达为：

(11)式中，这种将姿态矩阵链乘重构为aij矩阵的形式称为重力视运动的重构。采用参数辨识的方法可以有效地抑制随机噪声。因此(11)式中的η项可以忽略不计，因此(11)式可以改写为：

式中，cij为的元素，随着时间的变化而变化。可以整理为：

式中，x＝[a11a12a13a21a22a23a31a32a33▽x▽y▽z]^t，k＝[cos(ωiet)sin(ωiet)1]。(13)式即为包含加速度计常值漂移的重力视运动重构模型。

牛顿迭代优化方法的基本思想是：在现有极小值得附近对f(x)做二阶泰勒展开，进而找到极小点的下一个估计值。具体原理如下：函数在极值点处必然一阶导数为零，因此有f′(x)＝0，将函数f(x)进行二阶泰勒展开，有：

当δx无限趋近于0时，f(x+δx)＝f(x)，约去这两项，并对余项式对δx求导，有：

f′(x)+f″(x)δx＝0

进而得出牛顿迭代公式：

为了采用牛顿迭代寻优方法求解式(13)中的12个未知参数a11～33与▽x～z，我们将(13)式定义为函数：

并构建如下目标函数：

f(x)＝l^t(t)l(t)(18)

理论上当a11～33与▽x～z取真值时l＝03×1，同时此时a11～33与▽x～z的求解问题可转化为参数寻优问题。为充分利用观测数据，以更准确的估计a11～33与▽x～z值，本文将0～t时间段内的m次l^t(t)l(t)累加求和，构造如下目标函数：

对f(x)求解jacobian矩阵如下：

对f(x)求解hessian矩阵如下：

根据式(16)～(21)得到迭代计算模型：

牛顿迭代算法重构流程：

第一步：令k＝0，设置aij、ba初始值为0，即x＝012×1；

第二步：利用当前时刻的仪表输出，据式(17)计算l(t)，并利用(19)计算f(x)；

第三步：根据式(20)计算f(x)的一阶偏导数j，根据式(21)计算f(x)的二阶偏导

第四步：根据式(22)计算δx并更新xk；

第五步：令k＝k+1，跳到第三步，直到收敛精度或者最大的迭代步数；

第六步：根据迭代结果，得到重力视运动在惯性系的重构量和加速度计常值漂移量，基于重力视运动的三矢量对准运算，获取姿态矩阵。

第七步：跳至第二步，进行下一时刻的解算，直至结束。

由目标函数表达式(18)可知牛顿迭代方法依赖于长时间的数据信息，且在观测数据不断更新的过程中，每一组观测数据只利用了一次，数据利用程度低，造成信息的浪费，使得系统反应时间变长。而与此同时，针对过长时间的参数辨识，本文模型构建过程中未考虑陀螺仪误差的影响，因此模型的系统误差会随时间的累积逐渐显现最终影响参数辨识结果。另外，仪表误差自身是一个缓慢变化的过程，短期内可以认为是一个常值，但长期看，其并不稳定。辨识时间长短与参数辨识精度中间存在着上述的关系。基于此本专利设计以一种窗循环辨识的方法，在捷联惯性导航初始对准中，数学计算平台具有高速的数据处理能力，将窗时间段的采样数据按照时间序列存储下来，多次重复利用已采样数据信息，提高了数据利用率。

窗循环辨识算法设计如图2所示，在t0～tn时间内，sins仪表进行正常的仪表数据采集，获得加速度计测量值与陀螺测量值并存储；同时进行陀螺姿态矩阵更新获得每时每刻的将投影至ib0系下获得进行量测更新，tn+1时刻值是利用t1～tn+1时间段数据在tn时刻解算的基础上进行的窗循环解算，tn时刻的状态量x作为t1时刻初始迭代值，与时间相关的矩阵：姿态矩阵r阵需清零，运行过程由公式(19)～(22)决定。

基于重力视运动的窗循环解算和牛顿迭代参数估计方法流程：

第一步：在0～500s时间段内，按照牛顿迭代算法重构流程进行姿态角和加速度计常值漂移的参数估计，并令n＝1；

第二步：将所有涉及积分的变量清零，具体有jacobian矩阵、hessian矩阵，注意在n秒时需将姿态矩阵重置为单位阵；

第三步：利用第n秒至n+500秒窗时间段内的陀螺仪、加速度计输出数据，根据式(20)计算f(x)的一阶偏导数j，根据式(21)计算f(x)的二阶偏导数h；

第四步：根据式(22)计算xk并更新，跳到第三步，直到收敛精度或者最大的迭代步数；

第五步：根据迭代结果，得到重力视运动在惯性系的重构量和加速度计常值漂移量，据重力视运动的三矢量对准运算，获取姿态矩阵。

第六步：令n＝n+1，跳至第三步，进行下一时刻的解算，直至结束。

本发明的有益效果通过如下仿真得以验证：

为实现加表零偏的估计，本文采用正余弦函数描述摇摆激励的数学模型：

式中p,r和h分别表示纵摇角、横滚角和航向角，a,f,和θ分别表示幅度、摇摆频率、初始相位角和初始姿态角，载体初始位置设置为：东经118°、北纬32°，设置表1的摇摆激励条件：

表1晃动基座参数

为了验证并对比常规牛顿迭代算法(方法1)与窗循环算法(方法2)，将在表2所述的仿真条件下进行两种方法的解算，仿真时间持续2000s。仿真过程中，仪表测量数据与对准解算更新周期均为5ms，误差计算周期为1s。

表2传感器参数设置

结合窗循环的解算方法(方法2)因其每一个数据的解算仅利用滑动窗内数据进行解算，因而一定程度上克服了因忽略陀螺仪常值漂移误差而带来的系统误差。图3中(a)、(b)与(c)分别表示航、横与纵向对准误差，其中蓝色与绿色实线分别对应两种算法，红色点划线对应对准极限精度。图3的姿态角对准误差统计结果如表3所示。图4中(a)、(b)与(c)分别表示三轴加速度计的常值漂移估计，其中蓝色与绿色实线分别对应于两种算法，红色点划线为加速度计的常值漂移真值。三轴加速度计的常值漂移估计的统计结果如表4所示。图3和图4的曲线表明，两种算法均能完成姿态角对准及加速度计的常值漂移估计。表3表明两种方法姿态角对准误差均值相当，纵摇角和横滚角误差趋近于零，航向角误差靠近理论极限值。表4表明相比于方法1，方法2的加速度计常值漂移均值更靠近真值，但与此同时，方法2因使用的数据长度小，其方差也大于方法1。在0～500s的时间里，方法1与方法2进行了同样的运算，其均值和方差均相同。从501s开始，方法1在500s基础上继续迭代得到下一秒的值，而方法2则利用第500s时刻解算结果作为迭代初始值，然后利用前500s数据进行迭代解算。由于方法1解算所依赖的数据时间长度是从0时刻到当前时刻，而方法2解算仅依赖当前时刻前500s的数据，因此，方法2的方差会大一些。与此同时，方法2的窗循环算法对前面的数据进行了多次重复利用，因此其均值更靠近真值。另外，窗循环具有更好的时间敏感性，可以避免长时间辨识带来的系统误差。

表3姿态角误差统计特性(°).

表4加速度计偏差估计统计(μg)

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。