CN103353299B

CN103353299B - A high-precision vehicle-mounted road slope detection device and method

Info

Publication number: CN103353299B
Application number: CN201310246463.1A
Authority: CN
Inventors: 汪志伟; 李龙飞; 江申; 段泽能; 杜奕凡; 黄浪; 王晶
Original assignee: Xian Jiaotong University
Current assignee: Xian Jiaotong University
Priority date: 2013-06-20
Filing date: 2013-06-20
Publication date: 2015-07-08
Anticipated expiration: 2033-06-20
Also published as: CN103353299A

Abstract

The invention discloses a high-precision vehicle-mounted road grade detection device and method, which are applicable to the field of a car navigation system and smart car electronics. The device is composed of a mastering node and detection nodes, which are hooked on a vehicle-mounted CAN (controller area network) bus. Each detection node comprises a triaxial acceleration sensor, a microcontroller, a power supply and a CAN communication module. The detection method of the device comprises the following steps that: a dip angle of each detection node relative to a reference plane, namely a surface water plane, is calculated by the output value of the corresponding triaxial acceleration sensor and transmitted to the mastering node by the CAN bus; the mastering node acquires detection node data through polling and performs fusion processes including careless error elimination, least squares fit and Kalman filtering on the acquired data to obtain an optimal estimate value of the grade of a road on which a vehicle runs. According to the invention, the triaxial digital acceleration sensor is used to calculate the dip angle, and the fusion processes are performed on the data of the multiple sensors, so that the precision of the vehicle-mounted road grade detection can be improved to 0.5 degree.

Description

A high-precision vehicle-mounted road slope detection device and method

技术领域technical field

本发明涉及车载导航系统与智能汽车电子技术领域，特别涉及一种高精度车载道路坡度检测装置及方法。The invention relates to the technical field of vehicle navigation systems and intelligent vehicle electronics, in particular to a high-precision vehicle-mounted road slope detection device and method.

背景技术Background technique

随着自动化技术的快速发展，车载导航系统已成为智能汽车电子领域的重要组成部分之一，它不仅能给驾驶员提供全面的地理路况信息，还能帮助其做出安全可靠的控制策略。精确地检测车体当前行驶道路的坡度是车载导航系统所必备的功能之一。但是，现有的车载倾角检测装置普遍存在如下问题：（1）通过对当前车速微分，或对传统两轴加速度传感器的模拟信号进行A/D采样等方法获取车体加速度和倾斜角度信息，它们均存在检测精度低、适用范围较窄等缺陷和不足；（2）使用单一传感器节点进行数据检测，这种方式数据描述片面，而且易受温度、电压、电磁等因素的影响，导致数据可靠性差。因此，目前的检测装置无法满足高精度车载导航系统的实际需求。With the rapid development of automation technology, the car navigation system has become one of the important components in the field of smart car electronics. It can not only provide drivers with comprehensive geographic road condition information, but also help them make safe and reliable control strategies. Accurately detecting the slope of the road currently traveling on the vehicle body is one of the necessary functions of the vehicle navigation system. However, the existing vehicle-mounted inclination detection devices generally have the following problems: (1) The acceleration and inclination angle information of the vehicle body is obtained by differentiating the current vehicle speed or performing A/D sampling on the analog signal of the traditional two-axis acceleration sensor. All have defects and deficiencies such as low detection accuracy and narrow application range; (2) Using a single sensor node for data detection, this method of data description is one-sided, and is easily affected by factors such as temperature, voltage, and electromagnetics, resulting in poor data reliability . Therefore, the current detection device cannot meet the actual needs of high-precision vehicle navigation systems.

发明内容Contents of the invention

为了克服上述现有技术的缺陷，本发明的目的在于提供一种高精度车载道路坡度检测装置及方法，利用三轴数字加速度传感器设计检测节点，采集并计算车体倾斜角度，然后通过CAN总线将数据发送至主控节点；主控节点对不同位置的多个传感器节点的检测数据进行信息融合处理，获得车体倾斜角度的最优估计值，以实现对道路坡度的高精度检测。In order to overcome the defects of the above-mentioned prior art, the object of the present invention is to provide a high-precision vehicle-mounted road gradient detection device and method, which utilizes a three-axis digital acceleration sensor to design a detection node, collects and calculates the vehicle body tilt angle, and then passes the CAN bus. The data is sent to the main control node; the main control node performs information fusion processing on the detection data of multiple sensor nodes at different locations, and obtains the optimal estimated value of the inclination angle of the vehicle body, so as to realize high-precision detection of the road slope.

为了达到上述目的，本发明的技术方案是这样实现的：In order to achieve the above object, the technical solution of the present invention is achieved in that:

一种高精度车载道路坡度检测装置，包括分布在车体各处的检测节点1，检测节点1与主控节点2之间通过CAN总线通信；检测节点1所处平面与车体底盘平面平行，检测节点1包括传感器检测模块3、微控制模块4、CAN通信模块5和为各模块提供稳定的电压供应的电源管理模块6；传感器检测模块3采用三轴数字加速度传感器；CAN通信模块5由总线控制芯片7和驱动芯片8组成；总线控制芯片7通过SPI总线与微控制模块（4）相连，完成数据与控制信息的交互；各检测节点之间通过双绞屏蔽线进行总线式连接，并在首尾节点的CANH和CANL两端并联终端匹配电阻。A high-precision vehicle-mounted road gradient detection device, including detection nodes 1 distributed throughout the vehicle body, the detection node 1 communicates with the main control node 2 through the CAN bus; the plane where the detection node 1 is located is parallel to the plane of the vehicle body chassis, The detection node 1 includes a sensor detection module 3, a micro-control module 4, a CAN communication module 5 and a power management module 6 that provides stable voltage supply for each module; the sensor detection module 3 adopts a three-axis digital acceleration sensor; the CAN communication module 5 is connected by a bus The control chip 7 and the driver chip 8 are composed; the bus control chip 7 is connected with the micro-control module (4) through the SPI bus to complete the interaction of data and control information; the detection nodes are connected by the bus through twisted-pair shielded wires, and in the The CANH and CANL ends of the first and last nodes are connected in parallel with terminal matching resistors.

基于上述装置的检测方法，包括以下检测步骤：The detection method based on the above-mentioned device comprises the following detection steps:

步骤一、通过配置相关寄存器初始化三轴数字加速度传感器中三个轴的偏移量，完成初始倾角值复位；Step 1. Initialize the offsets of the three axes in the three-axis digital acceleration sensor by configuring the relevant registers to complete the reset of the initial inclination value;

步骤二、传感器检测模块3中的三轴数字加速度传感器测量X、Y、Z三个相互正交方向的动、静态的加速度，传感器检测模块3与微控制模块4进行数据和控制信息的交互；控制模块4计算出各检测节点1的倾斜角度；Step 2, the three-axis digital acceleration sensor in the sensor detection module 3 measures dynamic and static accelerations in three mutually orthogonal directions of X, Y, and Z, and the sensor detection module 3 and the micro control module 4 perform data and control information interaction; The control module 4 calculates the inclination angle of each detection node 1;

步骤三、主控节点2分别给各检测节点1发送远程帧，以此下达信息收集指令；检测节点1在收到发送给自己的远程帧时，将最新的倾斜角度信息回传给主控节点2，否则处于侦听或休眠状态；Step 3. The main control node 2 sends remote frames to each detection node 1 respectively, so as to issue information collection instructions; when the detection node 1 receives the remote frame sent to itself, it returns the latest tilt angle information to the main control node 2, otherwise it is in the listening or dormant state;

步骤四、主控节点2收集完网内所有检测节点1的倾斜角度信息后，便进行数据处理；数据处理按如下步骤进行：（一）通过疏失误差剔除法剔除无效的检测节点数据，消除倾角测量中的随机干扰；（二）通过最小二乘法拟合剩余的测量值，作为卡尔曼滤波器的观测值输入；（三）结合上一检测周期的最优估计结果，利用卡尔曼滤波器完成本周期内的道路坡度最优估计值预测，即高精度的车载道路坡度值；Step 4. After the main control node 2 collects the inclination angle information of all detection nodes 1 in the network, data processing is performed; the data processing is carried out in the following steps: (1) Eliminate invalid detection node data through the error elimination method to eliminate the inclination angle Random interference in the measurement; (2) Fit the remaining measured values by the least square method, and use them as the observation value input of the Kalman filter; (3) Combine the optimal estimation results of the previous detection cycle, and use the Kalman filter to complete Prediction of the optimal estimated value of the road slope in this cycle, that is, the high-precision vehicle road slope value;

步骤四所述的疏失误差剔除法，其具体实施方法为：设现场总线网络中共有n个检测节点，表示主控节点在第k个检测周期收集到的检测节点i的角度测量值，则第k个周期内所有检测节点角度测量值组成的集合为对集合A进行从小到大的排序得到集合B={x₁(k),x₂(k),x₃(k),…,x_n(k)}，那么疏失误差剔除后的集合C由式(3)～式(7)计算得到：The negligence error elimination method described in step 4, its specific implementation method is: set n detection nodes altogether in the field bus network, Indicates the angle measurement value of detection node i collected by the master control node in the kth detection cycle, then the set of angle measurement values of all detection nodes in the kth cycle is Sort the set A from small to large to get the set B={x ₁ (k), x ₂ (k), x ₃ (k),…,x _n (k)}, then the set C after the negligent errors are eliminated is given by Formula (3) ~ formula (7) can be calculated as follows:

${x x}_{M m} = = MED MED {{{x x}_{11} ((k k)),, {x x}_{22} ((k k)),, {x x}_{33} ((k k)),, \cdot \cdot \cdot \cdot \cdot \cdot,, {x x}_{n no} ((k k))}} = = \{\begin{matrix} {x x}_{\frac{n no + + 11}{22}} ((k k)) & n no = = 22 m m + + 11 \\ \frac{{x x}_{n no / / 22} ((k k)) + + {x x}_{n no / / 22 + + 11} ((k k))}{22},, & n no = = 22 m m \end{matrix} - - - - - - ((33))$

DS=x_U-x_D (4)DS=x _U -x _D (4)

x_U=MED{x_M,…,x_n(k)} (5)x _U =MED{x _M ,…,x _n (k)} (5)

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

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

其中x_M为集合B的中位数，DS为四分位离散度,x_U和x_D分别为其上四分位数和下四分位数，MED{·}为求集合中位数的运算。满足集合C的测量值为有效的，否则为误差值，应剔除。Among them, x _M is the median of set B, DS is the quartile dispersion, x _U and x _D are the upper and lower quartiles respectively, and MED{ } is the method for finding the set median operation. The measured value that satisfies the set C is valid, otherwise it is an error value and should be eliminated.

步骤四所述的最小二乘法拟合剩余测量值，其具体实施方法为：在剔除无效的传感器数据后，通过最小二乘法(Least Square Method)对集合C中的数据进行拟合，得到时刻k的观测值，如式(8)所示，其中LSM{·}为通过最小二乘法拟合集合数据的运算。The least square method described in step 4 fits the remaining measured values, and its specific implementation method is: after eliminating invalid sensor data, the data in the set C is fitted by the least square method (Least Square Method), and the time k is obtained. The observed value of , as shown in formula (8), where LSM{·} is the operation of fitting the set data by the least square method.

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

步骤四所述的卡尔曼最优估计值预测，其具体实施方法为：卡尔曼滤波针对式(9)和(10)所示的离散线性系统The Kalman optimal estimated value prediction described in step 4, its specific implementation method is: Kalman filter for the discrete linear system shown in formula (9) and (10)

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

Y(k)=H·X(k)+V(k) (10)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个检测周期倾角的最优估计值。Among them, X(k) is the estimated value at time k; Y(k) is the observed value at time k (the kth detection period), and its value is obtained by fitting and calculating by step (2) least square method; parameter Φ is the system matrix , H represents the rate of change of the estimated value at time k relative to time k-1; W(k) and V(k) are process noise and measurement noise at time k, respectively. For the discrete linear system described by formulas (9) and (10), the Kalman filter combines the optimal estimation results of the k-1th detection period, and calculates the inclination angle of the kth detection period through formulas (11) to (14). best estimate.

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

${K K}_{g g} ((k k)) = = \frac{P P ((k k | | k k - - 11)) \cdot \cdot {H h}^{' '}}{H h \cdot \cdot P P ((k k | | k k - - 11)) \cdot &Center Dot; {H h}^{' '} + + R R} - - - - - - ((1212))$

P(k|k)=P(k|k-1)·(1-K_g(k)·H) (13)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)X(k|k)=X(k|k-1)+K _g (k)·(Y(k)-H·X(k|k-1)) (14)

本发明在硬件上通过低成本、低功耗、高分辨率的三轴数字加速度传感器设计倾斜检测节点，并在车体不同位置分别布设检测节点，采集各自的倾角信息；软件上把这些分布在不同位置上的多只同类传感器节点所提供的不完整观测量，依次依据疏失误差剔除、最小二乘拟合和卡尔曼滤波等优化准则组合起来，产生对车体行驶道路坡度的一致性解释和描述，消除多只传感器信息之间可能在的冗余和矛盾，降低不确定性，从而获得精确的道路坡度估计。本发明实现了对车体行驶道路坡度的高精度检测，同时，检测节点与主控节点之间通过CAN总线通信，保证了数据的可靠性与及时性，适合应用于车载导航系统与智能汽车电子领域。The present invention designs inclination detection nodes through low-cost, low power consumption, and high-resolution three-axis digital acceleration sensors on hardware, and arranges detection nodes at different positions of the car body to collect their own inclination angle information; on software, these are distributed in The incomplete observations provided by multiple sensor nodes of the same type in different positions are combined in turn according to optimization criteria such as error elimination, least square fitting and Kalman filtering, to produce a consistent interpretation of the road gradient of the car body and Describe, eliminate possible redundancy and contradiction between multiple sensor information, reduce uncertainty, and obtain accurate road slope estimation. The present invention realizes the high-precision detection of the slope of the road on which the car body is traveling. At the same time, the CAN bus communication between the detection node and the main control node ensures the reliability and timeliness of the data, and is suitable for use in vehicle navigation systems and smart car electronics. field.

附图说明：Description of drawings:

图1是高精度车载道路坡度检测装置结构图。Figure 1 is a structural diagram of a high-precision vehicle-mounted road gradient detection device.

图2是检测节点功能模块图。FIG. 2 is a functional block diagram of a detection node.

图3是角度检测原理图。Figure 3 is a schematic diagram of angle detection.

图4是现场总线网络通信流程示意图。Fig. 4 is a schematic diagram of the fieldbus network communication flow.

具体实施方式Detailed ways

下面结合附图作进一步的详细描述和说明。Further detailed description and explanation will be made below in conjunction with the accompanying drawings.

参照图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为各模块提供稳定的电压供应；With reference to Fig. 1 and Fig. 2, a kind of high-accuracy vehicle-mounted road slope detection device, comprises main control node 2 and the detection node 1 that is distributed in vehicle body everywhere, between detection node 1 and main control node 2 communicate through CAN bus; The plane where node 1 is located is parallel to the plane of the chassis of the car body. The detection node 1 includes a sensor detection module 3, a micro-control module 4, a CAN communication module 5 and a power management module 6; the sensor detection module 3 adopts a three-axis digital acceleration sensor; the micro-control module 4. The ultra-low power consumption MSP430F149 MCU of TI Company is adopted; the CAN communication module 5 is composed of bus control chip 7 and driver chip 8; Bus V2.0B technical specification; MCP2510 is connected with the microcontroller module 4 through the SPI bus to complete the interaction of data and control information; the CAN driver chip is TJA1050 from Philips, which is fully compatible with the ISO11898 standard, and the maximum transmission rate can reach 1Mbps. And the bus can be connected to at least 110 nodes, and the detection nodes are connected by twisted-pair shielded wires, and 120Ω terminal matching resistors are connected in parallel at both ends of CANH and CANL of the first and last nodes to absorb signal reflection waves on the bus ; The power management module 6 provides a stable voltage supply for each module;

传感器检测模块3采用Analogy Devices公司的三轴数字加速度传感器ADXL345，它体积小、功耗低、分辨率高，测量范围达±16g（g为自由落体加速度），可测量X、Y、Z三个相互正交方向的动、静态的加速度，ADXL345同样通过SPI总线与微控制器模块4进行数据和控制信息的交互，可通过配置相关寄存器初始化三个轴的偏移量，复位初始倾角值。Sensor detection module 3 adopts the three-axis digital acceleration sensor ADXL345 from Analogy Devices, which has small size, low power consumption, high resolution, and a measurement range of ±16g (g is the acceleration of free fall), which can measure X, Y, and Z For the dynamic and static accelerations in mutually orthogonal directions, the ADXL345 also interacts with the microcontroller module 4 for data and control information through the SPI bus. The offsets of the three axes can be initialized by configuring the relevant registers, and the initial inclination value can be reset.

基于上述检测装置的检测方法，包括以下步骤：The detection method based on above-mentioned detection device, comprises the following steps:

图3是倾角检测原理图。图1已经规定初始情况下各检测节点以地表水平面作为参考平面，如图3(a)所示，A_x、A_y、A_z分别为传感器在三个方向上检测到的静止加速度。当车体未发生倾斜时，A_z与重力加速度的大小和方向均相同，而A_x，A_y上的分量为零；当车体发生了如图3(b)所示的倾斜时，其各轴的加速度变化如图3(c)所示，其中A_xoy为传感器在xoy平面上的加速度，其计算方法如公式(1)所示：Figure 3 is a schematic diagram of inclination detection. Figure 1 has stipulated that in the initial situation, each detection node takes the ground surface level as the reference plane, as shown in Figure 3(a), A _x , A _y , and A _z are the static accelerations detected by the sensor in three directions, respectively. When the car body is not tilted, the magnitude and direction of A _z and gravitational acceleration are the same, while the components on A _x and A _y are zero; when the car body is tilted as shown in Figure 3(b), its The acceleration changes of each axis are shown in Figure 3(c), where _Axoy is the acceleration of the sensor on the xoy plane, and its calculation method is shown in formula (1):

${A A}_{xoy xoy} = = \sqrt{{A A}_{x x}^{22} + + {A A}_{y the y}^{22}} - - - - - - ((11))$

此时该检测节点计算到的车体xoy平面与地表水平面XOY倾斜的角度α如公式(2)所示：At this time, the tilt angle α between the car body xoy plane and the surface horizontal plane XOY calculated by the detection node is shown in formula (2):

$α α = = arctan arctan \frac{{A A}_{xoy xoy}}{{A A}_{z z}} - - - - - - ((22))$

即通过此方法可将获取的一组加速度信息进行处理，得到检测节点各子平面与地表水平面之间的倾角。由于ADXL345三轴加速度传感器最高分辨率可达3.9mg/LSB，因此单个传感器能检测不到1.0°的倾斜角度变化。That is, through this method, the obtained set of acceleration information can be processed to obtain the inclination angle between each sub-plane of the detection node and the ground surface level. Since the highest resolution of the ADXL345 triaxial acceleration sensor can reach 3.9mg/LSB, a single sensor can detect less than a 1.0° tilt angle change.

步骤三、参照图4，网络的一个检测周期由数据获取和数据处理两个阶段组成，在数据获取阶段，主控节点2分别给各检测节点1发送远程帧，以此下达信息收集指令；检测节点1在收到发送给自己的远程帧时，将最新的倾斜角度信息回传给主控节点2，否则处于侦听或休眠状态。对于节点数目不多的网络，上述这种轮询通信可有效控制总线上的数据流量，避免拥塞与碰撞的发生，并保证数据的可靠性与实时性。Step 3, referring to Figure 4, a detection cycle of the network is composed of two stages of data acquisition and data processing. In the data acquisition stage, the master control node 2 sends remote frames to each detection node 1 respectively, so as to issue information collection instructions; When node 1 receives the remote frame sent to itself, it returns the latest tilt angle information to master control node 2, otherwise it is in a listening or dormant state. For a network with a small number of nodes, the above polling communication can effectively control the data flow on the bus, avoid congestion and collisions, and ensure data reliability and real-time performance.

步骤四、参照图4，主控节点2收集完网内所有检测节点1的倾斜角度信息后，便进入数据处理阶段；为解决单一传感器数据描述的片面性，消除其检测信息中的噪声，主控节点对收集的一轮数据进行融合处理，从而得到本次检测周期内车体行驶道路坡度的最优估计值；数据处理阶段按如下步骤进行：（一）通过疏失误差剔除法剔除无效的检测节点数据，消除倾角测量中的随机干扰；（二）通过最小二乘法拟合剩余的测量值，作为卡尔曼滤波器的观测值输入；（3）结合上一检测周期的最优估计结果，利用卡尔曼滤波器完成本周期内的道路坡度最优估计值预测，即高精度的车载道路坡度值。卡尔曼滤波保证了检测数据以最小均方差准则融合最优估计值。Step 4, referring to Figure 4, after the main control node 2 collects the inclination angle information of all detection nodes 1 in the network, it enters the data processing stage; in order to solve the one-sidedness of single sensor data description and eliminate the noise in its detection information, the main control node 2 The node performs fusion processing on the collected round of data, so as to obtain the optimal estimated value of the road gradient of the vehicle body in this detection period; the data processing stage is carried out as follows: (1) Eliminate invalid detection nodes by the negligent error elimination method data to eliminate the random interference in the inclination measurement; (2) fit the remaining measured values by the least squares method, and use them as the input of the observations of the Kalman filter; (3) combine the optimal estimation results of the previous detection The Mann filter completes the prediction of the optimal estimated value of the road slope in this period, that is, the high-precision vehicle road slope value. The Kalman filter ensures that the detection data is fused with the optimal estimated value with the minimum mean square error criterion.

步骤四所述的疏失误差剔除法，其具体实施方法为：设现场总线网络中共有n个检测节点，表示主控节点在第k个检测周期收集到的检测节点i的角度测量值，则第k个周期内所有检测节点角度测量值组成的集合为对集合A进行从小到大的排序得到集合B={x₁(k),x₂(k),x₃(k),…,x_n(k)}。那么疏失误差剔除后的集合C由式(3)～式(7)计算得到。The negligence error elimination method described in step 4, its specific implementation method is: suppose that there are n detection nodes altogether in the field bus network, Indicates the angle measurement value of detection node i collected by the master control node in the kth detection cycle, then the set of angle measurement values of all detection nodes in the kth cycle is Sort set A from small to large to get set B={x ₁ (k), x ₂ (k), x ₃ (k),...,x _n (k)}. Then the set C after negligent errors are eliminated is calculated by formula (3) ~ formula (7).

DS=x_U-x_D (4)DS=x _U -x _D (4)

x_U=MED{x_M,…,x_n(k)} (5)x _U =MED{x _M ,…,x _n (k)} (5)

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

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

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

步骤四所述的卡尔曼最优估计值预测，其具体实施方法为：考虑到对正在行驶车辆倾斜角度的测量是一个渐变的过程，即k时刻测得的角度应与k-1时刻相关，本发明采用卡尔曼滤波法估算角度的最优估计值。卡尔曼滤波针对式(9)和(10)所示的离散线性系统。The Kalman optimal estimated value prediction described in step 4, its specific implementation method is: considering that the measurement of the inclination angle of the running vehicle is a gradual process, that is, the angle measured at the k moment should be related to the k-1 moment, The present invention uses a Kalman filtering method to estimate the optimal estimated value of the angle. Kalman filtering is aimed at discrete linear systems shown in equations (9) and (10).

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

Y(k)=H·X(k)+V(k) (10)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个检测周期倾角的的最优估计值。Among them, X(k) is the estimated value at time k; Y(k) is the observed value at time k (the kth detection cycle), and its value is calculated by fitting and calculating by step (2) least square method; parameter Φ is the system matrix , H represents the rate of change of the estimated value at time k relative to time k-1; W(k) and V(k) are process noise and measurement noise at time k, respectively. For the discrete linear system described by formulas (9) and (10), the Kalman filter is combined with the optimal estimate of the k-1th detection period, and the k-th detection period inclination angle is calculated by formulas (11)~(14). best estimate of .

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

${K K}_{g g} ((k k)) = = \frac{P P ((k k | | k k - - 11)) \cdot &Center Dot; {H h}^{' '}}{H h \cdot \cdot P P ((k k | | k k - - 11)) \cdot &Center Dot; {H h}^{' '} + + R R} - - - - - - ((1212))$

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

其中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。Where Q and R are the covariances of W(k) and V(k) respectively, K _g (k) is the Kalman filter gain at time k, X(k|k) represents the optimal result at time k, X(k |k-1) represents the state prediction value at the previous moment, P(k|k) and P(k|k-1) are the covariances corresponding to X(k|k) and X(k|k-1) respectively . In this embodiment, take F=H=1, X(0)=1, P(0)=0; since the selected triaxial acceleration sensor works in full resolution mode, the sensitivity deviation at this time is 3.9mg /LSB, that is, the covariance R=3.9×10 ^-3 of the measurement noise V(k) ×10 ^-3 .

Claims

1. the vehicle-mounted road grade detection device of high precision, comprise and be distributed in car body detection node everywhere (1), communicated by CAN between detection node (1) with main controlled node (2), it is characterized in that, the residing plane of detection node (1) is parallel with chassis of vehicle body plane, the power management module (6) that detection node (1) comprises sensor detection module (3), micro-control module (4), CAN (5) and provides stable voltage to supply for each module; Sensor detection module (3) adopts three axle digital acceleration sensors; CAN (5) is made up of bus marco chip (7) and driving chip (8); Bus marco chip (7) is connected with micro-control module (4) by spi bus, completes the mutual of data and control information; Bus type connection is carried out by twisted-pair shielded wire between each detection node, and at CANH and the CANL two ends parallel termination resistance of head and the tail node.

2. based on the detection method of claim 1 device, it is characterized in that, comprise following detecting step:

Step one, side-play amount by three axles in configuration related register initialization three axle digital acceleration sensor, complete initial tilt value and reset;

Three axle digital acceleration sensors in step 2, sensor detection module (3) measure the dynamic and static acceleration in X, Y, Z tri-mutually orthogonal directions, and sensor detection module (3) and micro-control module (4) carry out the mutual of data and control information; Control module (4) calculates the angle of inclination of each detection node (1);

Step 3, main controlled node (2) send remote frame to respectively each detection node (1), assign information instruction with this; Detection node (1), when receiving the remote frame sending to oneself, by up-to-date angle of inclination information back to main controlled node (2), otherwise is in and intercepts or dormant state;

Step 4, main controlled node (2) just carry out data processing after collecting the angle of inclination information of all detection node (1) in net; Data processing is carried out as follows: (one) rejects invalid detection node data by blunder error scalping method, eliminates the random disturbance in measurement of dip angle; (2) by the remaining measured value of least square fitting, the observed reading as Kalman filter inputs; (3) combine the optimal estimation result of a upper sense cycle, utilize Kalman filter to complete road grade optimal estimation value prediction in this cycle, i.e. high-precision vehicle-mounted road grade value.

3. detection method according to claim 2, is characterized in that, the blunder error scalping method described in step 4, and its specific implementation method is: establish total n detection node in fieldbus networks, represent the angle measurement of the detection node i that main controlled node is collected in a kth sense cycle, then the set that in the kth cycle, all detection node angle measurement form is set B={ x is obtained to the set A sequence carried out from small to large ₁(k), x ₂(k), x ₃(k) ..., x _n(k) }, the set C so after blunder error rejecting is calculated by formula (3) ~ formula (7):

x_{M} = MED {x_{1} (k), x_{2} (k), x_{3} (k), . . ., x_{n} (k)} = \{\begin{matrix} x_{\frac{n + 1}{2}} (k), & n = 2 m + 1 \\ \frac{x_{n / 2} (k) + x_{n / 2 + 1} (k)}{2}, & n = 2 m \end{matrix} - - - (3)

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)

Wherein x _mfor the median of set B, DS is quartile dispersion, x _uand x _dbe respectively its upper quartile and lower quartile, MED{} is the computing asking set median, and the measured value meeting set C is effective, otherwise is error amount, should reject.

4. detection method according to claim 2, it is characterized in that, least square fitting residue measured value described in step 4, its specific implementation method is: after rejecting invalid sensing data, by least square method (Least Square Method), matching is carried out to the data in set C, obtain the observed reading of moment k, shown in (8), wherein LSM{} is the computing by least square fitting collective data:

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

5. detection method according to claim 2, is characterized in that, the Kalman's optimal estimation value prediction described in step 4, its specific implementation method for: Kalman filtering is for the discrete linear systems shown in formula (9) and (10)

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

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

Wherein X (k) is the estimated value in k moment; Y (k) is calculated by step (two) least square fitting for the observed reading of k moment (a kth sense cycle), its value; Parameter Φ is system matrix, and H characterizes the rate of change of k moment estimated value relative to the k-1 moment; W (k) and V (k) is respectively process noise and the measurement noises in k moment; For the discrete linear systems described by formula (9) and (10), Kalman filtering is in conjunction with the optimal estimation result of kth-1 sense cycle, and through type (11) ~ (14) calculate the optimal estimation value at a kth sense cycle inclination angle:

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

K_{g} (k) = \frac{P (k | k - 1) \cdot H^{'}}{H \cdot P (k | k - 1) \cdot H^{'} + R} - - - (12)

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)

Wherein Q and R is respectively the covariance of W (k) and V (k), K _gk Kalman filtering gain that () is the k moment, X (k|k) characterizes the optimal result of moment k, X (k|k-1) characterized the status predication value in a upper moment, and P (k|k) and P (k|k-1) is respectively covariance corresponding to X (k|k) and X (k|k-1).