本发明涉及一种汽车用道路坡度识别方法，特别涉及一种利用加速度计和陀螺仪进行汽车用道路坡度实时估计的方法。
道路坡度在愈发自动化的汽车上显得尤为重要，对于汽车的操作便捷和舒适性上起着重要作用。例如在带有amt的重型卡车上，特别是在崎岖的山路上，可以参考坡道的变化自动的升降档位，极大的减轻了驾驶员的负担，同时，也提升了汽车的燃油经济性。
普遍的坡度识别思想利用加速度计采集到的加速度和陀螺仪采集到的角速度进行数据融合，由于采集纵向方向的加速度计采集的值等于汽车本身的加速度与重力加速度坡度分量之和，这种方法在汽车静止或者匀速运动时效果良好，但是在车辆行驶时由于路况多变，车速是经常变化的，加速度计采集的加速度值往往含有很多除重力加速度在坡度上分量以外的其他成分，进而影响坡道的最优估计坡度值。
现有的专利中,如中国专利公开号为cna，公开日为2014年3月12日，发明名称为“一种利用加速度传感器和陀螺进行车辆行驶上下坡状态判定的方法和装置”，该发明考虑了悬架变形和汽车转弯对坡度识别的影响，但最重要是汽车本身的加减速对坡度识别带来的影响却没有除去；中国专利公开号为cna，公开日为2015年4月1日，发明名称为“一种商用车动态坡度测试系统及其测试方法”，该发明利用了汽车can网络、气压传感器、陀螺仪、加速度传感器等诸多数据来源对坡度进行识别，虽然识别坡度的精度高，但是传感器多，成本不容易控制；中国专利公开号为cna，公开日为2013年10月16日，发明名称为“一种高精度车载道路坡度检测装置及方法”，该发明利用多个放在汽车不同位置的加速度传感器融合得到最优坡度估计，但是没有考虑本身汽车加减速带来的影响，且安装难度大，应用成本高；中国专利公开号为cna，公开日为2009年12月9日，发明名称为“一种车辆行驶坡度与相对高度动态估计方法”，该发明虽然考虑汽车本身加速度，但是该加速度是通过汽车历程仪得到的且标定量过多。
综上所述，现有的坡道识别方法一类是利用加速度计来预测坡度，没有考虑汽车本身纵向加减速对坡度识别的影响，即使考虑汽车本身纵向加速度，也是通过汽车can网络或其他装置获取的；令一类考虑复杂的方法是通过汽车实时车速，轮速、气压等诸多相关的参数和相应的传感器，进行数据融合估计坡度，这类方法成本高且处理方法复杂，同时多个信号的准确性又难以保证；同时考虑汽车加减速影响，且仅利用加速度计和陀螺仪传感器，实现汽车用道路坡度实时估计方法的，目前尚未发现。因此，有必要提供这样一种汽车用道路坡度实时估计方法来弥补现有技术的不足。
本发明旨在解决现有技术存在标定量多，受汽车加减速影响大，利用传感器多，成本高，依赖汽车参数过多，模型复杂鲁棒性差的缺点，提出一种廉价、简便、准确，并迎合市场需求的汽车用道路坡度实时估计方法。
为解决上述问题，本发明采用如下技术方案：
一种汽车用道路坡度实时估计方法，利用可以采集三轴角速度的陀螺仪，采集到的三轴角速度分别为gx，gy，gz，利用可以采集三轴加速度的加速度计，采集到的三轴加速度分别为ax，ay，az。传感器x轴的正方向与汽车的前进方向一致，传感器z轴的正方向与重力加速度的反方向一致且与汽车的垂向在一条直线上，传感器x，y，z三轴的方向满足笛卡尔坐标系的规定。gy当汽车仰头时为正，反之为负。
步骤一、信号初始化，利用每次上电后一段时间t内的陀螺仪输出的角速度加速度计输出的三轴加速度，通过式(1)计算陀螺仪角速度零飘和当地重力加速度，并通过式(2)校准陀螺仪和加速度计的原始误差，包括以下步骤：
式中，gy_bias——陀螺仪输出角速度的零飘值
gyi——陀螺仪第i次汽车绕横向角速度测量值
axi，ayi，azi——加速度计第i次汽车纵向、横向、垂向加速度测量值
n——上电后的t时间内传感器测量总次数
g_corr——校正后的重力加速度值
式中，gy_corr——陀螺仪校准后输出的角速度值
x——代表加速度计在三个方向上采集的汽车纵向加速度ax、横向加速度ay、垂向加速度az
x_corr——代表在三个方向上校准后的汽车纵向加速度ax_corr、横向加速度ay_corr、垂向加速度az_corr
步骤二、信号预处理，对采集到的加速度和角速度进行信号滤波处理，得到处理后的待用信号。具体包括以下步骤：
a、信号门限限制，陀螺仪采集到的角速度和加速度计采集到的三轴加速度信号与各自信号的合理范围对应的门限值进行比较，当待检测的信号高于其门限上限时，让该信号处理后值等于该信号的上限值，当待检测的信号在上限值和下限值之间时，该信号处理后值等于原始信号值，当当待检信号小于下限值时，该信号处理后值等于下限值。
b、信号斜率限制，信号经过门限限制之后，进行延时处理得到上一时刻的信号值，并根据公式(3)计算信号的变化斜率，当斜率值超过信号斜率限制上门限值时，允许的信号变化斜率为上斜率门限值，当斜率值小于信号斜率限制下门限值时，允许的信号变化斜率为下斜率门限值，当斜率值在下门限值和上门限值之间时，允许的信号变化斜率为计算的信号斜率值。根据公式(4)得到斜率限制处理后的信号值。
式中，k(i)——待检信号斜率
x1(i)——当前时刻待检信号
——前一时刻经过斜率限制后输出信号
dt——获取前一时刻与获取当前时刻信号之间的时间间隔
式中，——允许的信号变化斜率
——当前时刻经过斜率限制后输出信号
c、信号滤波处理，信号通过前两步处理之后，依据(5)式滤波处理。
式中，y(k)——当前时刻滤波输出值
y(k-1)——前一时刻滤波输出值
x2(k)——当期时刻输入给滤波处理的信号值
alpha——滤波系数，代表了滤波的程度
d、滑动平均滤波，信号经过滑动平均滤波依据(6)式处理。
式中，x3(k)——当前时刻滑动加权平均滤波输出值
x3(k)——当期时刻输入给滤波处理的信号值
x3(k-1)——上一时刻输入给滤波处理的信号值
n——滑动平均的数据总个数
步骤三、加速度校正，利用短时角速度积分得到角度的可信性，用以去除加速度计中的汽车本身纵向加速度和其他噪声干扰。包括以下步骤：
a、从传感器上电开始，每隔ta时间，获取ta这段时间内的所有绕汽车横向的角速度信号，并对这些角速度值进行离散积分，得到在这段时间内的角度变化量β，该角度变化量等于真实的汽车道路倾角变化值与陀螺仪引起的误差变化值之和；
b、获取距当前时刻ta前的最优估计道路坡度值，若上电开始到执行该步骤时间不足ta，则距当前时刻ta前的最优估计道路坡度值按零处理；
c、采用式(7)计算允许的加速度计采集的信号斜率上限和下限；
式中，kl——允许的加速度计采集的信号斜率下限
kh——允许的加速度计采集的信号斜率上限
bias_acc——考虑角速度的零飘特性对该斜率计算带来的影响
β1——ta时间内真实的汽车道路倾角变化值
β2——ta时间内极限的角速度的零飘值，取值范围在0.1-0.2度/s
α——由步骤b中获取的坡度转换而成的倾角值
d、利用c计算的斜率上下限，用步骤二中的信号斜率限制处理方法对信号预处理后的汽车纵向加速度信号滤波，得到除去加速度信号中其他干扰信息保留重力加速度在坡道上的分量信息的加速度值。
步骤四、角速度和加速度数据融合，基于卡尔曼滤波算法，通过测量值和先验值，得到最优估计的方法。具体包括以下步骤：
a、先验估计道路倾角，利用角速度离散积分得到角度，依据(8)式得到角速度预测的道路倾角。
式中，angle(k)——当前时刻利用角速度预测的道路倾角
s(k-1)——上一时刻角速度和加速度数据融合输出的道路倾角，初始化为零
b、加权因数k的计算，角速度和加速度数据融合的加权因数k的获取方法按照以下步骤：
1)按照加速度信号的特征判断，从传感器上电开始，每隔tb时间，获取tb这段时间内的所有采集到的ax_corr，ay_corr，az_corr，统计tb时间内满足式(9)的计数值count，通过count来衡量加速度计数据的可信程度；
式中，δg——门限值，衡量当前三轴加速度与重力加速度的偏离程度
2)按照角速度信号的特征判断，取当前时刻陀螺仪输出校准的角速度gy_corrn和距离当前时刻之前最近的n-1个陀螺仪校准后的角速度值，即从gy_corrn-1到gy_corr1，按照式(10)判断tb时间段内的角度变化量是否大于门限值，若满足式(10)，则车体倾角正在变化；
式中，δθ——角度变化门限值
3)综合加速度信号和角速度信号特征，当步骤2)中判断车体倾角正在变化时，令加权因数k等于零，否则，按照下表对加权因数k进行赋值；
表中，k1、k2、k3、k4——数据融合中的加权因子
c、测量估计道路倾角，利用加速度和角速度两者共同计算得出测量估计道路倾角。具体包括以下步骤：
1)利用校正后的纵向加速度信号，采用式(11)得到仅利用加速度计算的道路倾角；
式中，ax_limit——经过加速度校正步骤校正后的纵向加速度信号
za(k)——仅利用纵向加速度信号得到的道路倾角
2)从加速度和角速度数据融合中加权因数的计算方法中判断车体倾角没有变化时，加速度计算的道路倾角按式(12)得到，反之，加速度计算的道路倾角等于被限制斜率后的za(k)，其斜率限制门限值方法按式(13)计算得到，且限制斜率的方法与步骤二中的信号斜率限制处理方法相同；
式中，kmax——最大斜率限制门限
kmin——最小斜率限制门限
maxup——汽车上坡时道路倾角最大斜率限制门限值
minup——汽车上坡时道路倾角最小斜率限制门限值
maxdn——汽车下坡时道路倾角最大斜率限制门限值
mindn——汽车下坡时道路倾角最小斜率限制门限值
d、角速度和加速度数据融合得到道路倾角，按照式(14)得到估计道路倾角。
式中，s(k)——当前时刻角速度和加速度数据融合输出的道路倾角
e、把加权融合得到的道路倾角按照式(15)转换为道路坡度。
式中，xe(k)——当前时刻角速度和加速度数据融合输出的道路坡度
步骤五、坡度校正，主要校正汽车车体本身的弹性变形和外界环境对传感器随机干扰造成的影响，使其信号尽量平稳；同时校正传感器的机械安装误差，使得最后得到的最优估计值与真实的道路坡度更加接近。
与现有技术相比本发明的有益效果是：
1.在传感器上电时初始化，对角速度和重力加速度校正，从源头对传感器采集的信号进行校准；
2.利用角速度信号的短时积分可信性对纵向加速度信号校正，除去了加速度信号中的汽车本身纵向加速度和其他噪声干扰，极大的提高了数据融合的结果精度；
3.通过计数和角速度积分的方式对数据融合中加权因数的确定，可以除去不正常的加速度信号带来的影响，通过角速度积分判定上下坡，进而对上下坡时的数据融合中的测量值进行斜率限制，进一步提高了数据融合的准确度；
4.通过坡度校正和坡度调零的方法，可以校正汽车车体的悬架弹性变形和外界环境对传感器随机干扰造成的影响，以及安装传感器时的机械误差，使其信号尽量平稳可靠；
下面结合附图对本发明作进一步的说明：
图1为本发明所述的一种汽车用道路坡度实时估计方法整体流程图；
图2为本发明所述的传感器的方向示意和在传感器在汽车俯视方向上的方向关系图；
图3为本发明所述的步骤三加速度校正中汽车路况变化图；
图4为本发明所述的步骤三加速度校正中校正纵向加速度信号的流程图；
图5为本发明所述的步骤四角速度和加速度数据融合中加权因数获取方法时计算count值流程图；
图6为本发明所述的步骤四角速度和加速度数据融合中加权因数获取方法和加速度坡度计算的判定方法流程图；
图7为本发明所述的步骤五中坡度校正的流程图；
图8为本发明所述的一种汽车用道路坡度实时估计方法核心思想；
图9为本发明所述的一种汽车用道路坡度实时估计方法示例中经过步骤三得到的原始传感器采集到的纵向加速度和校正后的结果图；
图10为本发明所述的一种汽车用道路坡度实时估计方法示例中恒定加权因子和动态变化加权因子的坡度估计结果对比图；
图11为本发明所述的一种汽车用道路坡度实时估计方法示例中经过坡度校正和没有经过校正的坡度估计对比图；
下面结合附图对本发明作详细的描述：
参阅图1，本发明所述的一种汽车用道路坡度实时估计方法分为三大组成部分，分别是初始化部分、预处理部分、主处理部分，初始化是对角速度的零飘和当地重力加速度初始化。预处理部分是对信号进行门限限制、斜率限制、低通滤波、平均滤波、初步校正。主处理部分基于卡尔曼滤波算法，对信号进行数据融合，其中测量值是由校正加速度和斜率限制共同修正得到的，加权系数是通过测量值和先验估计两者得到的，经过数据融合后得到的初步汽车道路坡度又经过坡度校正处理和坡度调零，最后得到最优的道路坡度估计。其中该方法附属的硬件采集的是陀螺仪的三轴的角速度和加速度计的三轴加速度信息。该方法附属的硬件是通过汽车的can总线进行信息交互。
参阅图2，传感器x轴的正方向与汽车的前进方向一致，传感器z轴的正方向与重力加速度的反方向一致且与汽车的垂向在一条直线上，传感器x，y，z三轴的方向满足笛卡尔坐标系的规定。当汽车仰头采集到角速度数值时为正，则gy等于采集到角速度数值。若当汽车仰头时采集到角速度数值为负，则令gy等于采集到角速度数值的相反数。同理当汽车仰头时采集到纵向加速度数值为正，则ax等于采集到加速度数值。当汽车仰头时采集到纵向加速度数值为负，则ax等于采集到加速度数值的相反数。图2右侧一半为传感器在汽车俯视方向上的方向关系图。
参阅图3，结合校正纵向加速度中的处理方法，其β为当前时刻之前的time1时间段内的所有角速度值进行离散积分，即这段时间内的角度变化量。α为距当前时刻t1前的最优估计道路坡度值转化的倾角值。若上电开始到获取α的时间不足t1，则距当前时刻t1前的最优估计道路坡度值按零处理。一般可选取的t1≈1。
参阅图4，该图为加速度校正的流程图，利用短时角速度积分得到角度的可信性，用以去除加速度计中的汽车本身纵向加速度和其他噪声干扰。包括以下步骤：
a、初始化一计时器，且计时器输出当前计时时间time1，首次执行该步骤时计时器按清零处理，且计时器一直计时；
b、判断time1＜t1是否成立，若成立，则取距当前时刻之前的time1时间段内的所有绕汽车横向的角速度值进行离散积分，得到在这段时间内的角度变化量β，并返回a，若不成立，则清零计时器和离散积分器，并进入c；
c、获取距当前时刻t1前的最优估计道路坡度值，若上电开始到执行该步骤时间不足t1，则距当前时刻t1前的最优估计道路坡度值按零处理，并转换为倾角值α；
d、采用式(1)计算允许的加速度计采集的信号斜率上限和下限；
式中，kl——允许的加速度计采集的信号斜率下限
kh——允许的加速度计采集的信号斜率上限
bias_acc——考虑角速度的零飘特性对该斜率计算带来的影响
β1——t1时间内真实的汽车道路倾角变化值
β2——t1时间内极限的角速度的零飘值，取值范围在0.1-0.2度/s
e、利用d计算的斜率上下限，用信号斜率限制处理方法对信号预处理后的汽车纵向加速度信号滤波，得到除去加速度信号中其他干扰信息保留重力加速度在坡道上的分量信息的加速度值。
参阅图5，该图为角速度和加速度数据融合中加权因数获取方法时计算count值流程图，其包含如下步骤：
a、初始化一计时器，且计时器输出当前计时时间time2，首次执行该步骤时计时器按清零处理，且计时器一直计时；
b、采集经过步骤一校准后的汽车纵向加速度ax_corr、横向加速度ay_corr、垂向加速度az_corr，判断当前时刻式(2)是否成立，当满足式(2)时，count＝count+1，当不满足式(2)时，count＝count+0；
式中，δg——门限值，衡量当前三轴加速度与重力加速度的偏离程度
c、判断time2＜t2是否成立，若成立，输出上一次time2＜t2不成立时得到的count值，若不成立，输出这一次计算得到的count值，并清零count值，清零计时器，为下一次统计t2时间内满足式(2)的个数做准备；
一般δg可选的值能够区别三轴加速度与重力加速度的离散程度大小，当汽车处于急加速，急减速，拐弯，上下颠簸的路面时，就应统计处于这些路况下的情况，因为这些路况情况变化很大，导致采集数据不可信。一般δg在0.01g-0.03g之间。
参阅图6，该图为加权因数获取方法和测量值判定方法流程图，其中加权因数获取方法按照以下步骤：
a、创建数组gy_corr[n],初始数组gy_corr[n]值全为零，每采集一次数据，对数组进行更新，更新的方法按照式(3)进行(其中数组的首位索引规定为0)；
gy_corr——步骤一中被校正过的陀螺仪当前时刻测量的角速度值
b、每当采集一次数据，对a中建立的数组gy_corr[n]中的元素按照式(4)计算，判断t2时间段内的角度变化量是否大于门限值，若满足式(4)，则车体倾角正在变化；
式中，δθ——角度变化门限值
dt——获取前一时刻与获取当前时刻信号之间的时间间隔
c、当b中判断车体倾角正在变化时，令加权因数k等于零，若不满足式(4)，则按照下表对加权因数k进行赋值。
表中，k1、k2、k3、k4——数据融合中的加权因子
其中加速度坡度计算的判定方法按照以下步骤：
a、利用校正后的纵向加速度信号，并采用式(5)得到利用纵向加速度信号得到的道路倾角；
式中，ax_limit——经过加速度校正步骤校正后的纵向加速度信号
za(k)——仅利用纵向加速度信号得到的道路倾角
b、从加速度和角速度数据融合中加权因数的计算方法中判断车体倾角没有变化时，测量值按式(6)得到。反之，加速度计算的道路倾角等于被限制斜率后的za(k)，其斜率限制门限值方法按式(7)计算得到，且限制斜率的方法与步骤二中的信号斜率限制处理方法相同；
式中，kmax——最大斜率限制门限
kmin——最小斜率限制门限
maxup——汽车上坡时道路倾角最大斜率限制门限值
minup——汽车上坡时道路倾角最小斜率限制门限值
maxdn——汽车下坡时道路倾角最大斜率限制门限值
mindn——汽车下坡时道路倾角最小斜率限制门限值
参阅图7，该图为步骤五中坡度校正的流程图，主要校正汽车车体本身的弹性变形和外界环境对传感器随机干扰造成的影响，使其信号尽量平稳。按照以下步骤：
a、从传感器上电开始，初始化一计时器，读取计时的时间为time3，只要该估计方法依附的硬件不断电，计时器一直计时；
b、获取数据加权融合后的初步道路坡度估计值xe(k)，并判断式(8)是否成立，若不成立，执行式(9)，不清零计时器；反之执行式(10)，并清零计时器；
式中，xo(k-1)——前一时刻经过坡度校正后得到的最优道路坡度估计值，初次执行时该值为零
δst——门限值，该值对数值输出的平稳化程度有关
式中，——得到的最优道路坡度估计值的中间变量
c、取定时器的当前计时时间time3，判断式(11)是否成立，若不成立，执行式(12)，不清零计时器；反之执行式(13)，并清零计时器；
式中，δt——消除稳态误差的时间门限值
δse——门限值，弥补门限值δst带来的稳定误差而提出的更小门限值
式中，xo(k)——平稳后的最优坡度估计值
一般推荐的δst≈1，δse≈0.5，δt≈2。
d、定义道路坡度的偏差值常量slope_bias，没有赋值之前按零初始化，被赋值之后保持被赋予的值不变，将得到平稳后的最优坡度估计值减去道路坡度的偏差值slope_bias，即可得到最优的道路估计坡度，道路坡度的偏差值slope_bias赋值方法按照以下步骤：
1)将汽车停置在水平的道路上，该估计方法依附的硬件一直监控是否通过can总线接收到事先规定好的特定调平报文；
2)当用户有调平的意图时，操作某一调平的设备，使得这一设备向can总线上发送了一条调平报文，当该估计方法依附的硬件接收到该报文时，该硬件就从接收到该条报文后采集最近一段时间内输出的最优的道路估计坡度值，并取这些值的平均值，将该平均值赋予slope_bias；
参阅图8，为本发明所述的一种汽车用道路坡度实时估计方法核心思想，总结如下：
1)利用短时间内角速度积分得到角度变化量的可信性，对加速度计进行校正；
2)利用加速度计受到固定不变的重力加速度作用，对陀螺仪积分得到的角度进行牵制；
3)以卡尔曼滤波算法为主导思想，通过合理的数据加权融合，得到最优估算坡度值。
参阅图9，为在标准坡实际测试中，先上8％的坡，然后紧接着下20％的坡的纵向加速度校正前后结果对比，可以看出，校正前的原始加速度计采集到的纵向加速度含有很多干扰成分，特别是掺杂了汽车本身的纵向加速度，而校正后的加速度能够体现出了重力加速度在坡道上的分量。在数据融合时，可以得到准确的通过加速度计算出的坡度。
参阅图10，为在标准坡实际测试中，先上8％的坡，然后紧接着下20％的坡的恒定加权因数和动态变化加权因数k的对比结果图，可以看出，当加权因数k动态变化时，能够判断当前的测量值状态的可信程度，因此可以通过不同路况的变化而自适应加权系数的大小。因此得到的最有道路估计坡度也更加反应真实的道路坡度信息。
参阅图11，为在标准坡实际测试中，先上8％的坡，然后紧接着下20％的坡的平稳前后结果对比图，可以看出，经过数据融合后，输出结果能够表现出坡道信息，但坡道信息含有很多微小的震动成分。经过坡度校正后，基本上消除了由汽车本身的震动带来的影响，与实际的坡道信息更加吻合。
点击联系发帖人
