CN101598549B - 一种车辆行驶坡度与相对高度动态估计方法 - Google Patents

Abstract

本发明公开了一种车辆行驶坡度与相对高度动态估计方法，该方法是在单轴加速度计和里程仪硬件基础上运用算法计算出道路坡度及车辆行驶相对高度信息。单轴加速度计传感器测量车辆纵轴方向上的加速度accy；里程仪可精确测量车辆行驶速度，这样就可以提供车辆加速度信息a_车，由accy和a_车两者之间的几何关系，可得到坡度值及高度变化值，然后由高度变化和限制规则判断出车辆上下坡。在具体实施过程中要考虑单轴加速度计安装误差、加速度数据的噪声处理、两种传感器数据的对准以及上下坡的判断规则问题。该方法测量精度高；提高了***的稳定可靠性；实现了实时检测车辆坡度和高度，很高的灵敏度和响应速度；功耗小，启动快，简单易行。

Description

一种车辆行驶坡度与相对高度动态估计方法

技术领域

本发明属于车载导航技术领域，具体涉及一种导航***中动态检测车辆行驶道路坡度和高度的方法。

背景技术

在现有的车载导航***中，通过GPS可以获得海拔高度信息，但是由于GPS***测量特性所限，高度信息误差很大，因此GPS接收机直接输出的高度信息不能满足车辆上下坡判定，以及行驶相对高度变化估计的需要。

当前，测量倾角(即道路坡度)的方法主要有两种：传统的水泡式框式(条式)水平仪：其检测方法依然是“水泡移动，肉眼分辨”。这种原始的检测方法有诸多缺点，如测量值因人而易、功能单一、测量范围小等；另外一种是基于加速度传感器的倾角仪：它与传统的水泡式框式(条式)水平仪相比具有电子检测、测量精度高、测量范围大、使用及携带方便等特点，但是这种方法不能实现加速运动情况下角度测量，应用受到较大限制。

发明内容

本发明的目的是为了解决车载导航中判断车辆上下坡的问题，从而实现更精确定位，提供了一种动态检测车辆行驶道路坡度及行驶相对高度的方法，该方法采用了中位值滤波和信息融合技术手段，达到了实时判断车辆上下坡技术效果。

本发明在单轴加速度计和里程仪测量信息基础上，利用加速度的分配关系计算车辆行驶坡度角。若单轴加速度计敏感方向和车辆纵轴方向一致，此单轴加速度计传感器测量车辆纵轴方向上的加速度accy；里程仪可精确测量车辆行驶速度，这样就可以提供车辆加速度信息a_车，由加速度accy车辆加速度和a_车两者之间的几何关系，可得到坡度值及高度变化值，然后由高度变化判断出车辆上下坡。在具体设计过程中要考虑单轴加速度计安装误差、加速度数据的噪声处理、两种传感器数据的对准以及上下坡的判断规则问题。此方法相对于三轴、双轴加速度计测量倾角稳定性更好。

本发明提出的一种车辆行驶坡度与相对高度动态估计方法是在现有车载导航***的硬件设备基础上，首先实时检测出车辆行驶的坡度，然后结合车辆行驶的距离信息得到行驶相对高度。车辆坡度计算结果的好坏直接影响着高度的精确性，因此本方法的关键就在于坡度的计算。具体步骤为：

步骤一、安装角α装定。

设备安装角的获得方式有两种：

①根据装定的安装角初始化安装角α。

②测量安装角α。当无法获得装定时候的安装角信息时，采用单轴加速度计方式，即利用静态情况下坡度计算公式粗标设备安装角α，具体如下：

单轴加速度计测量的加速度accy感应的是非引力加速度，即车辆推力F、支持力N和摩擦力f分别在此方向分量的合力，即有

accy＝(F×cosα+N×sinα-f×cosα)/m    (1)

式中m为车辆质量。

根据车辆纵轴和横轴方向上的受力平衡有：

a_车＝(F-f)/m-G0×sinθ                  (2)

N＝m×G0×cosθ

式中G0为车辆重力加速度，θ为车辆所在位置的坡度。

由式(1)、式(2)得到：

accy＝a_车×cosα+G0×sinθ×cosα+G0×cosθ×sinα       (3)

accy＝a_车×cosα+G0×sin(θ+α)                          (4)

由式(4)得到坡度的计算公式：

当α＝0时，由由式(4)得到：

accy＝a_车+G0×sinθ                        (6)

此时的坡度θ计算公式为：

当车辆静止，不考虑设备安装角时，即静态情况下：a_车＝0，α＝0，

$\mathrm{\θ}={\sin }^{-1}\left(\frac{\mathrm{accy}}{G0}\right)---\left(8\right)$

采集车辆静止时的加速度计输出数据accy，尽量保证车辆在水平面，此时测量的坡度θ就是设备的安装角α： $\mathrm{\α}=\mathrm{\θ}={\sin }^{-1}\left(\frac{\mathrm{accy}}{G0}\right).$ 为下面计算坡度过程中设备安装角的补偿提供依据。在此过程中尽管不能完全保证车辆静止在水平面上，但安装角的这种估计误差对高度变化小时误差不大，不影响判断上下坡趋势。

步骤二、对单轴加速度计传感器输出的数据进行中位值滤波处理。

对加速度计数据accy进行长度为n(一般取5)的中位值滤波处理以得到剔除野点后的数据再进行后续工作。本方法中单轴加速度计敏感车辆纵轴方向的加速度，其数据记为accy，因此对加速度accy进行中位值滤波得到滤波后的结果记为accy_filted；

步骤三、对加速度计传感器和里程仪传感器传输的信息进行时间对准。

利用信息融合方法对加速度计传感器和里程仪传感器传输的信息进行时间对准。同时利用两种传感器的信息，保证这二者的同步是最为关键的，考虑硬件成本需选择合适的数据传输频率，将信息量较大的加速度计的采集频率设为n1 Hz，而信息量较小的里程仪的采样频率设为n2 Hz，n2＜n1，本文中取n1＝50Hz，n2＝1Hz，即里程仪输出的脉冲数是1s一次输出，而单轴加速度计输出的数据是1s有50次输出，因此在时间对准的原则下统一两部分数据。若***具有GPS，以GPS时间(也即里程仪时间)为基准，对应GPS当前时间n秒的加速度输出处理是这样的：对加速度传感器的时间在n和n+1之间(包括n秒的数据)的所有加速度数据取均值，把这个均值作为与GPS时间n秒对应的加速度数据accy_filted_AVG，这样就实现了在时间对准原则下加速度计和里程仪数据的融合；若***无GPS，则以加速度计采集次数作为与里程仪脉冲对准依据。

最后数据格式是每一秒对应有一个加速度值accy_filted_AVG和一个里程仪脉冲数odopulse。

步骤四、计算车辆的加速度a_车。

由里程仪传感器每秒输出的脉冲数odopulse和里程仪的刻度因数K_od(封装好的参数)求得车辆每秒行驶的距离Dis_ODM＝odopulse×K_od，然后根据距离差可以计算出车辆每秒的加速度a_车：

a_车(t)＝Dis_ODM(t)-Dis_ODM(t-1)。

步骤五、计算车辆坡度θ。

本方法融合了里程仪和惯性传感器的信息，这样就避免了单方面信息的不足带来的影响，实现实时检测到坡度。

将步骤三中得到的加速度数据accy_filted_AVG代入坡度计算公式(5)：

得到车辆坡度θ：

其中G0为车辆的重力加速度，accy_filted_AVG为融合对准后的加速度值。

步骤六、计算高度变化：

由车辆每秒行驶的距离Dis_ODM和每秒坡度θ，计算出每秒的高度差high：

high＝Dis_ODM×sin(θ)                    (10)

步骤七、判断上下坡。

由步骤六可以实时得到车辆每秒行驶的高度，由于是进行单秒计算，此高度差即使是在上下坡也不会很大，其中数据误差、计算误差等一些随机误差在此高度差中所占比重就会比较大，因此由每秒的高度差判断上下坡就很不可靠。可选择分段计算高度差，降低误差的影响，此处取3秒作为一个时间段，即每3秒进行积分，计算出一个高度差。此积分时间若太长则会产生积分累积误差，若太短，则积分后的高度差值比较小容易产生误判，3秒是由多组实验数据进行统计分析确定的经验值。下面以这些分段高度差作为依据，判断出车辆是否上下坡：

(1)分段计算高度：设置每段时间为3秒，，即每3秒进行一次积分，每3秒计算出一个高度差high_step。

(2)设置上下坡高度阈值：上坡阈值Delt_UP＝0.4，下坡阈值Delt_DOWN＝-0.4。当high_step＞Delt_UP时则认为是在上坡，并记录下满足此条件的时刻，构造满足上坡条件的时刻序列T1；

当high_step＜Delt_DOWN则认为是在下坡，并记录下满足此条件的时刻，构造满足下坡条件的时刻序列T2；

(3)若时刻序列T1中相邻元素T1(n)、T1(n+1)间的时刻差Δt＝T1(n+1)-T1(n)≤6s则认为此时间段[T1(n)，T1(n+1)]为上坡段，反之则不是。

若时刻序列T2中相邻元素T2(n)、T2(n+1)间的时刻差Δt＝T2(n+1)-T2(n)≤6s则认为此时间段[T2(n)，T2(n+1)]为下坡段，反之则不是。

本发明的优点在于：

(1)本方法考虑了设备安装角以及对惯性数据采用了去噪处理，大大提高了测量精度；

(2)本方法利用单轴加速度计与里程仪信息融合实现坡度识别，避免了完全依赖加速度计传感器的弊端，提高了***的稳定性与可靠性；

(3)本方法实现了实时检测车辆坡度和高度，有较高的灵敏度和响应速度；

(4)功耗小，启动快，简单易行。

附图说明

图1是本发明方法的流程图；

图2是车辆行驶在坡度上的受力分析示意图；

图3是本发明中车辆行驶坡度计算的原理示意图；

图4是判断上下坡仿真图；

图5是仿真坡度结果图；

图6是仿真高度结果图。

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本方法为了提高车辆上下坡度估计结果的精度，考虑了安装角的补偿、加速度数据的去噪处理和信息的融合处理，本发明提供的方法流程如图1所示，具体通过如下步骤实现：

步骤一、粗标设备安装角α。

加速度计被加工安装在车辆上后，通常存在设备安装角α。设备安装角α是指综合的安装角，也就是惯性器件安装平面与水平面的夹角，在进行坡度计算之前必须对该安装角α进行标定，以便在坡度计算时对该角进行补偿。设备安装角的获得方式有两种：

(1)根据装定的安装角初始化安装角α；

(2)测量安装角α；若无法方便提供安装信息，可采用单轴加速度计方式，即利用静态情况下坡度计算公式粗标设备安装角α，具体如下：

单轴加速度计测量的加速度accy感应的是非引力加速度，即车辆推力F、支持力N和摩擦力f分别在此方向分量的合力，如图3所示即有：

accy＝(F×cosα+N×sinα-f×cosα)/m    (1)

根据车辆纵轴和横轴方向上的受力平衡，如图2所示有

a_车＝(F-f)/m-G0×sinθ                  (2)

N＝m×G0×cosθ

式中G0为重力加速度，m为车辆的质量，θ为车辆所在位置的坡度。

由式(1)、式(2)得到：

accy＝a_车×cosα+G0×sinθ×cosα+G0×cosθ×sinα     (3)

accy＝a_车×cosα+G0×sin(θ+α)                       (4)

由式(4)得到坡度的计算公式：

当α＝0时，由式(4)得到：

accy＝a_车+G0×sinθ                             (6)

此时的坡度θ计算公式为：

当车辆静止，不考虑设备安装角时，即静态情况下：a_车＝0，α＝0，

$\mathrm{\θ}={\sin }^{-1}\left(\frac{\mathrm{accy}}{G0}\right)---\left(8\right)$

采集车辆静止时的加速度计输出数据accy，尽量保证车辆在水平面，此时测量的坡度θ就是设备的安装角α： $\mathrm{\α}=\mathrm{\θ}={\sin }^{-1}\left(\frac{\mathrm{accy}}{G0}\right).$ 为下面计算坡度过程中设备安装角的补偿提供依据。

步骤二、对加速度计传感器输出的数据进行中位值滤波处理。

由于惯性器件对外界环境的敏感导致其输出很不稳定，因此本发明中对加速度计数据进行窗口宽度为n(n为自然数，一般取5)的中位值滤波处理以得到剔除野点后的数据再进行后续工作。本方法中单轴加速度计敏感车辆纵轴方向的加速度accy，因此对accy进行中位值滤波得到滤波后的结果记为accy_filted。

步骤三、对加速度计传感器和里程仪传感器传输的信息进行时间对准。

本发明通过数据融合的方式进行时间对准。数据融合作为一种数据处理方法，其基本思想是综合利用***各个方面的数据，最大限度地抽取有关对象或环境的有效信息，以达到更准确、更全面地认识观测对象或环境的目的。本发明综合了单轴加速度计和里程仪两方面的信息，同时利用二种传感器的信息，保证这二者的同步是最为关键的，考虑硬件成本需选择合适的数据传输频率，将信息量较大的加速度计的采集频率设为n1 Hz，而信息量较小的里程仪的采样频率设为n2 Hz，n2＜n1，本文中取n1＝50Hz，n2＝1Hz，即里程仪输出的脉冲数是1s一次输出，而单轴加速度计输出的数据是1s有50次输出，因此在时间对准的原则下统一两部分数据。若***具有GPS，以GPS时间(也即里程仪时间)为基准，对应GPS当前时间n秒的加速度输出处理是这样的：对加速度传感器的时间在n和n+1之间(包括n秒的数据)的所有加速度数据取均值，把这个均值作为与GPS时间n秒对应的加速度数据accy_filted_AVG，这样就实现了在时间对准原则下加速度计和里程仪数据的融合；若***无GPS，可以加速度计采集次数作为与里程仪脉冲对准依据。

最后数据格式是每一秒对应有一个加速度值accy_filted_AVG和一个里程仪脉冲数odopulse。

步骤四、计算车辆的加速度a_车。

由里程仪传感器每秒输出的脉冲数odopulse和里程仪的刻度因数K_od(封装好的参数)求得车辆每秒行驶的距离Dis_ODM＝odopulse×K_od，然后就可以计算出车辆每秒的加速度a_车：

a_车(t)＝Dis_ODM(t)-Dis_ODM(t-1)。

步骤五、计算车辆坡度θ。

计算每秒的坡度，并对安装角进行补偿，得到坡度如下：

其中G0为车辆的重力加速度。

步骤六、计算高度变化。

由车辆每秒行驶的距离Dis_ODM和每秒坡度θ计算出每秒的高度值：

high＝Dis_ODM×sin(θ)。

步骤七、判断上下坡。

由上面可以实时得到车辆每秒行驶的高度，由于是进行单秒计算，此高度差即使是在上下坡也不会很大，其中数据误差、计算误差等一些随机误差在此高度差中所占比重就会比较大，因此由每秒的高度差判断上下坡就很不可靠。可选择分段计算高度差，降低误差的影响，此处取3秒作为一个时间段，即每3秒进行积分，计算出一个高度差。此积分时间若太长则会产生积分累积误差，若太短，则积分后的高度差值比较小容易产生误判，3秒是由多组实验数据进行统计分析确定的经验值。下步则以这些分段高度差作为依据，再结合一些限制规则判断出车辆是否上下坡。

(1)分段计算高度：设置每段时间为3秒，即每3秒进行一次积分，每3秒计算出一个高度差high_step。

(2)设置上下坡高度阈值：上坡阈值Delt_UP＝0.4，下坡阈值Delt_DOWN＝-0.4。

当high_step＞Delt_UP时则认为是在上坡，并记录下满足此条件的时刻构造满足上坡条件的时刻序列T1；

当high_step＜Delt_DOWN则认为是在下坡，并记录下满足此条件的时刻构造满足下坡条件的时刻序列T2；

(3)若时刻序列T1中相邻元素T1(n)、T1(n+1)间的时刻差Δt＝T1(n+1)-T1(n)≤6s则认为此时间段[T1(n)，T1(n+1)]为上坡段，反之则不是。

若时刻序列T2中相邻元素T2(n)、T2(n+1)间的时刻差Δt＝T2(n+1)-T2(n)≤6s则认为此时间段[T2(n)，T2(n+1)]为下坡段，反之则不是。

图4是车辆行驶在一条上下坡交替道路上的三维轨迹图，x轴表示的是东向位置，y轴表示的是北向位置，z轴表示的是高度，点线._：代表平路；粗体实线-：代表是上坡；星线＊：代表下坡；图中标出了此段轨迹的上坡段和下坡段下以及上下坡的高度，可以看出上下坡的结果和实际是吻合的。

仿真实验生成两组数据：单轴加速度计输出的加速度值和里程仪输出的脉冲数，这两组数据是无噪声的，但由于实际情况中加速度计和里程仪测量的数据都有噪声，因此在仿真时对理想的加速度值和由理想的脉冲数获得的车辆加速度值分别加10％的噪声，图5和图6分别是通过matlab仿真得到的坡度和高度结果图：其中图5中实线表示的是理想的坡度曲线，虚线是对噪声数据应用本发明方法得到的坡度曲线，图6中实线表示的是理想的高度曲线，虚线是对噪声数据应用本发明方法得到的坡度曲线，从图中可以看出通过本发明方法获得的坡度值和高度值与无噪声的理想值非常接近，坡度误差在0.4934°以内，高度误差在1.8692m以内，达到了比较好的效果。

Claims (1)

1.一种车辆行驶坡度与相对高度动态估计方法，其特征在于：

步骤一、粗标设备安装角α；

设备安装角α的获得方式有如下两种：

(1)根据装定的安装角初始化安装角α；

(2)测量安装角α；

若无法提供安装信息，设备的安装角α通过如下公式获得：

其中G0为车辆的重力加速度，accy为单轴加速度计感应的加速度信息，θ为坡度；

步骤二、对单轴加速度计传感器输出的数据accy进行中位值滤波处理；

对加速度计数据进行窗口宽度为n的中位值滤波处理，得到滤波后的结果记为accy_filted，n为自然数；

步骤三、对单轴加速度计传感器和里程仪传感器传输的信息进行时间对准；

若***具有GPS，以GPS时间为基准，对应GPS当前时间n秒的加速度输出处理是这样的：对加速度传感器的时间在n和n+1之间的所有加速度数据取均值，把这个均值作为与GPS时间n秒对应的加速度数据accy_filted_AVG，这样就实现了在时间对准原则下加速度计和里程仪数据的融合；若***无GPS，以加速度计采集次数作为与里程仪脉冲对准依据；

步骤四、计算车辆的加速度a_车；

由里程仪传感器每秒输出的脉冲数odopulse和里程仪的刻度因数K_od求得车辆每秒行驶的距离Dis_ODM＝odopulse×K_od，然后计算出车辆每秒的加速度a_车：

a_车(t)＝Dis_ODM(t)-Dis_ODM(t-1)；

步骤五、计算车辆坡度θ；

计算每秒的坡度，并对安装角进行补偿，得到坡度如下：

其中G0为车辆的重力加速度；

步骤六、计算高度变化；

由车辆每秒行驶的距离Dis_ODM和每秒坡度θ计算出每秒的高度差high：

high＝Dis_ODM×sin(θ)；

步骤七、上下坡判断；

(1)分段计算高度：设置每段时间为3秒，即每3秒进行一次积分，每3秒计算出一个高度差high_step；

(2)设置上下坡高度阈值：上坡阈值Delt_UP＝0.4，下坡阈值Delt_DOWN＝-0.4；

当high_step＞Delt_UP时则认为是在上坡，并记录下满足此条件的时刻，构造满足上坡条件的时刻序列T1；

当high_step＜Delt_DOWN则认为是在下坡，并记录下满足此条件的时刻，构造满足下坡条件的时刻序列T2；

(3)若时刻序列T1中相邻元素T1(n)、T1(n+1)间的时刻差Δt＝T1(n+1)-T1(n)≤6s则认为此时间段[T1(n)，T1(n+1)]为上坡段，反之则不是；

若时刻序列T2中相邻元素T2(n)、T2(n+1)间的时刻差Δt＝T2(n+1)-T2(n)≤6s则认为此时间段[T2(n)，T2(n+1)]为下坡段，反之则不是。

Images