CN107144284A - 基于ckf滤波的车辆动力学模型辅助惯导组合导航方法 - Google Patents

基于ckf滤波的车辆动力学模型辅助惯导组合导航方法 Download PDF

Info

Publication number
CN107144284A
CN107144284A CN201710253845.5A CN201710253845A CN107144284A CN 107144284 A CN107144284 A CN 107144284A CN 201710253845 A CN201710253845 A CN 201710253845A CN 107144284 A CN107144284 A CN 107144284A
Authority
CN
China
Prior art keywords
msub
mrow
msubsup
mover
mfrac
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.)
Pending
Application number
CN201710253845.5A
Other languages
English (en)
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 CN201710253845.5A priority Critical patent/CN107144284A/zh
Publication of CN107144284A publication Critical patent/CN107144284A/zh
Pending legal-status Critical Current

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/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
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/343—Calculating itineraries, i.e. routes leading from a starting point to a series of categorical destinations using a global route restraint, round trips, touristic trips
    • 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/34—Route searching; Route guidance
    • G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
    • 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
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

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

本发明公开一种基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，包括如下步骤：根据微惯性器件输出的角增量和比力，利用惯导数值更新算法解算车辆的姿态、速度和位置；建立三自由度车辆动力学模型，以方向盘转角和纵向力为控制输入量，利用四阶龙格库塔法实时解算载体速度；以惯导误差方程为状态方程，动力学模型和惯导解算的速度差为观测量，设计CKF滤波器对组合导航系统进行状态估计；CKF估计得到的位置、速度和姿态误差对捷联惯导解算结果进行输出校正，陀螺和加表误差对惯导进行反馈校正。此种方法针对惯导误差随时间积累，无法长时间保持导航精度的问题，可提高车辆导航系统的精确度和可靠性。

Description

基于CKF滤波的车辆动力学模型辅助惯导组合导航方法
技术领域
本发明属于车辆组合导航技术领域，特别涉及一种基于CKF滤波的车辆动力学模型辅助惯导组合导航方法。
背景技术
随着微惯性器件的迅速发展，惯性导航系统(inertial navigation system，INS)可满足车辆导航系统低成本、微型化的需求。但INS会产生随时间积累的误差，无法长时间保持导航精度，故需要引入辅助传感器测量的信息与INS组成组合导航系统进行工作。其中最常见的是惯导/卫星组合导航系统。但在实际应用中，卫星导航系统(Global navigationsatellite system,GNSS)自主性差，易受干扰，GNSS信号容易受到阻隔，在GNSS失效阶段导航系统仍然相当于INS单独工作。这种情况下，如果保持长时间的导航精度，需要其它状态信息辅助惯导系统。而车辆动力学模型(Vehicle dynamic model,VDM)对载体状态估计，不需依赖外部条件，具有较强的自主性和适用性，可以利用VDM有效地辅助INS进行导航。
目前，基于VDM的车辆状态估计主要有两类，一类是以速度为已知量，利用非线性VDM对侧偏角等状态进行估计，该类方法对速度的精度要求较高。第二类是以轮胎力为已知量，基于扩展卡尔曼滤波器、一阶斯梯林插值滤波器、递推最小二乘法滤波器，估计车速、横摆角速度、质心侧偏角等状态。但第二类方法中将非线性模型简化为线性时会造成一定的误差，降低测量精度，并且VDM估计的状态信息没有用于辅助其他导航系统，具有一定的局限性。
基于以上分析，现有的惯性导航技术精度较低，有待改进，本案由此产生。
发明内容
本发明的目的，在于提供一种基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，其针对惯导误差随时间积累，无法长时间保持导航精度的问题，可提高车辆导航系统的精确度和可靠性。
为了达成上述目的，本发明的解决方案是：
一种基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，包括如下步骤：
步骤1，根据微惯性器件输出的角增量和比力，利用惯导数值更新算法解算车辆的姿态、速度和位置；
步骤2，建立三自由度车辆动力学模型，以方向盘转角和纵向力为控制输入量，利用四阶龙格库塔法实时解算载体速度；
步骤3，以惯导误差方程为状态方程，动力学模型和惯导解算的速度差为观测量，设计CKF滤波器对组合导航系统进行状态估计；
步骤4，CKF估计得到的位置、速度和姿态误差对捷联惯导解算结果进行输出校正，陀螺和加表误差对惯导进行反馈校正。
上述步骤1的详细内容是：利用六轴加速度计和陀螺仪测量载体的角增量和比力，解算出载体当前时刻的导航参数，导航坐标系n采用东北天地理坐标系，载体坐标系b采用右前上坐标系；惯导数值更新算法包含姿态微分方程、比力微分方程和位置微分方程。
上述姿态微分方程的表达式为：
其中，Q为姿态四元数，为载体系相对于导航系的角速度，为陀螺输出的载体系相对于惯性系的角速度，为地球自转引起的导航系旋转角速度，为载体运动产生的位移角速度，为姿态旋转矩阵。
上述比力微分方程的表达式为：
其中，vI＝[vIe vIn vIu]T为惯导解算的东向、北向和天向速度，为加速度计测量的比力，为由载体运动和地球自转引起的哥氏加速度，为由载体运动引起的对地向心加速度，gn为重力加速度，为由载体系转换为导航系的转换矩阵。
上述位置微分方程的表达式为：
其中，RM和RN分别为子午圈和卯酉圈主曲率半径，L、λ和h分别为惯导解算的纬度、经度和高度，vIe、vIn和vIu分别为惯导解算的东向速度、北向速度和天向速度。
上述步骤2的详细内容是：建立横向、纵向、横摆三自由度非线性车辆动力学模型，忽略左右轮差异，模型等效简化为前、后车轮分别集中在车辆前、后轴中点而构成的一个自行车模型，xnoyn为导航坐标系，xn轴向东，yn轴向北；xboyb为固定在车辆质心上的载体坐标系，xb轴与载体横轴重合，向右为正，yb轴与载体纵轴重合，向前为正；
根据牛顿力学，车辆的动力学模型为：
其中，和分别为车辆的横向速度、纵向速度和横摆角速度；m和Iz分别为车辆的质量、绕竖轴的转动惯量；a、b分别为汽车前轮和后轮轮轴中心到质心的距离；δf为前轮转向角，由方向盘转角传感器测得的方向盘转角δ除以从方向盘到前轮的转向传动比来确定；Cd为空气阻力系数；Af为车辆前向面积；ρa为空气密度；Ftf、Ftr分别为作用在单个前轮和后轮上的纵向力；Fsf、Fsr分别为作用在单个前轮和后轮上的横向力；对于行驶在一般道路交通环境下的车辆，将作用在各轮上的侧向力表示为：
Fsf＝Cαfαf
Fsr＝Cαrαr
式中，Cαf、Cαr分别为前、后轮胎的侧偏刚度；αf、αr分别为前、后轮胎的侧偏角，近似表示为：
将侧向力表达式、侧偏角表达式代入动力学模型表达式，并考虑到δf通常是小角度，经整理后得：
利用方向盘转角传感器和车轮力传感器信息得到车辆动力学模型的控制输入向量U＝[δf Ftf Ftr]T，通过四阶龙格库塔法求解上式得载体坐标系下车辆的横向速度纵向速度和横摆角速度根据惯导解算的姿态矩阵，得到导航坐标系下由动力学模型计算的速度：
上述步骤3中，CKF滤波器的误差状态变量包括3个姿态误差、2个速度误差、2个位置误差、3个陀螺常值漂移、2个加速度计常值零偏，即
CKF滤波器的状态方程为
式中，A为相应的状态转移矩阵，由INS误差方程构成；w为过程噪声；
将惯导和车辆动力学模型解算的东向速度之差、北向速度之差作为CKF滤波器的观测量，即观测向量为Z＝[vIe-vMe vIn-vMn]T，则系统的量测方程为：
Z＝HX+v
式中，H＝[02×3 I2×2 02×7]为系统量测矩阵，v为量测噪声；
对前述系统状态方程和量测方程进行离散化，得到k时刻的离散化系统状态模型为：
Xk＝FXk-1+wk-1
Zk＝HXk+vk
式中，F＝exp(A×ΔT)为离散化的系统状态转移矩阵，ΔT为采样时间；wk-1为均值为0，方差为Qk-1的高斯白噪声，vk为均值为0，方差为Rk的高斯白噪声且wk-1和vk相互独立。
上述步骤3中，CKF滤波器的状态估计包括时间更新和量测更新两步：
(1)时间更新
构造容积点
式中，Pk-1k-1、为k-1时刻协方差和状态估计值；m＝2n，n是状态向量X维数；[1]i是点集[1]的第i列；
计算经状态方程传播容积点：
一步预测状态和误差协方差阵：
(2)量测更新
构造容积点：
计算经量测方程传播容积点：
Zi,k/k-1＝HXi,k/k-1
计算观测量预测值：
估计新息自协方差矩阵：
估计互协方差矩阵：
CKF滤波器增益矩阵：
估计k时刻的状态量：
估计k时刻状态误差协方差阵：
上述步骤4的详细内容是；对微惯性器件输出的角增量比力进行反馈校正得：
对INS解算的纬度L、经度λ进行输出校正得组合导航位置：
Lout＝L-δL
λout＝λ-δλ
对惯导解算的姿态进行输出校正得组合导航姿态：
θout＝θ-δθ
γout＝γ-δγ
对惯导解算的东向速度vIe、北向速度vIn进行输出校正得组合导航速度：
Veout＝vIe-δvx
Vnout＝vIn-δvy
其中，θ表示俯仰角，γ表示横滚角，表示航向角。
采用上述方案后，本发明提出基于CKF滤波的VDM辅助INS组合导航方法，建立车辆三自由度非线性动力学模型，利用四阶龙格库塔法实时解算速度信息。以INS误差方程为状态方程，VDM与INS解算的速度差为观测量，设计了CKF，并用估计的状态误差对INS进行校正。本发明中所采用的CKF算法无需对状态模型线性化，直接通过非线性系统方程对容积点进行传播，估计精度高。VDM辅助INS的组合导航方法可有效抑制INS导航误差发散，改善车辆导航系统的可靠性。本发明能够有效抑制纯惯导误差随时间累积问题，提高车辆的导航精度和可靠性，具有实时性好、自主性强等特点。
附图说明
图1是本发明的原理图；
图2是车辆动力学模型示意图。
具体实施方式
以下将结合附图，对本发明的技术方案进行详细说明。
如图1所示，本发明提供一种基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，包括如下步骤：
步骤1，根据微惯性器件输出的角增量和比力，利用惯导(INS)数值更新算法解算车辆的姿态、速度和位置；
其中，INS数值更新算法利用六轴加速度计和陀螺仪测量载体的比力和角增量，根据已知的初始条件，解算出载体当前时刻的导航参数。导航坐标系n采用东北天地理坐标系，载体坐标系b采用右前上坐标系。INS基本方程包含姿态微分方程、比力微分方程和位置微分方程。其中，姿态四元数微分表达式为
其中，Q为姿态四元数，为载体系相对于导航系的角速度，为陀螺输出的载体系相对于惯性系的角速度，为地球自转引起的导航系旋转角速度，为载体运动产生的位移角速度，为姿态旋转矩阵。
比力微分方程为
其中，vI＝[vIe vIn vIu]T为惯导解算的东向、北向和天向速度，为加速度计测量的比力，为由载体运动和地球自转引起的哥氏加速度，为由载体运动引起的对地向心加速度，gn为重力加速度，为由载体系转换为导航系的转换矩阵。
INS的位置(纬度、经度和高度)微分方程式如下
其中，RM和RN分别为子午圈和卯酉圈主曲率半径，L、λ和h分别为惯导解算的纬度、经度和高度，vIe、vIn和vIu分别为惯导解算的东向速度、北向速度和天向速度。
步骤2，建立三自由度车辆动力学模型，以方向盘转角和纵向力为控制输入量，利用四阶龙格库塔法实时解算载体速度；
VDM利用载体自身的输入量，解算出载体的部分运动参数，如速度、角速率等。VDM解算结果辅助校正INS的输出，从而构成INS/VDM的组合导航系统，提高车辆导航精度。：建立横向、纵向、横摆三自由度非线性车辆动力学模型，忽略左右轮差异，模型等效简化为前、后车轮分别集中在车辆前、后轴中点而构成的一个自行车模型，xnoyn为导航坐标系，xn轴向东，yn轴向北；xboyb为固定在车辆质心上的载体坐标系，xb轴与载体横轴重合，向右为正，yb轴与载体纵轴重合，向前为正。
据牛顿力学，车辆的动力学模型为：
其中，和分别为车辆的横向速度、纵向速度和横摆角速度；m和Iz分别为车辆的质量、绕竖轴的转动惯量；a、b分别为汽车前轮和后轮轮轴中心到质心的距离；δf为前轮转向角，由方向盘转角传感器测得的方向盘转角δ除以从方向盘到前轮的转向传动比来确定；Cd为空气阻力系数；Af为车辆前向面积；ρa为空气密度；Ftf、Ftr分别为作用在单个前轮和后轮上的纵向力；Fsf、Fsr分别为作用在单个前轮和后轮上的横向力；对于行驶在一般道路交通环境下的车辆，将作用在各轮上的侧向力表示为：
式中，Cαf、Cαr分别为前、后轮胎的侧偏刚度；αf、αr分别为前、后轮胎的侧偏角，近似表示为：
将式(5)、(6)代入式(4)，并考虑到，并考虑到δf通常是小角度，经整理后得：
利用方向盘转角传感器和车轮力传感器信息得到车辆动力学模型的控制输入向量U＝[δf Ftf Ftr]T，通过四阶龙格库塔法求解上式得载体坐标系下车辆的横向速度纵向速度和横摆角速度根据惯导解算的姿态矩阵，得到导航坐标系下由动力学模型计算的速度：
步骤3，以惯导误差方程为状态方程，动力学模型和惯导解算的速度差为观测量，设计CKF滤波器对组合导航系统进行状态估计；
CKF滤波器的误差状态变量包括3个姿态误差、2个速度误差、2个位置误差、3个陀螺常值漂移、2个加速度计常值零偏，即
即
CKF滤波器的状态方程为
式中，A为相应的状态转移矩阵，由INS误差方程构成；w为过程噪声；
将惯导和车辆动力学模型解算的东向速度之差、北向速度之差作为CKF滤波器的观测量，即观测向量为Z＝[vIe-vMe vIn-vMn]T，则系统的量测方程为：
Z＝HX+v
式中，H＝[02×3 I2×2 02×7]为系统量测矩阵，v为量测噪声；
对前述系统状态方程和量测方程进行离散化，得到k时刻的离散化系统状态模型为：
式中，F＝exp(A×ΔT)为离散化的系统状态转移矩阵，ΔT为采样时间；wk-1为均值为0，方差为Qk-1的高斯白噪声，vk为均值为0，方差为Rk的高斯白噪声且wk-1和vk相互独立。
CKF滤波器的状态估计包括时间更新和量测更新两步：
(1)时间更新
构造容积点
式中，Pk-1/k-1、为k-1时刻协方差和状态估计值；m＝2n，n是状态向量X维数；[1]i是点集[1]的第i列；
计算经状态方程传播容积点：
一步预测状态和误差协方差阵：
(2)量测更新
构造容积点：
计算经量测方程传播容积点：
Zi,k/k-1＝HXi,k/k-1
计算观测量预测值：
估计新息自协方差矩阵：
估计互协方差矩阵：
CKF滤波器增益矩阵：
估计k时刻的状态量：
估计k时刻状态误差协方差阵：
步骤4，CKF估计得到的位置、速度和姿态误差对捷联惯导解算结果进行输出校正，陀螺和加表误差对惯导进行反馈校正；
对微惯性器件输出的角增量比力进行反馈校正得：
对INS解算的纬度L、经度λ进行输出校正得组合导航位置：
Lout＝L-δL
λout＝λ-δλ
对惯导解算的姿态进行输出校正得组合导航姿态：
θout＝θ-δθ
γout＝γ-δγ
对惯导解算的东向速度vIe、北向速度vIn进行输出校正得组合导航速度：
Veout＝vIe-δvx
Vnout＝vIn-δvy
其中，θ表示俯仰角，γ表示横滚角，表示航向角。
以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (9)

1.一种基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，其特征在于包括如下步骤：
步骤1，根据微惯性器件输出的角增量和比力，利用惯导数值更新算法解算车辆的姿态、速度和位置；
步骤2，建立三自由度车辆动力学模型，以方向盘转角和纵向力为控制输入量，利用四阶龙格库塔法实时解算载体速度；
步骤3，以惯导误差方程为状态方程，动力学模型和惯导解算的速度差为观测量，设计CKF滤波器对组合导航系统进行状态估计；
步骤4，CKF估计得到的位置、速度和姿态误差对捷联惯导解算结果进行输出校正，陀螺和加表误差对惯导进行反馈校正。
2.如权利要求1所述的基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，其特征在于：所述步骤1的详细内容是：利用六轴加速度计和陀螺仪测量载体的角增量和比力，解算出载体当前时刻的导航参数，导航坐标系n采用东北天地理坐标系，载体坐标系b采用右前上坐标系；惯导数值更新算法包含姿态微分方程、比力微分方程和位置微分方程。
3.如权利要求2所述的基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，其特征在于：所述姿态微分方程的表达式为：
<mrow> <mover> <mi>Q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mrow> <mo>(</mo> <msubsup> <mi>w</mi> <mrow> <mi>n</mi> <mi>b</mi> </mrow> <mi>b</mi> </msubsup> <mo>&amp;times;</mo> <mo>)</mo> </mrow> <mi>Q</mi> </mrow>
<mrow> <msubsup> <mi>w</mi> <mrow> <mi>n</mi> <mi>b</mi> </mrow> <mi>b</mi> </msubsup> <mo>=</mo> <msubsup> <mi>w</mi> <mrow> <mi>i</mi> <mi>b</mi> </mrow> <mi>b</mi> </msubsup> <mo>-</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <msubsup> <mi>w</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> <mi>n</mi> </msubsup> <mo>+</mo> <msubsup> <mi>w</mi> <mrow> <mi>e</mi> <mi>n</mi> </mrow> <mi>n</mi> </msubsup> <mo>)</mo> </mrow> </mrow>
其中，Q为姿态四元数，为载体系相对于导航系的角速度，为陀螺输出的载体系相对于惯性系的角速度，为地球自转引起的导航系旋转角速度，为载体运动产生的位移角速度，为姿态旋转矩阵。
4.如权利要求2所述的基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，其特征在于：所述比力微分方程的表达式为：
<mrow> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>I</mi> </msub> <mo>=</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <msubsup> <mi>f</mi> <mrow> <mi>s</mi> <mi>f</mi> </mrow> <mi>b</mi> </msubsup> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> <mi>n</mi> </msubsup> <mo>+</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>e</mi> <mi>n</mi> </mrow> <mi>n</mi> </msubsup> <mo>)</mo> </mrow> <mo>&amp;times;</mo> <msub> <mi>v</mi> <mi>I</mi> </msub> <mo>+</mo> <msup> <mi>g</mi> <mi>n</mi> </msup> </mrow>
其中，vI＝[vIe vIn vIu]T为惯导解算的东向、北向和天向速度，为加速度计测量的比力，为由载体运动和地球自转引起的哥氏加速度，为由载体运动引起的对地向心加速度，gn为重力加速度，为由载体系转换为导航系的转换矩阵。
5.如权利要求2所述的基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，其特征在于：所述位置微分方程的表达式为：
<mrow> <mover> <mi>L</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mfrac> <msub> <mi>v</mi> <mrow> <mi>I</mi> <mi>n</mi> </mrow> </msub> <mrow> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> </mrow>
<mrow> <mover> <mi>&amp;lambda;</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mfrac> <mrow> <msub> <mi>v</mi> <mrow> <mi>I</mi> <mi>e</mi> </mrow> </msub> <mi>sec</mi> <mi> </mi> <mi>L</mi> </mrow> <mrow> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> </mrow>
<mrow> <mover> <mi>h</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>I</mi> <mi>u</mi> </mrow> </msub> </mrow>
其中，RM和RN分别为子午圈和卯酉圈主曲率半径，L、λ和h分别为惯导解算的纬度、经度和高度，vIe、vIn和vIu分别为惯导解算的东向速度、北向速度和天向速度。
6.如权利要求1所述的基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，其特征在于：所述步骤2的详细内容是：建立横向、纵向、横摆三自由度非线性车辆动力学模型，忽略左右轮差异，模型等效简化为前、后车轮分别集中在车辆前、后轴中点而构成的一个自行车模型，xnoyn为导航坐标系，xn轴向东，yn轴向北；xboyb为固定在车辆质心上的载体坐标系，xb轴与载体横轴重合，向右为正，yb轴与载体纵轴重合，向前为正；
根据牛顿力学，车辆的动力学模型为：
<mrow> <mi>m</mi> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>x</mi> </mrow> </msub> </msub> <mo>=</mo> <msub> <mi>mv</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>y</mi> </mrow> </msub> </msub> <msub> <mi>w</mi> <msub> <mi>M</mi> <mi>z</mi> </msub> </msub> <mo>-</mo> <mn>2</mn> <msub> <mi>F</mi> <mrow> <mi>t</mi> <mi>f</mi> </mrow> </msub> <msub> <mi>sin&amp;delta;</mi> <mi>f</mi> </msub> <mo>-</mo> <mn>2</mn> <msub> <mi>F</mi> <mrow> <mi>s</mi> <mi>f</mi> </mrow> </msub> <msub> <mi>cos&amp;delta;</mi> <mi>f</mi> </msub> <mo>-</mo> <mn>2</mn> <msub> <mi>F</mi> <mrow> <mi>s</mi> <mi>r</mi> </mrow> </msub> </mrow>
<mrow> <mi>m</mi> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>y</mi> </mrow> </msub> </msub> <mo>=</mo> <mo>-</mo> <msub> <mi>mv</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>x</mi> </mrow> </msub> </msub> <msub> <mi>w</mi> <msub> <mi>M</mi> <mi>z</mi> </msub> </msub> <mo>+</mo> <mn>2</mn> <msub> <mi>F</mi> <mrow> <mi>t</mi> <mi>f</mi> </mrow> </msub> <msub> <mi>cos&amp;delta;</mi> <mi>f</mi> </msub> <mo>-</mo> <mn>2</mn> <msub> <mi>F</mi> <mrow> <mi>s</mi> <mi>f</mi> </mrow> </msub> <msub> <mi>sin&amp;delta;</mi> <mi>f</mi> </msub> <mo>+</mo> <mn>2</mn> <msub> <mi>F</mi> <mrow> <mi>t</mi> <mi>r</mi> </mrow> </msub> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msub> <mi>C</mi> <mi>d</mi> </msub> <msub> <mi>A</mi> <mi>f</mi> </msub> <msub> <mi>&amp;rho;</mi> <mi>a</mi> </msub> <msubsup> <mi>v</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>y</mi> </mrow> </msub> <mn>2</mn> </msubsup> </mrow>
<mrow> <msub> <mi>I</mi> <mi>z</mi> </msub> <msub> <mover> <mi>w</mi> <mo>&amp;CenterDot;</mo> </mover> <msub> <mi>M</mi> <mi>z</mi> </msub> </msub> <mo>=</mo> <mn>2</mn> <msub> <mi>aF</mi> <mrow> <mi>t</mi> <mi>f</mi> </mrow> </msub> <msub> <mi>sin&amp;delta;</mi> <mi>f</mi> </msub> <mo>+</mo> <mn>2</mn> <msub> <mi>aF</mi> <mrow> <mi>s</mi> <mi>f</mi> </mrow> </msub> <msub> <mi>cos&amp;delta;</mi> <mi>f</mi> </msub> <mo>-</mo> <mn>2</mn> <msub> <mi>bF</mi> <mrow> <mi>s</mi> <mi>r</mi> </mrow> </msub> </mrow>
其中，和分别为车辆的横向速度、纵向速度和横摆角速度；m和Iz分别为车辆的质量、绕竖轴的转动惯量；a、b分别为汽车前轮和后轮轮轴中心到质心的距离；δf为前轮转向角，由方向盘转角传感器测得的方向盘转角δ除以从方向盘到前轮的转向传动比来确定；Cd为空气阻力系数；Af为车辆前向面积；ρa为空气密度；Ftf、Ftr分别为作用在单个前轮和后轮上的纵向力；Fsf、Fsr分别为作用在单个前轮和后轮上的横向力；对于行驶在一般道路交通环境下的车辆，将作用在各轮上的侧向力表示为：
Fsf＝Cαfαf
Fsr＝Cαrαr
式中，Cαf、Cαr分别为前、后轮胎的侧偏刚度；αf、αr分别为前、后轮胎的侧偏角，近似表示为：
<mrow> <msub> <mi>&amp;alpha;</mi> <mi>f</mi> </msub> <mo>=</mo> <msub> <mi>&amp;delta;</mi> <mi>f</mi> </msub> <mo>-</mo> <mfrac> <mrow> <msub> <mi>aw</mi> <msub> <mi>M</mi> <mi>z</mi> </msub> </msub> <mo>-</mo> <msub> <mi>v</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>x</mi> </mrow> </msub> </msub> </mrow> <msub> <mi>v</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>y</mi> </mrow> </msub> </msub> </mfrac> </mrow>
<mrow> <msub> <mi>&amp;alpha;</mi> <mi>r</mi> </msub> <mo>=</mo> <mfrac> <mrow> <msub> <mi>bw</mi> <msub> <mi>M</mi> <mi>z</mi> </msub> </msub> <mo>+</mo> <msub> <mi>v</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>x</mi> </mrow> </msub> </msub> </mrow> <msub> <mi>v</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>y</mi> </mrow> </msub> </msub> </mfrac> </mrow>
将侧向力表达式、侧偏角表达式代入动力学模型表达式，并考虑到δf通常是小角度，经整理后得：
<mrow> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>x</mi> </mrow> </msub> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mi>m</mi> </mfrac> <mo>&amp;lsqb;</mo> <msub> <mi>mv</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>y</mi> </mrow> </msub> </msub> <msub> <mi>w</mi> <msub> <mi>M</mi> <mi>z</mi> </msub> </msub> <mo>-</mo> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>&amp;delta;</mi> <mi>f</mi> </msub> <mo>-</mo> <mfrac> <mrow> <msub> <mi>aw</mi> <msub> <mi>M</mi> <mi>z</mi> </msub> </msub> <mo>-</mo> <msub> <mi>v</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>x</mi> </mrow> </msub> </msub> </mrow> <msub> <mi>v</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>y</mi> </mrow> </msub> </msub> </mfrac> <mo>)</mo> </mrow> <msub> <mi>C</mi> <mrow> <mi>&amp;alpha;</mi> <mi>f</mi> </mrow> </msub> <mo>-</mo> <mn>2</mn> <msub> <mi>C</mi> <mrow> <mi>&amp;alpha;</mi> <mi>r</mi> </mrow> </msub> <mfrac> <mrow> <msub> <mi>bw</mi> <msub> <mi>M</mi> <mi>z</mi> </msub> </msub> <mo>+</mo> <msub> <mi>v</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>x</mi> </mrow> </msub> </msub> </mrow> <msub> <mi>v</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>y</mi> </mrow> </msub> </msub> </mfrac> <mo>&amp;rsqb;</mo> <mo>-</mo> <mfrac> <mn>2</mn> <mi>m</mi> </mfrac> <msub> <mi>F</mi> <mrow> <mi>t</mi> <mi>f</mi> </mrow> </msub> <msub> <mi>&amp;delta;</mi> <mi>f</mi> </msub> </mrow>
<mrow> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>y</mi> </mrow> </msub> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mi>m</mi> </mfrac> <mo>&amp;lsqb;</mo> <mo>-</mo> <msub> <mi>mv</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>x</mi> </mrow> </msub> </msub> <msub> <mi>w</mi> <msub> <mi>M</mi> <mi>z</mi> </msub> </msub> <mo>+</mo> <mn>2</mn> <mfrac> <mrow> <msub> <mi>aw</mi> <msub> <mi>M</mi> <mi>z</mi> </msub> </msub> <mo>-</mo> <msub> <mi>v</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>x</mi> </mrow> </msub> </msub> </mrow> <msub> <mi>v</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>y</mi> </mrow> </msub> </msub> </mfrac> <msub> <mi>C</mi> <mrow> <mi>&amp;alpha;</mi> <mi>f</mi> </mrow> </msub> <msub> <mi>&amp;delta;</mi> <mi>f</mi> </msub> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msub> <mi>C</mi> <mi>d</mi> </msub> <msub> <mi>A</mi> <mi>f</mi> </msub> <msub> <mi>&amp;rho;</mi> <mi>a</mi> </msub> <msubsup> <mi>v</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>y</mi> </mrow> </msub> <mn>2</mn> </msubsup> <mo>&amp;rsqb;</mo> <mo>+</mo> <mfrac> <mn>2</mn> <mi>m</mi> </mfrac> <mrow> <mo>(</mo> <msub> <mi>F</mi> <mrow> <mi>t</mi> <mi>f</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>F</mi> <mrow> <mi>t</mi> <mi>r</mi> </mrow> </msub> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mover> <mi>w</mi> <mo>&amp;CenterDot;</mo> </mover> <msub> <mi>M</mi> <mi>z</mi> </msub> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <msub> <mi>I</mi> <mi>z</mi> </msub> </mfrac> <mo>&amp;lsqb;</mo> <mn>2</mn> <mi>a</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;delta;</mi> <mi>f</mi> </msub> <mo>-</mo> <mfrac> <mrow> <mo>-</mo> <msub> <mi>v</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>x</mi> </mrow> </msub> </msub> <mo>+</mo> <msub> <mi>aw</mi> <msub> <mi>M</mi> <mi>z</mi> </msub> </msub> </mrow> <msub> <mi>v</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>y</mi> </mrow> </msub> </msub> </mfrac> <mo>)</mo> </mrow> <msub> <mi>C</mi> <mrow> <mi>&amp;alpha;</mi> <mi>f</mi> </mrow> </msub> <mo>-</mo> <mn>2</mn> <msub> <mi>bC</mi> <mrow> <mi>&amp;alpha;</mi> <mi>r</mi> </mrow> </msub> <mfrac> <mrow> <msub> <mi>bw</mi> <msub> <mi>M</mi> <mi>z</mi> </msub> </msub> <mo>+</mo> <msub> <mi>v</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>x</mi> </mrow> </msub> </msub> </mrow> <msub> <mi>v</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>y</mi> </mrow> </msub> </msub> </mfrac> <mo>&amp;rsqb;</mo> <mo>+</mo> <mfrac> <mrow> <mn>2</mn> <mi>a</mi> </mrow> <msub> <mi>I</mi> <mi>z</mi> </msub> </mfrac> <msub> <mi>F</mi> <mrow> <mi>t</mi> <mi>f</mi> </mrow> </msub> <msub> <mi>&amp;delta;</mi> <mi>f</mi> </msub> </mrow>
利用方向盘转角传感器和车轮力传感器信息得到车辆动力学模型的控制输入向量U＝[δf Ftf Ftr]T，通过四阶龙格库塔法求解上式得载体坐标系下车辆的横向速度纵向速度和横摆角速度根据惯导解算的姿态矩阵，得到导航坐标系下由动力学模型计算的速度：
<mrow> <msub> <mi>v</mi> <mi>M</mi> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>v</mi> <mrow> <mi>M</mi> <mi>e</mi> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mrow> <mi>M</mi> <mi>n</mi> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mrow> <mi>M</mi> <mi>u</mi> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>v</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>x</mi> </mrow> </msub> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <msub> <mi>M</mi> <mrow> <mi>b</mi> <mi>y</mi> </mrow> </msub> </msub> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow>
7.如权利要求1所述的基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，其特征在于：所述步骤3中，CKF滤波器的误差状态变量包括3个姿态误差、2个速度误差、2个位置误差、3个陀螺常值漂移、2个加速度计常值零偏，即
CKF滤波器的状态方程为
<mrow> <mover> <mi>X</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mi>A</mi> <mi>X</mi> <mo>+</mo> <mi>w</mi> </mrow>
式中，A为相应的状态转移矩阵，由INS误差方程构成；w为过程噪声；
将惯导和车辆动力学模型解算的东向速度之差、北向速度之差作为CKF滤波器的观测量，即观测向量为Z＝[vIe-vMe vIn-vMn]T，则系统的量测方程为：
Z＝HX+v
式中，H＝[02×3 I2×2 02×7]为系统量测矩阵，v为量测噪声；
对前述系统状态方程和量测方程进行离散化，得到k时刻的离散化系统状态模型为：
Xk＝FXk-1+wk-1
Zk＝HXk+vk
式中，F＝exp(A×ΔT)为离散化的系统状态转移矩阵，ΔT为采样时间；wk-1为均值为0，方差为Qk-1的高斯白噪声，vk为均值为0，方差为Rk的高斯白噪声且wk-1和vk相互独立。
8.如权利要求1所述的基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，其特征在于：所述步骤3中，CKF滤波器的状态估计包括时间更新和量测更新两步：
(1)时间更新
构造容积点
<mrow> <msub> <mi>X</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msqrt> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msqrt> <msub> <mi>&amp;xi;</mi> <mi>i</mi> </msub> <mo>+</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mo>...</mo> <mo>,</mo> <mi>m</mi> </mrow>
式中，Pk-1/k-1、为k-1时刻协方差和状态估计值；m＝2n，n是状态向量X维数；[1]i是点集[1]的第i列；
计算经状态方程传播容积点：
<mrow> <msubsup> <mi>X</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>*</mo> </msubsup> <mo>=</mo> <msub> <mi>FX</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow>
一步预测状态和误差协方差阵：
<mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mi>m</mi> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>m</mi> </munderover> <msubsup> <mi>X</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>*</mo> </msubsup> </mrow>
<mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mi>m</mi> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>m</mi> </munderover> <msubsup> <mi>X</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>*</mo> </msubsup> <msubsup> <mi>X</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mo>*</mo> <mi>T</mi> </mrow> </msubsup> <mo>-</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow>
(2)量测更新
构造容积点：
<mrow> <msub> <mi>X</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msqrt> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msqrt> <msub> <mi>&amp;xi;</mi> <mi>i</mi> </msub> <mo>+</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow>
计算经量测方程传播容积点：
Zi,k/k-1＝HXi,k/k-1
计算观测量预测值：
<mrow> <msub> <mover> <mi>Z</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mi>m</mi> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>m</mi> </munderover> <msub> <mi>Z</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow>
估计新息自协方差矩阵：
<mrow> <msub> <mi>P</mi> <mrow> <mi>z</mi> <mi>z</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mi>m</mi> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>m</mi> </munderover> <msub> <mi>Z</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>Z</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>-</mo> <msub> <mover> <mi>Z</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mover> <mi>Z</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>R</mi> <mi>k</mi> </msub> </mrow>
估计互协方差矩阵：
<mrow> <msub> <mi>P</mi> <mrow> <mi>x</mi> <mi>z</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mi>m</mi> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>m</mi> </munderover> <msub> <mi>X</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>Z</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>-</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mover> <mi>Z</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> </mrow>
CKF滤波器增益矩阵：
<mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>P</mi> <mrow> <mi>x</mi> <mi>z</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msubsup> <mi>P</mi> <mrow> <mi>z</mi> <mi>z</mi> <mo>,</mo> <mi>k</mi> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> </mrow>
估计k时刻的状态量：
<mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mover> <mi>Z</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow>
估计k时刻状态误差协方差阵：
<mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>P</mi> <mrow> <mi>z</mi> <mi>z</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>.</mo> </mrow>
9.如权利要求1所述的基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，其特征在于：所述步骤4的详细内容是；对微惯性器件输出的角增量比力进行反馈校正得：
<mrow> <msubsup> <mi>w</mi> <mrow> <mi>i</mi> <mi>b</mi> </mrow> <mrow> <mo>&amp;prime;</mo> <mi>b</mi> </mrow> </msubsup> <mo>=</mo> <msubsup> <mi>w</mi> <mrow> <mi>i</mi> <mi>b</mi> </mrow> <mi>b</mi> </msubsup> <mo>-</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>b</mi> </msubsup> <msup> <mrow> <mo>&amp;lsqb;</mo> <msub> <mi>&amp;delta;&amp;epsiv;</mi> <mi>x</mi> </msub> <mo>,</mo> <msub> <mi>&amp;delta;&amp;epsiv;</mi> <mi>y</mi> </msub> <mo>,</mo> <msub> <mi>&amp;delta;&amp;epsiv;</mi> <mi>z</mi> </msub> <mo>&amp;rsqb;</mo> </mrow> <mi>T</mi> </msup> </mrow>
<mrow> <msubsup> <mi>f</mi> <mrow> <mi>s</mi> <mi>f</mi> </mrow> <mrow> <mo>&amp;prime;</mo> <mi>b</mi> </mrow> </msubsup> <mo>=</mo> <msubsup> <mi>f</mi> <mrow> <mi>s</mi> <mi>f</mi> </mrow> <mi>b</mi> </msubsup> <mo>-</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>b</mi> </msubsup> <msup> <mrow> <mo>&amp;lsqb;</mo> <mi>&amp;delta;</mi> <msub> <mo>&amp;dtri;</mo> <mi>x</mi> </msub> <mo>,</mo> <mi>&amp;delta;</mi> <msub> <mo>&amp;dtri;</mo> <mi>y</mi> </msub> <mo>,</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> </mrow> <mi>T</mi> </msup> </mrow>
对INS解算的纬度L、经度λ进行输出校正得组合导航位置：
Lout＝L-δL
λout＝λ-δλ
对惯导解算的姿态进行输出校正得组合导航姿态：
θout＝θ-δθ
γout＝γ-δγ
对惯导解算的东向速度vIe、北向速度vIn进行输出校正得组合导航速度：
Veout＝vIe-δvx
Vnout＝vIn-δvy
其中，θ表示俯仰角，γ表示横滚角，表示航向角。
CN201710253845.5A 2017-04-18 2017-04-18 基于ckf滤波的车辆动力学模型辅助惯导组合导航方法 Pending CN107144284A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710253845.5A CN107144284A (zh) 2017-04-18 2017-04-18 基于ckf滤波的车辆动力学模型辅助惯导组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710253845.5A CN107144284A (zh) 2017-04-18 2017-04-18 基于ckf滤波的车辆动力学模型辅助惯导组合导航方法

Publications (1)

Publication Number Publication Date
CN107144284A true CN107144284A (zh) 2017-09-08

Family

ID=59775310

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710253845.5A Pending CN107144284A (zh) 2017-04-18 2017-04-18 基于ckf滤波的车辆动力学模型辅助惯导组合导航方法

Country Status (1)

Country Link
CN (1) CN107144284A (zh)

Cited By (23)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107561943A (zh) * 2017-09-13 2018-01-09 青岛理工大学 一种汽车最速操纵逆动力学数学模型的建立方法
CN108394413A (zh) * 2018-01-26 2018-08-14 辽宁工业大学 一种四轮独立驱动与转向的电动汽车状态与参数校正方法
CN108592911A (zh) * 2018-03-23 2018-09-28 南京航空航天大学 一种四旋翼飞行器动力学模型/机载传感器组合导航方法
CN108981733A (zh) * 2018-04-26 2018-12-11 杭州中恒云能源互联网技术有限公司 一种电动汽车充电导航系统的速度预测方法
CN109000640A (zh) * 2018-05-25 2018-12-14 东南大学 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法
CN109466558A (zh) * 2018-10-26 2019-03-15 重庆邮电大学 一种基于ekf和bp神经网络的路面附着系数估计方法
CN109827572A (zh) * 2019-03-12 2019-05-31 北京星网宇达科技股份有限公司 一种检测车位置预测的方法及装置
CN109900295A (zh) * 2017-12-11 2019-06-18 上海交通大学 基于自主式传感器的车辆运动状态的检测方法及系统
CN110244293A (zh) * 2018-03-07 2019-09-17 德尔福技术有限公司 用于确定移动物体的指向角的方法和系统
CN111174974A (zh) * 2020-02-17 2020-05-19 燕山大学 一种车辆悬架升沉测量方法及系统
CN111238467A (zh) * 2020-02-07 2020-06-05 西北工业大学 一种仿生偏振光辅助的无人作战飞行器自主导航方法
CN111532254A (zh) * 2019-02-07 2020-08-14 通用汽车环球科技运作有限责任公司 用于控制自主车辆的系统和方法
CN111750897A (zh) * 2020-07-03 2020-10-09 南京晓庄学院 一种基于龙贝格观测器的横摆角速度陀螺仪偏差估计方法
CN112014122A (zh) * 2020-07-21 2020-12-01 无锡卡尔曼导航技术有限公司 一种车轮转角测量装置及运行方法
CN112595342A (zh) * 2020-11-13 2021-04-02 东南大学 一种基于窗循环的加速度计零偏迭代估计方法及系统
CN113008229A (zh) * 2021-02-26 2021-06-22 南京理工大学 一种基于低成本车载传感器的分布式自主组合导航方法
CN113267183A (zh) * 2021-03-25 2021-08-17 浙江大学 一种多加速度计惯导系统的组合导航方法
CN113744558A (zh) * 2021-09-06 2021-12-03 西安建筑科技大学 基于mems传感器的车位级停车惯性导航系统及导航方法
CN113821891A (zh) * 2021-09-30 2021-12-21 上海电机学院 一种全向移动机器人动力学模型的建模方法
CN113916220A (zh) * 2021-08-30 2022-01-11 西北工业大学 一种具有协方差反馈控制的动态自适应导航定位方法
CN114234972A (zh) * 2022-03-01 2022-03-25 长沙金维信息技术有限公司 用于惯性导航系统的惯性导航方法
CN114394130A (zh) * 2021-12-27 2022-04-26 中国矿业大学 一种煤矿辅助运输车辆定位方法及定位系统
CN114659488A (zh) * 2022-02-08 2022-06-24 东风悦享科技有限公司 一种基于误差卡尔曼滤波的高动态车辆姿态估计方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101706281A (zh) * 2009-11-13 2010-05-12 南京航空航天大学 惯性/天文/卫星高精度组合导航系统及其导航方法
KR20110108208A (ko) * 2010-03-26 2011-10-05 위드로봇 주식회사 주행 모드 판별법, 상기 주행 모드 판별을 이용한 항법 시스템 및 그 제공방법
CN102529976A (zh) * 2011-12-15 2012-07-04 东南大学 一种基于滑模观测器的车辆运行状态非线性鲁棒估计方法
CN102654406A (zh) * 2012-04-11 2012-09-05 哈尔滨工程大学 基于非线性预测滤波与求容积卡尔曼滤波相结合的动基座初始对准方法
CN103727941A (zh) * 2014-01-06 2014-04-16 东南大学 基于载体系速度匹配的容积卡尔曼非线性组合导航方法
CN103759733A (zh) * 2013-10-29 2014-04-30 南昌大学 基于联邦滤波的ins/vkm/vdm车载导航系统
CN104034332A (zh) * 2014-06-20 2014-09-10 东南大学 一种基于卡尔曼滤波的救援清障车姿态角估计方法
WO2016198958A1 (en) * 2015-06-10 2016-12-15 Ecole Polytechnique Federale De Lausanne (Epfl) Epfl-Tto Autonomous and non-autonomous navigation system for unmanned vehicles based on vehicle dynamic model
CN106515740A (zh) * 2016-11-14 2017-03-22 江苏大学 基于icdkf的分布式电驱动汽车行驶状态参数估计算法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101706281A (zh) * 2009-11-13 2010-05-12 南京航空航天大学 惯性/天文/卫星高精度组合导航系统及其导航方法
KR20110108208A (ko) * 2010-03-26 2011-10-05 위드로봇 주식회사 주행 모드 판별법, 상기 주행 모드 판별을 이용한 항법 시스템 및 그 제공방법
CN102529976A (zh) * 2011-12-15 2012-07-04 东南大学 一种基于滑模观测器的车辆运行状态非线性鲁棒估计方法
CN102654406A (zh) * 2012-04-11 2012-09-05 哈尔滨工程大学 基于非线性预测滤波与求容积卡尔曼滤波相结合的动基座初始对准方法
CN103759733A (zh) * 2013-10-29 2014-04-30 南昌大学 基于联邦滤波的ins/vkm/vdm车载导航系统
CN103727941A (zh) * 2014-01-06 2014-04-16 东南大学 基于载体系速度匹配的容积卡尔曼非线性组合导航方法
CN104034332A (zh) * 2014-06-20 2014-09-10 东南大学 一种基于卡尔曼滤波的救援清障车姿态角估计方法
WO2016198958A1 (en) * 2015-06-10 2016-12-15 Ecole Polytechnique Federale De Lausanne (Epfl) Epfl-Tto Autonomous and non-autonomous navigation system for unmanned vehicles based on vehicle dynamic model
CN106515740A (zh) * 2016-11-14 2017-03-22 江苏大学 基于icdkf的分布式电驱动汽车行驶状态参数估计算法

Non-Patent Citations (7)

* Cited by examiner, † Cited by third party
Title
PHILIPP CROCOLL等: "《Unified Model Technique for Inertial Navigation》", 《NAVIGATION》 *
李烨等: "飞行器动力学信息辅助MEMS惯导系统", 《系统工程与电子技术》 *
王美玲等: "一种车辆模型辅助的MEMS-SINS导航方法", 《中国惯性技术学报》 *
秦永元等: "《卡尔曼滤波与组合导航原理》", 30 June 2015 *
褚文博等: "分布式电驱动车辆的无味粒子滤波状态参数联合观测", 《机械工程学报》 *
马昕等: "车辆动态数学模型辅助的惯性导航系统 ", 《山东大学学报(工学版)》 *
马昕等: "车辆动态数学模型辅助的惯性导航系统", 《山东大学学报(工学版)》 *

Cited By (37)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107561943A (zh) * 2017-09-13 2018-01-09 青岛理工大学 一种汽车最速操纵逆动力学数学模型的建立方法
CN109900295A (zh) * 2017-12-11 2019-06-18 上海交通大学 基于自主式传感器的车辆运动状态的检测方法及系统
CN108394413A (zh) * 2018-01-26 2018-08-14 辽宁工业大学 一种四轮独立驱动与转向的电动汽车状态与参数校正方法
CN108394413B (zh) * 2018-01-26 2019-08-23 辽宁工业大学 一种四轮独立驱动与转向的电动汽车状态与参数校正方法
CN110244293A (zh) * 2018-03-07 2019-09-17 德尔福技术有限公司 用于确定移动物体的指向角的方法和系统
CN108592911A (zh) * 2018-03-23 2018-09-28 南京航空航天大学 一种四旋翼飞行器动力学模型/机载传感器组合导航方法
CN108592911B (zh) * 2018-03-23 2021-09-17 南京航空航天大学 一种四旋翼飞行器动力学模型/机载传感器组合导航方法
CN108981733B (zh) * 2018-04-26 2020-11-24 杭州中恒云能源互联网技术有限公司 一种电动汽车充电导航系统的速度预测方法
CN108981733A (zh) * 2018-04-26 2018-12-11 杭州中恒云能源互联网技术有限公司 一种电动汽车充电导航系统的速度预测方法
CN109000640A (zh) * 2018-05-25 2018-12-14 东南大学 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法
CN109000640B (zh) * 2018-05-25 2021-09-28 东南大学 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法
CN109466558A (zh) * 2018-10-26 2019-03-15 重庆邮电大学 一种基于ekf和bp神经网络的路面附着系数估计方法
CN111532254A (zh) * 2019-02-07 2020-08-14 通用汽车环球科技运作有限责任公司 用于控制自主车辆的系统和方法
CN111532254B (zh) * 2019-02-07 2023-04-11 通用汽车环球科技运作有限责任公司 用于控制自主车辆的系统和方法
CN109827572A (zh) * 2019-03-12 2019-05-31 北京星网宇达科技股份有限公司 一种检测车位置预测的方法及装置
CN111238467A (zh) * 2020-02-07 2020-06-05 西北工业大学 一种仿生偏振光辅助的无人作战飞行器自主导航方法
CN111238467B (zh) * 2020-02-07 2021-09-03 西北工业大学 一种仿生偏振光辅助的无人作战飞行器自主导航方法
CN111174974A (zh) * 2020-02-17 2020-05-19 燕山大学 一种车辆悬架升沉测量方法及系统
CN111750897A (zh) * 2020-07-03 2020-10-09 南京晓庄学院 一种基于龙贝格观测器的横摆角速度陀螺仪偏差估计方法
CN112014122A (zh) * 2020-07-21 2020-12-01 无锡卡尔曼导航技术有限公司 一种车轮转角测量装置及运行方法
CN112595342B (zh) * 2020-11-13 2022-04-19 东南大学 一种基于窗循环的加速度计零偏迭代估计方法及系统
CN112595342A (zh) * 2020-11-13 2021-04-02 东南大学 一种基于窗循环的加速度计零偏迭代估计方法及系统
CN113008229A (zh) * 2021-02-26 2021-06-22 南京理工大学 一种基于低成本车载传感器的分布式自主组合导航方法
CN113008229B (zh) * 2021-02-26 2024-04-05 南京理工大学 一种基于低成本车载传感器的分布式自主组合导航方法
CN113267183A (zh) * 2021-03-25 2021-08-17 浙江大学 一种多加速度计惯导系统的组合导航方法
CN113267183B (zh) * 2021-03-25 2023-01-06 浙江大学 一种多加速度计惯导系统的组合导航方法
CN113916220A (zh) * 2021-08-30 2022-01-11 西北工业大学 一种具有协方差反馈控制的动态自适应导航定位方法
CN113916220B (zh) * 2021-08-30 2023-06-23 西北工业大学 一种具有协方差反馈控制的动态自适应导航定位方法
CN113744558A (zh) * 2021-09-06 2021-12-03 西安建筑科技大学 基于mems传感器的车位级停车惯性导航系统及导航方法
CN113821891A (zh) * 2021-09-30 2021-12-21 上海电机学院 一种全向移动机器人动力学模型的建模方法
CN113821891B (zh) * 2021-09-30 2023-10-13 上海电机学院 一种全向移动机器人动力学模型的建模方法
CN114394130A (zh) * 2021-12-27 2022-04-26 中国矿业大学 一种煤矿辅助运输车辆定位方法及定位系统
CN114394130B (zh) * 2021-12-27 2022-11-11 中国矿业大学 一种煤矿辅助运输车辆定位方法及定位系统
CN114659488A (zh) * 2022-02-08 2022-06-24 东风悦享科技有限公司 一种基于误差卡尔曼滤波的高动态车辆姿态估计方法
CN114659488B (zh) * 2022-02-08 2023-06-23 东风悦享科技有限公司 一种基于误差卡尔曼滤波的高动态车辆姿态估计方法
CN114234972A (zh) * 2022-03-01 2022-03-25 长沙金维信息技术有限公司 用于惯性导航系统的惯性导航方法
CN114234972B (zh) * 2022-03-01 2022-05-24 长沙金维信息技术有限公司 用于惯性导航系统的惯性导航方法

Similar Documents

Publication Publication Date Title
CN107144284A (zh) 基于ckf滤波的车辆动力学模型辅助惯导组合导航方法
CN104061899B (zh) 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法
CN105606094B (zh) 一种基于mems/gps组合系统的信息条件匹配滤波估计方法
CN109000640B (zh) 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法
CN105737823B (zh) 一种基于五阶ckf的gps/sins/cns组合导航方法
CN104697526A (zh) 用于农业机械的捷联惯导系统以及控制方法
CN106568449B (zh) 一种基于mems的车辆模型辅助和约束的gnss/ins组合导航方法
CN105698822B (zh) 基于反向姿态跟踪的自主式惯性导航行进间初始对准方法
CN112629538A (zh) 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
CN102809377A (zh) 飞行器惯性/气动模型组合导航方法
JP7036080B2 (ja) 慣性航法装置
CN102621570B (zh) 基于双全球定位和惯性测量的汽车动力学参数测量方法
JP2001221652A (ja) 自動車用ナビゲーション・システムのための慣性誘導装置及び方法
CN112504275B (zh) 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
CN104713555A (zh) 应用全天域中性点辅助定向的车辆自主导航方法
CN105318876A (zh) 一种惯性里程计组合高精度姿态测量方法
CN110285804B (zh) 基于相对运动模型约束的车辆协同导航方法
CN102830414A (zh) 一种基于sins/gps的组合导航方法
CN103759733B (zh) 基于联邦滤波的ins/vkm/vdm车载导航系统
CN104977002A (zh) 基于sins/双od的惯性组合导航系统及其导航方法
CN103217158A (zh) 一种提高车载sins/od组合导航精度的方法
Gao et al. An integrated land vehicle navigation system based on context awareness
Liu et al. Interacting multiple model UAV navigation algorithm based on a robust cubature Kalman filter
CN110095117A (zh) 一种无陀螺惯性量测系统与gps组合的导航方法
CN103616026A (zh) 一种基于h∞滤波的auv操纵模型辅助捷联惯导组合导航方法

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
RJ01 Rejection of invention patent application after publication

Application publication date: 20170908

RJ01 Rejection of invention patent application after publication