CN110221332A - 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法 - Google Patents

一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法 Download PDF

Info

Publication number
CN110221332A
CN110221332A CN201910288631.0A CN201910288631A CN110221332A CN 110221332 A CN110221332 A CN 110221332A CN 201910288631 A CN201910288631 A CN 201910288631A CN 110221332 A CN110221332 A CN 110221332A
Authority
CN
China
Prior art keywords
lever arm
vehicle
error
compensation
ins
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
CN201910288631.0A
Other languages
English (en)
Other versions
CN110221332B (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.)
Tongji University
Original Assignee
Tongji 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 Tongji University filed Critical Tongji University
Priority to CN201910288631.0A priority Critical patent/CN110221332B/zh
Publication of CN110221332A publication Critical patent/CN110221332A/zh
Application granted granted Critical
Publication of CN110221332B publication Critical patent/CN110221332B/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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
    • 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

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

本发明涉及一种车载GNSS/INS组合导航的动态杆臂误差估计和补偿方法，本发明考虑了GNSS与INS作为导航子***的数据信号优劣差异分别处理，并且考虑车载导航中由于车辆状态变化产生变形引起杆臂变化导致误差变动，将杆臂分为静态和动态杆臂两部分，并对动态杆臂建模和建立补偿机制，对杆臂效应产生的速度和位置误差进行反馈和补偿，实现基于位置不重合导致的组合导航定位精度提高方法。

Description

一种车载GNSS/INS组合导航的动态杆臂误差估计和补偿方法
技术领域
本发明涉及车载导航与定位技术领域，尤其是涉及一种车载GNSS/INS组合导航的动态杆臂误差估计和补偿方法。
背景技术
惯性导航***(Inertial Navigation System，INS)具有高自主性、抗干扰性、高的短期精度、高数据输出率、完备的导航信息、适应范围广等特点，但是其***误差具有周期振荡的特性，某些导航参数误差具有随时间积累的特性且初始对准所需时间较长；而GNSS输出定位精度高，不要求初始化，但其信号会被干扰或者遮挡，无法稳定的提供连续导航参数和提供较为准确的车辆姿态，所以利用GNSS导航误差不随时间积累的外部参考信息源，定期或不定期地对惯性导航***进行导航参数校正和对惯性器件的漂移进行补偿，从而为车辆提供连续、长时和短时精度均较高和完整的导航参数，从而实现车载高精度定位。惯性导航一般以惯组(IMU)的几何中心作为导航定位或测速的参考基准，而卫星导航则以接收机天线的相位中心作为参考基准，在车辆载体同时使用时，它们在安装位置上存在一定的偏差，而该偏差在实际车辆运行中会导致速度和位置的差异，称为杆臂误差，所以在组合导航中需要将该误差进行估计并补偿。
目前关于组合导航杆臂误差的估计与补偿主要分为三种：力学补偿/机械补偿、动态在线标定/估计与数字滤波补偿。其中对于误差的估计主要为标定与状态拓展，而补偿校正主要将杆臂误差作用于输出结果或组合导航滤波器，即输出校正与反馈校正。输出校正是将输出结果进行修正，而不改变累积误差，该方法随着时间的累计，待估计的导航参数误差变大，导致INS***误差模型变为非线性，组合导航滤波器的精度降低，而反馈校正时导航参数误差估计值归零，因此在GNSS/INS组合导航时必须对导航参数误差进行反馈校正。当前的导航参数误差反馈校正方案根据校正方法以及校正的状态参数分为：混合校正(初始采用输出校正，后期采用反馈校正)、不完全反馈(只对位置、速度、姿态误差进行反馈校正)和完全反馈校正(对位置、速度、姿态误差以及惯性器件随机常值误差进行反馈校正)。由于惯性器件随机常值误差的校正对于GNSS信号弱及缺失下的***输出影响显著，此时必须考虑惯性器件随机常值误差的反馈校正方案，同时应当加入杆臂的反馈校正。
如专利申请号201110220018.9的航空遥感用位置和姿态测量***(POS)动态杆臂补偿方法公开了如下内容：“针对三轴惯性稳定平台框架转动导致惯性测量单元(IMU)量测中心与GPS天线相位中心之间的杆臂实时变化的问题，通过实时计算三轴惯性稳定平台中心与IMU量测中心之间的动态杆臂，得到IMU量测中心与GPS天线相位中心之间的实际杆臂，并实时计算三轴惯性稳定平台初始坐标系相对当地地理坐标系在三轴惯性稳定平台初始坐标系下的角速度，进行动态杆臂补偿”，但是该方法相当于建立角速度与杆臂误差的映射关系，从而航空遥感发生姿态运动时候进行补偿，而实际车辆的平动以及其他因素的耦合关系，可能导致角速度测量值偏差会引起错误的杆臂补偿。
如专利申请号201310289324.7的一种基于杆臂估计的INS/GPS组合导航***反馈校正方法公开了如下内容：“将杆臂误差扩展至***误差状态向量中进行估计，通过比较杆臂估计值与真实值之差是否在误差阈值范围内，来判断惯性器件随机误差是否得到准确估计，该方法可实现对惯性器件随机常值误差进行有效估计并进行全反馈校正，能够有效提高INS/GPS组合导航***的精度”，但是该方法中杆臂的测量值即为真值，在不完全反馈校正中，杆臂估计结果只作为一个校正方法切换的判断条件，而在全状态反馈中杆臂误差则是作为机械补偿，而忽略了运行中由于车辆的态变化引起的车辆挠曲变形导致的杆臂变化。
GNSS/INS组合导航***的两种杆臂估计补偿方案均存在以下不足：
1.杆臂误差缺乏对车辆动态杆臂的估计与补偿，从而降低组合导航精度；
2.目前的杆臂误差动态补偿需要稳定平台建立，不适应车辆使用情况；
3.动态误差补偿时候与车辆本身状态耦合关系考虑不全，只考虑了角速度与杆臂误差的对应关系。
发明内容
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种车载GNSS/INS组合导航的动态杆臂误差估计和补偿方法，考虑车辆运行过程中INS与GNSS间的杆臂变化导致的误差，将杆臂分为静态和动态杆臂两部分，并对动态杆臂建模和建立补偿机制，对杆臂效应产生的速度和位置误差进行反馈和补偿，实现基于位置不重合导致的组合导航定位精度提高方法。
本发明的目的可以通过以下技术方案来实现：
一种车载GNSS/INS组合导航的动态杆臂误差估计和补偿方法，包括以下步骤：
步骤1：车辆行驶前利用测量仪器测量GNSS相位中心与惯组中心在车辆坐标系下的杆臂值并在车辆静止状态下进行INS的初始化；
步骤2：采集车辆行驶过程中的INS原始导航数据和GNSS数据；
步骤3：对GNSS数据进行可用性判断、异常点去除、信号插值和滤波后将得到的输出PVT信息和航向信息以及信号的可用性信息均输入到组合滤波器中；
步骤4：将INS原始导航数据进行器件补偿和姿态解算后分别进入动态杆臂补偿判断和导航计算后将得到的判断结果及补偿量、INS导航相关信息以及速度增量姿态、速度和位置均输入到组合滤波器中；
步骤5：当组合滤波器的数据输入完毕后，建立组合导航***的状态方程，对状态方程进行估计，每次滤波后，利用滤波估计出的结果对INS解算结果进行反馈校正。
进一步地，所述步骤3中的可用性判断，其描述公式为：
式中，QGNSS表示信号可用性。
进一步地，其特征在于，所述步骤4中的动态杆臂补偿判断具体包括：根据当前输入三轴角速度和三轴加速度值辅以设定的权重值求解车辆工况的表征参数并设定不同阈值表征急加速急减速大转向或者剧烈工况，若车辆工况的表征参数小于等于设定阈值时，则不进行动态杆臂补偿，若车辆工况的表征参数大于设定阈值时，则进行动态杆臂补偿，所述车辆工况的表征参数的计算公式为：
式中，|R|为车辆工况的表征参数，k1、k2和k3分别为三轴对应权重值， 和分别为当前三轴比力值，和分别为当前输入三轴角速度。
进一步地，所述步骤4中的补偿量的计算公式为：
且|R|≤|R|th2
式中，为静态测量值，δlb为杆臂值，和分别为静态下GNSS相位中心与惯组中心在车辆坐标系下三个坐标轴的杆臂，φx、φy和φz分别为三个坐标轴的车辆姿态角，|R|th和|R|th2为用于划定判定区间的设定阈值，为动态补偿杆臂值。
进一步地，所述步骤4中的速度增量姿态、速度和位置采用二子样圆锥误差补偿算法计算，其对应的计算方程组为：
式中，Δθm1和Δθm2为陀螺两次等间隔采样对应的角增量，T为采样时间，为以惯性坐标系作为参考基准，载体系从tm-1时刻到tm时刻的旋转变化，为以惯性坐标系作为参考基准，地理系从tm时刻到tm-1时刻的旋转变化，下标i表示惯导***解算值，上下标b表示载体系，上下标n表示地理系，(m)表示tm时刻，(m-1)表示tm-1时刻，φ带上下标表示对应姿态，I表示单位矩阵，为常值。
进一步地，所述步骤5包括以下分步骤：
步骤51：建立***方程；
步骤52：建立量测方程；
步骤53：建立kalman滤波***方程并使量测方程离散化；
步骤54：利用kalman滤波***方程进行反馈校正。
进一步地，所述步骤51中的***方程，其描述公式为：
X＝[φE φN φU δvE δvN δvU δL δλ δh εx εy εz ▽x ▽y ▽z]T
式中，X为状态向量，φE、φN和φU分别为东-北-天地理坐标系下的姿态误差，δvE、δvN和δvU分别为东-北-天地理坐标系下的速度误差，δL、δλ和δh为经度、纬度和高度的位置误差，εx、εy和εz分别为陀螺仪三个坐标轴的零偏，▽x、▽y和▽z分别为加速度计的三个坐标轴的零偏；
式中，为地理系相对于惯性系的角速度，为地球系相对于惯性系的角速度误差，为地理系相对于地球系的角速度误差，为载体系到地理系的坐标变换矩阵，为载体系相对于惯性系的角速度误差，为载体系相对于地理系下的惯导***加速度计的输出比力，vn为载体在地理系下的速度，为地球系相对于惯性系的角速度，为地理系相对于地球系的角速度，δvn为载体在地理系下的速度误差，为载体系相对于地理系下的惯导***加速度计的输出比力误差，δgn为重力加速度误差，RM为卯酉圈半径，h为当地海拔高度，L为当地纬度，RN为子午圈半径，单独φ表示捷联惯导***中的数学平台误差角。
进一步地，所述步骤52中的量测方程，其描述公式为：
式中，上标n表示为地理系，Z为量测方程，下标INS表示惯性***，下标GNSS表示卫星导航，上标～表示实际值，v表示速度，p表示位置，表示载体系相对于地球系的角速度，RMh＝RM+h，RNh＝RN+h。
进一步地，所述步骤5还包括：将kalman滤波后的陀螺和加速度零偏反馈至器件补偿处进行校正，将姿态反馈至姿态更新补偿处，将速度、位置误差反馈至INS解算的输出值上进行校正，即：由修正后的可解得航向角ψ、俯仰角θ和滚动角γ，一次滤波反馈后，误差状态归0。
进一步地，所述由修正后的可解得航向角ψ、俯仰角θ和滚动角γ，其对应的描述公式为：
式中，(数字1，数字2)表示矩阵内具体对应的矩阵元素。
与现有技术相比，本发明具有以下优点：
(1)本发明中，考虑了GNSS与INS作为导航子***的数据信号优劣差异分别处理，并且考虑车载导航中由于车辆状态变化产生变形引起杆臂变化导致误差变动，将杆臂分为静态和动态杆臂两部分，并对动态杆臂建模和建立补偿机制，对杆臂效应产生的速度和位置误差进行反馈和补偿，提高了GNSS/INS组合导航***的精度。
附图说明
图1为本发明GNSS天线与INS惯组中心相对位置的杆臂简图；
图2为本发明组合导航***结构框图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。
实施例
本发明通过对INS惯组中心至GNSS天线之间的杆臂值进行测量，并区分动态杆臂与静态杆臂做误差补偿，主要分为四个阶段：第一阶段为静止测量标定与初始化、第二阶段为GNSS与INS的原始数据处理及信号可用性判断，第三阶段为动态杆臂补偿判断和导航计算，第四阶段为传感器融合滤波和反馈校正。
本发明的具体实施步骤如图2所示：
1)车辆行驶前，利用测量仪器测量GNSS相位中心与惯组(IMU)中心在车
辆坐标系下的杆臂作为静态以及小动态下的杆臂，如图1所示；
2)车辆静止状态下进行INS的初始化；
3)车辆行驶过程采集GNSS/INS组合导航***数据，包括惯性测量数据：三轴陀螺仪数据和三轴加速度计数据，GNSS数据:载波、扩频码/测距码，保留原始导航电文信息(信号状态)，同时通过导航处理器确定位置、速度和时间(Position，Velocity，and Time，PVT)，伪距与伪距率。其中位置包括纬度L，经度λ及高度h，速度包括东向速度VE，北向速度VN，天向速度VU；
4)对3)中GNSS信号进行处理，具体包括信号可用性判断、异常点去除、信号的插值和滤波，输出速度位置信息和航向信息以及信号的可用性QGNSS，输入到组合滤波器中；其中，
速度位置测量值为：
信号的可用性为：
5)对3)中INS的原始导航数据，进行器件补偿和姿态解算后分别进入动态杆臂补偿的判断和导航计算，得到是否进行动态杆臂补偿及补偿值[δlb Glb]，以及得到INS导航状态的三轴加速度、三轴角速度以及角度、速度增量姿态、速度和位置等信息，均输入到组合滤波器中；
杆臂动态补偿判断：
根据当前输入三轴角速度：三轴比力值，当前车辆的状态根据急加速急减速大转向或者剧烈工况来表征，设定阈值|R|th和|R|th2。
其中：
当|R|≤|R|th，不进行动态杆臂补偿；|R|＞|R|th进行补偿。
动态杆臂补偿量计算
由于杆臂为静态与动态之和：
为静态测量值，为动态杆臂，Glb为动态杆臂的反馈系数。
由于车辆变形与其受力/扭矩大小和车辆本身的扭转刚度相关以及实际的扭转角相关，而受力/扭矩大小反应为车辆的运动状态改变，扭转刚度在运动过程中视为不变，而实际的扭转角难以测量，但与车辆的姿态角相关，所以建立变形量与车辆自身姿态角和静态杆臂之间的函数关系：
动态杆臂：
且|R|≤|R|th2
6)当(4)(5)中数据输入滤波器后，采用误差状态向量具体包括位置、速度、姿态、陀螺随机常值漂移ε、加速度计随机常值零偏▽的共15维误差状态量，对15维误差状态向量进行估计。每次滤波后，利用滤波估计出的位置误差速度误差失准角误差陀螺随机常值漂移加速度计随机常值零偏对INS解算结果进行反馈校正。
一.步骤5)中的姿态计算
选“东-北-天(E-N-U)”地理坐标系(g系)作为捷联惯导***的导航参考坐标系，重新记为n系，则以n系作为参考系的姿态微分方程为：
其中，矩阵表示以i系(惯性坐标系)作为参考基准，b系从tm-1时刻到tm时刻的旋转变化，可由陀螺角速度确定；表示以i系作为参考基准，n系从tm时刻到tm-1时刻的旋转变化，可由计算角速度确定，和分别表示tm-1和tm时刻的捷联姿态矩阵。若陀螺在时间段[tm-1,tm]内(T＝tm-tm-1)进行了两次等间隔采样，角增量分别为Δθm1和Δθm2，采用二子样圆锥误差补偿算法，有：
取四阶截断，并取近似：
导航更新周期[tm-1,tm]内，可以认为由速度和位置引起的变化很小，即可视为常值，记为则有：
二.步骤(6)中的滤波解算
1.滤波解算：
建立***方程
其中：X：误差状态向量；
F：***矩阵；
G：噪声分配矩阵；
w：零均值高斯白噪声向量；
Z：量测向量；
H：量测矩阵；
V：量测噪声状态向量；
相关上下标位置处的b表示载体系，n表示地理系，e表示地球系，i表示惯性系。
X＝[φE φN φU δvE δvN δvU δL δλ δh εx εy εz ▽x ▽y ▽z]T
式中，X为状态向量，φE、φN和φU分别为东-北-天地理坐标系下的姿态误差，δvE、δvN和δvU分别为东-北-天地理坐标系下的速度误差，δL、δλ和δh为经度、纬度和高度的位置误差，εx、εy和εz分别为陀螺仪三个坐标轴的零偏，▽x、▽y和▽z分别为加速度计的三个坐标轴的零偏；
式中，为地理系相对于惯性系的角速度，为地球系相对于惯性系的角速度误差，为地理系相对于地球系的角速度误差，为载体系到地理系的坐标变换矩阵，为载体系相对于惯性系的角速度误差，为载体系相对于地理系下的惯导***加速度计的输出比力，vn为载体在地理系下的速度，为地球系相对于惯性系的角速度，为地理系相对于地球系的角速度，δvn为载体在地理系下的速度误差，为载体系相对于地理系下的惯导***加速度计的输出比力误差，δgn为重力加速度误差，RM为卯酉圈半径，h为当地海拔高度，L为当地纬度，RN为子午圈半径，单独φ表示捷联惯导***中的数学平台误差角。
载体系下的陀螺零偏：
载体系下的加速计零偏：
以下将方程(姿态-速度-位置)依次展开：
其中
其中：
为陀螺测量误差，m带不同a、x、y、z下标表示为陀螺测量中的两轴间的交叉耦合系数，s带a、x、z下标表示为陀螺测量中的比例因子误差。
其中：
为加速度计测量误差，m带不同g、x、y、z下标表示为加速度计测量中的交叉耦合系数，s带g、x、z下标表示为加速度计测量中比例因子误差。
根据WGS-84(World Geodetic System 1984)地球系给出的地球参数为：
半长轴：Re＝6378137m，扁率：f＝1/298.257223563，
地心引力常数(含大气层)：μ＝3.986004418×1014m3/s2，
地球自转角速率：ωie＝7.2921151467×10-5rad/s
ge和gp分别为赤道重力和极点重力，地球重力扁率为：
β1表示与赤道重力的比值：
β2表示重力随高度下降梯度：
整理有式1-5有：
F15＝03×3
F24＝03×3，F31＝03×3
F34＝03×3，F35＝03×3，F41＝F42＝F43＝F44＝F45＝F51＝F52＝F53＝F54＝F55＝03×3
2.建立量测方程：
式中，上标n表示为地理系，Z为量测方程，下标INS表示惯性***，下标GNSS表示卫星导航，上标～表示实际值，v表示速度，p表示位置，表示载体系相对于地球系的角速度，RMh＝RM+h，RNh＝RN+h。
整理有：
3.Kalman滤波***方程和量测方程离散化
做近似离散化等效：
Xk＝Φk/k-1Xk-1+Γk-1Wk-1
其中设定离散化时间间隔Ts＝tk-tk-1，则状态转移矩阵取一阶截断，有：
Wk-1为***噪声向量，Vk为量测噪声向量，两者都是零均值的高斯白噪声向量序列(服从正态分布)，且它们之间互不相关，即满足：
{E[Wk]＝0,E[Vk]＝0,
在Kalman滤波状态空间模型中对于噪声要求的基本假设，一般要求Qk是半正定的且Rk是正定的，即Qk≥0且Rk＞0。Kalman滤波全套算法，可划分为五个基本公式，如下：
(1)状态一步预测
(2)状态一步预测均方误差
(3)滤波增益
(4)状态估计
(5)状态估计均方误差
Pk＝(I-KkHk)Pk/k-1
4.反馈校正
将kalman滤波后的陀螺和加速度零偏反馈至器件补偿处进行校正，将姿态反馈至姿态更新补偿处，将速度、位置误差反馈至INS解算的输出值上进行校正，反馈后，误差状态归0。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (10)

1.一种车载GNSS/INS组合导航的动态杆臂误差估计和补偿方法，其特征在于，包括以下步骤：
步骤1：车辆行驶前利用测量仪器测量GNSS相位中心与惯组中心在车辆坐标系下的杆臂值并在车辆静止状态下进行INS的初始化；
步骤2：采集车辆行驶过程中的INS原始导航数据和GNSS数据；
步骤3：对GNSS数据进行可用性判断、异常点去除、信号插值和滤波后将得到的输出PVT信息和航向信息以及信号的可用性信息均输入到组合滤波器中；
步骤4：将INS原始导航数据进行器件补偿和姿态解算后分别进入动态杆臂补偿判断和导航计算后将得到的判断结果及补偿量、INS导航相关信息以及速度增量姿态、速度和位置均输入到组合滤波器中；
步骤5：当组合滤波器的数据输入完毕后，建立组合导航***的状态方程，对状态方程进行估计，每次滤波后，利用滤波估计出的结果对INS解算结果进行反馈校正。
2.根据权利要求1所述的一种车载GNSS/INS组合导航的动态杆臂误差估计和补偿方法，其特征在于，所述步骤3中的可用性判断，其描述公式为：
式中，QGNSS表示信号可用性。
3.根据权利要求1所述的一种车载GNSS/INS组合导航的动态杆臂误差估计和补偿方法，其特征在于，所述步骤4中的动态杆臂补偿判断具体包括：根据当前输入三轴角速度和三轴加速度值辅以设定的权重值求解车辆工况的表征参数并设定不同阈值表征急加速急减速大转向或者剧烈工况，若车辆工况的表征参数小于等于设定阈值时，则不进行动态杆臂补偿，若车辆工况的表征参数大于设定阈值时，则进行动态杆臂补偿，所述车辆工况的表征参数的计算公式为：
式中，|R|为车辆工况的表征参数，k1、k2和k3分别为三轴对应权重值， 和分别为当前三轴比力值，和分别为当前输入三轴角速度。
4.根据权利要求1所述的一种车载GNSS/INS组合导航的动态杆臂误差估计和补偿方法，其特征在于，所述步骤4中的补偿量的计算公式为：
且|R|≤|R|th2
式中，为静态测量值，δlb为杆臂值，和分别为静态下GNSS相位中心与惯组中心在车辆坐标系下三个坐标轴的杆臂，φx、φy和φz分别为三个坐标轴的车辆姿态角，|R|th和|R|th2为用于划定判定区间的设定阈值，为动态补偿杆臂值。
5.根据权利要求1所述的一种车载GNSS/INS组合导航的动态杆臂误差估计和补偿方法，其特征在于，所述步骤4中的速度增量姿态、速度和位置采用二子样圆锥误差补偿算法计算，其对应的计算方程组为：
式中，Δθm1和Δθm2为陀螺两次等间隔采样对应的角增量，T为采样时间，为以惯性坐标系作为参考基准，载体系从tm-1时刻到tm时刻的旋转变化，为以惯性坐标系作为参考基准，地理系从tm时刻到tm-1时刻的旋转变化，下标i表示惯导***解算值，上下标b表示载体系，上下标n表示地理系，(m)表示tm时刻，(m-1)表示tm-1时刻，φ带上下标表示对应姿态，I表示单位矩阵，为常值。
6.根据权利要求1所述的一种车载GNSS/INS组合导航的动态杆臂误差估计和补偿方法，其特征在于，所述步骤5包括以下分步骤：
步骤51：建立***方程；
步骤52：建立量测方程；
步骤53：建立kalman滤波***方程并使量测方程离散化；
步骤54：利用kalman滤波***方程进行反馈校正。
7.根据权利要求6所述的一种车载GNSS/INS组合导航的动态杆臂误差估计和补偿方法，其特征在于，所述步骤51中的***方程，其描述公式为：
式中，X为状态向量，φE、φN和φU分别为东-北-天地理坐标系下的姿态误差，δvE、δvN和δvU分别为东-北-天地理坐标系下的速度误差，δL、δλ和δh为经度、纬度和高度的位置误差，εx、εy和εz分别为陀螺仪三个坐标轴的零偏，和分别为加速度计的三个坐标轴的零偏；
式中，为地理系相对于惯性系的角速度，为地球系相对于惯性系的角速度误差，为地理系相对于地球系的角速度误差，为载体系到地理系的坐标变换矩阵，为载体系相对于惯性系的角速度误差，为载体系相对于地理系下的惯导***加速度计的输出比力，vn为载体在地理系下的速度，为地球系相对于惯性系的角速度，为地理系相对于地球系的角速度，δvn为载体在地理系下的速度误差，为载体系相对于地理系下的惯导***加速度计的输出比力误差，δgn为重力加速度误差，RM为卯酉圈半径，h为当地海拔高度，L为当地纬度，RN为子午圈半径，单独φ表示捷联惯导***中的数学平台误差角。
8.根据权利要求7所述的一种车载GNSS/INS组合导航的动态杆臂误差估计和补偿方法，所述步骤52中的量测方程，其描述公式为：
式中，上标n表示为地理系，Z为量测方程，下标INS表示惯性***，下标GNSS表示卫星导航，上标～表示实际值，v表示速度，p表示位置，表示载体系相对于地球系的角速度，RMh＝RM+h，RNh＝RN+h。
9.根据权利要求1所述的一种车载GNSS/INS组合导航的动态杆臂误差估计和补偿方法，其特征在于，所述步骤5还包括：将kalman滤波后的陀螺和加速度零偏反馈至器件补偿处进行校正，将姿态反馈至姿态更新补偿处，将速度、位置误差反馈至INS解算的输出值上进行校正，即：由修正后的可解得航向角ψ、俯仰角θ和滚动角γ，一次滤波反馈后，误差状态归0。
10.根据权利要求9所述的一种车载GNSS/INS组合导航的动态杆臂误差估计和补偿方法，其特征在于，所述由修正后的可解得航向角ψ、俯仰角θ和滚动角γ，其对应的描述公式为：
式中，(数字1，数字2)表示矩阵内具体对应的矩阵元素。
CN201910288631.0A 2019-04-11 2019-04-11 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法 Active CN110221332B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910288631.0A CN110221332B (zh) 2019-04-11 2019-04-11 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910288631.0A CN110221332B (zh) 2019-04-11 2019-04-11 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法

Publications (2)

Publication Number Publication Date
CN110221332A true CN110221332A (zh) 2019-09-10
CN110221332B CN110221332B (zh) 2023-02-10

Family

ID=67822558

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910288631.0A Active CN110221332B (zh) 2019-04-11 2019-04-11 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法

Country Status (1)

Country Link
CN (1) CN110221332B (zh)

Cited By (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110672124A (zh) * 2019-09-27 2020-01-10 北京耐威时代科技有限公司 离线杠杆臂估计方法
CN111469855A (zh) * 2020-04-20 2020-07-31 北京易控智驾科技有限公司 一种车辆运动参数的计算方法
CN111678538A (zh) * 2020-07-29 2020-09-18 中国电子科技集团公司第二十六研究所 一种基于速度匹配的动态水平仪误差补偿方法
US10996345B2 (en) 2018-06-11 2021-05-04 Honeywell International Inc. Signal fault detection for global navigation satellite system using multiple antennas
CN113029139A (zh) * 2021-04-07 2021-06-25 中国电子科技集团公司第二十八研究所 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法
CN113465628A (zh) * 2021-06-17 2021-10-01 杭州鸿泉物联网技术股份有限公司 惯性测量单元数据补偿方法及***
CN113841025A (zh) * 2020-10-14 2021-12-24 深圳市大疆创新科技有限公司 用于可移动平台的位置姿态确定方法、相关装置和***
CN113865583A (zh) * 2021-07-20 2021-12-31 北京航天控制仪器研究所 一种加速度计组合动态安装偏差矩阵确定及补偿方法
CN113971846A (zh) * 2020-07-22 2022-01-25 郑州宇通客车股份有限公司 一种自动驾驶车辆的定位失效检测方法及装置
CN114076610A (zh) * 2020-08-12 2022-02-22 千寻位置网络（浙江）有限公司 Gnss/mems车载组合导航***的误差标定、导航方法及其装置
CN114167493A (zh) * 2021-11-23 2022-03-11 武汉大学 Gnss双天线辅助陀螺的地震旋转测量***及方法
CN114563004A (zh) * 2022-04-29 2022-05-31 季华实验室 组合导航***杆臂值在线测量方法及***
CN114894222A (zh) * 2022-07-12 2022-08-12 深圳元戎启行科技有限公司 Imu-gnss天线的外参数标定方法和相关方法、设备
US11536854B2 (en) 2020-04-24 2022-12-27 Honeywell International Inc. Multiple faulty global navigation satellite system signal detecting system

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060224321A1 (en) * 2005-03-29 2006-10-05 Honeywell International Inc. Method and apparatus for high accuracy relative motion determination using inertial sensors
CN103344259A (zh) * 2013-07-11 2013-10-09 北京航空航天大学 一种基于杆臂估计的ins/gps组合导航***反馈校正方法
CN107270893A (zh) * 2017-05-27 2017-10-20 东南大学 面向不动产测量的杆臂、时间不同步误差估计与补偿方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060224321A1 (en) * 2005-03-29 2006-10-05 Honeywell International Inc. Method and apparatus for high accuracy relative motion determination using inertial sensors
CN103344259A (zh) * 2013-07-11 2013-10-09 北京航空航天大学 一种基于杆臂估计的ins/gps组合导航***反馈校正方法
CN107270893A (zh) * 2017-05-27 2017-10-20 东南大学 面向不动产测量的杆臂、时间不同步误差估计与补偿方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
潘献飞等: "考虑机抖激光陀螺信号滤波特性的圆锥算法修正", 《中国惯性技术学报》 *
王海峰等: "高动态GPS/BD2组合导航信号模拟器关键技术研究", 《宇航计测技术》 *
缪玲娟等: "减小动态误差的捷联***姿态算法研究", 《兵工学报》 *
肖虎等: "光纤陀螺捷联惯导***速度算法的改进研究", 《弹箭与制导学报》 *
钱闯: "弱ＧＮＳＳ信号区域的ＧＮＳＳ／ＩＮＳ／ＬｉＤＡＲ高精度定位方法及其应用研究", 《中国优秀博硕士学位论文全文数据库(博士)信息科技辑》 *

Cited By (21)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10996345B2 (en) 2018-06-11 2021-05-04 Honeywell International Inc. Signal fault detection for global navigation satellite system using multiple antennas
CN110672124A (zh) * 2019-09-27 2020-01-10 北京耐威时代科技有限公司 离线杠杆臂估计方法
CN111469855A (zh) * 2020-04-20 2020-07-31 北京易控智驾科技有限公司 一种车辆运动参数的计算方法
US11536854B2 (en) 2020-04-24 2022-12-27 Honeywell International Inc. Multiple faulty global navigation satellite system signal detecting system
CN113971846A (zh) * 2020-07-22 2022-01-25 郑州宇通客车股份有限公司 一种自动驾驶车辆的定位失效检测方法及装置
CN111678538A (zh) * 2020-07-29 2020-09-18 中国电子科技集团公司第二十六研究所 一种基于速度匹配的动态水平仪误差补偿方法
CN111678538B (zh) * 2020-07-29 2023-06-09 中国电子科技集团公司第二十六研究所 一种基于速度匹配的动态水平仪误差补偿方法
CN114076610B (zh) * 2020-08-12 2024-05-28 千寻位置网络（浙江）有限公司 Gnss/mems车载组合导航***的误差标定、导航方法及其装置
CN114076610A (zh) * 2020-08-12 2022-02-22 千寻位置网络（浙江）有限公司 Gnss/mems车载组合导航***的误差标定、导航方法及其装置
CN113841025A (zh) * 2020-10-14 2021-12-24 深圳市大疆创新科技有限公司 用于可移动平台的位置姿态确定方法、相关装置和***
CN113029139A (zh) * 2021-04-07 2021-06-25 中国电子科技集团公司第二十八研究所 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法
CN113029139B (zh) * 2021-04-07 2023-07-28 中国电子科技集团公司第二十八研究所 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法
CN113465628A (zh) * 2021-06-17 2021-10-01 杭州鸿泉物联网技术股份有限公司 惯性测量单元数据补偿方法及***
CN113865583B (zh) * 2021-07-20 2024-02-09 北京航天控制仪器研究所 一种加速度计组合动态安装偏差矩阵确定及补偿方法
CN113865583A (zh) * 2021-07-20 2021-12-31 北京航天控制仪器研究所 一种加速度计组合动态安装偏差矩阵确定及补偿方法
CN114167493A (zh) * 2021-11-23 2022-03-11 武汉大学 Gnss双天线辅助陀螺的地震旋转测量***及方法
CN114167493B (zh) * 2021-11-23 2023-08-04 武汉大学 Gnss双天线辅助陀螺的地震旋转测量***及方法
CN114563004B (zh) * 2022-04-29 2022-07-22 季华实验室 组合导航***杆臂值在线测量方法及***
CN114563004A (zh) * 2022-04-29 2022-05-31 季华实验室 组合导航***杆臂值在线测量方法及***
CN114894222A (zh) * 2022-07-12 2022-08-12 深圳元戎启行科技有限公司 Imu-gnss天线的外参数标定方法和相关方法、设备
CN114894222B (zh) * 2022-07-12 2022-10-28 深圳元戎启行科技有限公司 Imu-gnss天线的外参数标定方法和相关方法、设备

Also Published As

Publication number Publication date
CN110221332B (zh) 2023-02-10

Similar Documents

Publication Publication Date Title
CN110221332A (zh) 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法
CN110501024A (zh) 一种车载ins/激光雷达组合导航***的量测误差补偿方法
US9541392B2 (en) Surveying system and method
US6459990B1 (en) Self-contained positioning method and system thereof for water and land vehicles
CN111156994B (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN106289246B (zh) 一种基于位置和姿态测量***的柔性杆臂测量方法
EP2259023B1 (en) Inertial navigation system error correction
CN110221333A (zh) 一种车载ins/od组合导航***的量测误差补偿方法
CN103994763B (zh) 一种火星车的sins/cns深组合导航***及其实现方法
CN100541132C (zh) 大失准角下船用光纤陀螺捷联航姿***系泊精对准方法
CN107270893A (zh) 面向不动产测量的杆臂、时间不同步误差估计与补偿方法
CN101571394A (zh) 基于旋转机构的光纤捷联惯性导航***初始姿态确定方法
CN101858748A (zh) 高空长航无人机的多传感器容错自主导航方法
CN101963512A (zh) 船用旋转式光纤陀螺捷联惯导***初始对准方法
CN107677292B (zh) 基于重力场模型的垂线偏差补偿方法
CN109916395A (zh) 一种姿态自主冗余组合导航算法
CN108387227A (zh) 机载分布式pos的多节点信息融合方法及***
CN109931955A (zh) 基于状态相关李群滤波的捷联惯性导航***初始对准方法
CN111947653A (zh) 一种月面巡视探测器双模式惯性/视觉/天文导航方法
CN113916222B (zh) 基于卡尔曼滤波估计方差约束的组合导航方法
CN109612460A (zh) 一种基于静止修正的垂线偏差测量方法
CN104049269A (zh) 一种基于激光测距和mems/gps组合导航***的目标导航测绘方法
CN105606093B (zh) 基于重力实时补偿的惯性导航方法及装置
CN110068325A (zh) 一种车载ins/视觉组合导航***的杆臂误差补偿方法
CN108303120A (zh) 一种机载分布式pos的实时传递对准的方法及装置

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