基于姿态差分补偿的垂线偏差测量方法、系统、终端、介质
阅读说明：本技术 基于姿态差分补偿的垂线偏差测量方法、系统、终端、介质 (Method, system, terminal and medium for measuring plumb line deviation based on attitude difference compensation ) 是由 何泓洋 邸建琛 詹国强 安文 江鹏飞 许江宁 于 2021-09-09 设计创作，主要内容包括：本发明属于垂线偏差测量技术领域,公开了一种基于姿态差分补偿的垂线偏差测量方法、系统、终端、介质,用姿态差对惯导解算中的重力项进行垂线偏差补偿,在消除垂线偏差误差的同时又引入了特性明确的常值误差、缓变误差和周期误差；把缓变误差建模为一阶高斯马尔科夫过程,周期误差作为输入,建立垂线偏差补偿后的SINS/GNSS组合导航方程,针对系统噪声变化的问题,将渐消记忆Kalman滤波用于状态估计,估计出的姿态不受垂线偏差影响,以该姿态作为姿态基准与垂线偏差未补偿时SINS/GNSS组合解算的姿态做差,差分结果即为垂线偏差。(The invention belongs to the technical field of vertical deviation measurement, and discloses a vertical deviation measurement method, a system, a terminal and a medium based on attitude difference compensation, wherein the attitude difference is used for performing vertical deviation compensation on a gravity item in inertial navigation solution, and a constant error, a slowly varying error and a periodic error with definite characteristics are introduced while the vertical deviation error is eliminated; modeling the slowly-varying error into a first-order Gaussian Markov process, taking the periodic error as input, establishing an SINS/GNSS combined navigation equation after vertical deviation compensation, aiming at the problem of system noise change, using fading memory Kalman filtering for state estimation, wherein the estimated attitude is not influenced by the vertical deviation, taking the attitude as an attitude reference, and making a difference with the attitude solved by the SINS/GNSS combination when the vertical deviation is not compensated, wherein the difference result is the vertical deviation.)
技术领域
本发明属于垂线偏差测量技术领域，尤其涉及一种基于姿态差分补偿的垂 线偏差测量方法、系统、终端、介质。
背景技术
目前，由于地球形状不规则，内部质量不均匀，真实重力与正常重力并不 相等，两者之间的差异称为重力扰动，包括重力异常和垂线偏差。精确的垂线 偏差数据在大地水准面确定、地震监测、工程勘测、空间探测、国防安全、国 家综合PNT体系构建等领域都有重要价值。
基于捷联惯性导航系统(Strapdown Inertial Navigation System,SINS)和全球导航卫星系统(Global Navigation Satellite System,GNSS)组合的垂线偏差测 量是一种正在研究中的垂线偏差测量方案。目前常用的基于SINS/GNSS组合的 垂线偏差测量方法主要分为直接求差法和建模估计法。直接求差法基于比力方 程，它将加速度计测量的比力与GNSS测量的惯性加速度做差以得到重力矢量， 然后通过简单的数学运算计算出垂线偏差；建模估计法通过在SINS误差方程中 引入垂线偏差，并对垂线偏差进行建模，然后利用GNSS提供的参考信息结合 Kalman滤波器对垂线偏差进行估计。
当用直接求差法求解垂线偏差时，需要利用方向余弦矩阵将比力从载体系 转换到导航坐标系，这意味方向余弦矩阵的误差会影响到垂线偏差的计算。但 方向余弦矩阵的计算误差又受垂线偏差的影响，这形成一个难以打破的闭环。 建模估计法恢复出的垂线偏差的精度取决于重力模型与真实重力矢量的符合程 度。高精度的垂线偏差需要高精度的重力场模型，但实际工作中较难提前获知 未知区域的重力场模型特征。因此，亟需一种新的基于姿态差分补偿的垂线偏 差恢复方法。
通过上述分析，现有技术存在的问题及缺陷为：
(1)现有的直接求差法中，方向余弦矩阵的计算误差又受垂线偏差的影响， 这形成一个难以打破的闭环。
(2)现有的建模估计法中，高精度的垂线偏差需要高精度的重力场模型， 但实际工作中较难提前获知未知区域的重力场模型特征。
解决以上问题及缺陷的难度为：
(1)误差控制；
(2)高精度重力场模型获取。
解决以上问题及缺陷的意义为：本发明提供了一种新的垂线偏差测量方法， 它既解除了方向余弦矩阵计算误差与垂线偏差的耦合，又不需要对未知垂线偏 差进行建模，避免了模型不准确误差。
发明内容
针对现有技术存在的问题，本发明提供了一种基于姿态差分补偿的垂线偏 差测量方法、系统、终端、介质。
本发明是这样实现的，一种基于姿态差分补偿的垂线偏差测量方法，所述 基于姿态差分补偿的垂线偏差测量方法包括以下步骤：
步骤一，利用SINS和GNSS进行组合导航，解算出姿态旋转矩阵
步骤二，利用SINS中的陀螺组件Gyros和GNSS进行组合，解算出姿态旋 转矩阵
步骤三，用和进行差分，所述差分结果包含垂线偏差和特征明确的 Gyros/GNSS姿态误差项；
步骤四，将Gyros/GNSS姿态误差项中的低频趋势项建模为一阶马尔科夫过 程，Gyros/GNSS姿态误差中的周期项作为先验信息；
步骤五，用姿态差分结果对SINS惯性解算方程中的重力项进行垂线偏差补 偿，消除垂线偏差项，引入已经建模的特征已知的Gyros/GNSS姿态误差项；
步骤六，对补偿后的SINS惯性解算模型进行渐消记忆Kalman滤波，估计 出不含垂线偏差信息的姿态旋转矩阵
步骤七，用做基准，并与步骤一中SINS/GNSS组合导航解算出姿态旋转 矩阵做差，差分结果即为恢复的航迹上的垂线偏差信息；
步骤八，一种以纯惯性解算速度误差之和最小为目标函数的渐消因子筛选 方法，为步骤六中渐消记忆Kalman滤波器找出最优渐消因子；
步骤九，渐消因子初值为1，步长为0.01；每遍历一个渐消因子，就进行一 次渐消记忆Kalman滤波，判断滤波是否发散，如果是，就终止算法；如果否， 则估计出一对水平失准角
步骤十，对滤波器估计的水平失准角进行傅立叶变换，判断其频谱是否在ωin处存在一个尖峰；如果否，说明滤波结果不准确，结束本次循环；如果是就说 明滤波结果符合先验条件，进而根据步骤七计算垂线偏差；
步骤十一，用计算出的垂线偏差η(i)，ξ(i)对SINS纯惯性解算中的重力项进 行补偿和修正，计算速度解算误差之和；
步骤十二，观察速度误差之和的曲线，速度误差之和最小时对应的垂线偏 差即为恢复出的垂线偏差，此次循环的渐消因子即为最优渐消因子。
进一步，步骤一中，所述利用SINS/GNSS组合导航解算出的方向余弦矩阵 记为
进一步，步骤二中，所述Gyros/GNSS组合导航解算出的方向余弦矩阵记为
进一步，步骤三中，所述用和进行差分，消除反映真实姿态的方向余 弦矩阵包括：
忽略二阶小量，式(3)写作：
定义θ＝φ-ψ，θ在东向和北向投影写作：
用θE和θN进行垂线偏差补偿，补偿后的速度微分方程写作：
式(6)中，Vc为计算的速度，为计算的方向余弦矩阵，为包含加速度 计器件误差的比力测量，和分别为和的计算值；补偿后的速度误差方 程写作：
式中，表示垂线偏差补偿后惯性解算的姿态失准角。
进一步，步骤四中，所述Gyros/GNSS姿态误差ψ包含常值项，缓慢增长项 以及周期项，周期项的角频率等于的大小，故将式(7)的最后一项建模为：
式中，aE1(t)和aN1(t)表示时间相关的函数，aE2(ωint)和aN2(ωint)表示角频率为ωin的 周期函数。aE1(t)，aN1(t)均包含常值项和缓慢增长项，同陀螺漂移的特征非常相似，常值误差项类似于陀螺漂移的零偏重复性误差，缓慢增长项类似于陀螺的 零偏稳定性误差；aE1(t)和aN1(t)的模型表示为：
式中，aE11(t)，aN11(t)分别为aE1(t)和aN1(t)中的常值项，aE12(t)，aN12(t)分别为aE1(t) 和aN1(t)中的缓慢增长项，建模为一阶马尔可夫过程，τE和τN为相关时间，wE和wN为白噪声。
进一步，步骤五中，所述垂线偏差补偿后的SINS/GNSS组合导航系统的误 差方程写作：
式中，x为状态量，w为噪声，u为输入量。将aE1(t)和aN1(t)作为状态变量， 周期项aE2(ωint)和aN2(ωint)作为输入量； u＝(01×3aE2aN2001×13)T，省略时间变量t；噪声矢量 w＝(wg wax+wE0 way+wN0 waz 01×10 wE 0 wN)T，其中wg表示陀螺的噪声，wax、way和 waz均为加速度计的噪声，wE0和wN0为该垂线偏差补偿模型中的ψ建模不精确产生 的建模噪声。
进一步，所述组合导航过程分为惯性解算过程和滤波过程，惯性解算的姿 态误差传递过程具有低通特征。输入量的频率ωin很小，在误差传递过程中几乎 没有衰减，将完全体现在姿态误差中，这使得姿态误差的频谱在ωin处存在一个 尖峰。将这一分析结论作为先验信息，式(11)可以简化为标准的组合导航状 态方程：
用载波相位差分GNSS提供的位置信息作为参考，垂线偏差补偿后的 SINS/GNSS组合导航系统的量测方程写作：
y＝Hx+v (13)
式中，y为量测量，y＝δP，H为量测矩阵，v为量测噪声；其中，A和H的 表达式为：
H＝[03×6 I3×3 03×10]。
式(12)和式(13)离散后表示为：
式中，Φk/k-1表示离散化后的系统矩阵，wk和vk表示高斯白噪声序列，并且满 足以下关系：
式中，δjk为狄拉克(Dirac)δ函数。
进一步，步骤六中，所述渐消记忆Kalman滤波器在滤波当前时刻将历史噪 声方差阵的参数重置，滤波器更注重新量测和新状态的作用，对噪声变化和模 型不准确比较宽容，所述离散模型下的渐消记忆Kalman滤波方程如下：
(1)一步预测：
(2)一步预测方差矩阵：
(3)一步预测的量测：
(4)新息：
(5)新息的二阶距：
(6)
(7)滤波增益：
(8)状态估计：
(9)状态估计方差：
其中，Dk＝diag(d1k d2k d3k … dnk)为渐消因子矩阵，是一个对角矩阵，对角 元素是对不同状态量赋予的渐消因子，当di＝1，1≤i≤n时，说明对第i个状态量 不渐消。由于方程中干扰量只施加于东向和北向速度误差，因此 Dk＝diag([ones(1,3) dk4 dk5 1 ones(1,9)])。
本发明的另一目的在于提供一种基于姿态差分补偿的垂线偏差测量系统， 包括：姿态旋转矩阵获取模块，用于利用SINS和GNSS进行组合导航，解算 出姿态旋转矩阵还用于利用SINS中的陀螺组件Gyros和GNSS进行组合， 解算出姿态旋转矩阵
先验信息获取模块，用于用和进行差分，所述差分结果包含垂线偏差 和特征明确的Gyros/GNSS姿态误差项；并将Gyros/GNSS姿态误差项中的低频 趋势项建模为一阶马尔科夫过程，Gyros/GNSS姿态误差中的周期项作为先验信 息；
不含垂线偏差信息姿态旋转矩阵估计模块，用于用姿态差分结果对SINS惯 性解算方程中的重力项进行垂线偏差补偿，消除垂线偏差项，引入已经建模的 特征已知的Gyros/GNSS姿态误差项；对补偿后的SINS惯性解算模型进行渐消 记忆Kalman滤波，估计出不含垂线偏差信息的姿态旋转矩阵
最优渐消因子获取模块，用于用做基准，并与SINS/GNSS组合导航解算 出姿态旋转矩阵做差，差分结果即为恢复的航迹上的垂线偏差信息；以纯惯 性解算速度误差之和最小为目标函数的渐消因子筛选方法，为渐消记忆Kalman 滤波器找出最优渐消因子；
垂线偏差计算模块，用于渐消因子初值为1，步长为0.01；每遍历一个渐消 因子，就进行一次渐消记忆Kalman滤波，判断滤波是否发散，如果是，就终止 算法；如果否，则估计出一对水平失准角对滤波器估计的水平失准角 进行傅立叶变换，判断其频谱是否在ωin处存在一个尖峰；如果否，说明滤波结 果不准确，结束本次循环；如果是就说明滤波结果符合先验条件，进而计算垂 线偏差；
最优渐消因子获取模块，用于用计算出的垂线偏差η(i)，ξ(i)对SINS纯惯性 解算中的重力项进行补偿和修正，计算速度解算误差之和；分析速度误差之和 的曲线，速度误差之和最小时对应的垂线偏差即为恢复出的垂线偏差，此次循 环的渐消因子即为最优渐消因子。
本发明的另一目的在于提供一种计算机设备，所述计算机设备包括存储器 和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行 时，使得所述处理器执行所述基于姿态差分补偿的垂线偏差测量方法。
本发明的另一目的在于提供一种计算机可读存储介质，存储有计算机程序， 所述计算机程序被处理器执行时，使得所述处理器执行所述基于姿态差分补偿 的垂线偏差测量方法。
本发明的另一目的在于提供一种信息数据处理终端，所述信息数据处理终 端用于实现所述基于姿态差分补偿的垂线偏差测量方法。
结合上述的所有技术方案，本发明所具备的优点及积极效果为：本发明提 供的基于姿态差分补偿的垂线偏差测量方法，与直接求差法不同，直接求差法 是对加速度进行差分，本发明是对姿态进行差分，并且本发明不需要提前获知 未知区域的重力场特征，不需要对未知垂线偏差进行建模，避免了建模估计法 中出现的模型不准确误差。
本发明利用SINS中的陀螺组件与GNSS组合测量出的姿态不受垂线偏差影 响，只受陀螺误差的影响，姿态误差特征明确。将Gyros/GNSS组合测量的姿态 与SINS/GNSS测量的姿态做差，可以消除真实姿态，姿态差中包含垂线偏差、 常值误差项、缓变误差项和周期误差项。用姿态差对惯导解算中的重力项进行 垂线偏差补偿，在消除垂线偏差误差的同时又引入了特性明确的常值误差、缓 变误差和周期误差。把缓变误差建模为一阶高斯马尔科夫过程，周期误差作为 输入，建立垂线偏差补偿后的SINS/GNSS组合导航方程，针对系统噪声变化的 问题，将渐消记忆Kalman滤波用于状态估计，估计出的姿态不受垂线偏差影响，以该姿态作为姿态基准与垂线偏差未补偿时SINS/GNSS组合解算的姿态做差， 差分结果即为垂线偏差。为了找到最优渐消因子，将恢复出的垂线偏差用于纯 惯性解算中的垂线偏差补偿，速度误差之和最小时对应的渐消因子即为最优渐 消因子，对应的垂线偏差即为沿轨迹的垂线偏差。
对比的技术效果或者实验效果。
本发明用一段仿真试验数据对方法进行了验证。模拟一段海洋测试航线，如 图1(a)，航线上的垂线偏差如图1(b)。载船航行速度设为10m/s，测量使用 的SINS和GNSS误差参数如表1。
表1 SINS和GNSS的误差参数
当解算过程使用正常重力时，分别进行SINS/GNSS组合导航和Gyros/GNSS 姿态解算，两种方法求解的水平姿态如图2，水平姿态误差如图3。从图2可以 看出在SINS/GNSS组合导航解算中，惯导的真实姿态与垂线偏差耦合在一起，难 以区分。Gyros/GNSS姿态解算不受垂线偏差影响，在真实姿态的基础上，还表 现出周期近似为24h的周期振荡和缓慢漂移。
将SINS/GNSS姿态同Gyros/GNSS姿态做差，能消除共有的真实姿态，差分 结果如图4，不仅包含垂线偏差，还包含Gyros/GNSS水平姿态误差。
用姿态差分结果对SINS解算中使用的重力进行“垂线偏差”补偿，理论上 能消除垂线偏差对SINS姿态解算结果的影响，但在垂线偏差得以补偿的同时， Gyros/GNSS姿态误差会被引入SINS解算过程，成为新的误差源。在SINS/GNSS 组合导航模式下，该误差将几乎无衰减地反映在水平姿态误差中。因此，用姿 态差分结果进行垂线偏差补偿后，SINS水平姿态误差中一定包含ωin周期项，如 图5所示。
Gyros/GNSS姿态误差的估计结果如图6，北向姿态误差的估计值与真实值之 间存在一个偏置，该偏置会引起SINS水平姿态误差，最终造成垂线偏差的求解 误差。
对Gyros/GNSS误差引起的SINS姿态误差进行修正之后，SINS水平姿态既不 含垂线偏差也不含Gyros/GNSS姿态误差，如图7。滤波稳定后，SINS水平姿态 误差几乎在零附近，但由于Gyros/GNSS姿态解算的北向分量存在常值误差，该 误差使得SINS的横摇方向也存在一个常值偏置误差。将不含垂线偏差的姿态作 为基准，再与无垂线偏差补偿时的SINS/GNSS姿态做差，差分结果如图8。可以 看出，基于姿态差分补偿的垂线偏差测量方法能够恢复出航迹上的垂线偏差。 但由于北向姿态误差存在常值偏置，最终使得卯酉面垂线偏差的恢复结果存在 一个常值误差，但该误差可以通过端点匹配得到较好的修正，图8(a)也给出 了修正后的卯酉面垂线偏差。修正后卯酉面垂线偏差外符合精度为3.04"，子午 面垂线偏差外符合精度为3.86"。误差主要位于前10个小时，这个时间段内， 算法处于收敛过程，存在较大超调。除去这个过程，后62小时η和ξ的外符合 精度分别为1.06″和1.17″。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对本发明实施例中所 需要使用的附图做简单的介绍，显而易见地，下面所描述的附图仅仅是本发明 的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下 还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的载船航迹和航迹上的垂线偏差图。图1(a)载船 航迹；图1(b)航迹上的垂线偏差。
图2是本发明实施例提供的不同方法求解的水平姿态图。图2(a)纵摇角； 图3(b)横摇角。
图3是本发明实施例提供的不同方法求解的水平姿态误差图。
图3(a)φE；图3(b)φN。
图4是本发明实施例提供的SINS/GNSS与Gyros/GNSS的水平姿态差分图。 图4(a)东向姿态差分；图4(b)北向姿态差分.
图5是本发明实施例提供的“垂线偏差”补偿后的水平姿态误差图。图5(a) φE；图5(b)φN。
图6是本发明实施例提供的“垂线偏差”补偿后的水平姿态误差的估计图； 图6(a)φE；图7(b)φN。
图7是本发明实施例提供的垂线偏差和Gyros/GNSS姿态误差补偿后的水平 姿态误差图。图7(a)φE；图7(b)φN。
图8是本发明实施例提供的恢复的垂线偏差图。图8(a)η；图8(b)ξ。
图9是本发明实施例提供的基于姿态差分补偿的垂线偏差恢复方法流程图。
图10是本发明实施例提供的最优渐消因子搜索算法流程图。
图11是本发明实施例提供的垂线偏差计算流程图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例， 对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以 解释本发明，并不用于限定本发明。
针对现有技术存在的问题，本发明提供了一种基于姿态差分补偿的垂线偏 差测量方法、系统、终端、介质，下面结合附图对本发明作详细的描述。
本发明实施例提供的基于姿态差分补偿的垂线偏差测量方法包括以下步 骤：
步骤1，利用SINS和GNSS进行组合导航，解算出姿态旋转矩阵
步骤2，利用SINS中的陀螺组件Gyros和GNSS进行组合，解算出姿态旋 转矩阵
步骤3，用和进行差分，所述差分结果包含垂线偏差和特征明确的 Gyros/GNSS姿态误差项；
步骤4，将Gyros/GNSS姿态误差项中的低频趋势项建模为一阶马尔科夫过 程，Gyros/GNSS姿态误差中的周期项作为先验信息；
步骤5，用姿态差分结果对SINS惯性解算方程中的重力项进行垂线偏差补 偿，消除垂线偏差项，引入已经建模的特征明确的Gyros/GNSS姿态误差项；
步骤6，对补偿后的SINS解算模型进行渐消记忆Kalman滤波，估计出不 含垂线偏差信息的姿态旋转矩阵
步骤7，用做基准，并与SINS/GNSS组合导航解算出姿态旋转矩阵做 差，差分结果即恢复的航迹上的垂线偏差信息；
步骤8，种以纯惯性解算速度误差之和最小为目标函数的渐消因子筛选方 法，为渐消记忆Kalman滤波器找出最优渐消因子；
步骤9，渐消因子初值为1，步长为0.01；每遍历一个渐消因子，就进行一 次渐消记忆Kalman滤波，判断滤波是否发散，如果是，就终止算法；如果否， 则估计出一对水平失准角
步骤10，对滤波器估计的水平失准角进行傅立叶变换，判断其频谱是否在ωin处存在一个尖峰；如果否，说明滤波结果不准确，结束本次循环；如果是就说 明滤波结果符合先验条件，进而计算垂线偏差；
步骤11，用计算出的垂线偏差η(i)，ξ(i)对纯惯性解算中的重力项进行补偿 和修正，计算速度解算误差之和；
步骤12，观察速度误差之和的曲线，速度误差之和最小时对应的垂线偏差 即为恢复出的垂线偏差，此次循环的渐消因子即为最优渐消因子。
本发明还提供一种基于姿态差分补偿的垂线偏差测量系统，包括：姿态旋 转矩阵获取模块，用于利用SINS和GNSS进行组合导航，解算出姿态旋转矩阵 还用于利用SINS中的陀螺组件Gyros和GNSS进行组合，解算出姿态旋转 矩阵
先验信息获取模块，用于用和进行差分，所述差分结果包含垂线偏差 和特征明确的Gyros/GNSS姿态误差项；并将Gyros/GNSS姿态误差项中的低频 趋势项建模为一阶马尔科夫过程，Gyros/GNSS姿态误差中的周期项作为先验信 息；
不含垂线偏差信息姿态旋转矩阵估计模块，用于用姿态差分结果对SINS惯 性解算方程中的重力项进行垂线偏差补偿，消除垂线偏差项，引入已经建模的 特征已知的Gyros/GNSS姿态误差项；对补偿后的SINS惯性解算模型进行渐消 记忆Kalman滤波，估计出不含垂线偏差信息的姿态旋转矩阵
最优渐消因子获取模块，用于用做基准，并与SINS/GNSS组合导航解算 出姿态旋转矩阵做差，差分结果即为恢复的航迹上的垂线偏差信息；以纯惯 性解算速度误差之和最小为目标函数的渐消因子筛选方法，为渐消记忆Kalman 滤波器找出最优渐消因子；
垂线偏差计算模块，用于渐消因子初值为1，步长为0.01；每遍历一个渐消 因子，就进行一次渐消记忆Kalman滤波，判断滤波是否发散，如果是，就终止 算法；如果否，则估计出一对水平失准角对滤波器估计的水平失准角 进行傅立叶变换，判断其频谱是否在ωin处存在一个尖峰；如果否，说明滤波结 果不准确，结束本次循环；如果是就说明滤波结果符合先验条件，进而计算垂 线偏差；
最优渐消因子获取模块，用于用计算出的垂线偏差η(i)，ξ(i)对SINS纯惯性 解算中的重力项进行补偿和修正，计算速度解算误差之和；分析速度误差之和 的曲线，速度误差之和最小时对应的垂线偏差即为恢复出的垂线偏差，此次循 环的渐消因子即为最优渐消因子。
下面结合实施例对本发明作进一步描述。
实施例1
本发明提出了一种基于姿态差分补偿的垂线偏差测量方法。该方法与直接 求差法不同，直接求差法是对加速度进行差分，本方法是对姿态进行差分。并 且该方法不需要提前获知未知区域的重力场特征，不需要对未知垂线偏差进行 建模，避免了建模估计法中出现的模型不准确误差。
本发明提出一种基于姿态差分补偿的垂线偏差恢复方法，算法流程如图9 所示，具体包括：
(1)首先用SINS和GNSS进行组合导航，解算出姿态旋转矩阵
(2)然后再利用SINS中的陀螺组件Gyros和GNSS进行组合，解算出姿 态旋转矩阵
(3)用和进行差分。差分结果包含垂线偏差和特征明确的Gyros/GNSS 姿态误差项；
(4)将Gyros/GNSS姿态误差项中的低频趋势项建模为一阶马尔科夫过程， Gyros/GNSS姿态误差中的周期项作为先验信息；
(5)用姿态差分结果对SINS惯性解算方程中的重力项进行垂线偏差补偿， 能够消除垂线偏差项，但是引入了已经建模的特征已知的Gyros/GNSS姿态误差 项；
(6)对补偿后的SINS解算模型进行渐消记忆Kalman滤波，估计出不含垂 线偏差信息的姿态旋转矩阵
(7)最后用做基准，并与SINS/GNSS组合导航解算出姿态旋转矩阵做 差，差分结果即为恢复的航迹上的垂线偏差信息；
(8)为了为渐消记忆Kalman滤波器找出最优渐消因子，提出一种以纯惯 性解算速度误差之和最小为目标函数的渐消因子筛选方法，如图10所示；
(9)渐消因子初值为1，步长为0.01；每遍历一个渐消因子，就进行一次 渐消记忆Kalman滤波，判断滤波是否发散，如果是，就终止算法；如果否，则 估计出一对水平失准角
(10)然后对滤波器估计的水平失准角进行傅立叶变换，判断其频谱是否 在ωin处存在一个尖峰，如果否，说明滤波结果不准确，结束本次循环；如果是 就说明滤波结果符合先验条件，进而计算垂线偏差；
(11)垂线偏差的计算流程如图11所示，用计算出的垂线偏差η(i)，ξ(i)对 纯惯性解算中的重力项进行补偿和修正，计算速度解算误差之和；
(12)观察速度误差之和的曲线。速度误差之和最小时对应的垂线偏差即 为恢复出的垂线偏差，此次循环的渐消因子即为最优渐消因子。
实施例2
本发明提出的基于姿态差分补偿的垂线偏差恢复方法，详细数学过程如下：
SINS/GNSS组合导航解算出的方向余弦矩阵记为Gyros/GNSS组合导 航解算出的方向余弦矩阵记为
对式(1)和式(2)进行姿态差分，可以消除反映真实姿态的方向余弦矩 阵如式(3)，
忽略二阶小量，式(3)写作：
定义θ＝φ-ψ，θ在东向和北向投影写作：
用θE和θN进行垂线偏差补偿，补偿后的速度微分方程写作：
式(6)中，Vc为计算的速度，为计算的方向余弦矩阵，为包含加速度 计器件误差的比力测量，和分别为和的计算值。补偿后的速度误差方 程写作：
式中，表示垂线偏差补偿后惯性解算的姿态失准角。相较于垂线偏差未补 偿时的SINS速度误差方程式，新的误差方程只在最后一项有变化。新方程将难 以建模的垂线偏差项转换成了特征明确，易于建模的Gyros/GNSS姿态误差项。 Gyros/GNSS姿态误差ψ包含常值项，缓慢增长项以及周期项，周期项的角频率 等于的大小。因此将式(7)的最后一项建模为：
式中，aE1(t)和aN1(t)表示时间相关的函数，aE2(ωint)和aN2(ωint)表示角频率为ωin的 周期函数。aE1(t)，aN1(t)均包含常值项和缓慢增长项，同陀螺漂移的特征非常相似，常值误差项类似于陀螺漂移的零偏重复性误差，缓慢增长项类似于陀螺的 零偏稳定性误差。aE1(t)和aN1(t)的模型表示为：
式中，aE11(t)，aN11(t)分别为aE1(t)和aN1(t)中的常值项，aE12(t)，aN12(t)分别为aE1(t) 和aN1(t)中的缓慢增长项，建模为一阶马尔可夫过程，τE和τN为相关时间，wE和wN为白噪声。
垂线偏差补偿后的SINS/GNSS组合导航系统的误差方程写作：
式中，x为状态量，w为噪声，u为输入量。将aE1(t)和aN1(t)作为状态变量， 周期项aE2(ωint)和aN2(ωint)作为输入量。 u＝(01×3aE2 aN2001×13)T，这里的表示省略了时间变量t。噪声矢量 w＝(wg wax+wE0 way+wN0 waz01×10wE 0wN)T，其中wg表示陀螺的噪声，wax、way和 waz均为加速度计的噪声，wE0和wN0为该垂线偏差补偿模型中的ψ建模不精确产生 的建模噪声。
组合导航过程分为惯性解算过程和滤波过程，惯性解算的姿态误差传递过 程具有低通特征。输入量的频率ωin很小，在误差传递过程中几乎没有衰减，将 完全体现在姿态误差中，这使得姿态误差的频谱在ωin处存在一个尖峰。将这一 分析结论作为先验信息，式(11)可以简化为标准的组合导航状态方程，
用载波相位差分GNSS提供的位置信息作为参考，垂线偏差补偿后的 SINS/GNSS组合导航系统的量测方程写作：
y＝Hx+v (13)
式中，y为量测量，y＝δP，H为量测矩阵，v为量测噪声。式(24)和式(25) 即为SINS/GNSS的组合方程，其中，A和H的表达式见附录一。
式(12)和式(13)离散后表示为：
式中，Φk/k-1表示离散化后的系统矩阵，wk和vk表示高斯白噪声序列，并且满 足以下关系：
式中，δjk为狄拉克(Dirac)δ函数。
垂线偏差补偿后的SINS/GNSS组合导航模型的噪声矢量包含ψ建模不精确 产生的建模噪声，但其大小未知。为了提高滤波精度，希望滤波器能够对噪声 方差矩阵Q比较宽容。渐消记忆Kalman滤波器在滤波当前时刻将历史噪声方差 阵的参数重置，滤波器更注重新量测和新状态的作用，对噪声变化和模型不准 确比较宽容。离散模型下的渐消记忆Kalman滤波方程如下：
(1)一步预测：
(2)一步预测方差矩阵：
(3)一步预测的量测：
(4)新息：
(5)新息的二阶距：
(6)
(7)滤波增益：
(8)状态估计：
(9)状态估计方差：
Dk＝diag(d1k d2k d3k … dnk)为渐消因子矩阵，是一个对角矩阵，对角元素是 对不同状态量赋予的渐消因子，当di＝1，1≤i≤n时，说明对第i个状态量不渐消。 由于方程中干扰量只施加于东向和北向速度误差，因此 Dk＝diag([ones(1,3) dk4 dk5 1 ones(1,9)])。
渐消记忆Kalman滤波中渐消因子dk的选择影响滤波效果。dk选择不当，滤 波估计结果将偏离真实，甚至出现滤波发散。如何选择渐消因子d4k和d5k？本发明提出一种以纯惯性解算误差最小为目标函数的渐消因子筛选方法，如图10所 示。渐消因子初值为1，步长为0.01。每遍历一个渐消因子，就进行一次渐消记 忆Kalman滤波，判断滤波是否发散，如果是，就终止算法；如果否，则估计出 一对水平失准角然后对滤波器估计的水平失准角进行傅立叶变换，判 断其频谱是否在ωin处存在一个尖峰，如果否，说明滤波结果不准确，结束本次 循环；如果是就说明滤波结果符合先验条件，进而计算垂线偏差，垂线偏差的计算流程如图11所示。用计算出的垂线偏差η(i)，ξ(i)对纯惯性解算中的重力项 进行补偿和修正，计算速度解算误差之和，观察速度误差之和的曲线。速度误 差之和最小时对应的垂线偏差即为恢复出的垂线偏差，此次循环的渐消因子即 为最优渐消因子。
垂线偏差的计算过程如图11所示。首先用θE和θN补偿后的SINS解算模型 进行纯惯性解算，然后再对式(2)表示的误差模型进行开环渐消记忆Kalman 滤波，用估计出的失准角对纯惯性解算出的姿态角进行修正，修正后的姿态角 不包含垂线偏差。以该姿态角为基准，并与未做垂线偏差补偿的SINS/GNSS组 合导航模型解算出的姿态角做差，便能恢复出航迹上的垂线偏差。
本发明利用SINS中的陀螺组件Gyros与GNSS组合测量出的SINS姿态不受垂 线偏差影响，只受Gyros/GNSS姿态误差的影响，特征明确。将Gyros/GNSS组合 测量的姿态与SINS/GNSS测量的姿态做差，可以消除真实姿态，姿态差中包含 垂线偏差、常值误差项、缓变误差项和周期误差项。用姿态差对SINS惯性解算 中的重力项进行垂线偏差补偿，在消除垂线偏差误差的同时又引入了特性明确 的常值误差、缓变误差和周期误差。把缓变误差建模为一阶高斯马尔科夫过程， 周期误差作为输入，建立垂线偏差补偿后的SINS/GNSS组合导航方程，针对系 统噪声变化的问题，将渐消记忆Kalman滤波用于状态估计，估计出的姿态不受 垂线偏差影响，以该姿态作为姿态基准与垂线偏差未补偿时SINS/GNSS组合解 算的姿态做差，差分结果即为垂线偏差。为了找到最优渐消因子，将恢复出的 垂线偏差用于纯惯性解算中的垂线偏差补偿，速度误差之和最小时对应的渐消 因子即为最优渐消因子，对应的垂线偏差即为沿轨迹的垂线偏差。
附录一：
H＝[03×6 I3×3 03×10]。
下面结合具体仿真实验对本发明的技术效果作进一步描述。
算例：本发明用一段仿真试验数据对方法进行了验证。模拟一段海洋测试航 线，如图1(a)，航线上的垂线偏差如图1(b)。载船航行速度设为10m/s，测 量使用的SINS和GNSS误差参数如表1。
表1 SINS和GNSS的误差参数
当解算过程使用正常重力时，分别进行SINS/GNSS组合导航和Gyros/GNSS 姿态解算，两种方法求解的水平姿态如图2，水平姿态误差如图3。从图2可以 看出在SINS/GNSS组合导航解算中，惯导的真实姿态与垂线偏差耦合在一起，难 以区分。Gyros/GNSS姿态解算不受垂线偏差影响，在真实姿态的基础上，还表 现出周期近似为24h的周期振荡和缓慢漂移。
将SINS/GNSS姿态同Gyros/GNSS姿态做差，能消除共有的真实姿态，差分 结果如图4，不仅包含垂线偏差，还包含Gyros/GNSS水平姿态误差。
用姿态差分结果对SINS解算中使用的重力进行“垂线偏差”补偿，理论上 能消除垂线偏差对SINS姿态解算结果的影响，但在垂线偏差得以补偿的同时， Gyros/GNSS姿态误差会被引入SINS解算过程，成为新的误差源。在SINS/GNSS 组合导航模式下，该误差将几乎无衰减地反映在水平姿态误差中。因此，用姿 态差分结果进行垂线偏差补偿后，SINS水平姿态误差中一定包含ωin周期项，如 图5所示。
Gyros/GNSS姿态误差的估计结果如图6，北向姿态误差的估计值与真实值之 间存在一个偏置，该偏置会引起SINS水平姿态误差，最终造成垂线偏差的求解 误差。
对Gyros/GNSS误差引起的SINS姿态误差进行修正之后，SINS水平姿态既不 含垂线偏差也不含Gyros/GNSS姿态误差，如图7。滤波稳定后，SINS水平姿态 误差几乎在零附近，但由于Gyros/GNSS姿态解算的北向分量存在常值误差，该 误差使得SINS的横摇方向也存在一个常值偏置误差。将不含垂线偏差的姿态作 为基准，再与无垂线偏差补偿时的SINS/GNSS姿态做差，差分结果如图8。可以 看出，基于姿态差分补偿的垂线偏差测量方法能够恢复出航迹上的垂线偏差。 但由于北向姿态误差存在常值偏置，最终使得卯酉面垂线偏差的恢复结果存在 一个常值误差，但该误差可以通过端点匹配得到较好的修正，图8(a)也给出 了修正后的卯酉面垂线偏差。修正后卯酉面垂线偏差外符合精度为3.04"，子午 面垂线偏差外符合精度为3.86"。误差主要位于前10个小时，这个时间段内， 算法处于收敛过程，存在较大超调。除去这个过程，后62小时η和ξ的外符合 精度分别为1.06″和1.17″。
在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组 合来实现。当使用全部或部分地以计算机程序产品的形式实现，所述计算机程 序产品包括一个或多个计算机指令。在计算机上加载或执行所述计算机程序指 令时，全部或部分地产生按照本发明实施例所述的流程或功能。所述计算机可 以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。所述计算 机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向 另一个计算机可读存储介质传输，例如，所述计算机指令可以从一个网站站点、 计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线(DSL)或 无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据 中心进行传输)。所述计算机可读取存储介质可以是计算机能够存取的任何可用 介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。 所述可用介质可以是磁性介质，(例如，软盘、硬盘、磁带)、光介质(例如，DVD)、 或者半导体介质(例如固态硬盘Solid State Disk(SSD))等。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于 此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明 的精神和原则之内所作的任何修改、等同替换和改进等，都应涵盖在本发明的 保护范围之内。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种物流机器人及其定位方法