基于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为惯导解算的东向、北向和天向速度，为加速度计测量的比力，为由载体运动和地球自转引起的哥氏加速度，为由载体运动引起的对地向心加速度，gⁿ为重力加速度，为由载体系转换为导航系的转换矩阵。

上述位置微分方程的表达式为：

其中，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＝[δ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-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＝[vievinviu]^t为惯导解算的东向、北向和天向速度，为加速度计测量的比力，为由载体运动和地球自转引起的哥氏加速度，为由载体运动引起的对地向心加速度，gⁿ为重力加速度，为由载体系转换为导航系的转换矩阵。

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＝[δfftfftr]^t，通过四阶龙格库塔法求解上式得载体坐标系下车辆的横向速度纵向速度和横摆角速度根据惯导解算的姿态矩阵，得到导航坐标系下由动力学模型计算的速度：

步骤3，以惯导误差方程为状态方程，动力学模型和惯导解算的速度差为观测量，设计ckf滤波器对组合导航系统进行状态估计；

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时刻的离散化系统状态模型为：

式中，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

其中，θ表示俯仰角，γ表示横滚角，表示航向角。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。