CN111220151B - 载体系下考虑温度模型的惯性和里程计组合导航方法 - Google Patents

载体系下考虑温度模型的惯性和里程计组合导航方法 Download PDF

Info

Publication number
CN111220151B
CN111220151B CN201911329826.1A CN201911329826A CN111220151B CN 111220151 B CN111220151 B CN 111220151B CN 201911329826 A CN201911329826 A CN 201911329826A CN 111220151 B CN111220151 B CN 111220151B
Authority
CN
China
Prior art keywords
error
odometer
navigation
under
representing
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
CN201911329826.1A
Other languages
English (en)
Other versions
CN111220151A (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.)
General Designing Institute of Hubei Space Technology Academy
Original Assignee
General Designing Institute of Hubei Space Technology Academy
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 General Designing Institute of Hubei Space Technology Academy filed Critical General Designing Institute of Hubei Space Technology Academy
Priority to CN201911329826.1A priority Critical patent/CN111220151B/zh
Publication of CN111220151A publication Critical patent/CN111220151A/zh
Application granted granted Critical
Publication of CN111220151B publication Critical patent/CN111220151B/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/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
    • 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/20—Instruments for performing navigational calculations
    • 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/24—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for cosmonautical navigation

Abstract

本发明公开了一种载体系下考虑温度模型的惯性和里程计组合导航方法，涉及航空航天捷联惯性导航技术领域中的惯性组合导航领域，该方法包括按照预设的采样周期实时采集惯组脉冲数与里程计脉冲数，进行惯性导航解算，得到惯导算法输出的导航参数，同时求解载体系下惯导位移矢量累计值和载体系下里程计位移矢量累计值，并完成里程计当量温度效应误差补偿计算；根据状态向量建立状态微分方程；根据里程计位移增量和惯组位移增量建立量测方程；进行卡尔曼滤波以实时修正惯性导航系统参数误差、里程计参数误差和器件参数误差，实现导航数据输出。本发明能够极大地提高惯性和里程计组合导航精度。

Description

载体系下考虑温度模型的惯性和里程计组合导航方法
技术领域
本发明涉及航空航天捷联惯性导航技术领域中的惯性组合导航领域，具体涉及一种载体系下考虑温度模型的惯性和里程计组合导航方法。
背景技术
捷联惯导系统具有反应时间短、可靠性高、自主性好以及全天候应用等优点，广泛应用于航空、航天、车载等军用和民用导航领域，在国防和经济建设中发挥着重要作用。惯性导航系统能实时地输出载体位置、速度、姿态等信息，但是其难以克服导航误差随时间累积的缺点。里程计是一种自主的距离信息测量传感器，它与惯性导航系统组合之后能够起到抑制惯导误差发散的作用，因此采用惯性和里程计组合技术能够建立高精度自主定位定向导航系统。
通常情况下，惯性和里程计组合信息融合算法，是采用航位推算方法将里程计测量信息投影至导航坐标系，得到导航坐标系下测量位移，然后与惯性导航测量的导航系下位移作差比较，比较值作为组合导航卡尔曼滤波器的量测输入值。但是，采用该算法处理时量测值为惯性与里程计测量位移差在导航系下的投影，不能直观地体现出量测值的物理意义，同时，车辆轮胎随着运输过程胎内温度逐渐升高，里程计刻度系数会随着运动过程发生变化，里程计刻度系数误差建模方法为时间的一阶马尔科夫过程模型，难以准确描述实际情况的变化。
发明内容
针对现有技术中存在的缺陷，本发明的目的在于提供一种载体系下考虑温度模型的惯性和里程计组合导航方法，能够极大地提高惯性和里程计组合导航精度。
为达到以上目的，本发明采取的技术方案是，包括以下步骤：
按照预设的采样周期实时采集惯组脉冲数与里程计脉冲数，进行惯性导航解算，得到惯导算法输出的导航参数，同时求解载体系下惯导位移矢量累计值和载体系下里程计位移矢量累计值，并完成里程计当量温度效应误差补偿计算；
根据状态向量建立状态微分方程，所述状态微分方程为
Figure RE-GDA0002463472190000021
其中，
Figure RE-GDA0002463472190000022
表示状态微分方程，F(t)为状态转移矩阵， w(t)为系统噪声，状态向量表示为：
Figure RE-GDA0002463472190000023
其中，X表示状态向量，φ为姿态误差状态向量，δvn为速度误差状态向量，δP为位置误差状态向量，
Figure RE-GDA0002463472190000024
为陀螺漂移误差向量，
Figure RE-GDA0002463472190000025
为加速度计偏置误差向量，ξod＝[δKod,δαθ，δαψ]，δKod为里程计刻度系数误差，δαθ为俯仰安装偏角误差，δαψ为航向安装偏角误差，dCT为里程计温度系数误差，T表示卡尔曼滤波离散化周期；
根据里程计位移增量和惯组位移增量建立量测方程，所述量测方程为：
z＝H(t)X+v(t)
其中，z表示量测方程，H(t)表示量测矩阵，v(t)表示量测噪声。
基于所建立的状态微分方程和量测方程，进行卡尔曼滤波以实时修正惯性导航系统参数误差、里程计参数误差和器件参数误差，实现导航数据输出。
在上述技术方案的基础上，所述求解载体系下惯导位移矢量累计值和载体系下里程计位移矢量累计值，其中求解载体系下里程计位移矢量累计值的具体过程为：
根据采集的里程计脉冲数计算采样周期内路程增量，计算公式为：
Figure RE-GDA0002463472190000031
其中，
Figure RE-GDA0002463472190000032
表示路程增量，
Figure RE-GDA0002463472190000033
中的k表示第k个采样周期， NOdom为里程计脉冲输出数，KOdom为里程计当量；
基于所计算的路程增量，得到载体系下当前采样周期的里程计位移矢量：
Figure RE-GDA0002463472190000034
其中，
Figure RE-GDA0002463472190000035
表示载体系下当前采样周期的里程计位移矢量；
根据载体系下当前采样周期的里程计位移矢量计算载体系下里程计位移矢量累计值，计算公式为：
Figure RE-GDA0002463472190000036
其中，
Figure RE-GDA0002463472190000037
表示载体系下里程计位移矢量累计值，
Figure RE-GDA0002463472190000038
表示前一个采样周期的载体系下里程计位移矢量累计值。
在上述技术方案的基础上，所述求解载体系下惯导位移矢量累计值和载体系下里程计位移矢量累计值，其中求解载体系下惯导位移矢量累计值的具体过程为：
根据惯导速度计算导航系下惯导位移矢量：
Figure RE-GDA0002463472190000039
其中，
Figure RE-GDA00024634721900000310
表示惯导位移矢量，Δts表示采样周期，Δts中的s表示采样周期计数，
Figure RE-GDA00024634721900000311
表示导航系下的速度，
Figure RE-GDA00024634721900000312
表示当前采样周期的导航系下的速度，
Figure RE-GDA00024634721900000313
表示前一个采样周期的导航系下的速度；
基于计算得到的惯导位移矢量，计算载体系下惯导位移矢量累计值：
Figure RE-GDA0002463472190000041
其中，
Figure RE-GDA0002463472190000042
表示载体系下惯导位移矢量累计值，
Figure RE-GDA0002463472190000043
表示前一个周期的载体系下惯导位移矢量累计值，
Figure RE-GDA0002463472190000044
表示当前周期的导航系下姿态。
在上述技术方案的基础上，所述完成里程计当量温度效应误差补偿计算，计算公式为：
Figure RE-GDA0002463472190000045
其中，KOdom表示里程计当量温度效应误差补偿，CT、ΔTMax和 STempRise均表示里程计当量温度系数，
Figure RE-GDA0002463472190000046
表示截止至当前采样周期里程计累积行驶里程的绝对值。
在上述技术方案的基础上，所述系统噪声具体为：
Figure RE-GDA0002463472190000047
其中，
Figure RE-GDA0002463472190000048
表示第k个采样周期内的姿态余弦矩阵，
Figure RE-GDA0002463472190000049
表示陀螺仪随机噪声，
Figure RE-GDA00024634721900000410
表示加速度计随机噪声。
在上述技术方案的基础上，
所述姿态误差状态向量具体为φ＝[φE φN φU]T，其中，φE为东向姿态误差，φN为北向姿态误差，φU为天向姿态误差；
所述速度误差状态向量具体为
Figure RE-GDA00024634721900000411
其中，
Figure RE-GDA00024634721900000412
为东向速度误差，
Figure RE-GDA00024634721900000413
为北向速度误差，
Figure RE-GDA00024634721900000414
为天向速度误差；
所述位置误差状态向量具体为δP＝[δL δλ δh]T，其中，δL为纬度误差、δλ为经度误差、δh为高度误差；
所述陀螺漂移误差向量具体为
Figure RE-GDA0002463472190000051
其中，
Figure RE-GDA0002463472190000052
为x 轴陀螺误差，
Figure RE-GDA0002463472190000053
为y轴陀螺误差，
Figure RE-GDA0002463472190000054
为z轴陀螺误差；
所述加速度计偏置误差向量具体为
Figure RE-GDA0002463472190000055
其中，
Figure RE-GDA0002463472190000056
为x轴加速度计误差，
Figure RE-GDA0002463472190000057
为y轴加速度计误差，
Figure RE-GDA0002463472190000058
为z轴加速度计误差。
在上述技术方案的基础上，所述量测方程中的量测值计算方式为：
Figure RE-GDA0002463472190000059
其中，
Figure RE-GDA00024634721900000510
表示量测值，
Figure RE-GDA00024634721900000511
表示载体系下里程计位移矢量累计值，
Figure RE-GDA00024634721900000512
表示载体系下惯导位移矢量累计值。
在上述技术方案的基础上，所述基于所建立的状态微分方程和量测方程，进行卡尔曼滤波以实时修正惯性导航系统参数误差、里程计参数误差和器件参数误差，实现导航数据输出，具体步骤包括：
对状态微分方程离散化，得到离散形式的状态方程，建立卡尔曼滤波器；
根据状态微分方程和量测方程，设定系统噪声矩阵、量测噪声矩阵初值、滤波初值和滤波状态误差初值；
根据惯组数据和里程计数据实时进行导航计算，并将量测值输入卡尔曼滤波器，通过滤波估计后得到各误差状态的估计值，；
利用得到的估计值修正惯性导航系统参数误差、里程计参数误差和器件参数误差，得到修正后的导航参数，实现导航数据输出。
在上述技术方案的基础上，所述离散形式的状态方程具体为：
Xk+1＝Φk+1,kXk+Wk
其中，
Figure RE-GDA0002463472190000061
Xk表示离散形式的状态向量，F(tk)表示状态转移矩阵，T表示卡尔曼滤波离散化周期，τ表示时间积分量，I3表示3维单位矩阵。
在上述技术方案的基础上，所述利用得到的估计值修正惯性导航系统参数误差、里程计参数误差和器件参数误差，修正公式为：
Figure RE-GDA0002463472190000062
Figure RE-GDA0002463472190000063
Figure RE-GDA0002463472190000064
Figure RE-GDA0002463472190000065
Figure RE-GDA0002463472190000066
Figure RE-GDA0002463472190000067
Figure RE-GDA0002463472190000068
其中，
Figure RE-GDA0002463472190000069
表示导航姿态矩阵、
Figure RE-GDA00024634721900000610
表示导航速度、
Figure RE-GDA00024634721900000611
分别表示导航的维度、经度和高程，CT表示里程计当量温度系数，所述误差状态的估计值包括姿态误差估计值
Figure RE-GDA00024634721900000612
速度误差估计值
Figure RE-GDA00024634721900000613
位置误差估计值
Figure RE-GDA00024634721900000614
里程计刻度系数误差
Figure RE-GDA00024634721900000615
俯仰安装偏角误差估计值
Figure RE-GDA00024634721900000616
方位安装角误差估计值
Figure RE-GDA00024634721900000617
和里程计刻度系数温度系数估计值
Figure RE-GDA00024634721900000618
(CT)+的上角标“+”表示修正之后的值，(CT)-的上角标“-”表示修正之前的值，
Figure RE-GDA00024634721900000619
表示里程计刻度系数，
Figure RE-GDA00024634721900000620
表示俯仰安装角，
Figure RE-GDA00024634721900000621
表示防伪安装角。
与现有技术相比，本发明的优点在于：在采用惯性和里程计组合导航时，考虑温度变化对里程计的影响，即考虑里程计刻度系数误差的温度模型，提升里程计测量精度，从而极大地提高惯性和里程计组合导航精度。
附图说明
图1为本发明实施例中一种载体系下考虑温度模型的惯性和里程计组合导航方法的流程图。
具体实施方式
本发明实施例提供一种载体系下考虑温度模型的惯性和里程计组合导航方法，考虑里程计刻度系数误差的温度模型，极大提高惯性和里程计组合导航精度。以下结合附图及实施例对本发明作进一步详细说明。
参见图1所示，本发明实施例提供一种载体系下考虑温度模型的惯性和里程计组合导航方法，具体包括以下步骤：
S1：按照预设的采样周期实时采集惯组脉冲数与里程计脉冲数，进行惯性导航解算，得到惯导算法输出的导航参数，同时求解载体系下惯导位移矢量累计值和载体系下里程计位移矢量累计值，并完成里程计当量温度效应误差补偿计算；
本发明实施例中，惯导和里程计固定安装于车辆上，具体的，里程计安装于车体上，与车辆的变速转轴连接，通过转轴的转动传递使里程计周期转动，惯导设备安装固定于车辆内部。初始时，车体保持静止，惯导通电预热10分钟后开始初始对准，车辆开始运动后，按照预设的采样周期实时采集惯组脉冲数与里程计脉冲数，本发明实施例中的采样周期优选为0.01s。车辆开始运动后，通过采集原始脉冲数，进行惯性导航解算，得到惯导姿态、速度和位置。
本发明实施例中，求解载体系下惯导位移矢量累计值和载体系下里程计位移矢量累计值，其中求解载体系下里程计位移矢量累计值的具体过程为：
S111：根据采集的里程计脉冲数计算采样周期内路程增量，计算公式为：
Figure RE-GDA0002463472190000081
其中，
Figure RE-GDA0002463472190000082
表示路程增量，
Figure RE-GDA0002463472190000083
中的k表示第k个采样周期， NOdom为里程计脉冲输出数，KOdom为里程计当量；
S112：基于所计算的路程增量，得到载体系下当前采样周期的里程计位移矢量：
Figure RE-GDA0002463472190000084
其中，
Figure RE-GDA0002463472190000085
表示载体系下当前采样周期的里程计位移矢量；
S113：根据载体系下当前采样周期的里程计位移矢量计算载体系下里程计位移矢量累计值，计算公式为：
Figure RE-GDA0002463472190000086
其中，
Figure RE-GDA0002463472190000087
表示载体系下里程计位移矢量累计值，
Figure RE-GDA0002463472190000088
表示前一个采样周期的载体系下里程计位移矢量累计值。
本发明实施例中，求解载体系下惯导位移矢量累计值和载体系下里程计位移矢量累计值，其中求解载体系下惯导位移矢量累计值的具体过程为：
S121：根据惯导速度计算导航系下惯导位移矢量：
Figure RE-GDA0002463472190000089
其中，
Figure RE-GDA00024634721900000810
表示惯导位移矢量，Δts表示采样周期，Δts中的s表示采样周期计数，
Figure RE-GDA0002463472190000091
表示导航系下的速度，
Figure RE-GDA0002463472190000092
表示当前采样周期的导航系下的速度，
Figure RE-GDA0002463472190000093
表示前一个采样周期的导航系下的速度；
S122：基于计算得到的惯导位移矢量，计算载体系下惯导位移矢量累计值：
Figure RE-GDA0002463472190000094
其中，
Figure RE-GDA0002463472190000095
表示载体系下惯导位移矢量累计值，
Figure RE-GDA0002463472190000096
表示前一个周期的载体系下惯导位移矢量累计值，
Figure RE-GDA0002463472190000097
表示当前周期的导航系下姿态。
本发明实施例中，完成里程计当量温度效应误差补偿计算，计算公式为：
Figure RE-GDA0002463472190000098
其中，KOdom表示里程计当量温度效应误差补偿，CT、ΔTMax和 STempRise均表示里程计当量温度系数，
Figure RE-GDA0002463472190000099
表示截止至当前采样周期里程计累积行驶里程的绝对值。
S2：根据状态向量建立状态微分方程，所述状态微分方程为
Figure RE-GDA00024634721900000910
其中，
Figure RE-GDA00024634721900000911
表示状态微分方程，F(t)为状态转移矩阵， w(t)为系统噪声，状态向量表示为：
Figure RE-GDA00024634721900000912
其中，X表示状态向量，φ为姿态误差状态向量，δvn为速度误差状态向量，δP为位置误差状态向量，
Figure RE-GDA00024634721900000913
为陀螺漂移误差向量，
Figure RE-GDA00024634721900000914
为加速度计偏置误差向量，ξod＝[δKod,δαθ，δαψ]，δKod为里程计刻度系数误差，δαθ为俯仰安装偏角误差，δαψ为航向安装偏角误差，dCT为里程计温度系数误差，T表示卡尔曼滤波离散化周期。
本发明实施例中，系统噪声具体为：
Figure RE-GDA0002463472190000101
其中，
Figure RE-GDA0002463472190000102
表示第k个采样周期内的姿态余弦矩阵，
Figure RE-GDA0002463472190000103
表示陀螺仪随机噪声，
Figure RE-GDA0002463472190000104
表示加速度计随机噪声。
本发明实施例中，姿态误差状态向量具体为φ＝[φE φN φU]T，其中，φE为东向姿态误差，φN为北向姿态误差，φU为天向姿态误差；速度误差状态向量具体为
Figure RE-GDA0002463472190000105
其中，
Figure RE-GDA0002463472190000106
为东向速度误差，
Figure RE-GDA0002463472190000107
为北向速度误差，
Figure RE-GDA0002463472190000108
为天向速度误差；位置误差状态向量具体为δP＝[δL δλ δh]T，其中，δL为纬度误差、δλ为经度误差、δh为高度误差；陀螺漂移误差向量具体为
Figure RE-GDA0002463472190000109
其中，
Figure RE-GDA00024634721900001010
为x轴陀螺误差，
Figure RE-GDA00024634721900001011
为y轴陀螺误差，
Figure RE-GDA00024634721900001012
为z轴陀螺误差；加速度计偏置误差向量具体为
Figure RE-GDA00024634721900001013
其中，
Figure RE-GDA00024634721900001014
为x轴加速度计误差，
Figure RE-GDA00024634721900001015
为y轴加速度计误差，
Figure RE-GDA00024634721900001016
为z轴加速度计误差。
本发明实施例中，状态转移矩阵F(t)具体表示如下：
Figure RE-GDA00024634721900001017
S3：根据里程计位移增量和惯组位移增量建立量测方程，所述量测方程为：
z＝H(t)X+v(t)
其中，z表示量测方程，H(t)表示量测矩阵，具体为 H＝[03×25 I3×3 03×4]，v(t)表示量测噪声；
量测方程中的量测值计算方式为：
Figure RE-GDA0002463472190000111
其中，
Figure RE-GDA0002463472190000112
表示量测值，
Figure RE-GDA0002463472190000113
表示载体系下里程计位移矢量累计值，
Figure RE-GDA0002463472190000114
表示载体系下惯导位移矢量累计值。
S4：基于所建立的状态微分方程和量测方程，进行卡尔曼滤波以实时修正惯性导航系统参数误差、里程计参数误差和器件参数误差，实现导航数据输出。该步骤具体包括：
S401：对状态微分方程离散化，得到离散形式的状态方程，建立卡尔曼滤波器；
本发明实施例中，离散形式的状态方程具体为：
Xk+1＝Φk+1,kXk+Wk
其中，
Figure RE-GDA0002463472190000115
Xk+1表示离散形式的状态向量，F(tk)表示状态转移矩阵，T表示卡尔曼滤波离散化周期，τ表示时间积分量,I3表示3维单位矩阵。对于卡尔曼滤波离散化周期，优选设置为0.1秒。
S402：根据状态微分方程和量测方程，设定系统噪声矩阵、量测噪声矩阵初值、滤波初值和滤波状态误差初值；
S403：根据惯组数据和里程计数据实时进行导航计算，并将量测值输入卡尔曼滤波器，通过滤波估计后得到各误差状态的估计值，误差状态的估计值包括姿态误差估计值
Figure RE-GDA0002463472190000121
速度误差估计值
Figure RE-GDA0002463472190000122
位置误差估计值
Figure RE-GDA0002463472190000123
里程计刻度系数误差及安装偏角误差估计值
Figure RE-GDA0002463472190000124
S404：利用得到的估计值修正惯性导航系统参数误差、里程计参数误差和器件参数误差，得到修正后的导航参数，实现导航数据输出。
本发明实施例中，系统噪声矩阵Qk、量测噪声矩阵初值、滤波初值X0和滤波状态误差初值，均为卡尔曼滤波的初始值，卡尔曼滤波初始状态向量设置为0，即：
Xo＝032×1
卡尔曼滤波的初始协方差矩阵Po为对角矩阵，对角线元素的定义如下表1所示。
表1
序号 对角线序号 数值 备注
1. 1～3 [10/6378137 10/6378137 10]^2
2. 4～6 [0.1 0.1 0.1]^2
3. 7～9 [[300 300 300]*pi/180/3600]^2
4. 10 (10*pi/180/3600)^2
5. 11～13 [[20 20 20]*1.0e-6*g0]^2
6. 14～16 [[20 20 20]*1.0e-6]^2
7. 17～19 [[0.003 0.003 0.003]*pi/180/3600]^2
8. 20～22 [[20 20 20]*1.0e-6]^2
9. 23～25 [0.003 0.003 0.003]^2
10. 26～28 [0.1 0.1 0.1]^2
11. 29～31 [2 2 2]^2
12. 32 (1.0e-4)^2
系统噪声矩阵Qk为对角矩阵，初始化过程如下：
Figure RE-GDA0002463472190000131
Figure RE-GDA0002463472190000132
其中，RWC表示6*1向量，分别为3个加速度计的速度随机游走系数
Figure RE-GDA0002463472190000133
和3个陀螺的角度随机游走系数
Figure RE-GDA0002463472190000134
INSNoisePSD表示器件状态的过程噪声的功率谱密度，具体数值为： [2.66e-14 2.66e-14 2.66e-14 0 0 0 5.87e-22 5.87e-225.87e-22 0 0 0]； aidNoisePSD表示辅助导航系统的过程噪声功率谱密度，具体数值为：[1e-012 1e-020 1e-020 1e-012 1e-012 1e-012 0 0 0 0]；量测噪声矩阵Rk为对角矩阵，对角线元素如下表2所示。
表2
Rk(1,1) (0.1)^2
Rk(2,2) (0.1)^2
Rk(3,3) (0.1)^2
本发明实施例中的建立卡尔曼滤波器，其中的推导状态向量更新计算过程，具体如下：
a：计算状态一步预测
Figure RE-GDA0002463472190000135
Figure RE-GDA0002463472190000136
b：计算一步预测均方差阵Pk/k-1：
Figure RE-GDA0002463472190000141
c：计算滤波增益矩阵Kk：
Figure RE-GDA0002463472190000142
d：计算状态最优估计
Figure RE-GDA0002463472190000143
Figure RE-GDA0002463472190000144
e：计算状态估计均方差阵Pk：
Pk＝(I-KkHk)Pk/k-1
本发明实施例中，利用得到的估计值修正惯性导航系统参数误差、里程计参数误差和器件参数误差，修正公式为：
Figure RE-GDA0002463472190000145
Figure RE-GDA0002463472190000146
Figure RE-GDA0002463472190000147
Figure RE-GDA0002463472190000148
Figure RE-GDA0002463472190000149
Figure RE-GDA00024634721900001410
Figure RE-GDA00024634721900001411
其中，
Figure RE-GDA00024634721900001412
表示导航姿态矩阵、
Figure RE-GDA00024634721900001413
表示导航速度、
Figure RE-GDA00024634721900001414
分别表示导航的维度、经度和高程，CT表示里程计当量温度系数，误差状态的估计值包括姿态误差估计值
Figure RE-GDA00024634721900001415
速度误差估计值
Figure RE-GDA00024634721900001416
位置误差估计值
Figure RE-GDA00024634721900001417
里程计刻度系数误差
Figure RE-GDA00024634721900001418
俯仰安装偏角误差估计值
Figure RE-GDA00024634721900001419
方位安装角误差估计值
Figure RE-GDA00024634721900001420
和里程计刻度系数温度系数估计值
Figure RE-GDA00024634721900001421
(CT)+的上角标“+”表示修正之后的值，(CT)-的上角标“-”表示修正之前的值，
Figure RE-GDA0002463472190000151
表示里程计刻度系数，
Figure RE-GDA0002463472190000152
表示俯仰安装角，
Figure RE-GDA0002463472190000153
表示防伪安装角。
本发明根据惯导输出的位投影转换至载体系得到惯导在载体系的位移，将采样周期内载体系下惯导位移增量与里程计位移增量的差值作为量测值，考虑了里程计的刻度系数误差及惯导安装偏角误差，另外在此基础上考虑了里程计刻度系数误差的温度模型，提高里程计测量精度，通过卡尔曼滤波器对所有模型误差进行估计，进而通过估计值修正误差量，能大幅度提高组合导航精度，且计算简便，易于实现，具有良好的工程应用价值。
本发明实施例的载体系下考虑温度模型的惯性和里程计组合导航方法，在采用惯性和里程计组合导航时，考虑温度变化对里程计的影响，即考虑里程计刻度系数误差的温度模型，提升里程计测量精度，从而极大地提高惯性和里程计组合导航精度。
本发明不局限于上述实施方式，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围之内。本说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (7)

1.一种载体系下考虑温度模型的惯性和里程计组合导航方法，其特征在于，包括以下步骤：
按照预设的采样周期实时采集惯组脉冲数与里程计脉冲数，进行惯性导航解算，得到惯导算法输出的导航参数，同时求解载体系下惯导位移矢量累计值和载体系下里程计位移矢量累计值，并完成里程计当量温度效应误差补偿计算；
根据状态向量建立状态微分方程，所述状态微分方程为
Figure FDA0003105912800000011
其中，
Figure FDA0003105912800000012
表示状态微分方程，F(t)为状态转移矩阵，w(t)为系统噪声，状态向量表示为：
Figure FDA0003105912800000013
其中，X表示状态向量，φ为姿态误差状态向量，δvn为速度误差状态向量，δP为位置误差状态向量，
Figure FDA0003105912800000014
为陀螺漂移误差向量，
Figure FDA0003105912800000015
为加速度计偏置误差向量，ξod＝[δKod,δαθ，δαψ]，δKod为里程计刻度系数误差，δαθ为俯仰安装偏角误差，δαψ为航向安装偏角误差，dCT为里程计温度系数误差，T表示卡尔曼滤波离散化周期；
根据里程计位移增量和惯组位移增量建立量测方程，所述量测方程为：
z＝H(t)X+v(t)
其中，z表示量测方程，H(t)表示量测矩阵，v(t)表示量测噪声；
基于所建立的状态微分方程和量测方程，进行卡尔曼滤波以实时修正惯性导航系统参数误差、里程计参数误差和器件参数误差，实现导航数据输出；
其中，所述求解载体系下惯导位移矢量累计值和载体系下里程计位移矢量累计值，其中求解载体系下里程计位移矢量累计值的具体过程为：
根据采集的里程计脉冲数计算采样周期内路程增量，计算公式为：
Figure FDA0003105912800000021
其中，
Figure FDA0003105912800000022
表示路程增量，
Figure FDA0003105912800000023
中的k表示第k个采样周期，NOdom为里程计脉冲输出数，KOdom为里程计当量；
基于所计算的路程增量，得到载体系下当前采样周期的里程计位移矢量：
Figure FDA0003105912800000024
其中，
Figure FDA0003105912800000025
表示载体系下当前采样周期的里程计位移矢量；
根据载体系下当前采样周期的里程计位移矢量计算载体系下里程计位移矢量累计值，计算公式为：
Figure FDA0003105912800000026
其中，
Figure FDA0003105912800000027
表示载体系下里程计位移矢量累计值，
Figure FDA0003105912800000028
表示前一个采样周期的载体系下里程计位移矢量累计值；
其中，所述求解载体系下惯导位移矢量累计值和载体系下里程计位移矢量累计值，其中求解载体系下惯导位移矢量累计值的具体过程为：
根据惯导速度计算导航系下惯导位移矢量：
Figure FDA0003105912800000029
其中，
Figure FDA00031059128000000210
表示惯导位移矢量，Δts表示采样周期，Δts中的s表示采样周期计数，
Figure FDA00031059128000000211
表示导航系下的速度，
Figure FDA00031059128000000212
表示当前采样周期的导航系下的速度，
Figure FDA00031059128000000213
表示前一个采样周期的导航系下的速度；
基于计算得到的惯导位移矢量，计算载体系下惯导位移矢量累计值：
Figure FDA0003105912800000031
其中，
Figure FDA0003105912800000032
表示载体系下惯导位移矢量累计值，
Figure FDA0003105912800000033
表示前一个周期的载体系下惯导位移矢量累计值，
Figure FDA0003105912800000034
表示当前周期的导航系下姿态；
其中，所述完成里程计当量温度效应误差补偿计算，计算公式为：
Figure FDA0003105912800000035
其中，KOdom表示里程计当量温度效应误差补偿，CT、ΔTMax和STempRise均表示里程计当量温度系数，
Figure FDA0003105912800000036
表示截止至当前采样周期里程计累积行驶里程的绝对值。
2.如权利要求1所述的一种载体系下考虑温度模型的惯性和里程计组合导航方法，其特征在于，所述系统噪声具体为：
Figure FDA0003105912800000037
其中，
Figure FDA0003105912800000038
表示第k个采样周期内的姿态余弦矩阵，
Figure FDA0003105912800000039
表示陀螺仪随机噪声，
Figure FDA00031059128000000310
表示加速度计随机噪声。
3.如权利要求1所述的一种载体系下考虑温度模型的惯性和里程计组合导航方法，其特征在于：
所述姿态误差状态向量具体为φ＝[φE φN φU]T，其中，φE为东向姿态误差，φN为北向姿态误差，φU为天向姿态误差；
所述速度误差状态向量具体为
Figure FDA00031059128000000311
其中，
Figure FDA00031059128000000312
为东向速度误差，
Figure FDA00031059128000000313
为北向速度误差，
Figure FDA00031059128000000314
为天向速度误差；
所述位置误差状态向量具体为δP＝[δL δλ δh]T，其中，δL为纬度误差、δλ为经度误差、δh为高度误差；
所述陀螺漂移误差向量具体为
Figure FDA0003105912800000041
其中，
Figure FDA0003105912800000042
为x轴陀螺误差，
Figure FDA0003105912800000043
为y轴陀螺误差，
Figure FDA0003105912800000044
为z轴陀螺误差；
所述加速度计偏置误差向量具体为
Figure FDA0003105912800000045
其中，
Figure FDA0003105912800000046
为x轴加速度计误差，
Figure FDA0003105912800000047
为y轴加速度计误差，
Figure FDA0003105912800000048
为z轴加速度计误差。
4.如权利要求1所述的一种载体系下考虑温度模型的惯性和里程计组合导航方法，其特征在于，所述量测方程中的量测值计算方式为：
Figure FDA0003105912800000049
其中，
Figure FDA00031059128000000410
表示量测值，
Figure FDA00031059128000000411
表示载体系下里程计位移矢量累计值，
Figure FDA00031059128000000412
表示载体系下惯导位移矢量累计值。
5.如权利要求4所述的一种载体系下考虑温度模型的惯性和里程计组合导航方法，其特征在于，所述基于所建立的状态微分方程和量测方程，进行卡尔曼滤波以实时修正惯性导航系统参数误差、里程计参数误差和器件参数误差，实现导航数据输出，具体步骤包括：
对状态微分方程离散化，得到离散形式的状态方程，建立卡尔曼滤波器；
根据状态微分方程和量测方程，设定系统噪声矩阵、量测噪声矩阵初值、滤波初值和滤波状态误差初值；
根据惯组数据和里程计数据实时进行导航计算，并将量测值输入卡尔曼滤波器，通过滤波估计后得到各误差状态的估计值；
利用得到的估计值修正惯性导航系统参数误差、里程计参数误差和器件参数误差，得到修正后的导航参数，实现导航数据输出。
6.如权利要求5所述的一种载体系下考虑温度模型的惯性和里程计组合导航方法，其特征在于，所述离散形式的状态方程具体为：
Xk+1＝Φk+1,kXk+Wk
其中，
Figure FDA0003105912800000051
Xk表示离散形式的状态向量，F(tk)表示状态转移矩阵，T表示卡尔曼滤波离散化周期，τ表示时间积分量，I3表示3维单位矩阵。
7.如权利要求6所述的一种载体系下考虑温度模型的惯性和里程计组合导航方法，其特征在于，所述利用得到的估计值修正惯性导航系统参数误差、里程计参数误差和器件参数误差，修正公式为：
Figure FDA0003105912800000052
Figure FDA0003105912800000053
Figure FDA0003105912800000054
Figure FDA0003105912800000055
Figure FDA0003105912800000056
Figure FDA0003105912800000057
Figure FDA0003105912800000058
其中，
Figure FDA0003105912800000059
表示导航姿态矩阵、
Figure FDA00031059128000000510
表示导航速度、
Figure FDA00031059128000000511
分别表示导航的维度、经度和高程，CT表示里程计当量温度系数，所述误差状态的估计值包括姿态误差估计值
Figure FDA00031059128000000512
速度误差估计值
Figure FDA00031059128000000513
位置误差估计值
Figure FDA00031059128000000514
里程计刻度系数误差
Figure FDA00031059128000000515
俯仰安装偏角误差估计值
Figure FDA0003105912800000061
方位安装角误差估计值
Figure FDA0003105912800000062
和里程计刻度系数温度系数估计值
Figure FDA0003105912800000063
(CT)+的上角标“+”表示修正之后的值，(CT)-的上角标“-”表示修正之前的值，
Figure FDA0003105912800000064
表示里程计刻度系数，
Figure FDA0003105912800000065
表示俯仰安装角，
Figure FDA0003105912800000066
表示防伪安装角。
CN201911329826.1A 2019-12-20 2019-12-20 载体系下考虑温度模型的惯性和里程计组合导航方法 Active CN111220151B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911329826.1A CN111220151B (zh) 2019-12-20 2019-12-20 载体系下考虑温度模型的惯性和里程计组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911329826.1A CN111220151B (zh) 2019-12-20 2019-12-20 载体系下考虑温度模型的惯性和里程计组合导航方法

Publications (2)

Publication Number Publication Date
CN111220151A CN111220151A (zh) 2020-06-02
CN111220151B true CN111220151B (zh) 2021-08-03

Family

ID=70827873

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911329826.1A Active CN111220151B (zh) 2019-12-20 2019-12-20 载体系下考虑温度模型的惯性和里程计组合导航方法

Country Status (1)

Country Link
CN (1) CN111220151B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112461236B (zh) * 2020-11-23 2022-10-04 中国人民解放军火箭军工程大学 一种车载高精度容错组合导航方法及系统
CN114061574B (zh) * 2021-11-20 2024-04-05 北京唯实深蓝科技有限公司 一种基于位置不变约束及零速校正的采煤机定姿定向方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104977004A (zh) * 2015-07-13 2015-10-14 湖北航天技术研究院总体设计所 一种激光惯组与里程计组合导航方法及系统
CN106595715A (zh) * 2016-12-30 2017-04-26 中国人民解放军信息工程大学 基于捷联惯导/卫星组合导航系统里程计标定方法及装置
CN109269500A (zh) * 2018-11-16 2019-01-25 北京电子工程总体研究所 一种基于惯性导航系统和里程计的管道定位方法和系统
EP3462132A2 (de) * 2017-09-12 2019-04-03 Valeo Schalter und Sensoren GmbH Verfahren zum bestimmen einer aktuellen position eines kraftfahrzeugs, steuereinrichtung, fahrzeugmodul, positionsbestimmungssystem sowie kraftfahrzeug
CN110514221A (zh) * 2019-08-13 2019-11-29 中国航空工业集团公司西安飞行自动控制研究所 一种里程仪初始参数快速计算方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104977004A (zh) * 2015-07-13 2015-10-14 湖北航天技术研究院总体设计所 一种激光惯组与里程计组合导航方法及系统
CN106595715A (zh) * 2016-12-30 2017-04-26 中国人民解放军信息工程大学 基于捷联惯导/卫星组合导航系统里程计标定方法及装置
EP3462132A2 (de) * 2017-09-12 2019-04-03 Valeo Schalter und Sensoren GmbH Verfahren zum bestimmen einer aktuellen position eines kraftfahrzeugs, steuereinrichtung, fahrzeugmodul, positionsbestimmungssystem sowie kraftfahrzeug
CN109269500A (zh) * 2018-11-16 2019-01-25 北京电子工程总体研究所 一种基于惯性导航系统和里程计的管道定位方法和系统
CN110514221A (zh) * 2019-08-13 2019-11-29 中国航空工业集团公司西安飞行自动控制研究所 一种里程仪初始参数快速计算方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
Influence of Tire Temperature on Odometer’s Scale factor;Zhu Libin;《2012 Fifth International Conference on Intelligent Computation Technology and Automation》;20121231;第690-693页 *
基于强跟踪滤波的车载行进间对准;赵小明等;《中国惯性技术学报》;20150430(第02期);第41-44页 *
捷联惯导/里程计组合导航技术;马智渊等;《火力与指挥控制》;20170228(第02期);第183-186页 *
车载惯导航位推算组合导航系统误差补偿研究;李旦等;《计算机测量与控制》;20111231(第02期);第389-391页 *

Also Published As

Publication number Publication date
CN111220151A (zh) 2020-06-02

Similar Documents

Publication Publication Date Title
CN110487301B (zh) 一种雷达辅助机载捷联惯性导航系统初始对准方法
CN108180925B (zh) 一种里程计辅助车载动态对准方法
CN108226980B (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
CN112629538B (zh) 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
CN111156994B (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
CN106767752B (zh) 一种基于偏振信息的组合导航方法
CN106990426B (zh) 一种导航方法和导航装置
CN103245359B (zh) 一种惯性导航系统中惯性传感器固定误差实时标定方法
CN109974697A (zh) 一种基于惯性系统的高精度测绘方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN105318876A (zh) 一种惯性里程计组合高精度姿态测量方法
CN104764467B (zh) 空天飞行器惯性传感器误差在线自适应标定方法
CN109186597B (zh) 一种基于双mems-imu的室内轮式机器人的定位方法
CN109931955B (zh) 基于状态相关李群滤波的捷联惯性导航系统初始对准方法
CN103697894B (zh) 基于滤波器方差阵修正的多源信息非等间隔联邦滤波方法
CN102853837B (zh) 一种mimu和gnss信息融合的方法
CN110243377B (zh) 一种基于分层式结构的集群飞行器协同导航方法
CN111024074B (zh) 一种基于递推最小二乘参数辨识的惯导速度误差确定方法
CN112762961B (zh) 一种车载惯性里程计组合导航在线标定方法
CN102519470A (zh) 多级嵌入式组合导航系统及导航方法
CN104215262A (zh) 一种惯性导航系统惯性传感器误差在线动态辨识方法
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
CN110954102A (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN111220151B (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