CN111006675B - 基于高精度重力模型的车载激光惯导系统自标定方法 - Google Patents

基于高精度重力模型的车载激光惯导系统自标定方法 Download PDF

Info

Publication number
CN111006675B
CN111006675B CN201911378995.4A CN201911378995A CN111006675B CN 111006675 B CN111006675 B CN 111006675B CN 201911378995 A CN201911378995 A CN 201911378995A CN 111006675 B CN111006675 B CN 111006675B
Authority
CN
China
Prior art keywords
inertial navigation
gravity
vehicle
speed
module
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.)
Active
Application number
CN201911378995.4A
Other languages
English (en)
Other versions
CN111006675A (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.)
Xian University of Technology
Original Assignee
Xian University of Technology
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 Xian University of Technology filed Critical Xian University of Technology
Priority to CN201911378995.4A priority Critical patent/CN111006675B/zh
Publication of CN111006675A publication Critical patent/CN111006675A/zh
Application granted granted Critical
Publication of CN111006675B publication Critical patent/CN111006675B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass

Landscapes

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

Abstract

本发明提供了基于高精度重力模型的车载激光惯导系统自标定方法，包括速度测量设备VMS、激光陀螺惯性导航系统、重力模型参数、高精度GNSS；激光陀螺惯性导航系统连接捷联惯性导航算法更新模块；激光陀螺惯性导航系统角速度输出连接VMS速度计算模块，VMS速度计算模块连接速度测量设备VMS；所述重力模型参数连接重力计算模块；所述高精度GNSS连接所述重力计算模块，所述重力计算模块连接捷联惯性导航算法更新模块；车载标定滤波误差计算模块连接标定判断模块。该方法不用将激光接连广行导航系统从在车上拆卸下来，减少标定时间；操作方法简单；不需要高精度的转位机构和平台设备。

Description

基于高精度重力模型的车载激光惯导系统自标定方法
技术领域
本发明属于车载导航技术领域，涉及一种基于高精度重力模型的车载激光惯性导航系统自标定方法。
背景技术
高精度激光捷联惯性导航系统长时间使用后，由于器件的原因会造成精度降低的现象，需要进行重新标定后才可以满足使用要求。一种方法是将激光捷联惯性导航系统从载体上拆卸下来，返回实验室进行标定工作，此方法虽可以较完整的标定出激光捷联惯性导航系统的所有参数，精度较高，但是标定周期较长。另一种方法是利用车上回转机构带动激光捷联惯性导航系统进行旋转，进行4个位置标定，此方法对车体调平精度，以及回转机构旋转精度要求较高，而且只能标定水平方向陀螺和加速度计的零偏，且天向陀螺零偏的可观测性弱。
发明内容
本发明的目的在于提供基于高精度重力模型的车载激光惯导自标定系统，解决了现有技术中存在的标定工作需要将激光捷联惯性导航系统从载体上拆卸下来，导致标定周期长的问题。
本发明的另一目的在于提供基于高精度重力模型的车载激光惯导自标定方法。
为了实现上述目的，本发明采用如下技术方案：
基于高精度重力模型的车载激光惯导自标定系统，包括速度测量设备VMS、激光陀螺惯性导航系统、重力模型参数、高精度GNSS；激光陀螺惯性导航系统连接捷联惯性导航算法更新模块；激光陀螺惯性导航系统角速度输出连接VMS速度计算模块，VMS速度计算模块连接速度测量设备VMS；所述重力模型参数连接重力计算模块；所述高精度GNSS连接所述重力计算模块，重力计算模块的输出值同时与捷联惯性导航算法更新模块位置输出进行做差运算后连接车载标定滤波误差计算模块；所述重力计算模块连接捷联惯性导航算法更新模块；所述VMS速度计算模块VMS速度输出与捷联惯性导航算法更新模块速度输出做差后连接车载标定滤波误差计算模块；车载标定滤波误差计算模块连接标定判断模块。
本发明的特点还在于：
激光陀螺惯性导航系统包括激光陀螺和挠性加速度计。
基于高精度重力模型的车载激光惯导自标定方法，采用基于高精度重力模型的车载激光惯导自标定系统进行实施，具体步骤为：
步骤1：速度测量设备VMS测量载车的车体速度
Figure BDA0002341762260000021
设测速设备单位时间输出的脉冲为
Figure BDA0002341762260000022
则载体系下的速度矢量
Figure BDA0002341762260000023
其中，kVMS为测速设备的刻度系数；
步骤2：根据激光陀螺惯性导航系统输出角速率的
Figure BDA0002341762260000024
和速度测量设备VMS输出的车体速度
Figure BDA0002341762260000025
进行航位推算，输出导航坐标系下的VMS速度
Figure BDA0002341762260000026
计算公式如下：
Figure BDA0002341762260000031
其中，姿态矩阵
Figure BDA0002341762260000032
的更新过程可以在捷联导航算法更新模块中直接获得；
步骤3：激光陀螺惯性导航系统输出载体坐标系下的角速度
Figure BDA0002341762260000033
和加速度
Figure BDA0002341762260000034
然后采用线性误差模型对输出脉冲进行标定补偿，即
Figure BDA0002341762260000035
其中，
Figure BDA0002341762260000036
分别为陀螺仪、加速度计温补后的输出脉冲；KG、KA分别为陀螺仪、加速度计的标定安装矩阵；εb、▽b分别为陀螺仪、加速度计的零位；
步骤4：捷联惯性导航算法更新模块根据输入的角速度
Figure BDA0002341762260000037
加速度
Figure BDA0002341762260000038
和重力补偿值δgn进行导航解算，实现速度
Figure BDA0002341762260000039
和位置
Figure BDA00023417622600000310
的更新输出。
步骤5：在车载标定滤波误差计算模块中，惯性导航速度
Figure BDA00023417622600000311
与VMS速度
Figure BDA00023417622600000312
做差后得到速度量测δvn，惯性导航位置
Figure BDA00023417622600000313
与GNSS提供位置
Figure BDA00023417622600000314
做差后得到位置量测δpn。车载标定滤波误差计算模块根据速度量测δvn和位置量测δpn，采用最优Kalman滤波技术实时估计系统的状态变量
Figure BDA00023417622600000315
更新协方差矩阵
Figure BDA00023417622600000316
步骤6：在重力计算模块中，保存全球高阶重力模型的缔合勒让德球谐参数，单位化后的参数包括
Figure BDA00023417622600000317
且
Figure BDA00023417622600000318
式中，
Figure BDA00023417622600000319
为高阶球谐重力模型的勒让德参数；
Figure BDA00023417622600000320
为WGS-84正常地球引力勒让德参数，n在这里只取有限阶偶数项(n≤10)；
步骤7：高精度GNSS可实时为载车提供高精度的位置信息
Figure BDA0002341762260000041
和秒脉冲信号，秒脉冲信号用于同步惯性导航系统与GNSS的时间；GNSS输出相对惯性导航的延迟时间
δt＝tGNSS-tPPS
式中，tGNSS为惯性导航接收到GNSS发送的位置信息数据帧的时刻；tPPS为这一帧位置数据对应的秒脉冲时，显然δt≥0。
步骤8：重力计算模块，根据重力模型参数和
Figure BDA0002341762260000042
计算的到重力补偿值δgn。
Figure BDA0002341762260000043
其中
Figure BDA0002341762260000044
Figure BDA0002341762260000045
其中，重力补偿值计算中的勒让德参数
Figure BDA0002341762260000046
存放在重力模型参数模块当中；
步骤9：标定判断模块输出并保存当前时刻的估计值
Figure BDA0002341762260000047
类似的，εb、▽b的所有误差状态都小于门限且滤波时间t＞1h时，则标定过程结束，标定得到参数可发送给上位机或存储在惯性导航系统的内部flash上。
步骤4中导航算法要求跑车方式为闭合路径行车；
Figure BDA0002341762260000051
其中
Figure BDA0002341762260000052
Figure BDA0002341762260000053
Figure BDA0002341762260000054
其中，L,λ,h分别为载体所在的纬度、经度和高度；下标E、N、U表示沿当地坐标系的东向、北向和天向；RM、RN分别为载体所在当地子午圈和卯酉圈半径；速度和位置的初始值
Figure BDA0002341762260000055
由GNSS提供；姿态矩阵
Figure BDA0002341762260000056
通过捷联惯性导航系统自对准完成；
Figure BDA0002341762260000057
为通过标准重力模型计算得到的重力值，其计算公式一般采用WGS-84模型
Figure BDA0002341762260000058
这里，
Figure BDA0002341762260000059
的计算单位为m/s2。
步骤5具体步骤为：选择惯性导航系统的误差状态为速度误差δvn、姿态误差向量φ、位置误差δpn、陀螺零偏εb、加速度计零位▽b，同时考虑里程计的标度因数误差δKOD、航向安装偏角误差αψ和俯仰安装偏角误差αθ，则标定滤波器的误差向量
Figure BDA00023417622600000510
标定滤波器的状态方程
Figure BDA0002341762260000061
其中，WG为陀螺噪声；WA为加速度计噪声。
时间更新方程状态转移矩阵
Figure BDA0002341762260000062
其中，FSINS为惯性导航系统误差方程对应的转移矩阵。其中的非零元素有：
Figure BDA0002341762260000063
Figure BDA0002341762260000064
Figure BDA0002341762260000065
F1，10＝-C1,1,F1，11＝-C1,2,F1，12＝-C1,3
F2，10＝-C2,1,F2，11＝-C2,2,F2，12＝-C2,3
F3，10＝-C3,1,F3，11＝-C3,2,F3，12＝-C3,3
Figure BDA0002341762260000066
Figure BDA0002341762260000067
Figure BDA0002341762260000068
Figure BDA0002341762260000069
Figure BDA00023417622600000610
Figure BDA00023417622600000611
F4，13＝C1,1,F4，14＝C1,2,F4，15＝C1,3
F5，13＝C2,1,F5，14＝C2,2,F5，15＝C2,3
F6，13＝C3,1,F6，14＝C3,2,F6，15＝C3,3
F7，4＝1,F8,5＝1,F9,6＝1
Figure BDA00023417622600000612
其中，Fi,j表示矩阵FSINS中第i行第j列的元素。之所以不将重力扰动δgn列入滤波方程，一是由于δgn本质上是相对于位置的函数而非时间，采用Kalman滤波状态更新方程的形式建模一般无法做到十分准确，二是由于模块(6)中采用了全球高阶重力模型对实时重力扰动矢量进行计算，且在模块(4)中进行了补偿，其对导航参数的影响可以忽略；
标定滤波的量测量
Figure BDA0002341762260000071
其中，Vv为VMS的速度测量噪声；Vp为GNSS的位置测量噪声。
位置转换矩阵和量测矩阵
Figure BDA0002341762260000072
H＝[06×3 I6 06×9]
其中，I表示单位矩阵；
步骤1中速度测量设备VMS(1)，可以是里程计、激光雷达测速仪等设备。
本发明的有益效果是
本发明在于提供一种车载激光捷联惯性导航系统自标定方法。该方法不用将激光接连广行导航系统从在车上拆卸下来，减少标定时间；该方法只需将载车按照正常的行驶方法行驶1.5～2小时行程，形式轨迹为闭合轨迹，但对闭合轨迹中的线路和路况没有特殊要求，操作方法简单；不需要高精度的转位机构和平台设备。
附图说明
图1是本发明基于高精度重力模型的车载激光惯导自标定方法的算法框图；
图2是本发明基于高精度重力模型的车载激光惯导自标定方法两种标定模式下3个陀螺仪零位的协方差估计曲线。
图中，1.速度测量设备VMS，2.VMS速度计算模块3.激光陀螺惯性导航系统，4.捷联惯性导航算法更新模块，5.车载标定滤波误差计算模块，6.重力模型参数，7.高精度GNSS，8.重力计算模块，9.标定判断模块。
具体实施方式
下面结合附图及具体实施方式对本发明做进一步的描述。
基于高精度重力模型的车载激光惯导自标定系统，其特征在于，包括速度测量设备VMS 1、激光陀螺惯性导航系统3、重力模型参数6、高精度GNSS 7；激光陀螺惯性导航系统3连接捷联惯性导航算法更新模块4；激光陀螺惯性导航系统3角速度输出连接VMS速度计算模块2，VMS速度计算模块2连接速度测量设备VMS1；所述重力模型参数6连接重力计算模块8；所述高精度GNSS 7连接所述重力计算模块8，重力计算模块8的输出值同时与捷联惯性导航算法更新模块4位置输出进行做差运算后连接车载标定滤波误差计算模块5；所述重力计算模块8连接捷联惯性导航算法更新模块4；所述VMS速度计算模块2VMS速度输出与捷联惯性导航算法更新模块4速度输出做差后连接车载标定滤波误差计算模块5；车载标定滤波误差计算模块5连接标定判断模块9。
激光陀螺惯性导航系统3包括激光陀螺和挠性加速度计。
基于高精度重力模型的车载激光惯导自标定方法，采用基于高精度重力模型的车载激光惯导自标定系统进行实施，具体步骤为：
步骤1：速度测量设备VMS 1测量载车的车体速度
Figure BDA0002341762260000091
设测速设备单位时间输出的脉冲为
Figure BDA0002341762260000092
则载体系下的速度矢量
Figure BDA0002341762260000093
其中，kVMS为测速设备的刻度系数；
步骤2：根据激光陀螺惯性导航系统3输出角速率的
Figure BDA0002341762260000094
和速度测量设备VMS输出的车体速度
Figure BDA0002341762260000095
进行航位推算，输出导航坐标系下的VMS速度
Figure BDA0002341762260000096
计算公式如下：
Figure BDA0002341762260000097
其中，姿态矩阵
Figure BDA0002341762260000098
的更新过程可以在捷联导航算法更新模块4中直接获得；
步骤3：激光陀螺惯性导航系统3输出载体坐标系下的角速度
Figure BDA0002341762260000099
和加速度
Figure BDA00023417622600000910
然后采用线性误差模型对输出脉冲进行标定补偿，即
Figure BDA00023417622600000911
其中，
Figure BDA00023417622600000912
分别为陀螺仪、加速度计温补后的输出脉冲；KG、KA分别为陀螺仪、加速度计的标定安装矩阵；εb、▽b分别为陀螺仪、加速度计的零位；
步骤4：捷联惯性导航算法更新模块4根据输入的角速度
Figure BDA00023417622600000913
加速度
Figure BDA00023417622600000914
和重力补偿值δgn进行导航解算，实现速度
Figure BDA00023417622600000915
和位置
Figure BDA00023417622600000916
的更新输出。
步骤5：在车载标定滤波误差计算模块中，惯性导航速度
Figure BDA0002341762260000101
与VMS速度
Figure BDA0002341762260000102
做差后得到速度量测δvn，惯性导航位置
Figure BDA0002341762260000103
与GNSS提供位置
Figure BDA0002341762260000104
做差后得到位置量测δpn。车载标定滤波误差计算模块根据速度量测δvn和位置量测δpn，采用最优Kalman滤波技术实时估计系统的状态变量
Figure BDA0002341762260000105
更新协方差矩阵
Figure BDA0002341762260000106
步骤6：在重力计算模块中，保存全球高阶重力模型的缔合勒让德球谐参数，单位化后的参数包括
Figure BDA0002341762260000107
且
Figure BDA0002341762260000108
式中，
Figure BDA0002341762260000109
为高阶球谐重力模型的勒让德参数；
Figure BDA00023417622600001010
为WGS-84正常地球引力勒让德参数，n在这里只取有限阶偶数项(n≤10)；
步骤7：高精度GNSS可实时为载车提供高精度的位置信息
Figure BDA00023417622600001011
和秒脉冲信号，秒脉冲信号用于同步惯性导航系统与GNSS的时间；GNSS输出相对惯性导航的延迟时间
δt＝tGNSS-tPPS
式中，tGNSS为惯性导航接收到GNSS发送的位置信息数据帧的时刻；tPPS为这一帧位置数据对应的秒脉冲时，显然δt≥0。
步骤8：重力计算模块，根据重力模型参数和
Figure BDA00023417622600001012
计算的到重力补偿值δgn。
Figure BDA00023417622600001013
其中
Figure BDA0002341762260000111
Figure BDA0002341762260000112
其中，重力补偿值计算中的勒让德参数
Figure BDA0002341762260000113
存放在重力模型参数模块6当中；
步骤9：标定判断模块输出并保存当前时刻的估计值
Figure BDA0002341762260000114
类似的，εb、▽b的所有误差状态都小于门限且滤波时间t＞1h时，则标定过程结束，标定得到参数可发送给上位机或存储在惯性导航系统的内部flash上。
步骤4中导航算法要求跑车方式为闭合路径行车；
Figure BDA0002341762260000115
其中
Figure BDA0002341762260000116
Figure BDA0002341762260000117
Figure BDA0002341762260000118
其中，L,λ,h分别为载体所在的纬度、经度和高度；下标E、N、U表示沿当地坐标系的东向、北向和天向；RM、RN分别为载体所在当地子午圈和卯酉圈半径；速度和位置的初始值
Figure BDA0002341762260000119
由GNSS提供；姿态矩阵
Figure BDA00023417622600001110
通过捷联惯性导航系统自对准完成；
Figure BDA00023417622600001111
为通过标准重力模型计算得到的重力值，其计算公式一般采用WGS-84模型
Figure BDA0002341762260000121
这里，
Figure BDA0002341762260000122
的计算单位为m/s2。
步骤5具体步骤为：选择惯性导航系统的误差状态为速度误差δvn、姿态误差向量φ、位置误差δpn、陀螺零偏εb、加速度计零位▽b，同时考虑里程计的标度因数误差δKOD、航向安装偏角误差αψ和俯仰安装偏角误差αθ，则标定滤波器的误差向量
Figure BDA0002341762260000123
标定滤波器的状态方程
Figure BDA0002341762260000124
其中，WG为陀螺噪声；WA为加速度计噪声。
时间更新方程状态转移矩阵
Figure BDA0002341762260000125
其中，FSINS为惯性导航系统误差方程对应的转移矩阵。其中的非零元素有：
Figure BDA0002341762260000131
Figure BDA0002341762260000132
Figure BDA0002341762260000133
F1，10＝-C1,1,F1，11＝-C1,2,F1，12＝-C1,3
F2，10＝-C2,1,F2，11＝-C2,2,F2，12＝-C2,3
F3，10＝-C3,1,F3，11＝-C3,2,F3，12＝-C3,3
Figure BDA0002341762260000134
Figure BDA0002341762260000135
Figure BDA0002341762260000136
Figure BDA0002341762260000137
Figure BDA0002341762260000138
Figure BDA0002341762260000139
F4，13＝C1,1,F4，14＝C1,2,F4，15＝C1,3
F5，13＝C2,1,F5，14＝C2,2,F5，15＝C2,3
F6，13＝C3,1,F6，14＝C3,2,F6，15＝C3,3
F7，4＝1,F8,5＝1,F9,6＝1
Figure BDA00023417622600001310
其中，Fi,j表示矩阵FSINS中第i行第j列的元素。之所以不将重力扰动δgn列入滤波方程，一是由于δgn本质上是相对于位置的函数而非时间，采用Kalman滤波状态更新方程的形式建模一般无法做到十分准确，二是由于重力模型参数模块6中采用了全球高阶重力模型对实时重力扰动矢量进行计算，且在捷联惯性导航算法更新模块4中进行了补偿，其对导航参数的影响可以忽略；
标定滤波的量测量
Figure BDA0002341762260000141
其中，Vv为VMS的速度测量噪声；Vp为GNSS的位置测量噪声。
位置转换矩阵和量测矩阵
Figure BDA0002341762260000142
H＝[06×3I606×9]
其中，I表示单位矩阵；
步骤1中速度测量设备VMS 1，可以是里程计、激光雷达测速仪等设备。
实施例
1)速度测量设备(Velocity Measurement Sensor,VMS)，测量载车的车体速度
Figure BDA0002341762260000143
该设备可以是里程计、激光雷达测速仪等设备。假设测速设备单位时间输出的脉冲为
Figure BDA0002341762260000144
则载体系下的速度矢量
Figure BDA0002341762260000145
其中，kVMS为测速设备的刻度系数。
2)外部速度计算模块，根据激光陀螺惯性导航系统输出角速率的
Figure BDA0002341762260000146
和速度测量设备VMS输出的车体速度
Figure BDA0002341762260000147
进行航位推算，输出导航坐标系下的VMS速度
Figure BDA0002341762260000148
计算公式如下：
Figure BDA0002341762260000149
其中，姿态矩阵
Figure BDA00023417622600001410
的更新过程可以在捷联导航算法更新模块4中直接获得。
3)激光陀螺惯性导航系统，包含3个激光陀螺和3个挠性加速度计，主要输出载体坐标系下的角速度
Figure BDA00023417622600001411
和加速度
Figure BDA00023417622600001412
对于激光陀螺捷联惯性导航系统来说，一般采用线性误差模型对输出脉冲进行标定补偿，即
Figure BDA0002341762260000151
其中，
Figure BDA0002341762260000152
分别为陀螺仪、加速度计温补后的输出脉冲；KG、KA分别为陀螺仪、加速度计的标定安装矩阵；εb、▽b分别为陀螺仪、加速度计的零位。
4)捷联惯性导航算法更新模块，根据输入的角速度
Figure BDA0002341762260000153
加速度
Figure BDA0002341762260000154
和重力补偿值δgn进行导航解算，实现速度
Figure BDA0002341762260000155
和位置
Figure BDA0002341762260000156
的更新输出。导航算法要求跑车方式为闭合路径行车。
Figure BDA0002341762260000157
其中
Figure BDA0002341762260000158
Figure BDA0002341762260000159
Figure BDA00023417622600001510
其中，L,λ,h分别为载体所在的纬度、经度和高度；下标E、N、U表示沿当地坐标系的东向、北向和天向；RM、RN分别为载体所在当地子午圈和卯酉圈半径；速度和位置的初始值
Figure BDA00023417622600001511
由GNSS提供；姿态矩阵
Figure BDA00023417622600001512
通过捷联惯性导航系统自对准完成；
Figure BDA00023417622600001513
为通过标准重力模型计算得到的重力值，其计算公式一般采用WGS-84模型
Figure BDA0002341762260000161
这里，
Figure BDA0002341762260000162
的计算单位为m/s2。
5)车载标定滤波误差计算模块，惯性导航速度
Figure BDA0002341762260000163
与VMS速度
Figure BDA0002341762260000164
做差后得到速度量测δvn，惯性导航位置
Figure BDA0002341762260000165
与GNSS提供位置
Figure BDA0002341762260000166
做差后得到位置量测δpn。车载标定滤波误差计算模块根据速度量测δvn和位置量测δpn，采用最优Kalman滤波技术实时估计系统的状态变量
Figure BDA0002341762260000167
更新协方差矩阵
Figure BDA0002341762260000168
选择惯性导航系统的误差状态为速度误差δvn、姿态误差向量φ、位置误差δpn、陀螺零偏εb、加速度计零位▽b，同时考虑里程计的标度因数误差δKOD、航向安装偏角误差αψ和俯仰安装偏角误差αθ，则标定滤波器的误差向量
Figure BDA0002341762260000169
标定滤波器的状态方程
Figure BDA00023417622600001610
其中，WG为陀螺噪声；WA为加速度计噪声。
时间更新方程状态转移矩阵
Figure BDA00023417622600001611
其中，FSINS为惯性导航系统误差方程对应的转移矩阵。其中的非零元素有：
Figure BDA0002341762260000171
Figure BDA0002341762260000172
Figure BDA0002341762260000173
F1，10＝-C1,1,F1，11＝-C1,2,F1，12＝-C1,3
F2，10＝-C2,1,F2，11＝-C2,2,F2，12＝-C2,3
F3，10＝-C3,1,F3，11＝-C3,2,F3，12＝-C3,3
Figure BDA0002341762260000174
Figure BDA0002341762260000175
Figure BDA0002341762260000176
Figure BDA0002341762260000177
Figure BDA0002341762260000178
Figure BDA0002341762260000179
F4，13＝C1,1,F4，14＝C1,2,F4，15＝C1,3
F5，13＝C2,1,F5，14＝C2,2,F5，15＝C2,3
F6，13＝C3,1,F6，14＝C3,2,F6，15＝C3,3
F7，4＝1,F8,5＝1,F9,6＝1
Figure BDA00023417622600001710
其中，Fi,j表示矩阵FSINS中第i行第j列的元素。之所以不将重力扰动δgn列入滤波方程，一是由于δgn本质上是相对于位置的函数而非时间，采用Kalman滤波状态更新方程的形式建模一般无法做到十分准确，二是由于重力计算模块中采用了全球高阶重力模型对实时重力扰动矢量进行计算，且在捷联惯性导航算法更新模块4中进行了补偿，其对导航参数的影响可以忽略。
标定滤波的量测量
Figure BDA0002341762260000181
其中，Vv为VMS的速度测量噪声；Vp为GNSS的位置测量噪声。位置转换矩阵和量测矩阵
Figure BDA0002341762260000182
H＝[06×3 I6 06×9]
其中，I表示单位矩阵。
6)重力模型参数，保存全球高阶重力模型的缔合勒让德球谐参数，单位化后的参数包括
Figure BDA0002341762260000183
且
Figure BDA0002341762260000184
式中，
Figure BDA0002341762260000185
为高阶球谐重力模型的勒让德参数；
Figure BDA0002341762260000186
为WGS-84正常地球引力勒让德参数，n在这里只取有限阶偶数项(n≤10)。
WGS-84正常地球引力勒让德参数
Figure BDA0002341762260000187
通过球谐参数计算重力补偿值得方法在重力计算模块8进行。高阶球谐模型参数选用德国波茨坦地学研究中心发布的2190阶/次的EIGEN-6C4模型。该模型包括了地球重力场和海洋环流探测GOCE卫星全部的重力梯度数据，在全球都具有较好的重力符合精度。。
7)高精度GNSS，实时为载车提供高精度的位置信息
Figure BDA0002341762260000188
和秒脉冲信号，秒脉冲信号用于同步惯性导航系统与GNSS的时间。GNSS输出相对惯性导航的延迟时间
δt＝tGNSS-tPPS
式中，tGNSS为惯性导航接收到GNSS发送的位置信息数据帧的时刻；tPPS为这一帧位置数据对应的秒脉冲时，显然δt≥0。则考虑到车辆的运动环境相对平缓，真实的GNSS位置信息可通过简单的线性插值得到。
8)重力计算模块，根据重力模型参数和
Figure BDA0002341762260000191
计算的到重力补偿值δgn。
Figure BDA0002341762260000192
其中
Figure BDA0002341762260000193
Figure BDA0002341762260000194
其中，重力补偿值计算中的勒让德参数
Figure BDA0002341762260000195
存放在重力模型参数模块6当中。
9)标定判断模块9根据输入的系统状态变量
Figure BDA0002341762260000196
和协方差
Figure BDA0002341762260000197
进行计算和判断，在标定时长t到达后，实时输出待标定参数陀螺仪/加速度计零偏εb、▽b的估计值，并且可以输出标定状态，其状态包括标定成功、标定失败，以及标定中。
以z轴陀螺仪零位标定为例，如图1所示，标定判断模块9实时从滤波误差计算模块5中拾取z轴陀螺仪零位的协方差
Figure BDA0002341762260000198
设置标定完成时协方差门限TG,z，当
Figure BDA0002341762260000201
时，则认为z轴陀螺仪零位在此时已经得到准确估计，标定判断模块9输出并保存当前时刻的估计值
Figure BDA0002341762260000202
类似的，εb、▽b的所有误差状态都小于门限且滤波时间t＞1h时，则标定过程结束，标定得到参数可发送给上位机或存储在惯性导航系统的内部flash上。标定开始后，整个标定过程无需人工参与。
传统的标定方法由于惯性导航系统在单个位置没有移动，三个陀螺的可观测性只能由外部速度信息提供。而车辆动基座运动则引入了位置矢量信息，提高了惯性元件误差的可观测性。图2为静基座和动基座两种标定模式下陀螺仪零位的协方差估计曲线。静基座标定采用4方位标定模式，即航向相隔90°，每个位置静止约30min。动基座模式则令车辆沿一闭环跑车路线行驶约2小时，行驶过程中，重力补偿模块实时计算重力扰动矢量值并进行补偿，使得两种标定模式的标定结果基本一致。两种模式标定时长都截取前7000s。
由图2可以看出，动基座标定模式3个陀螺仪零位的协方差曲线收敛速度要快于静基座模式，说明了动基座自标定方法的优越性。相比静基座标定流程，动基座标定更加灵活机动，收敛速度也较快，实时高精度重力计算模块的引入补偿了车辆移动引入的额外误差，使得标定结果的可靠性大大增强。

Claims (3)

1.基于高精度重力模型的车载激光惯导系统自标定方法，其特征在于，采用基于高精度重力模型的车载激光惯导系统进行实施，基于高精度重力模型的车载激光惯导系统，包括速度测量设备VMS(1)、激光陀螺惯性导航系统(3)、重力模型参数模块(6)、高精度GNSS(7)；激光陀螺惯性导航系统(3)连接捷联惯性导航算法更新模块(4)；激光陀螺惯性导航系统(3)角速度输出连接VMS速度计算模块(2)，VMS速度计算模块(2)连接速度测量设备VMS(1)；所述重力模型参数模块(6)连接重力计算模块(8)；所述高精度GNSS(7)连接所述重力计算模块(8)，高精度GNSS(7)位置输出值与捷联惯性导航算法更新模块(4)位置输出值进行做差运算后连接车载标定滤波误差计算模块(5)；所述重力计算模块(8)连接捷联惯性导航算法更新模块(4)；所述VMS速度计算模块(2)VMS速度输出与捷联惯性导航算法更新模块(4)速度输出做差后连接车载标定滤波误差计算模块(5)；车载标定滤波误差计算模块(5)连接标定判定模块(9)；所述激光陀螺惯性导航系统(3)包括激光陀螺和挠性加速度计；
具体步骤为：
步骤1：速度测量设备VMS(1)测量载车的车体速度
Figure FDA0003836893080000011
设测速设备单位时间输出的脉冲为
Figure FDA0003836893080000012
则载体系下的速度矢量
Figure FDA0003836893080000013
其中，kVMS为测速设备的刻度系数；
步骤2：根据激光陀螺惯性导航系统(3)输出的角速度
Figure FDA0003836893080000014
和速度测量设备VMS输出的车体速度
Figure FDA0003836893080000015
进行航位推算，输出导航坐标系下的VMS速度
Figure FDA0003836893080000016
计算公式如下：
Figure FDA0003836893080000021
其中，姿态矩阵
Figure FDA0003836893080000022
的更新过程可以在捷联惯性导航算法更新模块(4)中直接获得；
步骤3：激光陀螺惯性导航系统(3)输出载体坐标系下的角速度
Figure FDA0003836893080000023
和加速度
Figure FDA0003836893080000024
然后采用线性误差模型对输出脉冲进行标定补偿，即
Figure FDA0003836893080000025
其中，
Figure FDA0003836893080000026
分别为陀螺仪、加速度计温补后的输出脉冲；KG、KA分别为陀螺仪、加速度计的标定安装矩阵；εb表示陀螺零偏，
Figure FDA0003836893080000027
为加速度计的零位；
步骤4：捷联惯性导航算法更新模块(4)根据输入的角速度
Figure FDA0003836893080000028
加速度
Figure FDA0003836893080000029
和重力补偿值δgn进行导航解算，实现速度
Figure FDA00038368930800000210
和位置
Figure FDA00038368930800000211
的更新输出；
所述步骤4中导航算法要求跑车方式为闭合路径行车；
Figure FDA00038368930800000212
其中
Figure FDA00038368930800000213
Figure FDA00038368930800000214
Figure FDA00038368930800000215
其中，L,λ,h分别为载体所在的纬度、经度和高度；下标E、N、U表示沿当地坐标系的东向、北向和天向；RM、RN分别为载体所在当地子午圈和卯酉圈半径；速度和位置的初始值
Figure FDA00038368930800000216
由GNSS提供；姿态矩阵
Figure FDA00038368930800000217
通过捷联惯性导航系统自对准完成；
Figure FDA00038368930800000218
为通过标准重力模型计算得到的重力值，其计算公式采用WGS-84模型
Figure FDA0003836893080000031
这里，
Figure FDA0003836893080000032
的计算单位为m/s2；
步骤5：在车载标定滤波误差计算模块中，惯性导航速度
Figure FDA0003836893080000033
与VMS速度
Figure FDA0003836893080000034
做差后得到速度误差δvn，惯性导航位置
Figure FDA0003836893080000035
与GNSS提供位置
Figure FDA0003836893080000036
做差后得到位置量测δpn，车载标定滤波误差计算模块根据速度误差δvn和位置误差δpn，采用最优Kalman滤波技术实时估计系统的状态变量
Figure FDA0003836893080000037
更新协方差矩阵
Figure FDA0003836893080000038
步骤6：在重力计算模块中，保存全球高阶重力模型的缔合勒让德球谐参数，单位化后的参数包括
Figure FDA0003836893080000039
且
Figure FDA00038368930800000310
式中，
Figure FDA00038368930800000311
为高阶球谐重力模型的勒让德参数；
Figure FDA00038368930800000312
为WGS-84正常地球引力勒让德参数，n在这里只取有限阶偶数项n≤10；
步骤7：高精度GNSS可实时为载车提供高精度的位置信息
Figure FDA00038368930800000313
和秒脉冲信号，秒脉冲信号用于同步惯性导航系统与GNSS的时间；GNSS输出相对惯性导航的延迟时间
δt＝tGNSS-tPPS
式中，tGNSS为惯性导航接收到GNSS发送的位置信息数据帧的时刻；tPPS为这一帧位置数据对应的秒脉冲时，显然δt≥0；
步骤8：重力计算模块，根据重力模型参数模块和
Figure FDA00038368930800000314
计算得到的重力补偿值δgn；
Figure FDA0003836893080000041
其中
Figure FDA0003836893080000042
Figure FDA0003836893080000043
其中，重力补偿值计算中的勒让德参数
Figure FDA0003836893080000044
存放在重力模型参数模块(6)当中；
步骤9：标定判定模块(9)输出并保存当前时刻的估计值
Figure FDA0003836893080000045
类似的，εb、
Figure FDA0003836893080000046
的所有误差状态都小于门限且滤波时间t>1h时，则标定过程结束，标定得到参数可发送给上位机或存储在惯性导航系统的内部flash上。
2.根据权利要求1所述的基于高精度重力模型的车载激光惯导系统自标定方法，其特征在于，所述步骤5具体步骤为：选择惯性导航系统的误差状态为速度误差δvn、姿态误差向量φ、位置误差δpn、陀螺零偏εb、加速度计零位
Figure FDA0003836893080000047
同时考虑里程计的标度因数误差δKOD、航向安装偏角误差αψ和俯仰安装偏角误差αθ，则标定滤波器的状态变量表示为：
Figure FDA0003836893080000048
标定滤波器的状态方程
Figure FDA0003836893080000049
其中，WG为陀螺噪声；WA为加速度计噪声；
时间更新方程状态转移矩阵
Figure FDA0003836893080000051
其中，FSINS为惯性导航系统误差方程对应的转移矩阵，其中的非零元素有：
Figure FDA0003836893080000052
Figure FDA0003836893080000053
Figure FDA0003836893080000054
F1，10＝-C1,1,F1，11＝-C1,2,F1，12＝-C1,3
F2，10＝-C2,1,F2，11＝-C2,2,F2，12＝-C2,3
F3，10＝-C3,1,F3，11＝-C3,2,F3，12＝-C3,3
F4,2＝-fsf,z,F4,3＝fsf,y,
Figure FDA0003836893080000055
Figure FDA0003836893080000056
F5，1＝fsf,z,F5，3＝-fsf,x,
Figure FDA0003836893080000057
Figure FDA0003836893080000058
F6，1＝fsf,y,F6，2＝-fsf,x,
Figure FDA0003836893080000059
Figure FDA00038368930800000510
F4，13＝C1,1,F4，14＝C1,2,F4，15＝C1,3
F5，13＝C2,1,F5，14＝C2,2,F5，15＝C2,3
F6，13＝C3,1,F6，14＝C3,2,F6，15＝C3,3
F7，4＝1,F8,5＝1,F9,6＝1
Figure FDA00038368930800000511
其中，Fi,j表示矩阵FSINS中第i行第j列的元素，之所以不将重力扰动δgn列入滤波方程，一是由于δgn本质上是相对于位置的函数而非时间，采用Kalman滤波状态更新方程的形式建模一般无法做到十分准确，二是由于重力模型参数模块(6)中采用了全球高阶重力模型对实时重力扰动矢量进行计算，且在捷联惯性导航算法更新模块(4)中进行了补偿，其对导航参数的影响可以忽略；
标定滤波的量测量
Figure FDA0003836893080000061
其中，Vv为VMS的速度测量噪声；Vp为GNSS的位置测量噪声，位置转换矩阵和量测矩阵
Figure FDA0003836893080000062
H＝[06×3 I6 06×9]
其中，I表示单位矩阵。
3.根据权利要求1所述的基于高精度重力模型的车载激光惯导系统自标定方法，其特征在于，所述步骤1中速度测量设备VMS(1)，是里程计或激光雷达测速仪。
CN201911378995.4A 2019-12-27 2019-12-27 基于高精度重力模型的车载激光惯导系统自标定方法 Active CN111006675B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911378995.4A CN111006675B (zh) 2019-12-27 2019-12-27 基于高精度重力模型的车载激光惯导系统自标定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911378995.4A CN111006675B (zh) 2019-12-27 2019-12-27 基于高精度重力模型的车载激光惯导系统自标定方法

Publications (2)

Publication Number Publication Date
CN111006675A CN111006675A (zh) 2020-04-14
CN111006675B true CN111006675B (zh) 2022-10-18

Family

ID=70119060

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911378995.4A Active CN111006675B (zh) 2019-12-27 2019-12-27 基于高精度重力模型的车载激光惯导系统自标定方法

Country Status (1)

Country Link
CN (1) CN111006675B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113190998B (zh) * 2021-04-29 2022-11-18 中山大学 一种波茨模型的高维复用计算方法及装置
CN113566850B (zh) * 2021-07-29 2024-03-08 深圳元戎启行科技有限公司 惯性测量单元的安装角度标定方法、装置和计算机设备
CN114184209B (zh) * 2021-10-29 2023-10-13 北京自动化控制设备研究所 用于低速检测平台系统的惯性误差抑制方法
CN114413933A (zh) * 2022-01-17 2022-04-29 广东星舆科技有限公司 一种加速度计动态较准方法、系统及存储介质

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103900614A (zh) * 2014-03-28 2014-07-02 哈尔滨工程大学 一种九加速度计无陀螺惯导系统的重力补偿方法
CN106950586A (zh) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机作业的gnss/ins/车辆组合导航方法
CN106990426A (zh) * 2017-03-16 2017-07-28 北京无线电计量测试研究所 一种导航方法和导航装置
CN108132060A (zh) * 2017-11-17 2018-06-08 北京计算机技术及应用研究所 一种捷联惯导系统无基准的系统级标定方法
CN108180925A (zh) * 2017-12-15 2018-06-19 中国船舶重工集团公司第七0七研究所 一种里程计辅助车载动态对准方法
CN108303119A (zh) * 2018-01-05 2018-07-20 西安理工大学 双纵模激光陀螺频率可调谐闭锁阈值检测系统及检测方法
CN109186591A (zh) * 2018-08-28 2019-01-11 贵州理工学院 一种基于系统状态估计的sins/gps高精度重力扰动补偿方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4853937B2 (ja) * 2003-04-28 2012-01-11 独立行政法人産業技術総合研究所 慣性センサの動的感度マトリックス計測装置およびその計測方法
US8825399B2 (en) * 2008-07-24 2014-09-02 Raytheon Company System and method of passive and autonomous navigation of space vehicles using an extended Kalman filter
CN103674064B (zh) * 2013-12-06 2017-02-08 广东电网公司电力科学研究院 捷联惯性导航系统的初始标定方法
US9764858B2 (en) * 2015-01-07 2017-09-19 Mitsubishi Electric Research Laboratories, Inc. Model predictive control of spacecraft
US9568320B2 (en) * 2015-05-05 2017-02-14 King Fahd University Of Petroleum And Minerals Method and apparatus for estimation of center of gravity using accelerometers
GB2555806A (en) * 2016-11-09 2018-05-16 Atlantic Inertial Systems Ltd A navigation system

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103900614A (zh) * 2014-03-28 2014-07-02 哈尔滨工程大学 一种九加速度计无陀螺惯导系统的重力补偿方法
CN106950586A (zh) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机作业的gnss/ins/车辆组合导航方法
CN106990426A (zh) * 2017-03-16 2017-07-28 北京无线电计量测试研究所 一种导航方法和导航装置
CN108132060A (zh) * 2017-11-17 2018-06-08 北京计算机技术及应用研究所 一种捷联惯导系统无基准的系统级标定方法
CN108180925A (zh) * 2017-12-15 2018-06-19 中国船舶重工集团公司第七0七研究所 一种里程计辅助车载动态对准方法
CN108303119A (zh) * 2018-01-05 2018-07-20 西安理工大学 双纵模激光陀螺频率可调谐闭锁阈值检测系统及检测方法
CN109186591A (zh) * 2018-08-28 2019-01-11 贵州理工学院 一种基于系统状态估计的sins/gps高精度重力扰动补偿方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Li Chan ; Cao Yuan ; Zhang Shi-feng.A new multi-position calibration method for accelerometers of the inertial navigation system.《The 27th Chinese Control and Decision Conference》.2015,全文. *
重力扰动对高精度激光陀螺惯导系统ZUPT 的影响分析与补偿;翁浚，卞肖云;《系统工程与电子技术》;20190903;论文摘要，第0-4节 *

Also Published As

Publication number Publication date
CN111006675A (zh) 2020-04-14

Similar Documents

Publication Publication Date Title
CN111006675B (zh) 基于高精度重力模型的车载激光惯导系统自标定方法
CN110501024B (zh) 一种车载ins/激光雷达组合导航系统的量测误差补偿方法
CN110031882B (zh) 一种基于sins/dvl组合导航系统的外量测信息补偿方法
CN107588769B (zh) 一种车载捷联惯导、里程计及高程计组合导航方法
CN110926468B (zh) 基于传递对准的动中通天线多平台航姿确定方法
CN112097763B (zh) 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
CN110017837B (zh) 一种姿态抗磁干扰的组合导航方法
CN104764467B (zh) 空天飞行器惯性传感器误差在线自适应标定方法
CN111156994A (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN111102993A (zh) 一种旋转调制型捷联惯导系统晃动基座初始对准方法
CN111024074B (zh) 一种基于递推最小二乘参数辨识的惯导速度误差确定方法
CN111399023B (zh) 基于李群非线性状态误差的惯性基组合导航滤波方法
CN113203418A (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN111722295B (zh) 一种水下捷联式重力测量数据处理方法
CN113340298A (zh) 一种惯导和双天线gnss外参标定方法
CN114966629A (zh) 一种基于ekf算法框架的车体激光雷达外参标定方法
CN113252031A (zh) 一种narx神经网络辅助组合导航的方法
CN109084755B (zh) 一种基于重力视速度与参数辨识的加速度计零偏估计方法
CN113008229A (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN113092822A (zh) 一种基于惯组的激光多普勒测速仪的在线标定方法和装置
CN117053782A (zh) 一种水陆两栖机器人组合导航方法
CN112798014A (zh) 一种基于重力场球谐模型补偿垂线偏差的惯导自对准方法
CN117053802A (zh) 一种基于旋转mems imu的车载导航系统定位误差减小的方法
CN116576849A (zh) 一种基于gmm辅助的车辆融合定位方法及系统

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