CN103353299B - 一种高精度车载道路坡度检测装置及方法 - Google Patents

Abstract

一种高精度车载道路坡度检测装置及方法，它适用于车载导航***与智能汽车电子领域。该装置由主控节点和检测节点组成，它们均挂接在车载CAN总线上，检测节点由三轴加速度传感器、微控制器、电源和CAN通信等模块组成；该装置的检测方法为：检测节点以地表水平面作为参考平面，通过三轴加速度传感器的输出值计算与参考平面倾角，并通过CAN总线将其发送给主控节点。主控节点通过轮询的方式收集检测节点数据，然后对其依次进行疏失误差剔除、最小二乘法拟合和卡尔曼滤波等融合处理，获得车体行驶道路坡度的最优估计值；本发明采用三轴数字加速度传感器计算倾角，并对多个传感器数据进行融合处理，提高了车载道路边坡检测的精度，检测精度可达0.5°。

Description

一种高精度车载道路坡度检测装置及方法

技术领域

本发明涉及车载导航***与智能汽车电子技术领域，特别涉及一种高精度车载道路坡度检测装置及方法。

背景技术

随着自动化技术的快速发展，车载导航***已成为智能汽车电子领域的重要组成部分之一，它不仅能给驾驶员提供全面的地理路况信息，还能帮助其做出安全可靠的控制策略。精确地检测车体当前行驶道路的坡度是车载导航***所必备的功能之一。但是，现有的车载倾角检测装置普遍存在如下问题：（1）通过对当前车速微分，或对传统两轴加速度传感器的模拟信号进行A/D采样等方法获取车体加速度和倾斜角度信息，它们均存在检测精度低、适用范围较窄等缺陷和不足；（2）使用单一传感器节点进行数据检测，这种方式数据描述片面，而且易受温度、电压、电磁等因素的影响，导致数据可靠性差。因此，目前的检测装置无法满足高精度车载导航***的实际需求。

发明内容

为了克服上述现有技术的缺陷，本发明的目的在于提供一种高精度车载道路坡度检测装置及方法，利用三轴数字加速度传感器设计检测节点，采集并计算车体倾斜角度，然后通过CAN总线将数据发送至主控节点；主控节点对不同位置的多个传感器节点的检测数据进行信息融合处理，获得车体倾斜角度的最优估计值，以实现对道路坡度的高精度检测。

为了达到上述目的，本发明的技术方案是这样实现的：

一种高精度车载道路坡度检测装置，包括分布在车体各处的检测节点1，检测节点1与主控节点2之间通过CAN总线通信；检测节点1所处平面与车体底盘平面平行，检测节点1包括传感器检测模块3、微控制模块4、CAN通信模块5和为各模块提供稳定的电压供应的电源管理模块6；传感器检测模块3采用三轴数字加速度传感器；CAN通信模块5由总线控制芯片7和驱动芯片8组成；总线控制芯片7通过SPI总线与微控制模块（4）相连，完成数据与控制信息的交互；各检测节点之间通过双绞屏蔽线进行总线式连接，并在首尾节点的CANH和CANL两端并联终端匹配电阻。

基于上述装置的检测方法，包括以下检测步骤：

步骤一、通过配置相关寄存器初始化三轴数字加速度传感器中三个轴的偏移量，完成初始倾角值复位；

步骤二、传感器检测模块3中的三轴数字加速度传感器测量X、Y、Z三个相互正交方向的动、静态的加速度，传感器检测模块3与微控制模块4进行数据和控制信息的交互；控制模块4计算出各检测节点1的倾斜角度；

步骤三、主控节点2分别给各检测节点1发送远程帧，以此下达信息收集指令；检测节点1在收到发送给自己的远程帧时，将最新的倾斜角度信息回传给主控节点2，否则处于侦听或休眠状态；

步骤四、主控节点2收集完网内所有检测节点1的倾斜角度信息后，便进行数据处理；数据处理按如下步骤进行：（一）通过疏失误差剔除法剔除无效的检测节点数据，消除倾角测量中的随机干扰；（二）通过最小二乘法拟合剩余的测量值，作为卡尔曼滤波器的观测值输入；（三）结合上一检测周期的最优估计结果，利用卡尔曼滤波器完成本周期内的道路坡度最优估计值预测，即高精度的车载道路坡度值；

步骤四所述的疏失误差剔除法，其具体实施方法为：设现场总线网络中共有n个检测节点，表示主控节点在第k个检测周期收集到的检测节点i的角度测量值，则第k个周期内所有检测节点角度测量值组成的集合为对集合A进行从小到大的排序得到集合B={x₁(k),x₂(k),x₃(k),…,x_n(k)}，那么疏失误差剔除后的集合C由式(3)～式(7)计算得到：

$x_M=\mathrm{MED}\{x_1\left(k\right),x_2\left(k\right),x_3\left(k\right),\·\·\·,x_n\left(k\right)\}=\left\{\begin{array}{cc}{x}_{\frac{n+1}{2}}\left(k\right)& n=2m+1\\ \frac{x_{n/2}\left(k\right)+x_{n/2+1}\left(k\right)}{2},& n=2m\end{array}\right.---\left(3\right)$

DS=x_U-x_D    (4)

x_U=MED{x_M,…,x_n(k)}   (5)

x_D=MED{x₁(k),…,x_M}   (6)

C={x_i(k)||x_i(k)-x_M≤DS}   (7)

其中x_M为集合B的中位数，DS为四分位离散度,x_U和x_D分别为其上四分位数和下四分位数，MED{·}为求集合中位数的运算。满足集合C的测量值为有效的，否则为误差值，应剔除。

步骤四所述的最小二乘法拟合剩余测量值，其具体实施方法为：在剔除无效的传感器数据后，通过最小二乘法(Least Square Method)对集合C中的数据进行拟合，得到时刻k的观测值，如式(8)所示，其中LSM{·}为通过最小二乘法拟合集合数据的运算。

Y(k)=LSM{C}    (8)

步骤四所述的卡尔曼最优估计值预测，其具体实施方法为：卡尔曼滤波针对式(9)和(10)所示的离散线性***

X(k)=Φ·X(k-1)+W(k)    (9)

Y(k)=H·X(k)+V(k)    (10)

其中X(k)为k时刻的估计值；Y(k)为k时刻（第k个检测周期）的观测值，其值由步骤（二）最小二乘法拟合计算得到；参数Φ为***矩阵，H表征了k时刻估计值相对于k-1时刻的变化率；W(k)和V(k)分别为k时刻的过程噪声和测量噪声。对于式(9)和(10)所描述的离散线性***，卡尔曼滤波结合第k-1个检测周期的最优估计结果，通过式(11)～(14)计算第k个检测周期倾角的最优估计值。

P(k|k-1)=Φ·P(k-1|k-1)+Q   (11)

$K_g\left(k\right)=\frac{P\left(k\right|k-1)\·H^{\′}}{H\·P\left(k\right|k-1)\·H^{\′}+R}---\left(12\right)$

P(k|k)=P(k|k-1)·(1-K_g(k)·H)   (13)

X(k|k)=X(k|k-1)+K_g(k)·(Y(k)-H·X(k|k-1))   (14)

其中Q和R分别为W(k)和V(k)的协方差，K_g(k)为k时刻的卡尔曼滤波增益，X(k|k)表征时刻k的最优结果，X(k|k-1)表征上一时刻的状态预测值，P(k|k)和P(k|k-1)分别为X(k|k)和X(k|k-1)对应的协方差。

本发明在硬件上通过低成本、低功耗、高分辨率的三轴数字加速度传感器设计倾斜检测节点，并在车体不同位置分别布设检测节点，采集各自的倾角信息；软件上把这些分布在不同位置上的多只同类传感器节点所提供的不完整观测量，依次依据疏失误差剔除、最小二乘拟合和卡尔曼滤波等优化准则组合起来，产生对车体行驶道路坡度的一致性解释和描述，消除多只传感器信息之间可能在的冗余和矛盾，降低不确定性，从而获得精确的道路坡度估计。本发明实现了对车体行驶道路坡度的高精度检测，同时，检测节点与主控节点之间通过CAN总线通信，保证了数据的可靠性与及时性，适合应用于车载导航***与智能汽车电子领域。

附图说明：

图1是高精度车载道路坡度检测装置结构图。

图2是检测节点功能模块图。

图3是角度检测原理图。

图4是现场总线网络通信流程示意图。

具体实施方式

下面结合附图作进一步的详细描述和说明。

参照图1和图2，一种高精度车载道路坡度检测装置，包括主控节点2和分布在车体各处的检测节点1，检测节点1与主控节点2之间通过CAN总线通信；检测节点1所处平面与车体底盘平面平行，检测节点1包括传感器检测模块3、微控制模块4、CAN通信模块5和电源管理模块6；传感器检测模块3采用三轴数字加速度传感器；微控制模块4采用了TI公司的超低功耗MSP430F149单片机；CAN通信模块5由总线控制芯片7和驱动芯片8组成；总线控制芯片7选用Microchip公司的MCP2510，该芯片性能稳定、功耗低，完全支持CAN总线V2.0B技术规范；MCP2510通过SPI总线与微控制器模块4连接，完成数据与控制信息的交互；CAN驱动芯片选用Philips公司的TJA1050，它与ISO11898标准完全兼容，最高传输速率可达1Mbps，且总线至少可挂接110个节点，各检测节点之间通过双绞屏蔽线进行总线式连接，并在首尾节点的CANH和CANL两端并联120Ω的终端匹配电阻，以吸收总线上的信号反射波；电源管理模块6为各模块提供稳定的电压供应；

传感器检测模块3采用Analogy Devices公司的三轴数字加速度传感器ADXL345，它体积小、功耗低、分辨率高，测量范围达±16g（g为自由落体加速度），可测量X、Y、Z三个相互正交方向的动、静态的加速度，ADXL345同样通过SPI总线与微控制器模块4进行数据和控制信息的交互，可通过配置相关寄存器初始化三个轴的偏移量，复位初始倾角值。

基于上述检测装置的检测方法，包括以下步骤：

步骤一、通过配置相关寄存器初始化三轴数字加速度传感器中三个轴的偏移量，完成初始倾角值复位；

步骤二、传感器检测模块3中的三轴数字加速度传感器测量X、Y、Z三个相互正交方向的动、静态的加速度，传感器检测模块3与微控制模块4进行数据和控制信息的交互；控制模块4计算出各检测节点1的倾斜角度；

图3是倾角检测原理图。图1已经规定初始情况下各检测节点以地表水平面作为参考平面，如图3(a)所示，A_x、A_y、A_z分别为传感器在三个方向上检测到的静止加速度。当车体未发生倾斜时，A_z与重力加速度的大小和方向均相同，而A_x，A_y上的分量为零；当车体发生了如图3(b)所示的倾斜时，其各轴的加速度变化如图3(c)所示，其中A_xoy为传感器在xoy平面上的加速度，其计算方法如公式(1)所示：

$A_{\mathrm{xoy}}=\sqrt{A_x^2+A_y^2}---\left(1\right)$

此时该检测节点计算到的车体xoy平面与地表水平面XOY倾斜的角度α如公式(2)所示：

$\mathrm{\α}=\arctan \frac{A_{\mathrm{xoy}}}{A_z}---\left(2\right)$

即通过此方法可将获取的一组加速度信息进行处理，得到检测节点各子平面与地表水平面之间的倾角。由于ADXL345三轴加速度传感器最高分辨率可达3.9mg/LSB，因此单个传感器能检测不到1.0°的倾斜角度变化。

步骤三、参照图4，网络的一个检测周期由数据获取和数据处理两个阶段组成，在数据获取阶段，主控节点2分别给各检测节点1发送远程帧，以此下达信息收集指令；检测节点1在收到发送给自己的远程帧时，将最新的倾斜角度信息回传给主控节点2，否则处于侦听或休眠状态。对于节点数目不多的网络，上述这种轮询通信可有效控制总线上的数据流量，避免拥塞与碰撞的发生，并保证数据的可靠性与实时性。

步骤四、参照图4，主控节点2收集完网内所有检测节点1的倾斜角度信息后，便进入数据处理阶段；为解决单一传感器数据描述的片面性，消除其检测信息中的噪声，主控节点对收集的一轮数据进行融合处理，从而得到本次检测周期内车体行驶道路坡度的最优估计值；数据处理阶段按如下步骤进行：（一）通过疏失误差剔除法剔除无效的检测节点数据，消除倾角测量中的随机干扰；（二）通过最小二乘法拟合剩余的测量值，作为卡尔曼滤波器的观测值输入；（3）结合上一检测周期的最优估计结果，利用卡尔曼滤波器完成本周期内的道路坡度最优估计值预测，即高精度的车载道路坡度值。卡尔曼滤波保证了检测数据以最小均方差准则融合最优估计值。

步骤四所述的疏失误差剔除法，其具体实施方法为：设现场总线网络中共有n个检测节点，表示主控节点在第k个检测周期收集到的检测节点i的角度测量值，则第k个周期内所有检测节点角度测量值组成的集合为对集合A进行从小到大的排序得到集合B={x₁(k),x₂(k),x₃(k),…,x_n(k)}。那么疏失误差剔除后的集合C由式(3)～式(7)计算得到。

$x_M=\mathrm{MED}\{x_1\left(k\right),x_2\left(k\right),x_3\left(k\right),\·\·\·,x_n\left(k\right)\}=\left\{\begin{array}{cc}{x}_{\frac{n+1}{2}}\left(k\right)& n=2m+1\\ \frac{x_{n/2}\left(k\right)+x_{n/2+1}\left(k\right)}{2},& n=2m\end{array}\right.---\left(3\right)$

DS=x_U-x_D   (4)

x_U=MED{x_M,…,x_n(k)}   (5)

x_D=MED{x₁(k),…,x_M}   (6)

C={x_i(k)||x_i(k)-x_M≤DS}   (7)

其中x_M为集合B的中位数，DS为四分位离散度,x_U和x_D分别为其上四分位数和下四分位数，MED{·}为求集合中位数的运算。满足集合C的测量值为有效的，否则为误差值，应剔除。

步骤四所述的最小二乘法拟合剩余测量值，其具体实施方法为：在剔除无效的传感器数据后，通过最小二乘法(Least Square Method)对集合C中的数据进行拟合，得到时刻k的观测值，如式(8)所示，其中LSM{·}为通过最小二乘法拟合集合数据的运算。

Y(k)=LSM{C}   (8)

步骤四所述的卡尔曼最优估计值预测，其具体实施方法为：考虑到对正在行驶车辆倾斜角度的测量是一个渐变的过程，即k时刻测得的角度应与k-1时刻相关，本发明采用卡尔曼滤波法估算角度的最优估计值。卡尔曼滤波针对式(9)和(10)所示的离散线性***。

X(k)=Φ·X(k-1)+W(k)   (9)

Y(k)=H·X(k)+V(k)   (10)

其中X(k)为k时刻的估计值；Y(k)为k时刻（第k个检测周期）的观测值，其值由步骤（二）最小二乘法拟合计算得到；参数Φ为***矩阵，H表征了k时刻估计值相对于k-1时刻的变化率；W(k)和V(k)分别为k时刻的过程噪声和测量噪声。对于式(9)和(10)所描述的离散线性***，卡尔曼滤波结合第k-1个检测周期的最优估计结合，通过式(11)～(14)计算第k个检测周期倾角的的最优估计值。

P(k|k-1)=Φ·P(k-1|k-1)+Q   (11)

$K_g\left(k\right)=\frac{P\left(k\right|k-1)\·H^{\′}}{H\·P\left(k\right|k-1)\·H^{\′}+R}---\left(12\right)$

P(k|k)=P(k|k-1)·(1-K_g(k)·H)   (13)

X(k|k)=X(k|k-1)+K_g(k)·(Y(k)-H·X(k|k-1))   (14)

其中Q和R分别为W(k)和V(k)的协方差，K_g(k)为k时刻的卡尔曼滤波增益，X(k|k)表征时刻k的最优结果，X(k|k-1)表征上一时刻的状态预测值，P(k|k)和P(k|k-1)分别为X(k|k)和X(k|k-1)对应的协方差。在本实施例中，取F=H=1，X(0)=1，P(0)=0；由于选用的三轴加速度传感器工作在全分辨率模式下，此时的灵敏度偏差为3.9mg/LSB，即测量噪声V(k)的协方差R=3.9×10^-3；设过程噪声服从高斯分布，为了得到比较合理的预测效果，需保证Q和R在一个数量级，本文取Q=1×10^-3。

Claims (5)

1.一种高精度车载道路坡度检测装置，包括分布在车体各处的检测节点(1)，检测节点(1)与主控节点(2)之间通过CAN总线通信，其特征在于，检测节点(1)所处平面与车体底盘平面平行，检测节点(1)包括传感器检测模块(3)、微控制模块(4)、CAN通信模块(5)和为各模块提供稳定的电压供应的电源管理模块(6)；传感器检测模块(3)采用三轴数字加速度传感器；CAN通信模块(5)由总线控制芯片(7)和驱动芯片(8)组成；总线控制芯片(7)通过SPI总线与微控制模块(4)相连，完成数据与控制信息的交互；各检测节点之间通过双绞屏蔽线进行总线式连接，并在首尾节点的CANH和CANL两端并联终端匹配电阻。

2.基于权利要求1装置的检测方法，其特征在于，包括以下检测步骤：

步骤一、通过配置相关寄存器初始化三轴数字加速度传感器中三个轴的偏移量，完成初始倾角值复位；

步骤二、传感器检测模块(3)中的三轴数字加速度传感器测量X、Y、Z三个相互正交方向的动、静态的加速度，传感器检测模块(3)与微控制模块(4)进行数据和控制信息的交互；控制模块(4)计算出各检测节点(1)的倾斜角度；

步骤三、主控节点(2)分别给各检测节点(1)发送远程帧，以此下达信息收集指令；检测节点(1)在收到发送给自己的远程帧时，将最新的倾斜角度信息回传给主控节点(2)，否则处于侦听或休眠状态；

步骤四、主控节点(2)收集完网内所有检测节点(1)的倾斜角度信息后，便进行数据处理；数据处理按如下步骤进行：(一)通过疏失误差剔除法剔除无效的检测节点数据，消除倾角测量中的随机干扰；(二)通过最小二乘法拟合剩余的测量值，作为卡尔曼滤波器的观测值输入；(三)结合上一检测周期的最优估计结果，利用卡尔曼滤波器完成本周期内的道路坡度最优估计值预测，即高精度的车载道路坡度值。

3.根据权利要求2所述的检测方法，其特征在于，步骤四所述的疏失误差剔除法，其具体实施方法为：设现场总线网络中共有n个检测节点，表示主控节点在第k个检测周期收集到的检测节点i的角度测量值，则第k个周期内所有检测节点角度测量值组成的集合为对集合A进行从小到大的排序得到集合B＝{x₁(k),x₂(k),x₃(k),…,x_n(k)}，那么疏失误差剔除后的集合C由式(3)～式(7)计算得到：

$x_M=\mathrm{MED}\{x_1\left(k\right),x_2\left(k\right),x_3\left(k\right),...,x_n\left(k\right)\}=\left\{\begin{array}{cc}{x}_{\frac{n+1}{2}}\left(k\right),& n=2m+1\\ \frac{x_{n/2}\left(k\right)+x_{n/2+1}\left(k\right)}{2},& n=2m\end{array}\right.---\left(3\right)$

DS＝x_U-x_D                 (4)

x_U＝MED{x_M,…,x_n(k)}                 (5)

x_D＝MED{x₁(k),…,x_M}                 (6)

C＝{x_i(k)||x_i(k)-x_M|≤DS}            (7)

其中x_M为集合B的中位数，DS为四分位离散度,x_U和x_D分别为其上四分位数和下四分位数，MED{·}为求集合中位数的运算，满足集合C的测量值为有效的，否则为误差值，应剔除。

4.根据权利要求2所述的检测方法，其特征在于，步骤四所述的最小二乘法拟合剩余测量值，其具体实施方法为：在剔除无效的传感器数据后，通过最小二乘法(Least Square Method)对集合C中的数据进行拟合，得到时刻k的观测值，如式(8)所示，其中LSM{·}为通过最小二乘法拟合集合数据的运算：

Y(k)＝LSM{C}                 (8)

5.根据权利要求2所述的检测方法，其特征在于，步骤四所述的卡尔曼最优估计值预测，其具体实施方法为：卡尔曼滤波针对式(9)和(10)所示的离散线性***

X(k)＝Φ·X(k-1)+W(k)                 (9)

Y(k)＝H·X(k)+V(k)                 (10)

其中X(k)为k时刻的估计值；Y(k)为k时刻(第k个检测周期)的观测值，其值由步骤(二)最小二乘法拟合计算得到；参数Φ为***矩阵，H表征了k时刻估计值相对于k-1时刻的变化率；W(k)和V(k)分别为k时刻的过程噪声和测量噪声；对于式(9)和(10)所描述的离散线性***，卡尔曼滤波结合第k-1个检测周期的最优估计结果，通过式(11)～(14)计算第k个检测周期倾角的最优估计值：

P(k|k-1)＝Φ·P(k-1|k-1)+Q                 (11)

$K_g\left(k\right)=\frac{P\left(k\right|k-1)\·H^{\′}}{H\·P\left(k\right|k-1)\·H^{\′}+R}---\left(12\right)$

P(k|k)＝P(k|k-1)·(1-K_g(k)·H)                 (13)

X(k|k)＝X(k|k-1)+K_g(k)·(Y(k)-H·X(k|k-1))             (14)

其中Q和R分别为W(k)和V(k)的协方差，K_g(k)为k时刻的卡尔曼滤波增益，X(k|k)表征时刻k的最优结果，X(k|k-1)表征上一时刻的状态预测值，P(k|k)和P(k|k-1)分别为X(k|k)和X(k|k-1)对应的协方差。