基于加速度校正的坡度估计算法与实现*

曾小华，钱琦峰，宋大凤，谷京哲，苑卫松

（1.吉林大学，汽车仿真与控制国家重点实验室，长春 130000； 2.一汽解放汽车有限公司，长春 130000）

前言

环境感知技术是现代智能运输系统的重要组成部分。汽车行驶在复杂多变的路况下，对路面坡度信息进行快速准确的估计，在汽车稳定性控制［1］、燃油经济性改善、自动变速器平顺换挡控制［2-3］等方面都具有重要的现实意义。例如重型商用车经常行驶在盘山多坡道工况，实时获取路面坡道变化，指导车辆自动换挡，调整系统工作模式，不仅能够减少发动机的燃油消耗，还可提高整车的驾驶感受。

当前普遍的坡度估计方法主要可以划分为基于汽车纵向动力学模型构造状态观测器和基于传感器信号时频分析［4］这两大类。基于汽车纵向动力学模型的方法利用汽车纵向动力学平衡方程中含有坡道这一待估计量，通过汽车CAN总线获取当前的各种信号量，采用模型参数辨识法［5］、卡尔曼滤波法［6-7］等估计道路纵向坡度。该方法依靠众多信号的影响，同时须对整车等关键参数进行细致给定，用来减小复杂多变的行驶环境带来的误差。基于传感器信号处理的方法，采用加速度传感器［8-9］、陀螺仪、GPS［10-11］和气压传感器等电子传感器获取汽车外界信息，通过多信息融合的方法对道路坡度进行估计。由于该方法不涉及汽车动力学模型，因此估计的结果精度完全取决于信号的滤波和融合处理方式。还有学者综合汽车动力学模型和采集的传感器信号，采用遗忘最小二乘法、卡尔曼滤波法［12］或模糊估计法［13］进行联合估计，其优势在于适应性好，但受到复杂度和成本的制约。

考虑到汽车纵向动力学模型涉及参数较多，CAN总线上的信号可靠性和准确度有待验证，而利用传感器过多造成信号冗余、成本增加，本文中仅利用加速度传感器和陀螺仪得到汽车的三轴加速度和单轴角速度，通过角速度的短时积分来校正加速度信号，并借助卡尔曼滤波的思想，对加速度和角速度得到的坡度结果进行数据融合。考虑到卡尔曼滤波须实时给出误差的协方差，在复杂多变的路况下，很难精确定量给出其大小。根据汽车实际工况特征，标定出坡度量测值和先验值误差的定量关系，然后建立相应的模糊规则，动态给定加权系数，进而融合加速度和角速度信号，对纵向坡道实时动态估计。得到的结果可靠性高、实时性好，避免了汽车速度和加速度等信号估计不准确引入的误差，克服了加速度信号易受汽车本身或噪声干扰的影响，解决了直接实施卡尔曼滤波算法时误差统计特征量难以给定的难点。

1 坡度估计整体实施方案

根据陀螺仪和加速度计采集的信号，采用坡度估计算法对加速度和角速度信号进行处理，得到最终的估计结果，最后利用CAN总线输出坡度估计算法所估计出的道路坡度，供给CAN总线上需坡度信号的电子控制单元。坡度估计整体实现方案如图1所示。

坡度估计算法首先根据角速度和上一时刻的估计结果得到坡度估计的先验值，然后利用经过角速度校正后的加速度计算得到信号融合的量测值。根据角速度短时积分和加速度短时计数这两个特征量作为模糊控制器的输入，用以评价当前路况条件，从侧面反映出坡度量测值和先验值误差的定量关系；根据在不同误差下离线计算出稳态卡尔曼增益，作为不同路况条件下模糊控制器输出信号融合系数的大小。最后联合融合系数、坡度的先验估计值、坡度的量测值，进行加权融合，得到坡度的估计结果。

图1 坡度估计整体实现方案

2 坡度估计算法

2.1 信号的初始化和预处理

加速度传感器和陀螺仪受到外界温度和地理位置的影响，给信号带来采样误差。为减少该误差，有必要对信号进行初始化处理。利用传感器在刚上电后的短时间内大致估计出角速度零漂和重力加速度，即

式中：ωbias为预估的角速度零漂；ωi为陀螺仪刚上电后在很短的时间内采集的角速度，代表车辆的俯仰角速度；axi、ayi、azi分别为加速度传感器刚上电后在很短时间内采集的三轴加速度，分别代表车辆在纵向、横向、垂向上的加速度；n为初始化阶段采样次数。

加速度传感器采集到的原始信号可以表示为

式中：ax为加速度传感器测量得到的纵向加速度信号；v为汽车速度，对其求导即为汽车加速度；g为当地的重力加速度；θ为道路倾角，g sinθ为重力加速度在坡道上的分量；w为加速度传感器测量时由于传感器本身的精度或外界的噪声而引入的测量误差。根据常识可知，重力加速度在坡道上的分量和汽车本身的加速度都是低频信号，汽车的振动导致的噪声加速度常属于高频成分。首先通过1阶低通滤波器除掉信号中的高频成分，即

式中ak为数字滤波器的第k次输出；ak-1为数字滤波器的上一次输出；araw为原始输入加速度信号；α为滤波系数。

经过滤波后，在2.2节须对加速度进一步校正，尽可能除去汽车本身的加速度成分。然后通过式（4）计算得到基于加速度传感器的量测值，即

式中：θz为基于加速度传感器的量测值；zk为经过一些校正方法后，尽可能保留g sinθ这一项信息的加速度信号。

2.2 加速度的校正

由式（2）和式（4）可以看出，除去加速度传感器测量值中的汽车本身加速度，是得到道路坡度量测值的关键。一般的处理方法是通过汽车CAN总线获取汽车车速进而差分，但在车辆起步、加速、急减速、转弯、换挡等过程得到的汽车加速度未必准确。本文中提出一种基于陀螺仪短时积分对加速度传感器测量值校正的方法，消除车辆加减速时带来的加速度。

根据陀螺仪输出的信号特点，长时间对角速度积分得到的角度值有明显的误差，但在短时间内积分得到的角度值却有较高的精度。在一定长度的时间窗内（例如1 s），结合该角度变化量和道路坡度的估计值，对原始加速度信号的变化幅度进行限制，尽可能剔除加速度传感器的采样信号中车辆本身纵向加速度。

图2 纵向坡度变化示意图

图2示出道路角度随着汽车位移变化过程的示意图，假设汽车在倾角为α的道路上行驶，经过一段时间后，道路坡度倾角变为θ，其中道路倾角的变化量为β。重力加速度沿坡道分量的变化满足式（5），即

式中Δax为道路角度变化时重力加速度沿坡道分量的变化量。

由于坡度估计算法得到的坡度结果不可能完全精确反映道路的倾角真实值α，只能尽可能趋近真实值。受到陀螺仪精度的影响，短时角速度积分能较准确地反映出倾角变化。因此，实际采集信号和实际路况之间的关系为

式中：α′为上一时间段内坡度估计算法得到的道路角度；Δα为估计误差；β′为一定长度的时间窗内角速度积分值；Δβ为这一时间窗内的角速度积分误差。

由图1可知，坡度估计结果是通过加权融合估计得到的，由式（4）可知量测值是由加速度导出的，其误差没有累加效应，因此估计出来的道路坡度误差Δα会逐步收敛于零。角速度零漂在经过初始化校正后，实际误差不超过0.2°/s，即-0.2°≤Δβ≤0.2°。因此可以按式（7）近似。

将式（6）和式（7）代入式（5），进行三角变换后求得加速度变化范围为

通过式（8）看出，坡度倾角变化时导致重力加速度沿坡道分量的变化量包括3项：第1项为已知量，可以直接通过坡道估计结果和角速度积分进行计算；第2项表示角速度的误差；第3项表示坡度估计输出的误差。

由于道路坡度角是待估计量，角速度误差也会受到外界因素的影响而不断变化，因此坡道估计算法输出的误差Δα和角速度积分的计算误差Δβ无法准确计算。但是由于普通公路最大的坡度不超过10%，考虑极端情况，一般车辆行驶的道路坡度也不超过30%，因此在式（8）中的第3项约为零。根据试验可以对Δβ的范围进行标定限制如下：

式中：eβ，L为一定时间内角速度积分误差下限阈值；eβ，H为一定时间内角速度积分误差上限阈值。

综合式（8）和式（9）进一步推导出Δax的变化门限值，即

式中：Δax，L为加速度变化下限阈值；Δax，H为加速度变化上限阈值。

由式（8）和式（9）得到阈值为

根据加速度上下变化阈值，对加速度进行校正，即

式中zk-1为上一计算时间窗内经过校正的加速度。从式（12）可以看出，过大或过小的加速度变化都属于加速度测量值不正常的变化情况，应根据其变化门限予以修正。

值得说明的是，根据式（11）求解得到的上下阈值是每隔一定长度的时间窗更新一次，因此在道路坡度短时间内变化较剧烈的工况下，被校正后的加速度信号相对于重力加速度沿坡道的分量，会出现明显的时延，这一问题在2.3节中解决。

2.3 信息融合坡度估计

由于行驶工况的复杂多变，坡度估计的先验值和量测值的误差随着时间动态变化，如果直接利用卡尔曼滤波算法进行数据融合得到的结果误差很大，本文中提出一种模糊系统结合稳态卡尔曼滤波的方法（简称F-KF算法），能够有效结合卡尔曼滤波器和模糊系统的优势，动态调节信号融合系数的大小，保证了算法的精度和鲁棒性。

首先对待估计的坡度信号搭建卡尔曼滤波算法，通过角速度对时间积分，可以得到卡尔曼滤波器的预测方程为

式中：θk为在第k步长下坡道角的先验值；为上一时刻卡尔曼滤波器输出的坡道角；ωk为在第k步长下的角速度；Δt为步长。

由式（13）可知，该系统为线性离散系统，结合卡尔曼滤波器核心公式，构建基于加速度和角速度的坡度估计卡尔曼滤波器。其中，式（13）为卡尔曼滤波器的先验状态估计方程，通过式（4）将校正后的加速度转化为坡道角，并将该角度作为卡尔曼滤波器中的量测值，即卡尔曼滤波器中的状态变量到量测值的转换矩阵H为1，卡尔曼滤波器的量测方程为θz＝θ＋ν，ν代表测量误差，θ为真实坡道角。式（14）为状态的预测方程，式（15）为先验估计协方差的更新方程，式（16）为卡尔曼增益系数计算方程，式（17）为估计状态的输出方程，式（18）为后验估计协方差计算方程。

根据上述变量取值，对卡尔曼滤波器估计中式（17）变形得到式（19）。可以看出，式中的计算结果是根据量测值和先验值的加权得到，其中Kk为加权系数。例如当Kk较大时，由式（4）得到的坡度角对于计算结果的占比更大。

本文所用的方法为稳态卡尔曼滤波，其增益系数Kk的大小仅与状态转移矩阵A、状态转移噪声协方差Q和测量噪声协方差R有关。针对坡度估计设计的卡尔曼滤波器，若给定恒定不变的Q和R，Kk在经过几次迭代后很快收敛到正常值。

然而实际道路环境下由于道路坡度未知，实时计算预测误差和测量噪声的统计特性基本不可能，即Q和R难以定量地随着环境的变化而精确给定；由式（2）和式（13）可以看出，汽车本身的加速度也严重影响着测量误差，且陀螺仪测量的角速度只能测量低频范围内的角速度；由于这些情况不能实时精确地传递给卡尔曼滤波器，因此在一些变化剧烈的工况下，其估计结果的精度受到很大影响，即Kk不能仅通过卡尔曼滤波中递推的方法给定。

为改善稳态卡尔曼滤波受到实际工况干扰的影响，首先通过离线数据，定量确定Q和R在实际道路工况下的几组典型定量统计，然后利用式（14）～式（18）描述的卡尔曼滤波器分别离线计算出每组Q和R下稳态时的卡尔曼增益系数，即通过预估出预测误差和测量误差大致的变化范围，得到对量测值θz和先验值θk不同的加权因数，如表1所示。考虑到实际汽车行驶工况的不确定性，如果仅通过信号的门限值来判断，适应力较弱，且不能实现一些复杂的逻辑关系。为增加系统的鲁棒性，根据常识经验判断，结合传感器采集的数据，提取出影响Q和R大小的关键特征量，并根据表1离线定量计算得到的卡尔曼增益变化情况和所选取的特征量，定性建立模糊规则，动态给出加权系数Kk的大小。最后结合式（4）和式（13）分别得到量测值θz和先验值θk，并利用式（19）计算出待估计的状态，改善卡尔曼滤波器在线计算受到环境噪声的影响。

表1 不同Q和R计算得到的Kk

对不同条件下的Q和R进行估计后，得到对应的Kk如表1所示，可以看出其大致的变化范围。

由式（19）看出，当Kk较大时，即量测方程得到的坡道角可信度大，须满足以下条件：

（1）车辆运行状态较平稳，没有剧烈的加减速运动；

（2）没有很大的侧向加速度，即没有出现急转弯；

（3）垂直方向上加速度接近重力加速度；

（4）车辆没有处于坡度正在变化的路面上。

当不太符合以上这些条件时，应该更加信任式（14）得到的先验估计值。其中条件（1）～条件（3）都是从车辆在XYZ三轴方向上的稳定程度考虑的，为简化模糊系统以便于实时计算，根据式（20）判断采集到的加速度与重力加速度的关系。

式中ΔG为预先设定的门限值，推荐取值ΔG=0.05gcorr，上式用来衡量当前车辆的车速均匀程度。

为避免个例采样错误而导致系统误判造成波动，而又确保车辆状态更新的实时性，以当前时刻为终点，向前取一定宽度的时间窗，统计该段时间内满足式（20）的采样点个数CountNum，如果满足上式的个数占整个时间窗内采样点的个数比重少时，则由量测方程得到的坡道角可信度大。

对于条件（4），当车辆处于坡度正在变化的路面上时，例如车辆从水平道路过渡到带有坡度道路的工况下，由于坡道阻力的变化，驾驶员往往会切换挡位或加大动力输出，因此会导致加速度传感器采集的纵向加速度信号难以处理，不能反映出当前的重力加速度在坡道上的分量信息，而角速度信号虽然有误差的存在，但是不会受到这一工况的干扰。因此须识别出这一工况，并提高对预测方程的可信度。

同样从当前时刻向前取一定宽度的时间窗，在该段时间窗内采集角速度的绝对值按照下式积分，根据短时积分值IntAngle的大小判断车辆处于的坡度变化剧烈程度。

式中n为时间窗内采集点的个数。

根据实际测试情况确定采样点计数值CountNum和短时积分角度变化IntAngle的变化范围，其中算法步长为10 ms，两个输入变量的时间窗宽度都为500 ms，隶属度函数为梯形，具体参数值如表2所示；模糊系统给定的卡尔曼增益Kk作为输出，隶属度函数为三角形，具体参数值如表3所示。

表2 模糊系统输入变量的参数

表3 模糊系统输出变量的参数

通过实际测试经验和离线的稳态卡尔曼增益变化情况及范围，给定模糊逻辑规则如下：

模糊系统建立完成后，得到的模糊系统输入输出关系如图3所示。

图3 模糊系统输入与输出的关系

从图3可以看出，当CountNum和IntAngle都较小时，表明车辆接近匀速行驶且所处的道路坡度变化较小，应该增大系数Kk，提高对量测值的可信度，利用加速度没有误差累积的特点及时对坡度估计结果进行牵制，避免角速度积分误差导致估计结果的发散。当IntAngle较大时，表明车辆处于坡度正在变化的工况下，须降低系数Kk，避免结果跟随大误差的加速度而大幅非正常变化。当IntAngle较小，CountNum较大时，表明车辆处于坡度基本不变的但加减速较大的工况，例如车辆在水平路面或一定坡度的路面上，进行起步、急转弯、急加速、急制动；快速通过凹凸不平的坏路，应降低Kk，提高先验值的可信度。

3 软硬件集成实现

借助MATLAB/Simulink平台，搭建应用层坡度估计算法，并利用数字字典对算法内部变量和参数定点处理并分配数据类型，进而借助代码生成工具，完成应用层代码的设计。之后基于STM32单片机的IO、CAN、定时器TIM和IIC通信等外部设备，利用IIC和陀螺仪及加速度传感器信息交互，利用定时器实现算法的定步长运行，CAN通信接口实现与整车信息的交互，完成底层硬件驱动的设计。最后在Keil编译器中对这两部分代码进行集成，实现软硬件之间的数据交互，整体的程序流程如图4所示。

图4 程序执行流程

利用电源管理芯片完成车载电源电压转换，为坡度传感器硬件提供稳定的电源输入，在外围电路、微机械传感器和CAN收发器的基础上，利用STM32F103型号单片机实现信号的采集、处理和输出。硬件电路架构如图5所示。

图5 硬件电路架构

图6中左图为贴片后的电路板图，右图为组装后实车测试前的坡度传感器。图7中左图为试验时的某款重型商用车，右图为测试车上的调试场景。

图6 硬件电路PCB与坡度传感器实物

图7 试验所用汽车与调试场景

4 实际测试结果

在一汽农安试验场进行标准坡道测试，传感器固定在驾驶舱，加速度传感器和陀螺仪的X、Y、Z轴分别与车辆坐标系下的纵向、侧向、垂向保持一致。试验工况为先上坡度为10%的坡道，然后紧接着下坡度为20%的坡道，CAN总线上采集的车速如图8所示。

图8 CAN总线上采集的车速

根据2.2节中校正加速度算法，加速度传感器实际测量得到的加速度与经过加速度校正后的对比如图9所示。

图9 原始加速度与经过加速度校正限制后的曲线对比

由图9可知，车辆在30 s左右开始上坡，由于坡道阻力的影响，为保持车辆匀速，车辆增加动力输出，原始加速度增大，到达40 s左右时，由于驾驶员进行挡位切换并增大动力输出，车辆本身加速度大幅抖动。50 s左右车辆到达坡道顶端，坡度开始下降；下坡时为稳定车速，对车辆进行了轻微制动，导致实际加速度传感器测量值（绝对值）比重力加速度在坡道上的分量（绝对值）小。在70 s后完成了整个坡道测试。从80～120 s可以看出，在汽车本身加减速作用下，原始加速度出现较小的变化。从校正后的加速度可以看出，其大小基本不会受到车辆本身加减速、换挡等过程的冲击而产生的加速度误差影响，反映出重力加速度在坡道上的分量成分，为F-KF滤波算法提供了良好的输入。

图10 校正加速度前后数据融合结果对比

为显示出加速度校正算法的明显优势，分别以低通滤波处理后的加速度和校正处理后的加速度作为量测值，并采用F-KF算法进行坡道估计。对比结果如图10所示。AccLowPass＋F-KF代表低通滤波后进行F-KF的估计结果。AccCorrection＋F-KF代表加速度校正结合F-KF的估计结果；加速度经过低通滤波后再进行F-KF算法数据融合处理，消除了大部分由汽车本身加速度带来的影响，但由于原始加速度的影响，其估计结果随着车速变化出现了较大的误差。而加速度校正结合F-KF算法估计的结果很好地反映出道路坡度的变化。

由于加速度校正算法采用的加速度变化阈值是每隔一定时间后的计算结果，当短时间道路坡度变化较大时，被限制后的加速度就不能体现出真实重力加速度沿坡道的分量，因此在F-KF算法中针对这类工况动态调节加权因数，增大对先验值的可信度，改善算法的实时性。

为体现算法的实时性，图11依次给出了校正后的加速度量测值直接换算成坡度的结果AccCorrection，校正后的加速度作为量测值经过F-KF算法的处理结果AccCorrection＋F-KF及单纯角速度积分得到的结果。由于陀螺仪零漂的影响，长时间积分得到的道路坡度会逐步偏离真实值，80～120 s时角速度的积分结果已明显发散。虽然角速度积分不能得到正确结果，但是可以反映出坡度变化的实时情况。利用这一性质，对另外两种方法在稳定上坡时间段36～50 s和稳定下坡时间段59～63 s的定量统计结果对比，如表4所示。

图11 坡度估计算法实时性对比结果

表4 定量统计结果对比

从表4可以看出，加速度校正后经过F-KF算法的结果相比于仅加速度校正后的结果，误差均值和误差标准差都更小。图11中50～60 s时间段也可反映出通过加速度校正的方法在这段坡度变化较快的工况下实时性较差。从80 s之后的工况看出，虽然校正后的加速度有些时延，但是由于时间窗长度很短，被限制后的加速度信号和经过F-KF处理后的基本一致，说明加速度校正的效果能很好地反映真实的重力加速度沿坡道的加速度分量，提高了量测方程的可信度。

综合对比可以看出，经过加速度校正的F-KF算法能有效克服加速度校正存在时延的缺点，同时保证了估计结果的准确性和实时性。

5 结论

仅采用三轴加速度计和陀螺仪采集的加速度和角速度信号，相比利用汽车动力学关系估计坡度的方法，从源头减少了误差的来源，降低了成本，且容易集成到整车系统。利用角速度短时积分得到的角度变化值误差小的特点，校正原始加速度信号，提取出加速度信号中重力加速度在坡道上的分量成分，滤除加速度原始数据中除重力加速度在坡道上分量的信号，包括车辆本身的加速度和测量噪声等。

考虑到在复杂路况下难以实时估计出误差统计特性，直接使用卡尔曼滤波出现估计误差大的缺点，为保证估计结果的准确度，同时又尽可能地提高算法的实时性，F-KF算法利用模糊系统的特点，根据离线计算不同的误差协方差输入给卡尔曼滤波算法，得到卡尔曼增益的大致范围，并以车辆所处的工况特征作为系统输入，以实际测试经验作为模糊逻辑规则，建立模糊系统，在不同的条件下动态调整卡尔曼增益值，保证了在复杂的道路环境下，估计算法的准确性和实时性。