传感器融合-路面坡度计算

本文主要介绍一种计算车辆坡度角的方法，利用有限的两轴加速度传感器并结合车辆动力学特性，实时估算车辆的坡度角。

青岛

基于加速度偏差坡度估计

车辆坐标系定义

车辆坐标下定义如下：

沿车辆中轴线确定x轴，车头方向为正方向; x轴与后轴的交点为原点o，过点o作一条直线与x轴垂直，将该直线定义为y轴，沿x轴逆时针方向为正。

加速度计定义

一般车辆加速度计的安装与车辆坐标系一致，车辆前进时，加速前进，传感测量值为正；减速前进，传感器测量值为负。车辆后退时，加速后退，传感器输出值为负；减速后退，传感器输出值为正。

同理，如果车辆前进上坡，静止在坡道上，传感器输出信号为正值；车辆前进下坡，静止在坡道上，传感器输出信号为负值。

计算坡度值

上坡静止状态

如上图所示，车辆上坡阶段，静止于坡道上时，加速度传感器在x轴上存在重力分量，可以表示为 (1)ax=gsin⁡(θ)

下坡静止状态

如上图所示，车辆下坡阶段，静止于坡道上时，加速度传感器在x轴上存在重力分量，可以表示为 (2)ax=gsin⁡(π−θ)=gsin⁡(−θ)

车辆加速度

如上图所示，通过采集相邻时刻k−1和k的纵向速度，计算车辆k−1时刻的加速度为 (3)v˙x(k−1)=v(k)−v(k−1)ΔT 其中，ΔT为采样步长，v(k)定义前进为正，后退为负。

坡度角计算

基于车辆在坡道上的受力分析，可以推算出当车辆运动时，加速度传感器输出信号ax与坡度角θ的关系如下： (4)ax=v˙x+gsin⁡(θ) 所以坡度角可以表示如下： (5)θ=arcsin⁡(ax−v˙xg)

基于纵向动力学坡度计算

车辆纵向动力学模型

如上图所示，车辆纵向方向主要受到发动机产生的牵引力Fx的影响，同时也受到重力分量、迎风阻力和地面摩擦力的影响。通过对车辆对上述力的分析，车辆的动力学模型可以表示如下：

(6)Fx−mgsin⁡(θ)−fmgcos⁡(θ)−12ρCdAvx2=mv˙x

其中，m为车辆质量，ρ为空气密度，Cd为风阻系数，A为迎风面积，g为重力加速度，θ为路面坡度，f为路面滚阻系数。上述等式(6)中，路面坡度θ为待求参数，其它参数对于某一车辆来说均可以认为是已知的或者通过测量能够得到。

令 y=Fx，u=mv˙x+12ρCdAvx2，b=mg(sin⁡(θ)+fcos⁡(θ))，将公式(6)简化为：

(7)y=u+b

上式中，y、u为已知量，b为未知量，由于b是参数θ的函数，因此对路面坡度的估计可以通过估算b^的值求得。

采用带有遗忘因子的最小二乘法估计b的值，假设代价函数为V(b^,k)，那么采用最小二乘法估计b的值，相当于求解参数b^，使得函数V(b^,k)在某一时刻k取得最小值。

(8)V(b^,k)=12∑i=1kλk−i(yi−ui−b^k)2

其中，λ为遗忘因子，k为采样时刻。

函数极小值的计算可以通过求解函数导数为0的参数，及

(9)∂V∂b^k=0

结合等式(8)、(9)得

(10)∑i=1kλk−i(yi−ui−b^k)=0

对等式(10)化简得

(11)∑i=1kλk−i(yi−ui)−∑i=1kλk−ib^k=0

(12)b^k∑i=1kλk−i=∑i=1kλk−i(yi−ui)

基于等式(12)，提取b^k得

(13)b^k=(∑i=1kλk−i)−1(∑i=1kλk−i(yi−ui))

从等式(13)可以看出，随着采样时刻k不断增加，估计值b^的计算量也不断增加。由于车辆行驶过程中的路面坡度估计需要实时计算，因此在实际应用中一般采用递推最小二乘估计来实时估计每一采样时刻b的估计值b^。

(14)b^k=b^k−1+Lk(yk−uk)

增益系数Lk为

(15)Lk=Pk−1λ+Pk−1

误差协方差Pk为

(16)Pk=1λ(1−Lk)Pk−1

根据公式(14)、(15)、(16)的更新过程

(17)D=b^mg

(18)θ=arcsin⁡(D−f1−D2+f21+f2)

坡度角融合

车辆行驶过程中路面坡度θ可以认为是高频信号和低频信号两部分组成，在对路面坡度进行估计时，基于纵向动力学估计方法依赖于车辆的多个参数，车辆的各个参数受高频噪声影响大；基于加速度偏差坡度估计的缺点是受惯性传感器的静态偏差影响较大，惯性传感器的静态偏差属于低频噪声，为了保证路面坡度估算结果的准确性，对θ1采用低通滤波器除去其高频部分，对θ2采用高通滤波器滤除其低频部分，最终得到更加准确的实时估计值θ^为：

(19)θ^=1τs+1θ1+τsτs+1θ2

其中，τ为时间常数，s为拉普拉斯算子，1τs+1为低通滤波器，τsτs+1为高通滤波器。