基于CKF滤波的车辆动力学模型辅助惯导组合导航方法技术

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

Inertial navigation integrated navigation method based on CKF filtering for vehicle dynamics model

The invention discloses a vehicle dynamics model of CKF filter Aided INS Integrated Navigation Based on the method comprises the following steps: according to the angle increment and more micro inertial components outputs, using the update algorithm of vehicle inertial navigation numerical attitude, velocity and position; the establishment of three degrees of freedom vehicle model, the steering wheel angle and vertical force as the input, using four order Runge Kutta method to calculate vehicle speed in real time; the SINS error equation as state equation, dynamics model and IMU speed difference measurements, CKF filter is designed to estimate the states of the integrated navigation system; CKF estimation of position, velocity and attitude error are obtained the strapdown inertial navigation solution results are output correction, gyro and accelerometer error feedback correction of ins. This method can improve the accuracy and reliability of vehicle navigation system, because the error of inertial navigation can not be maintained for long time.

【技术实现步骤摘要】
基于CKF滤波的车辆动力学模型辅助惯导组合导航方法
本专利技术属于车辆组合导航
，特别涉及一种基于CKF滤波的车辆动力学模型辅助惯导组合导航方法。
技术介绍
随着微惯性器件的迅速发展，惯性导航系统(inertialnavigationsystem，INS)可满足车辆导航系统低成本、微型化的需求。但INS会产生随时间积累的误差，无法长时间保持导航精度，故需要引入辅助传感器测量的信息与INS组成组合导航系统进行工作。其中最常见的是惯导/卫星组合导航系统。但在实际应用中，卫星导航系统(Globalnavigationsatellitesystem,GNSS)自主性差，易受干扰，GNSS信号容易受到阻隔，在GNSS失效阶段导航系统仍然相当于INS单独工作。这种情况下，如果保持长时间的导航精度，需要其它状态信息辅助惯导系统。而车辆动力学模型(Vehicledynamicmodel,VDM)对载体状态估计，不需依赖外部条件，具有较强的自主性和适用性，可以利用VDM有效地辅助INS进行导航。目前，基于VDM的车辆状态估计主要有两类，一类是以速度为已知量，利用非线性VDM对侧偏角等状态进行估计，该类方法对速度的精度要求较高。第二类是以轮胎力为已知量，基于扩展卡尔曼滤波器、一阶斯梯林插值滤波器、递推最小二乘法滤波器，估计车速、横摆角速度、质心侧偏角等状态。但第二类方法中将非线性模型简化为线性时会造成一定的误差，降低测量精度，并且VDM估计的状态信息没有用于辅助其他导航系统，具有一定的局限性。基于以上分析，现有的惯性导航技术精度较低，有待改进，本案由此产生。
技术实现思路
本专利技术的目的，在于提供一种基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，其针对惯导误差随时间积累，无法长时间保持导航精度的问题，可提高车辆导航系统的精确度和可靠性。为了达成上述目的，本专利技术的解决方案是：一种基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，包括如下步骤：步骤1，根据微惯性器件输出的角增量和比力，利用惯导数值更新算法解算车辆的姿态、速度和位置；步骤2，建立三自由度车辆动力学模型，以方向盘转角和纵向力为控制输入量，利用四阶龙格库塔法实时解算载体速度；步骤3，以惯导误差方程为状态方程，动力学模型和惯导解算的速度差为观测量，设计CKF滤波器对组合导航系统进行状态估计；步骤4，CKF估计得到的位置、速度和姿态误差对捷联惯导解算结果进行输出校正，陀螺和加表误差对惯导进行反馈校正。上述步骤1的详细内容是：利用六轴加速度计和陀螺仪测量载体的角增量和比力，解算出载体当前时刻的导航参数，导航坐标系n采用东北天地理坐标系，载体坐标系b采用右前上坐标系；惯导数值更新算法包含姿态微分方程、比力微分方程和位置微分方程。上述姿态微分方程的表达式为：其中，Q为姿态四元数，为载体系相对于导航系的角速度，为陀螺输出的载体系相对于惯性系的角速度，为地球自转引起的导航系旋转角速度，为载体运动产生的位移角速度，为姿态旋转矩阵。上述比力微分方程的表达式为：其中，vI＝[vIevInvIu]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αfFsr＝Cαrαr式中，Cαf、Cαr分别为前、后轮胎的侧偏刚度；αf、αr分别为前、后轮胎的侧偏角，近似表示为：将侧向力表达式、侧偏角表达式代入动力学模型表达式，并考虑到δf通常是小角度，经整理后得：利用方向盘转角传感器和车轮力传感器信息得到车辆动力学模型的控制输入向量U＝[δfFtfFtr]T，通过四阶龙格库塔法求解上式得载体坐标系下车辆的横向速度纵向速度和横摆角速度根据惯导解算的姿态矩阵，得到导航坐标系下由动力学模型计算的速度：上述步骤3中，CKF滤波器的误差状态变量包括3个姿态误差、2个速度误差、2个位置误差、3个陀螺常值漂移、2个加速度计常值零偏，即CKF滤波器的状态方程为式中，A为相应的状态转移矩阵，由INS误差方程构成；w为过程噪声；将惯导和车辆动力学模型解算的东向速度之差、北向速度之差作为CKF滤波器的观测量，即观测向量为Z＝[vIe-vMevIn-vMn]T，则系统的量测方程为：Z＝HX+v式中，H＝[02×3I2×202×7]为系统量测矩阵，v为量测噪声；对前述系统状态方程和量测方程进行离散化，得到k时刻的离散化系统状态模型为：Xk＝FXk-1+wk-1Zk＝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-δvxVnout＝vIn-δvy其中，θ表示俯仰角，γ表示横滚角，表示航向角。采用上述方案后，本专利技术提出基于CKF滤波的VDM辅助INS组合导航方法，建立车辆三自由度非线性动力学模型，利用四阶龙格库塔法实时解算速度信息。以INS误差方程为状态本文档来自技高网...

【技术保护点】
一种基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，其特征在于包括如下步骤：步骤1，根据微惯性器件输出的角增量和比力，利用惯导数值更新算法解算车辆的姿态、速度和位置；步骤2，建立三自由度车辆动力学模型，以方向盘转角和纵向力为控制输入量，利用四阶龙格库塔法实时解算载体速度；步骤3，以惯导误差方程为状态方程，动力学模型和惯导解算的速度差为观测量，设计CKF滤波器对组合导航系统进行状态估计；步骤4，CKF估计得到的位置、速度和姿态误差对捷联惯导解算结果进行输出校正，陀螺和加表误差对惯导进行反馈校正。

【技术特征摘要】
1.一种基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，其特征在于包括如下步骤：步骤1，根据微惯性器件输出的角增量和比力，利用惯导数值更新算法解算车辆的姿态、速度和位置；步骤2，建立三自由度车辆动力学模型，以方向盘转角和纵向力为控制输入量，利用四阶龙格库塔法实时解算载体速度；步骤3，以惯导误差方程为状态方程，动力学模型和惯导解算的速度差为观测量，设计CKF滤波器对组合导航系统进行状态估计；步骤4，CKF估计得到的位置、速度和姿态误差对捷联惯导解算结果进行输出校正，陀螺和加表误差对惯导进行反馈校正。2.如权利要求1所述的基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，其特征在于：所述步骤1的详细内容是：利用六轴加速度计和陀螺仪测量载体的角增量和比力，解算出载体当前时刻的导航参数，导航坐标系n采用东北天地理坐标系，载体坐标系b采用右前上坐标系；惯导数值更新算法包含姿态微分方程、比力微分方程和位置微分方程。3.如权利要求2所述的基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，其特征在于：所述姿态微分方程的表达式为：其中，Q为姿态四元数，为载体系相对于导航系的角速度，为陀螺输出的载体系相对于惯性系的角速度，为地球自转引起的导航系旋转角速度，为载体运动产生的位移角速度，为姿态旋转矩阵。4.如权利要求2所述的基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，其特征在于：所述比力微分方程的表达式为：其中，vI＝[vIevInvIu]T为惯导解算的东向、北向和天向速度，为加速度计测量的比力，为由载体运动和地球自转引起的哥氏加速度，为由载体运动引起的对地向心加速度，gn为重力加速度，为由载体系转换为导航系的转换矩阵。5.如权利要求2所述的基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，其特征在于：所述位置微分方程的表达式为：其中，RM和RN分别为子午圈和卯酉圈主曲率半径，L、λ和h分别为惯导解算的纬度、经度和高度，vIe、vIn和vIu分别为惯导解算的东向速度、北向速度和天向速度。6.如权利要求1所述的基于CKF滤波的车辆动力学模型辅助惯导组合导航方法，其特征在于：所述步骤2的详细内容是：建立横向、纵向、横摆三自由度非线性车辆动力学模型，忽略左右轮差异，模型等效简化为前、后车轮分别集中在车辆前、后轴中点而构成的一个自行车模型，xnoyn为导航坐标系，xn轴向东，yn轴向北；xboyb为固定在车辆质心上的载体坐标系，xb轴与载体横轴重合，向右为正，yb轴与载体纵轴重合，向前为正；根据牛顿力学，车辆的动力学模型为：其中，和分别为车辆的横向速度、纵向速度和横摆角速度；m和Iz分别为车辆的质量、绕竖轴的转动惯量；a、b分别为汽车前轮和后轮轮轴中心到质心的距离；δf为前轮转向角，由方向盘转角传感器测得的方向盘转角δ除以从方向盘到前轮的转向传动比来确定；Cd为空气阻力系数；Af为车辆前向面积；ρa为空气密度；Ftf、Ftr分别为作用在单个前轮和后轮上的纵向力；Fsf、Fsr分别为作用在单个前轮和后轮上的横向力；对于行驶在一般道路交通环境下的车辆，将作用在各轮上的侧向力表示为：Fsf＝CαfαfFsr＝Cαrαr式中，Cαf、Cαr分别为前、后轮胎的侧偏刚度；αf、αr分别为前、后轮胎的侧偏角，近似表示为：将侧向力表达式、侧偏角表达式代入动力学模型表达式，并考虑到δf通常是小角度，经整理后得：

【专利技术属性】
技术研发人员：王立辉，张月新，乔楠，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏,32