一种基于自适应扩展卡尔曼滤波的道路坡度估计方法[发明专利]

(19)中华人民共和国国家知识产权局
(12)发明专利申请
(10)申请公布号 (43)申请公布日 (21)申请号 201710054689.X
(22)申请日 2017.01.24
(71)申请人 重庆大学
地址 400044 重庆市沙坪坝区沙坪坝正街
174号
(72)发明人 孙棣华　赵敏　廖孝勇　黄秋光　
(74)专利代理机构 北京汇泽知识产权代理有限
公司 11228
代理人 武君
(51)Int.Cl.
G01C 9/00(2006.01)
(54)发明名称
一种基于自适应扩展卡尔曼滤波的道路坡
度估计方法
(57)摘要
本发明公开了一种基于自适应扩展卡尔曼
滤波的道路坡度估计方法，通过数据采集装置获
取车辆状态数据，结合车辆状态数据和车辆固有
参数计算模型参数：然后基于坡度与车辆状态数
据的关系模型，构建扩展卡尔曼滤波估计模型；
最后将卡尔曼滤波估计模型改进为自适应扩展
卡尔曼滤波算法模型。

本发明提供的方法利用了
能快速收敛和实时估计的自适应扩展卡尔曼滤
波算法，通过车辆驾驶状态数据动态、实时估计
道路坡度，从而为驾驶员提供实时的道路坡度信
息，为自动驾驶辅助决策、绿色驾驶、及自动变速
器换挡控制提供重要的依据，从而实现安全、经
济、舒适驾驶；提高在不确定性动态噪声影响下
坡度估计的精度和适用范围。

权利要求书3页 说明书8页 附图1页CN 106840097 A 2017.06.13
C N 106840097
A
1.一种基于自适应扩展卡尔曼滤波的道路坡度估计方法，其特征在于：包括以下步骤：
步骤一：通过数据采集装置获取车辆状态数据，结合车辆状态数据和车辆固有参数，计算模型相关参数：所述相关参数包括加速度a和变速器传动比i g；
步骤二：基于车辆动力学按照以下公式建立坡度与车辆状态数据的关系模型：
其中，F t为车辆驱动力，F f为滚动阻力，F i为坡度阻力，F v为空气阻力，F j为加速阻力；m为汽车质量；i g为变速器传动比；i0是传速器传动比，η为机械效率，f r为滚动阻力，ρ是空气密度；C D是空气阻力系数；A为车辆迎风面积，v为车速；θ为道路坡度；
步骤三：基于坡度与车辆状态数据的关系模型，构建扩展卡尔曼滤波估计模型；
步骤四：将卡尔曼滤波估计模型改进为自适应扩展卡尔曼滤波算法模型。

2.如权利要求1所述的基于自适应扩展卡尔曼滤波的道路坡度估计方法，其特征在于：所述自适应扩展卡尔曼滤波算法模型设置有自适应调整模块，所述自适应调整模块是通过测量协方差和过程协方差来实现动态自适应调整，形成自适应卡尔曼滤波算法。

3.如权利要求1所述的基于自适应扩展卡尔曼滤波的道路坡度估计方法，其特征在于：所述步骤三中的扩展卡尔曼滤波估计模型是通过以下步骤来实现的：
1)按照以下公式将关系模型转化成离散状态空间的形式；
按照以下公式建立常规下状态空间：
其中，是指状态变量，u∈R是系统输入，y∈R m是系统输出，A∈R n×n,B∈R n,C∈R m×n 表示系统参数矩阵；
按照以下公式建立状态方程：
式中，表示车辆行驶位移的微分(变化率)；表示车辆速度的微分(变化率)；σ表示车辆旋转质量系数；表示坡度的微分(变化率)；r表示车轮半径；
按照以下公式将状态方程离散化为离散状态空间：
其中，
式中，w(k)和v(k)分别为测量噪声和状态噪声，R(k)和Q(k)分别为相对应的噪声协方差；s(k+1)表示k+1时刻的行驶里程；s(k)表示k时刻的行驶里程；v(k+1)表示k+1时刻的速度；v(k)表示k时刻的速度；θ(k+1)表示k+1时刻的坡度值；θ(k)表示k时刻的坡度值；表示k时刻的状态转移矩阵；G(k)表示k时刻的系统输入矩阵；Z(k)表示k时刻的输出向量；H (k)表示k时刻的系统测量矩阵；U(k)表示k时刻的输入向量；Δt表示时间间隔；σ(k)表示k 时刻的车辆旋转质量系数。

4.如权利要求1所述的基于自适应扩展卡尔曼滤波的道路坡度估计方法，其特征在于：所述步骤四中的自适应扩展卡尔曼滤波算法模型中包括计算当前时刻的状态值和输出值，所述当前时刻的状态值和输出值是按照以下步骤来实现的：
1)利用基于上一时刻的状态值按照以下公式计算预测当前时刻的状态值和输出值：
式中，是k时刻的状态预测值；是k时刻的状态估计值；P(k)是状态协方差；δ(k)为渐消因子；
所述渐消因子δ(k)按照以下公式计算：
δ(k)＝e c(k-1)；
其中，c(k-1)表示指数修正因子。

5.如权利要求1所述的基于自适应扩展卡尔曼滤波的道路坡度估计方法，其特征在于：所述步骤四中的自适应扩展卡尔曼滤波算法模型中包括自适应模块，所述自适应模块包括以下步骤：
1)利用当前时刻的输出值按照以下公式计算当前时刻的估计值：
其中，e(k)是估计系统输出值与测量值的新息值；K(k)为卡尔曼滤波增益，为状态协方差的修正值；
2)基于新息值对测量误差协方差和状态误差协方差按照以下公式进行在线调整估计：
NV(k)＝R(k)+H(k)P(k)H(k)T；
其中，R(k)为测量噪声协方差；NK(k)定义为新息误差协方差，所述新息误差协方差按照以下公式进行计算：
其中，N代表信息序列的时间窗；k表示当前k时刻；i表示在时间窗内的第i个序列值。

通过卡尔曼滤波状态协方差按照以下公式计算状态误差协方差：
K(k)NV(k)K(k)T＝K(k)NV(k)(P(k)H(k)T NV(k)-1)T＝K(k)H(k)P(k)T；
且
式中，表示先验估计值的协方差，p(k)表示后验估计值协方差。

6.如权利要求1所述的基于自适应扩展卡尔曼滤波的道路坡度估计方法，其特征在于：所述状态噪声协方差按照以下公式进行计算：
一种基于自适应扩展卡尔曼滤波的道路坡度估计方法
技术领域
[0001]本发明涉及智能交通系统技术领域，特别是一种基于自适应扩展卡尔曼滤波的动态道路坡度估计方法。

背景技术
[0002]随着车辆智能化的发展，人们对出行驾驶的安全性、舒适性、经济性的需求越来越高。

而除了车辆本身的动力性能和人为驾驶操作外，道路坡度也是影响车辆驾驶的一个重要因素。

当车辆经过坡道时容易出现不合理的加速、减速、频繁换挡等不合理的驾驶操作导致安全隐患问题，增加车辆的燃油消耗和尾气排放，同时影响了驾驶的舒适性。

因此，实时获取准确的道路坡度信息对于提高车辆安全性和燃油经济性具有重要意义。

[0003]目前关于道路坡度信息获取的算法从实现原理上可以分为三类：基于加速度传感器的检测方法、结合GPS和GIS的地理信息查询方法和基于纵向动力学模型的估计方法。

基于加速度传感器的方法根据加速度传感器信号和由车辆速度计算得到运动加速度，结合运动学分析即可得到道路坡度角。

该算法比较简单，但是需要附加安装传感器设备，且容易受到设备噪声干扰的影响，因此其应用受到限制。

结合GPS和GIS的地理信息查询方法是根据GPS的定位技术，通过与GIS地理信息系统进行电子地图匹配的方法获得坡度信息。

然而该方法的估计精度依赖于GPS信息，且GPS信息容易因为城市高楼和隧道等的影响而容易丢失，无法获取坡度信息。

因此，目前国内基于车辆动力学的估计方法成为研究热点。

但现有研究主要侧重于坡度估计的实时性，忽略了因驾驶过程存在的各种不确定干扰和模型误差，这在实际应用中会大大降低道路坡度估计精度。

[0004]因此，需要一种基于车载数据的考虑非确定性干扰影响下的道路坡度实时估计方法。

发明内容
[0005]本发明的目的是提出基于自适应扩展卡尔曼滤波的动态道路坡度估计方法；该方法针对道路坡度估计误差及滤波发散问题，将基于车辆纵向动力学模型的常规扩展卡尔曼滤波改进为自适应滤波估计，基于新息序列和坡度估计值变化率对测量协方差和过程协方差进行动态地自适应调整，从而提高坡度估计精度。

[0006]本发明的目的是通过以下技术方案来实现的：
[0007]本发明提供的基于自适应扩展卡尔曼滤波的道路坡度估计方法，包括以下步骤：[0008]步骤一：通过数据采集装置获取车辆状态数据，结合车辆状态数据和车辆固有参数，计算模型相关参数：所述相关参数包括加速度a和变速器传动比i g；
[0009]步骤二：基于车辆动力学按照以下公式建立坡度与车辆状态数据的关系模型：
[0010]
[0011]其中，F t为车辆驱动力，F f为滚动阻力，F i为坡度阻力，F v为空气阻力，F j为加速阻
力；m为汽车质量；i g为变速器传动比；i0是传速器传动比，η为机械效率，f r为滚动阻力，ρ是空气密度；C D是空气阻力系数；A为车辆正向迎风面积，v为车速；θ为道路坡度；
[0012]步骤三：基于坡度与车辆状态数据的关系模型，构建扩展卡尔曼滤波估计模型；[0013]步骤四：将卡尔曼滤波估计模型改进为自适应扩展卡尔曼滤波算法模型。

[0014]进一步，所述自适应扩展卡尔曼滤波算法模型设置有自适应调整模块，所述自适应调整模块是通过测量协方差和过程协方差来实现动态自适应调整，形成自适应卡尔曼滤波算法。

[0015]进一步，所述步骤三中的扩展卡尔曼滤波估计模型是通过以下步骤来实现的：[0016]1)按照以下公式将关系模型转化成离散状态空间的形式；
[0017]按照以下公式建立常规下状态空间：
[0018]
[0019]其中，是指状态变量，u∈R是系统输入，y∈R m是系统输出，A∈R n×n,B∈R n,C∈R m×n表示系统参数矩阵；
[0020]按照以下公式建立状态方程：
[0021]
[0022]式中，表示车辆行驶里程的微分(变化率)；表示速度的微分(变化率)；σ表示车辆旋转质量系数；表示坡度的微分(变化率)；r表示车轮半径；
[0023]按照以下公式将状态方程离散化为离散状态空间：
[0024]
[0025]
[0026]其中，
[0027]
[0028]式中，w(k)和v(k)分别为测量噪声和状态噪声，R(k)和Q(k)分别为相对应的噪声协方差；s(k+1)表示k+1时刻的行驶里程；s(k)表示k时刻的行驶里程；v(k+1)表示k+1时刻
的速度；v(k)表示k时刻的速度；θ(k+1)表示k+1时刻的坡度值；θ(k)表示k时刻的坡度值；
表示k时刻的状态转移矩阵；G(k)表示k时刻的系统输入矩阵；Z(k)表示k时刻的输出向量；H(k)表示k时刻的系统测量矩阵；U(k)表示k时刻的输入矩阵；Δt表示时间间隔；σ(k)表示k时刻的车辆旋转质量系数。

[0029]进一步，所述步骤四中的自适应扩展卡尔曼滤波算法模型中包括计算当前时刻的状态值和输出值，所述当前时刻的状态值和输出值是按照以下步骤来实现的：
[0030]1)利用基于上一时刻的状态值按照以下公式计算预测当前时刻的状态值和输出值：
[0031]
[0032]式中，是k时刻的状态预测值；是k时刻的状态估计值；P(k)是状态协方差；δ(k)为渐消因子；
[0033]所述渐消因子δ(k)按照以下公式计算：
[0034]δ(k)＝e c(k-1)；
[0035]其中，定义c(k-1)表示指数修正因子。

[0036]进一步，所述步骤四中的自适应扩展卡尔曼滤波算法模型中包括自适应模块，所述自适应模块包括以下步骤：
[0037]1)利用当前时刻的输出值按照以下公式计算当前时刻的估计值：
[0038]
[0039]其中，e(k)是估计系统输出值与测量值的新息值；K(k)为卡尔曼滤波增益，为状态协方差的修正值；
[0040]2)基于新息值对测量误差协方差和状态误差协方差按照以下公式进行在线调整估计：
[0041]NV(k)＝R(k)+H(k)P(k)H(k)T；
[0042]其中，R(k)为测量噪声协方差；NK(k)定义为新息误差协方差，所述新息误差协方差按照以下公式进行计算：
[0043]
[0044]其中，N代表新息序列的时间窗；k表示当前k时刻；i表示在时间窗内的第i个序列值；
[0045]通过卡尔曼滤波状态协方差按照以下公式计算状态误差协方差：
[0046]
[0047]K(k)NV(k)K(k)T＝K(k)NV(k)(P(k)H(k)T NV(k)-1)T＝K(k)H(k)P(k)T；
[0048]且
[0049]式中，表示先验估计值协方差，p(k)表示后验估计值协方差。

[0050]进一步，所述状态噪声协方差按照以下公式进行计算：
[0051]
[0052]由于采用了上述技术方案，本发明具有如下的优点：
[0053]本发明提供的基于自适应扩展卡尔曼滤波的动态道路坡度估计方法，利用了能快速收敛和实时估计的自适应扩展卡尔曼滤波算法，通过车辆驾驶状态数据动态、实时估计道路坡度，从而为驾驶员提供实时的道路坡度信息，为自动驾驶辅助决策、绿色驾驶、及自动变速器换挡控制提供重要的依据，从而实现安全、经济、舒适驾驶。

本方法在不影响坡度估计实时性前提下，不但能克服因模型误差而导致的估计发散问题，同时，还能动态地调整卡尔曼滤波估计中的测量噪声协方差和过程噪声协方差，进而提高在不确定性动态噪声影响下坡度估计的精度和适用范围。

[0054]本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。

本发明的目标和其他优点可以通过下面的说明书来实现和获得。

附图说明
[0055]本发明的附图说明如下。

[0056]图1是本发明采用的基于OBD-II接口的数据采集框架图。

[0057]图2是车辆纵向动力分析图。

[0058]图3是本发明采用的自适应扩展卡尔曼滤波估计方法流程图。

具体实施方式
[0059]下面结合附图和实施例对本发明作进一步说明。

[0060]实施例1
[0061]如图所示，本实施例提供的基于自适应扩展卡尔曼滤波的动态道路坡度估计方法，包括以下四个步骤：
[0062]步骤一：通过数据采集装置获取车辆状态数据，结合车辆状态数据和车辆固有参数，计算模型的其他相关参数：
[0063]1)通过OBD-II接口获取实时的车辆状态数据，包括动机转矩T、车速v、发动机转速n、油门开度Th、刹车信号Br、档位信息Ge并通过蓝牙设备存储到移动设备终端。

[0064]2)基于相应的车辆固有参数(轮胎滚动半径r、主减速器传动比i0、道路滚动阻力系数f、传动系机械效率η、车辆空气阻力系数C d、车辆正向迎风面积A)，计算模型中其他参数信息。

[0065]加速度a可由速度v对时间差分获得，第k时刻的加速度可表示为：
[0066]
[0067]其中，Δt为数据采集软件模块的采样周期；
[0068]变速器传动比i g按照如下计算方式得到：
[0069]步骤二：基于车辆动力学建立坡度与车辆状态数据的关系模型；
[0070]车辆在行驶过程中受到滚动阻力，空气阻力和坡度阻力的作用，其车辆纵向动力与道路坡度i存在的关系:
[0071]
[0072]其中，F t为车辆驱动力，F f为滚动阻力，F i为坡度阻力，F v为空气阻力，F j为加速阻力；m为汽车质量、i g为变速器传动比、i0是传速器传动比，η为机械效率，f r为滚动阻力，ρ是空气密度、C D是空气阻力系数；
[0073]步骤三：基于坡度与车辆状态数据的关系模型，构建扩展卡尔曼滤波估计模型；[0074]1)将关系模型转化成离散状态空间的形式；
[0075]2)基于状态空间方程建立扩展卡尔曼滤波估计模型；
[0076]步骤四：改进为自适应扩展卡尔曼滤波算法模型
[0077]在常规卡尔曼滤波中增加自适应调整部分，包括增加渐消因子来增强新数据的作用而削弱旧数据对估计的影响、基于新息系列和坡度估计值变化率对测量协方差和过程协方差进行动态地自适应调整，形成自适应卡尔曼滤波算法，进而提高坡度估计精度。

[0078]实施例2
[0079]本实施例进一步的详细描述；通过OpenXC采集车辆状态，运用自适应扩展卡尔曼滤波算法实现道路坡度的动态估计；下面对四个步骤进行详细的说明：
[0080]步骤一：通过OpenXC数据采集装置获取车辆状态数据，基结合车辆状态数据和车辆固有参数，计算模型的其他相关参数：
[0081]1)附图1是车内传感器获取的车辆状态数据传输及获取的过程。

为了实现汽车的智能化控制，目前各大汽车都采用CAN总线连接车内部所具有的控制器、执行器以及传感器，并将数据统一传输到OBD-II接口。

实施例中采用美国福特汽车公司提供的OpenXC插入到OBD-II接口，然后开发基于数据采集软件APP，通过蓝牙设备接收实时的车辆状态数据(转矩T、车速v、发动机转速n、油门开度Th、刹车信号Br、档位信息Ge)，存于手机移动终端。

[0082]2)基于相应的车辆固有参数(轮胎滚动半径r、主减速器传动比i0、道路滚动阻力系数f、传动系机械效率η、车辆空气阻力系数C d、车辆正向迎风面积A)，计算模型中其他参数信息。

[0083]加速度a可由速度v对时间差分获得，第k时刻的加速度可表示为：
[0084]
[0085]其中，Δt为OpenXC数据数据采集软件模块的采样周期；
[0086]变速器传动比i g按照如下计算方式得到：
[0087]步骤二：基于车辆动力学建立坡度与车辆状态数据的关系模型，车辆纵向动力学模型如下：
[0088]σma＝F t-F f-F i-F v；
[0089]其中，F t为车辆驱动力，F f为滚动阻力，F i为坡度阻力，F aer o为空气阻力，F j为加速阻力。

[0090]驱动力对应的关系式为：
[0091]
[0092]滚动阻力的计算公式是：
[0093]F r＝mgf r cosθ；
[0094]由道路设计规范标准可以知道，道路坡度设计中一般不超过10度，因此为了模型简化，可以将cosθ近似等于1。

坡度阻力的跟重力和坡度大小相关，关系式为
[0095]F i＝mg sinθ；
[0096]同理可将sinθ近似为sinθ≈θ。

空气阻力的计算公式为
[0097]
[0098]其中，m为汽车质量、i g为变速器传动比、i0是传速器传动比，η为机械效率，f r为滚动阻力，ρ是空气密度、C D是空气阻力系数；
[0099]由以上式子，即可得到车辆状态数据与坡度的关系模型：
[0100]
[0101]步骤三：基于坡度与车辆状态数据的关系模型，构建扩展卡尔曼滤波估计模型。

[0102]1)将关系模型转化成离散状态空间的形式；
[0103]常规下状态空间描述的表达式为：
[0104]
[0105]其中，是指状态变量，u∈R是系统输入，y∈R m是系统输出，A∈R n×n,B∈R n,C∈R m×n表示系统参数矩阵；
[0106]假设道路坡度没有发生突变，则状态方程可以描述为：
[0107]
[0108]为了实现道路坡度的实时递推估计，将状态方程离散化
[0109]
[0110]以上是车辆最后实现离散状态空间描述
[0111]
[0112]其中，
[0113]
[0114]式中，w(k)和v(k)分别为测量噪声和状态噪声，其相对应的噪声协方差分别为R (k)和Q(k)。

[0115]步骤四：
[0116]由以上建立的常规扩展卡尔曼滤波算法，是在假设系统的测量噪声和系统噪声在滤波过程中是已知的且为定值的基础上。

然而车辆在行驶过程中的驾驶操作策略及周围环境不断变化的，其噪声统计特性也会变化，同时构建的部分模型参数不可测或随着路况环境发生变化，导致估计结果误差变大甚至引起滤波发散。

[0117]为了防止滤波发散且提高道路坡度估计精度，附图3中表示的是自适应扩展卡尔曼滤波算法的流程图，卡尔曼滤波算法包括两个计算过程：时间更新和测量更新。

时间更新方程向前推算先验状态估计值和先验误差协方差；测量更新方程将先验状态估计和测量变量结合产生状态的后验估计并更新估计状态的后验估计误差协方差。

由上一时刻的状态估计值和当前测量值便可估算下一时刻的坡度估计值，如此递推。

本实施例中的自适应是指基于卡尔曼滤波新息在坡度估计的过程中动态地调整量测噪声协方差矩阵及误差协方差阵。

[0118]1)基于卡尔曼滤波算法获取时间更新和测量更新值；
[0119]基于上一时刻的状态值预测当前时刻的状态值和输出值，计算公式为
[0120]
[0121]式中，是k时刻的状态预测值；是k时刻的状态估计值；P(k)是状态协方差。

δ(k)为渐消因子；其目的是当状态估计变化比较大的情况下，增强新数据的作用，弱化老数据对估计的影响，缩短坡度估计收敛时间；其计算公式为：
[0122]δ(k)＝e c(k-1)；
[0123]其中，
[0124]由以上公式可知，渐消因子δ(k)的大小取决于上一时状态值变化的大小，当上一时刻坡度估计值变化率大则渐消因子也增大，以此来增强新数据在估计中的作用。

[0125]获取时间更新的状态预测值后，可以利用当前时刻的输出值进一步的进行预测值更新，从而获取当前时刻的估计值，计算公式为：
[0126]
[0127]其中，e(k)是估计系统输出值与测量值的差值，又称新息值；K(k)为卡尔曼滤波增益，为状态协方差的修正值；
[0128]2)基于新息值对测量误差协方差和状态误差协方差进行在线调整估计；[0129]为了提高坡度估计的精度，在扩展卡尔曼滤波估计的基础上增加自适应模块，用于在线修正测量误差协方差和状态误差协方差，R(k)的计算公式为：
[0130]NV(k)＝R(k)+H(k)P(k)H(k)T；
[0131]其中NK(k)定义为新息误差协方差，如下表示：
[0132]
[0133]N代表新息序列时间窗，起到数据平滑的作用。

[0134]基于以上公式，可以推导出测量噪声协方差：
[0135]R(k)＝NV(k)-H(k)P(k)H(k)T；
[0136]最后通过卡尔曼滤波状态协方差计算公式，推导出状态误差协方差计算公式
[0137]
[0138]因为K(k)NV(k)K(k)T＝K(k)NV(k)(P(k)H(k)T NV(k)-1)T＝K(k)H(k)P(k)T；[0139]且所以状态噪声协方差可以转换为：
[0140]
[0141]通过以上式子对测量噪声协方差和状态噪声协方差进行自适应地调整，在不影响计算复杂度的情况下实现道路坡度的动态估计。

[0142]综上所述，本实施例提出的基于自适应卡尔曼滤波方法的道路坡度估计方法。

与现有的方法相比，该方法针对复杂工况下存在的不确定性干扰，能够自适应地调整相关参数，进一步提高坡度估计的鲁棒性和准确性。

[0143]最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的保护范围当中。

图1
图2
图3。