CN109612460A - 一种基于静止修正的垂线偏差测量方法 - Google Patents

一种基于静止修正的垂线偏差测量方法 Download PDF

Info

Publication number
CN109612460A
CN109612460A CN201811558086.4A CN201811558086A CN109612460A CN 109612460 A CN109612460 A CN 109612460A CN 201811558086 A CN201811558086 A CN 201811558086A CN 109612460 A CN109612460 A CN 109612460A
Authority
CN
China
Prior art keywords
error
inertial
vector
global positioning
static
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201811558086.4A
Other languages
English (en)
Other versions
CN109612460B (zh
Inventor
赵立业
沈翔
王健松
黄丽斌
李宏生
罗海天
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Southeast University
Original Assignee
Southeast University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Southeast University filed Critical Southeast University
Priority to CN201811558086.4A priority Critical patent/CN109612460B/zh
Publication of CN109612460A publication Critical patent/CN109612460A/zh
Application granted granted Critical
Publication of CN109612460B publication Critical patent/CN109612460B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/40—Correcting position, velocity or attitude
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/52—Determining velocity
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/53—Determining attitude

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于静止修正的垂线偏差测量方法，包括建立组合系统垂线偏差观测模型的姿态位置速度方程和测量方程；建立静止修正垂线偏差观测模型；判断载体是否匀速，若载体匀速，则将位置差值和航向角修正的测量值作为测量方程对垂线偏差方程进行改正；否则，将速度、位置差值及航向角修正的测量值作为观测量对垂线偏差方程进行改正；基于改正的静止修正的垂线偏差观测模型对惯性系统输出状态变量进行静止修正。本发明不仅使用速度、位置信息作为观测量，同时添加零角度修正观测量来限制航向角误差，通过更新角速度传感器的常值零偏来减少航向角漂移，解决了组合系统垂线偏差测量中天向姿态误差补偿不到位和航向角误差随时间积累增大的问题。

Description

一种基于静止修正的垂线偏差测量方法
技术领域
本发明涉及惯导与全球定位组合系统垂线偏差测量技术，特别是涉及一种基于静止修正的垂线偏差测量方法。
背景技术
将全球定位系统与惯性系统结合起来应用信息融合理论形成最优组合测量系统已经成为新型测量技术的发展方向。它们之间具备良好的互补特性，可以获得稳定、精度好、数据更新率高的三维位置、速度、姿态信息。在垂线偏差组合测量系统中通常以全球定位系统输出位置、速度测量值并通过递归滤波等算法来限制惯性系统误差的累积，但由于全球定位系统输出的位置、速度测量值含有误差。静止修正方法的原理是利用惯性测量系统在载体停车时的速度输出作为惯性导航系统速度误差的观测量，在此基础上纠正其他误差，还可用载体运动时垂向和横向速度为零作为约束条件进行动态修正。在实际应用中，由于速度误差可能无限制的增长，这将直接影响到姿态和位置误差，使得测量精度受到威胁。相对于单个系统而言，静止修正很好地改善了惯性测量精度。
当载体处于静止状态时，理论上速度输出始终为零，位置始终保持不变，此时如果以静止度作为测量值，则相当于用精度极高的速度测量值来限制惯性导航误差的累积，可显著提高惯导与全球定位组合系统垂线偏差测量导航在静止状态下的结果。在载体静止时，由于惯导与全球定位组合系统垂线偏差测量中对于天向姿态误差的补偿不到位，使得对其观测质量差，导致航向角误差的积累随时间逐渐增大，急需改正的垂线偏差观测模型来解决这些问题。
发明内容
发明目的：提供一种基于静止修正的垂线偏差测量方法，该方法不仅使用速度、位置信息作为观测量，同时添加零角度修正观测量来限制航向角误差通过更新角速度传感器的常值零偏来减少航向角漂移，旨在解决垂线偏差测量中天向姿态误差的补偿不到位和航向角误差随时间积累增大的问题。
技术方案：为实现上述发明目的，本发明采用以下技术方案：
一种基于静止修正的垂线偏差测量方法，包括以下步骤：
(1)建立惯导与全球定位组合系统垂线偏差观测模型的姿态位置速度方程和测量方程；
(2)建立惯导与全球定位组合系统静止修正垂线偏差观测模型；
(3)判断载体是否匀速，若载体匀速，则执行步骤(4)；否则，执行步骤(5)；
(4)将位置差值和航向角修正的测量值作为观测量对垂线偏差方程进行改正；
(5)将速度、位置差值以及航向角修正的测量值作为观测量对垂线偏差方程进行改正；
(6)基于步骤(4)或步骤(5)中改正的静止修正的垂线偏差观测模型对惯性系统输出状态变量进行真值估计。
进一步的，步骤(1)中惯导与全球定位组合系统垂线偏差观测模型的姿态位置速度方程建立方法如下：
惯导与全球定位组合系统的姿态位置速度方程的线性化形式表示为：
其中，为惯导与全球定位组合系统垂线偏差中系统的线性化状态向量，包括当地导航坐标系下的姿态线性化误差矢量速度线性化误差矢量位置线性化误差角速度传感器线性化误差矢量和加速度传感器线性化误差矢量X(t)为惯导与全球定位组合系统垂线偏差中的状态向量，包括当地导航坐标系下的姿态误差矢量φn、速度误差矢量δvn、位置误差δrn、角速度传感器误差矢量和加速度传感器误差矢量F(t)为动态系数矩阵，包括惯导与全球定位组合系统的位置、速度、姿态，其中，F11～F33分别表示F(t)内部的动态系数，导航坐标系与载体坐标系之间的转置矩阵为W(t)为惯导与全球定位组合系统的噪声向量，包括角速度传感器噪声向量和加速度传感器噪声向量G(t)为惯导与全球定位组合系统的噪声系数矩阵。
进一步的，步骤(1)中惯导与全球定位组合系统垂线偏差观测模型的测量方程建立方法如下：
(a)在当地导航系统中，惯导与全球定位组合系统垂线偏差观测模型的测量方程是惯导系统和全球定位系统的位置、速度的差值，其测量方程Z(t)表示为：
其中，是惯性系统的位置，是全球定位系统的位置，是惯性系统的速度，是全球定位系统的速度，是位置的随机噪声向量，是速度的随机噪声向量，φn是姿态误差矢量，δvn是速度误差矢量，δrn是位置误差，是角速度传感器误差矢量，是加速度传感器误差矢量；
(b)全球定位系统给出的位置信息为经度λ、纬度和高度h，将其转换为当地导航坐标系中量测方程中的位置之差为：
其中，RM为子午圆半径；RN为卯酉圈半径；λI是惯性系统的经度，λG是全球定位系统的经度，是惯性系统的纬度，是全球定位系统的纬度。
进一步的，步骤(2)中静止修正垂线偏差观测模型的建立方法如下：
(21)设置惯导与全球定位组合系统的初始参数，包括初始姿态信息，初始位置信息和初始速度信息；
(22)惯性系统初始校准，包括水平方向校准和方位校准，使平台坐标系与地理坐标系趋于一致；
(23)对惯性系统进行捷联解算，根据加速度传感器和角速度传感器信号，解算出惯性系统的航向，姿态，位置及速度；
(24)判断是否到达静止修正时间，若是，则转到步骤(25)，若否，则转到步骤(23)；其中，静止修正时间设定如下：
其中，t0为零点修正时间，δa为设置最大零漂误差，ε0为惯导与全球定位组合系统的零偏精度；
(25)进行递归静止修正解算；
(26)判断是否到达预先设定的迭代时间ti，若是，则转到步骤(27)，若否，则转到步骤(25)；
(27)判断是否到达预先设定的解算结束时间ts，若是，则转到步骤(28)，若否，则转到步骤(23)；
(28)解算结束。
更进一步的，步骤(25)中递归静止修正解算方法如下：
(251)对载体进行静止检测，根据惯性器件角速度传感器和加速度传感器三轴输出以及变化规律判断载体是否静止，并通过改变惯导与全球定位组合系统垂线偏差观测模型的测量方程来对载体静止时的误差进行修正；
(252)对载体进行静止修正，当载体静止时，认为两个历元间航向角的变化应该为零，并通过更新角速度传感器的常值零偏来减少航向角漂移，具体方法如下：
真实航向角的变化率表达式为：
其中，Ψ、φ、θ分别表示载体的真实航向角、真实横滚角和真实俯仰角；为角速度传感器的真实输出角速率；为载体静止时地球自转在导航坐标系中的角速率；
测量航向角的变化率为：
其中，分别表示载体的测量航向角、测量横滚角和测量俯仰角；为角速度传感器的测量输出角速率；为载体静止时地球自转在导航坐标系中的角速率；
载体处于静止状态，所以角速度传感器的输出为：
其中，bg表示角速度传感器的零偏矢量；ng表示角速度传感器的随机噪声；为角速度传感器的测量输出角速率；为角速度传感器的真实输出角速率；为角速度传感器相对导航坐标系的角速率；
设俯仰角、横滚角为恒定值，令δΨ表示航向角的误差，在运算过程中作近似处理，如cosδψ＝1、sinδψ＝δψ；则将测量航向角的变化率与真实航向角的变化率相减得：
其中，为航向角变化率误差；
去掉第一项地球的自转误差，静止修正的垂线偏差观测模型得：
其中，Δtk为航向角更新的时间，k表示迭代次数。
进一步的，步骤(4)中将位置差值和航向角修正的测量值作为测量方程对静止修正垂线偏差观测模型进行改正，在静止观测的基础上添加了航向角修正测量值，建立改正的测量方程Z1(t)，并用Z1(t)代替Z(t)：具体测量方程如下所示：
其中，为惯性系统相对导航坐标系的位置矢量；为全球定位系统相对导航坐标系的位置矢量；为第k次迭代的真实航向角的变化率；为第k-1次迭代的真实航向角的变化率；φn为姿态误差，δvn为速度误差，δrn为位置误差，为角速度传感器的零偏，为加速度传感器的零偏；I3×3是惯性系统的3×3向量矩阵；θ为俯仰角，ψ为航向角，φ为翻滚角；表示全球定位系统的位置随机噪声；wψ为航向角观测的随机噪声。
进一步的，步骤(5)中将速度、位置差值以及航向角修正的测量值作为观测量对静止修正垂线偏差观测模型进行改正，建立改正的测量方程Z2(t)，并用Z2(t)代替Z(t)：
其中，为惯性系统相对导航坐标系的位置矢量；为全球定位系统相对导航坐标系的位置矢量；为惯性系统相对导航坐标系的速度矢量；为全球定位系统相对导航坐标系的速度矢量；为第k次迭代的真实航向角的变化率；为第k-1次迭代的真实航向角的变化率；φn为姿态误差，δvn为速度误差，δrn为位置误差，为角速度传感器的零偏，为加速度传感器的零偏；I3×3是惯性系统的3×3向量矩阵；θ为俯仰角，ψ为航向角，φ为翻滚角；和分别表示全球定位系统的位置和速度的随机噪声；wψ为航向角观测的随机噪声。
进一步的，步骤(6)中基于改正的静止修正的垂线偏差观测模型对惯性系统输出状态变量的真值估计方案如下：
(61)惯性系统输出状态变量真值和系统误差值，输出值Xs(t)用下列姿态位置速度方程描述：
Xs(t)＝X(t)+G(t)W(t)；
上式中的状态向量X(t)分为两个向量来考虑为：
X(t)＝[X1(t),X2(t)]；
其中，G(t)为惯性系统的噪声系数矩阵，W(t)表示惯性系统的噪声向量，X1(t)为惯性导航系统的误差状态向量，X1(t)为传感器误差的增广状态向量，如下所示：
X1(t)＝[φE,φN,φU,δVE,δVN,δVU,δL,δλ,δh]T；
其中，φE、φN、φU分别为东向、北向和天向的平台误差角，δVE、δVN、δVU分别为东向、北向和天向的速度误差，δL、δλ、δh分别为纬度、经度和高度误差；εbE、εbN、εbU分别为东向、北向和天向的角速度传感器随机常数误差，εrE、εrN、εrU分别为东向、北向和天向的角速度传感器的一阶马尔可夫过程误差，分别为东向、北向和天向的加速度传感器的一阶马尔可夫过程误差。
将惯性系统输出值Xs(t)的公式展开得到：
其中，为角速度传感器噪声向量和为加速度传感器噪声向量，导航坐标系与载体坐标系之间的转置矩阵为
(62)将惯性系统输出状态变量值Xs(t)，结合静止修正测量噪声输入递归滤波器进行递归静止修正解算，去除惯性系统噪声，并输出惯性系统的状态变量真值估计。
有益效果：与现有技术相比，本发明通过对静止修正的垂线偏差观测模型的改正，不仅使用速度、位置信息作为观测量，同时添加零角度修正观测量来限制航向角误差通过更新角速度传感器的常值零偏来减少航向角漂移，解决了由于惯导与全球定位惯导与全球定位组合系统垂线偏差测量中对于天向姿态误差的补偿不到位，航向角误差的积累随时间逐渐增大的问题。
附图说明
图1是本发明方法流程图；
图2是递归静止修正模型流程图；
图3是静止修正的递归解算原理图；
图4(a)、(b)、(c)分别为经纬度误差曲线图、速度误差曲线图和姿态误差曲线图；
图5是垂线偏差扰动的测量精度曲线。
具体实施方式
下面结合附图和具体实施例对本发明的技术方案进行详细说明。
本发明的一种基于静止修正的垂线偏差测量方法，具体是惯导与全球定位惯导与全球定位组合系统垂线偏差观测模型姿态位置速度方程和测量方程的建立方法，针对载体匀速与否提出的静止修正的垂线偏差观测模型的改正方法和改正静止修正的垂线偏差观测模型的建立方法。该方法不仅使用速度、位置信息作为观测量，同时添加零角度修正观测量来限制航向角误差通过更新角速度传感器的常值零偏来减少航向角漂移，解决了惯导与全球定位惯导与全球定位组合系统垂线偏差测量中天向姿态误差的补偿不到位和航向角误差随时间积累增大的问题。
如图1所示，一种基于静止修正的垂线偏差测量方法，包括如下步骤：
(1)建立惯导与全球定位惯导与全球定位组合系统垂线偏差观测模型的姿态位置速度方程和测量方程；
(11)惯导与全球定位组合系统的姿态位置速度方程的线性化形式可表示为：
其中，为惯导与全球定位组合系统垂线偏差中系统的线性化状态向量，包括当地导航坐标系下的姿态线性化误差矢量速度线性化误差矢量位置线性化误差角速度传感器线性化误差矢量和加速度传感器线性化误差矢量X(t)为惯导与全球定位组合系统垂线偏差中的状态向量，包括当地导航坐标系下的姿态误差矢量φn、速度误差矢量δvn、位置误差δrn、角速度传感器误差矢量和加速度传感器误差矢量F(t)为动态系数矩阵，包括惯导与全球定位组合系统的位置、速度、姿态，其中，F11～F33分别表示F(t)内部的动态系数，导航坐标系与载体坐标系之间的转置矩阵为W(t)为惯导与全球定位组合系统的噪声向量，包括角速度传感器噪声向量和加速度传感器噪声向量G(t)为惯导与全球定位组合系统的噪声系数矩阵。
(12)在当地导航系统中，惯导与全球定位组合系统的测量方程是惯导系统和全球定位系统的位置、速度的差值，其测量方程Z(t)可以表示为：
其中，是惯性系统的位置，是全球定位系统的位置，是惯性系统的速度，是全球定位系统的速度，是位置的随机噪声向量，是速度的随机噪声向量。
(13)全球定位系统给出的位置信息为经度λ、纬度和高度h，将其转换为当地导航坐标系中量测方程中的位置之差为：
其中，RM为子午圆半径；RN为卯酉圈半径；λI是惯性系统的经度，λG是全球定位系统的经度，是惯性系统的纬度，是全球定位系统的纬度。
(2)建立静止修正垂线偏差观测模型，如图2所示；
利用系统停车时的速度输出作为观测量，建立递归滤波器来估计惯导与全球定位惯导与全球定位组合系统的速度误差、位置误差以及姿态角误差，然后将估计到的各项误差补偿到相应的变量中，得到相对比较接近于状态变量真值的估计。采用递归滤波进行静止修正的流程如图2所示，主要包括以下步骤：
(21)设置惯导与全球定位惯导与全球定位组合系统的初始参数，包括初始姿态信息，初始位置信息和初始速度信息；
(22)惯性系统初始校准，包括水平方向校准和方位校准，使平台坐标系与地理坐标系趋于一致。
(23)对惯性系统进行捷联解算，根据加速度传感器和角速度传感器信号，解算出惯性系统的航向，姿态，位置及速度。
(24)判断是否到达静止修正时间，若是，则转到步骤(25)，若否，则转到步骤(23)。其中，静止修正时间设定如下：
其中，t0为零点修正时间，δa为设置最大零漂误差，ε0为惯导与全球定位组合系统的零偏精度。
(25)进行递归静止修正解算，其基本原理如图3所示。
(251)对载体进行静止检测，根据惯性器件角速度传感器和加速度传感器三轴输出以及变化规律判断载体是否静止，并通过改变惯导与全球定位组合系统的测量方程来对载体静止时的误差进行修正。
(252)对载体进行静止修正。当载体静止时，认为两个历元间航向角的变化应该为零，并通过更新角速度传感器的常值零偏来减少航向角漂移，具体方法如下：
真实航向角的变化率表达式为：
其中，Ψ、φ、θ分别表示载体的真实航向角、真实横滚角和真实俯仰角；为角速度传感器的真实输出角速率；为载体静止时地球自转在导航坐标系中的角速率。
测量航向角的变化率为：
其中，分别表示载体的测量航向角、测量横滚角和测量俯仰角；为角速度传感器的测量输出角速率；为载体静止时地球自转在导航坐标系中的角速率。
载体处于静止状态，所以角速度传感器的输出可以写为：
其中，bg表示角速度传感器的零偏矢量；ng表示角速度传感器的随机噪声；为角速度传感器的测量输出角速率；为角速度传感器的真实输出角速率；为角速度传感器相对导航坐标系的角速率。
假设俯仰角、横滚角为恒定值，令δΨ表示航向角的误差。在运算过程中作近似处理，如cosδψ＝1、sinδψ＝δψ。则将式(7)和式(6)相减可得：
其中，Ψ、φ、θ分别表示载体的真实航向角、真实横滚角和真实俯仰角；为角速度传感器相对导航坐标系的角速率；bg表示角速度传感器的零偏矢量；ng表示角速度传感器的随机噪声；为测量航向角的变化率；为真实航向角的变化率；为航向角变化率误差。
去掉第一项地球的自转误差，静止修正的垂线偏差观测简化模型可得：
其中，Δtk为航向角更新的时间，k表示迭代次数。
(26)判断是否到达迭代时间，若是，则转到步骤(27)，若否，则转到步骤(25)。其中，迭代时间ti预先设定。
(27)判断是否到达解算结束时间，若是，则转到步骤(28)，若否，则转到步骤(23)。其中，解算结束时间ts预先设定。
(28)解算结束。
(3)判断载体是否匀速，若载体匀速，则执行步骤(4)；否则，执行步骤(5)；
(4)将位置差值和航向角修正的测量值作为观测量对静止修正垂线偏差观测模型进行改正，建立改正的测量方程Z1(t)，并用Z1(t)代替Z(t)。在静止观测的基础上添加了航向角修正测量值，具体测量方程如下所示：
其中，为惯性系统相对导航坐标系的位置矢量；为全球定位系统相对导航坐标系的位置矢量；为第k次迭代的真实航向角的变化率；为第k-1次迭代的真实航向角的变化率；φn为姿态误差，δvn为速度误差，δrn为位置误差，为角速度传感器的零偏，为加速度传感器的零偏；I3×3是惯性系统的3×3向量矩阵；θ为俯仰角，ψ为航向角，φ为翻滚角；表示全球定位系统的位置随机噪声；wψ为航向角观测的随机噪声。
(5)将速度、位置差值以及航向角修正的测量值作为观测量对静止修正垂线偏差观测模型进行改正，建立改正的测量方程Z2(t)，并用Z2(t)代替Z(t)；
其中，为惯性系统相对导航坐标系的位置矢量；为全球定位系统相对导航坐标系的位置矢量；为惯性系统相对导航坐标系的速度矢量；为全球定位系统相对导航坐标系的速度矢量；为第k次迭代的真实航向角的变化率；为第k-1次迭代的真实航向角的变化率；φn为姿态误差，δvn为速度误差，δrn为位置误差，为角速度传感器的零偏，为加速度传感器的零偏；I3×3是惯性系统的3×3向量矩阵；θ为俯仰角，ψ为航向角，φ为翻滚角；和分别表示全球定位系统的位置和速度的随机噪声；wψ为航向角观测的随机噪声。
(6)基于改正的静止修正的垂线偏差观测模型对惯性系统输出状态变量进行真值估计。
(61)惯性系统输出状态变量真值和系统误差值，输出值Xs(t)可用下列状态方程描述：
Xs(t)＝X(t)+G(t)W(t) (13)；
上式中的状态向量X(t)可分为两个向量来考虑：
X(t)＝[X1(t),X2(t)] (14)；
其中，G(t)为惯性系统的噪声系数矩阵，W(t)表示惯性系统噪声，X1(t)为惯性导航系统的误差状态向量，X1(t)为传感器误差的增广状态向量，如下所示：
X1(t)＝[φE,φN,φU,δVE,δVN,δVU,δL,δλ,δh]T(15)；
其中，φE、φN、φU分别为东向、北向和天向的平台误差角，δVE、δVN、δVU分别为东向、北向和天向的速度误差，δL、δλ、δh分别为纬度、经度和高度误差；εbE、εbN、εbU分别为东向、北向和天向的角速度传感器随机常数误差，εrE、εrN、εrU分别为东向、北向和天向的角速度传感器的一阶马尔可夫过程误差，分别为东向、北向和天向的加速度传感器的一阶马尔可夫过程误差。
式(13)展开得到：
其中，为角速度传感器噪声向量和为加速度传感器噪声向量，导航坐标系与载体坐标系之间的转置矩阵为
(62)将惯性系统输出状态变量值Xs(t)，结合静止修正测量噪声输入递归滤波器进行递归静止修正解算，去除惯性系统噪声，并输出惯性系统的状态变量真值估计。
为了验证载体在静止条件下使用静止修正技术时系统输出的导航信息修正效果，对垂线偏差测量导航系统进行了仿真实验，表1为惯性系统的主要指标。
表1惯性系统的主要指标
仿真结果如图4所示。具体地，图4(a)所示为经纬度误差曲线图，图4(b)所示为速度误差曲线图，图4(c)所示为姿态误差曲线图；从图4可以看出，改正后方案都可以有效的改善载体的姿态误差，提高惯性系统的姿态输出精度，即可以采用速度位置姿态匹配的方法，输出惯性系统姿态，并将其作为惯性测量单元姿态的初始值。最后利用Kalman滤波估算出垂线偏差。
图5是垂线偏差扰动的测量结果。由图可知，总体上改正方案精度在2″以内，证实了该方法的可行性。
为减小惯性系统输出的姿态误差对垂线偏差计算产生误差影响，本发明利用静止修正技术改善了姿态误差输出精度。在静止修正模式下对垂线偏差建立相应的姿态位置速度方程，采用速度、位置匹配建立测量方程，并在位置、速度差观测量的基础上添加姿态角观测量，使角速度传感器的零偏变成可直接测量的参数，提高了航向角的估计精度，解决了垂线偏差测量中天向姿态误差的补偿不到位和航向角误差随时间积累增大的问题。

Claims (8)

1.一种基于静止修正的垂线偏差测量方法，其特征在于，包括以下步骤：
(1)建立惯导与全球定位组合系统垂线偏差观测模型的姿态位置速度方程和测量方程；
(2)建立惯导与全球定位组合系统静止修正垂线偏差观测模型；
(3)判断载体是否匀速，若载体匀速，则执行步骤(4)；否则，执行步骤(5)；
(4)将位置差值和航向角修正的测量值作为观测量对垂线偏差方程进行改正；
(5)将速度、位置差值以及航向角修正的测量值作为观测量对垂线偏差方程进行改正；
(6)基于步骤(4)或步骤(5)中改正的静止修正的垂线偏差观测模型对惯性系统输出状态变量进行真值估计。
2.根据权利要求1所述的一种基于静止修正的垂线偏差测量方法，其特征在于，步骤(1)中惯导与全球定位组合系统垂线偏差观测模型的姿态位置速度方程建立方法如下：
惯导与全球定位组合系统的姿态位置速度方程的线性化形式表示为：
其中，为惯导与全球定位组合系统垂线偏差中系统的线性化状态向量，包括当地导航坐标系下的姿态线性化误差矢量速度线性化误差矢量位置线性化误差角速度传感器线性化误差矢量和加速度传感器线性化误差矢量X(t)为惯导与全球定位组合系统垂线偏差中的状态向量，包括当地导航坐标系下的姿态误差矢量φn、速度误差矢量δvn、位置误差δrn、角速度传感器误差矢量和加速度传感器误差矢量F(t)为动态系数矩阵，包括惯导与全球定位组合系统的位置、速度、姿态，其中，F11～F33分别表示F(t)内部的动态系数，导航坐标系与载体坐标系之间的转置矩阵为W(t)为惯导与全球定位组合系统的噪声向量，包括角速度传感器噪声向量和加速度传感器噪声向量G(t)为惯导与全球定位组合系统的噪声系数矩阵。
3.根据权利要求1所述的一种基于静止修正的垂线偏差测量方法，其特征在于，步骤(1)中惯导与全球定位组合系统垂线偏差观测模型的测量方程建立方法如下：
(a)在当地导航系统中，惯导与全球定位组合系统垂线偏差观测模型的测量方程是惯导系统和全球定位系统的位置、速度的差值，其测量方程Z(t)表示为：
其中，是惯性系统的位置，是全球定位系统的位置，是惯性系统的速度，是全球定位系统的速度，是位置的随机噪声向量，是速度的随机噪声向量，φn是姿态误差矢量，δvn是速度误差矢量，δrn是位置误差，是角速度传感器误差矢量，是加速度传感器误差矢量；
(b)全球定位系统给出的位置信息为经度λ、纬度和高度h，将其转换为当地导航坐标系中量测方程中的位置之差为：
其中，RM为子午圆半径；RN为卯酉圈半径；λI是惯性系统的经度，λG是全球定位系统的经度，是惯性系统的纬度，是全球定位系统的纬度。
4.根据权利要求1所述的一种基于静止修正的垂线偏差测量方法，其特征在于，步骤(2)中静止修正垂线偏差观测模型的建立方法如下：
(21)设置惯导与全球定位组合系统的初始参数，包括初始姿态信息，初始位置信息和初始速度信息；
(22)惯性系统初始校准，包括水平方向校准和方位校准，使平台坐标系与地理坐标系趋于一致；
(23)对惯性系统进行捷联解算，根据加速度传感器和角速度传感器信号，解算出惯性系统的航向，姿态，位置及速度；
(24)判断是否到达静止修正时间，若是，则转到步骤(25)，若否，则转到步骤(23)；其中，静止修正时间设定如下：
其中，t0为零点修正时间，δa为设置最大零漂误差，ε0为惯导与全球定位组合系统的零偏精度；
(25)进行递归静止修正解算；
(26)判断是否到达预先设定的迭代时间ti，若是，则转到步骤(27)，若否，则转到步骤(25)；
(27)判断是否到达预先设定的解算结束时间ts，若是，则转到步骤(28)，若否，则转到步骤(23)；
(28)解算结束。
5.根据权利要求4所述的一种基于静止修正的垂线偏差测量方法，其特征在于，步骤(25)中递归静止修正解算方法如下：
(251)对载体进行静止检测，根据惯性器件角速度传感器和加速度传感器三轴输出以及变化规律判断载体是否静止，并通过改变惯导与全球定位组合系统垂线偏差观测模型的测量方程来对载体静止时的误差进行修正；
(252)对载体进行静止修正，当载体静止时，认为两个历元间航向角的变化应该为零，并通过更新角速度传感器的常值零偏来减少航向角漂移，具体方法如下：
真实航向角的变化率表达式为：
其中，Ψ、φ、θ分别表示载体的真实航向角、真实横滚角和真实俯仰角；为角速度传感器的真实输出角速率；为载体静止时地球自转在导航坐标系中的角速率；
测量航向角的变化率为：
其中，分别表示载体的测量航向角、测量横滚角和测量俯仰角；为角速度传感器的测量输出角速率；为载体静止时地球自转在导航坐标系中的角速率；
载体处于静止状态，所以角速度传感器的输出为：
其中，bg表示角速度传感器的零偏矢量；ng表示角速度传感器的随机噪声；为角速度传感器的测量输出角速率；为角速度传感器的真实输出角速率；为角速度传感器相对导航坐标系的角速率；
设俯仰角、横滚角为恒定值，令δΨ表示航向角的误差，在运算过程中作近似处理，如cosδψ＝1sinδψ＝δψ；则将测量航向角的变化率与真实航向角的变化率相减得：
其中，为航向角变化率误差；
去掉第一项地球的自转误差，静止修正的垂线偏差观测模型得：
其中，Δtk为航向角更新的时间，k表示迭代次数。
6.根据权利要求1所述的一种基于静止修正的垂线偏差测量方法，其特征在于，步骤(4)中将位置差值和航向角修正的测量值作为测量方程对静止修正垂线偏差观测模型进行改正，在静止观测的基础上添加了航向角修正测量值，建立改正的测量方程Z1(t)，并用Z1(t)代替Z(t)：具体测量方程如下所示：
其中，为惯性系统相对导航坐标系的位置矢量；为全球定位系统相对导航坐标系的位置矢量；为第k次迭代的真实航向角的变化率；为第k-1次迭代的真实航向角的变化率；φn为姿态误差，δvn为速度误差，δrn为位置误差，为角速度传感器的零偏，为加速度传感器的零偏；I3×3是惯性系统的3×3向量矩阵；θ为俯仰角，ψ为航向角，φ为翻滚角；表示全球定位系统的位置随机噪声；wψ为航向角观测的随机噪声。
7.根据权利要求1所述的一种基于静止修正的垂线偏差测量方法，其特征在于，步骤(5)中将速度、位置差值以及航向角修正的测量值作为观测量对静止修正垂线偏差观测模型进行改正，建立改正的测量方程Z2(t)，并用Z2(t)代替Z(t)：
其中，为惯性系统相对导航坐标系的位置矢量；为全球定位系统相对导航坐标系的位置矢量；为惯性系统相对导航坐标系的速度矢量；为全球定位系统相对导航坐标系的速度矢量；为第k次迭代的真实航向角的变化率；为第k-1次迭代的真实航向角的变化率；φn为姿态误差，δvn为速度误差，δrn为位置误差，为角速度传感器的零偏，为加速度传感器的零偏；I3×3是惯性系统的3×3向量矩阵；θ为俯仰角，ψ为航向角，φ为翻滚角；和分别表示全球定位系统的位置和速度的随机噪声；wψ为航向角观测的随机噪声。
8.根据权利要求1所述的一种基于静止修正的垂线偏差测量方法，其特征在于，步骤(6)中基于改正的静止修正的垂线偏差观测模型对惯性系统输出状态变量的真值估计方案如下：
(61)惯性系统输出状态变量真值和系统误差值，输出值Xs(t)用下列姿态位置速度方程描述：
Xs(t)＝X(t)+G(t)W(t)；
上式中的状态向量X(t)分为两个向量来考虑为：
X(t)＝[X1(t),X2(t)]；
其中，G(t)为惯性系统的噪声系数矩阵，W(t)表示惯性系统的噪声向量，X1(t)为惯性导航系统的误差状态向量，X1(t)为传感器误差的增广状态向量，如下所示：
X1(t)＝[φE,φN,φU,δVE,δVN,δVU,δL,δλ,δh]T；
其中，φE、φN、φU分别为东向、北向和天向的平台误差角，δVE、δVN、δVU分别为东向、北向和天向的速度误差，δL、δλ、δh分别为纬度、经度和高度误差；εbE、εbN、εbU分别为东向、北向和天向的角速度传感器随机常数误差，εrE、εrN、εrU分别为东向、北向和天向的角速度传感器的一阶马尔可夫过程误差，分别为东向、北向和天向的加速度传感器的一阶马尔可夫过程误差。
将惯性系统输出值Xs(t)的公式展开得到：
其中，为角速度传感器噪声向量和为加速度传感器噪声向量，导航坐标系与载体坐标系之间的转置矩阵为
(62)将惯性系统输出状态变量值Xs(t)，结合静止修正测量噪声输入递归滤波器进行递归静止修正解算，去除惯性系统噪声，并输出惯性系统的状态变量真值估计。
CN201811558086.4A 2018-12-19 2018-12-19 一种基于静止修正的垂线偏差测量方法 Active CN109612460B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811558086.4A CN109612460B (zh) 2018-12-19 2018-12-19 一种基于静止修正的垂线偏差测量方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811558086.4A CN109612460B (zh) 2018-12-19 2018-12-19 一种基于静止修正的垂线偏差测量方法

Publications (2)

Publication Number Publication Date
CN109612460A true CN109612460A (zh) 2019-04-12
CN109612460B CN109612460B (zh) 2020-11-20

Family

ID=66010005

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811558086.4A Active CN109612460B (zh) 2018-12-19 2018-12-19 一种基于静止修正的垂线偏差测量方法

Country Status (1)

Country Link
CN (1) CN109612460B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110017849A (zh) * 2019-04-18 2019-07-16 菲曼(北京)科技有限公司 一种基于gnss接收机和imu传感器的测绘一体机的倾斜测量方法
CN111833208A (zh) * 2020-07-14 2020-10-27 宁夏大学 一种基于垂线偏差扰动的地下水储量监测方法及系统
CN112558125A (zh) * 2021-02-22 2021-03-26 腾讯科技（深圳）有限公司 一种车辆定位的方法、相关装置、设备以及存储介质
CN112684207A (zh) * 2020-12-17 2021-04-20 东南大学 一种深潜载人潜水器adcp速度估计与修正算法
CN113532477A (zh) * 2021-07-15 2021-10-22 青岛迈金智能科技有限公司 一种骑行码表设备及骑行码表初始姿态自动校准方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104061945A (zh) * 2014-06-30 2014-09-24 中国人民解放军国防科学技术大学 基于ins和gps组合的垂线偏差动态测量装置及方法
US20170227363A1 (en) * 2015-01-08 2017-08-10 Uti Limited Partnership Method and apparatus for enhanced pedestrian navigation based on wlan and mems sensors
CN108426574A (zh) * 2018-02-02 2018-08-21 哈尔滨工程大学 一种基于zihr的航向角修正算法的mems行人导航方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104061945A (zh) * 2014-06-30 2014-09-24 中国人民解放军国防科学技术大学 基于ins和gps组合的垂线偏差动态测量装置及方法
US20170227363A1 (en) * 2015-01-08 2017-08-10 Uti Limited Partnership Method and apparatus for enhanced pedestrian navigation based on wlan and mems sensors
CN108426574A (zh) * 2018-02-02 2018-08-21 哈尔滨工程大学 一种基于zihr的航向角修正算法的mems行人导航方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
徐海鑫等: "零角度修正在GNSS/INS组合导航中的应用", 《测绘科学技术学报》 *
徐海鑫等: "零速修正在GNSS/INS组合导航中的应用", 《全球定位系统》 *
杨裕翠等: "基于卡尔曼滤波器的零速修正技术在导弹发射车中的应用研究", 《电气自动化》 *
王凌轩等: "GNSS/INS组合导航滤波更新周期的影响研究", 《第六届中国卫星导航学术年会》 *

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110017849A (zh) * 2019-04-18 2019-07-16 菲曼(北京)科技有限公司 一种基于gnss接收机和imu传感器的测绘一体机的倾斜测量方法
CN111833208A (zh) * 2020-07-14 2020-10-27 宁夏大学 一种基于垂线偏差扰动的地下水储量监测方法及系统
CN111833208B (zh) * 2020-07-14 2022-03-11 宁夏大学 一种基于垂线偏差扰动的地下水储量监测方法及系统
CN112684207A (zh) * 2020-12-17 2021-04-20 东南大学 一种深潜载人潜水器adcp速度估计与修正算法
CN112684207B (zh) * 2020-12-17 2022-03-11 东南大学 一种深潜载人潜水器adcp速度估计与修正算法
CN112558125A (zh) * 2021-02-22 2021-03-26 腾讯科技（深圳）有限公司 一种车辆定位的方法、相关装置、设备以及存储介质
CN112558125B (zh) * 2021-02-22 2021-05-25 腾讯科技（深圳）有限公司 一种车辆定位的方法、相关装置、设备以及存储介质
CN113532477A (zh) * 2021-07-15 2021-10-22 青岛迈金智能科技有限公司 一种骑行码表设备及骑行码表初始姿态自动校准方法

Also Published As

Publication number Publication date
CN109612460B (zh) 2020-11-20

Similar Documents

Publication Publication Date Title
CN109612460B (zh) 一种基于静止修正的垂线偏差测量方法
CN110501024B (zh) 一种车载ins/激光雷达组合导航系统的量测误差补偿方法
CN113029199B (zh) 一种激光陀螺惯导系统的系统级温度误差补偿方法
CN110221332B (zh) 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法
CN108051866B (zh) 基于捷联惯性/gps组合辅助水平角运动隔离的重力测量方法
CN110031882B (zh) 一种基于sins/dvl组合导航系统的外量测信息补偿方法
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
CN107270893B (zh) 面向不动产测量的杆臂、时间不同步误差估计与补偿方法
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN103994763B (zh) 一种火星车的sins/cns深组合导航系统及其实现方法
CN100516775C (zh) 一种捷联惯性导航系统初始姿态确定方法
CN101788296B (zh) 一种sins/cns深组合导航系统及其实现方法
CN109870173A (zh) 一种基于校验点的海底管道惯性导航系统的轨迹修正方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN104501838B (zh) 捷联惯导系统初始对准方法
CN113063429B (zh) 一种自适应车载组合导航定位方法
CN101571394A (zh) 基于旋转机构的光纤捷联惯性导航系统初始姿态确定方法
CN101187567A (zh) 基于多普勒的光纤陀螺捷联惯导系统初始姿态确定方法
CN101893445A (zh) 摇摆状态下低精度捷联惯导系统快速初始对准方法
CN104697526A (zh) 用于农业机械的捷联惯导系统以及控制方法
CN104344837A (zh) 一种基于速度观测的冗余惯导系统加速度计系统级标定方法
CN112595350A (zh) 一种惯导系统自动标定方法及终端
Xue et al. In-motion alignment algorithm for vehicle carried SINS based on odometer aiding
CN103697911A (zh) 一种纬度未知情形下的捷联惯导系统初始姿态确定方法
CN104776847B (zh) 一种适用于水下导航系统单点估计陀螺漂移的方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant