CN103353299A - High-precision vehicle-mounted road grade detection device and method - Google Patents

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 kind of high precision vehicle-mounted road grade pick-up unit and method

Technical field

The present invention relates to onboard navigation system and intelligent automobile electronic technology field, particularly a kind of high precision vehicle-mounted road grade pick-up unit and method.

Background technology

Along with the fast development of automatic technology, onboard navigation system has become one of important component part of intelligent automobile electronic applications, and it can not only provide comprehensive geographical traffic information to the driver, can also help it to make safe and reliable control strategy.The gradient that accurately detects car body current driving road is one of essential function of onboard navigation system.But, the following problem of existing vehicle-mounted inclination detecting device ubiquity: (1) is passed through the current vehicle speed differential, or the simulating signal of traditional two axle acceleration sensors is carried out the methods such as A/D sampling obtain car body acceleration and angle of inclination information, they all exist defective and the deficiencies such as accuracy of detection is low, the scope of application is narrower; (2) use the single-sensor node to carry out Data Detection, this mode data description is unilateral, and is subject to the impact of the factors such as temperature, voltage, electromagnetism, causes data reliability poor.Therefore, present pick-up unit can't satisfy the actual demand of high precision onboard navigation system.

Summary of the invention

In order to overcome the defective of above-mentioned prior art, the object of the present invention is to provide a kind of high precision vehicle-mounted road grade pick-up unit and method, utilize three number of axle word acceleration transducers design detection node, gather and calculating tilting of car body angle, then send data to main controlled node by the CAN bus; Main controlled node carries out use processing to the detection data of a plurality of sensor nodes of diverse location, obtains the optimal estimation value of tilting of car body angle, to realize the high precision detection to road grade.

In order to achieve the above object, technical scheme of the present invention is achieved in that

A kind of high precision vehicle-mounted road grade pick-up unit comprises being distributed in car body detection node 1 everywhere, passes through the CAN bus communication between detection node 1 and the main controlled node 2; Detection node 1 plane of living in and chassis of vehicle body plane parallel, detection node 1 comprise sensor detection module 3, microcontroller module 4, CAN communication module 5 and the power management module 6 of stable voltage supply are provided for each module; Sensor detection module 3 adopts three number of axle word acceleration transducers; CAN communication module 5 is comprised of bus control chip 7 and driving chip 8; Bus control chip 7 links to each other with microcontroller module (4) by spi bus, finishes the mutual of data and control information; Carry out bus type by twisted-pair shielded wire between each detection node and connect, and at CANH and the CANL two ends parallel termination resistance of head and the tail node.

Based on the detection method of said apparatus, comprise following detecting step:

Step 1, by the side-play amount of three axles in configuration related register initialization three number of axle word acceleration transducers, finishing just, the primary dip value resets;

Three number of axle word acceleration transducers in step 2, the sensor detection module 3 are measured the dynamic and static acceleration of X, Y, three mutually orthogonal directions of Z, and sensor detection module 3 carries out the mutual of data and control information with microcontroller module 4; Control module 4 calculates the angle of inclination of each detection node 1;

Step 3, main controlled node 2 send remote frame for respectively each detection node 1, assign the information instruction with this; Detection node 1 returns to main controlled node 2 with up-to-date angle of inclination information when receiving the remote frame that sends to oneself, intercept or dormant state otherwise be in;

After step 4, main controlled node 2 collect the angle of inclination information of all detection node 1 in the net, just carry out data and process; Data are processed and carried out as follows: (one) rejects invalid detection node data by the blunder error scalping method, eliminates the random disturbance in the measurement of dip angle; (2) by the remaining measured value of least square fitting, as the observed reading input of Kalman filter; (3) in conjunction with the optimal estimation result of a upper sense cycle, utilize Kalman filter to finish interior road grade optimal estimation value prediction of this cycle, i.e. high-precision vehicle-mounted road grade value;

The described blunder error scalping method of step 4, its specific implementation method is: establish total n detection node in the fieldbus networks,

The angle measurement of the detection node i that the expression main controlled node is collected k sense cycle, then the set of k interior all detection node angle measurement compositions of cycle is The ordering that pair set A carries out from small to large obtains set B={ x ₁(k), x ₂(k), x ₃(k) ..., x _n(k) }, the set C after blunder error is rejected is so calculated by formula (3)～formula (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 wherein _MBe the median of set B, DS is the quartile dispersion, x _UAnd x _DBe respectively its upper quartile and lower quartile, MED{} is the computing of asking the set median.The measured value that satisfies set C is effectively, otherwise is error amount, should reject.

The described least square fitting residue of step 4 measured value, its specific implementation method is: after rejecting invalid sensing data, carry out match by the data among least square method (Least Square Method) the pair set C, obtain the constantly observed reading of k, shown in (8), wherein LSM{} is the computing by the least square fitting collective data.

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

The described Kalman's optimal estimation of step 4 value prediction, its specific implementation method is: 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 k estimated value constantly; Y (k) is the constantly observed reading of (k sense cycle) of k, and its value is calculated by step (two) least square fitting; Parameter Φ is system matrix, and H has characterized k moment estimated value with respect to k-1 rate of change constantly; W (k) and V (k) are respectively k process noise and measurement noise constantly.For formula (9) and (10) described discrete linear systems, Kalman filtering is in conjunction with the optimal estimation result of k-1 sense cycle, and the optimal estimation value at k sense cycle inclination angle is calculated in through type (11)～(14).

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)

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

The present invention designs the tilt detection node at hardware by low cost, low-power consumption, high-resolution three number of axle word acceleration transducers, and lays respectively detection node at the car body diverse location, gathers obliquity information separately; On the software these are distributed in the imperfect observed quantity that many same type of sensor nodes on the diverse location provide, combine according to Optimality Criterias such as blunder error rejecting, least square fitting and Kalman filterings successively, generation is explained the consistance of the car body travel gradient and is described, eliminate between many sensor informations may redundancy and contradiction, reducing uncertainty is estimated thereby obtain accurate road grade.The present invention has realized the high precision of the car body travel gradient is detected, and simultaneously, by the CAN bus communication, has guaranteed the reliability of the data and promptness between detection node and the main controlled node, is fit to be applied to onboard navigation system and intelligent automobile electronic applications.

Description of drawings:

Fig. 1 is high precision vehicle-mounted road grade structure of the detecting device figure.

Fig. 2 is the detection node functional block diagram.

Fig. 3 is that angle detects schematic diagram.

Fig. 4 is fieldbus networks communication process synoptic diagram.

Embodiment

Be described in further detail and illustrate below in conjunction with accompanying drawing.

See figures.1.and.2, a kind of high precision vehicle-mounted road grade pick-up unit comprises main controlled node 2 and is distributed in car body detection node 1 everywhere, passes through the CAN bus communication between detection node 1 and the main controlled node 2; Detection node 1 plane of living in and chassis of vehicle body plane parallel, detection node 1 comprises sensor detection module 3, microcontroller module 4, CAN communication module 5 and power management module 6; Sensor detection module 3 adopts three number of axle word acceleration transducers; Microcontroller module 4 has adopted the super low-power consumption MSP430F149 single-chip microcomputer of TI company; CAN communication module 5 is comprised of bus control chip 7 and driving chip 8; Bus control chip 7 is selected the MCP2510 of Microchip company, and this chip performance is stable, low in energy consumption, supports CAN bus V2.0B technical manual fully; MCP2510 is connected with micro controller module 4 by spi bus, finishes the mutual of data and control information; CAN drives the TJA1050 that chip is selected Philips company, it and ISO11898 standard are fully compatible, maximum transmission rate can reach 1Mbps, and bus can articulate 110 nodes at least, carrying out bus type by twisted-pair shielded wire between each detection node connects, and in the terminal build-out resistor of the CANH of head and the tail node and CANL two ends 120 Ω in parallel, to absorb the returning echo on the bus; Power management module 6 provides stable voltage supply for each module;

Sensor detection module 3 adopts three number of axle word acceleration transducer ADXL345 of Analogy Devices company, its volume is little, low in energy consumption, resolution is high, measurement range reaches ± and 16g(g is free-fall acceleration), can measure the dynamic and static acceleration of X, Y, three mutually orthogonal directions of Z, ADXL345 carries out the mutual of data and control information by spi bus and micro controller module 4 equally, can be by the side-play amount of three axles of configuration related register initialization, just primary dip value resets.

Detection method based on above-mentioned detection device may further comprise the steps:

Step 1, by the side-play amount of three axles in configuration related register initialization three number of axle word acceleration transducers, finishing just, the primary dip value resets;

Three number of axle word acceleration transducers in step 2, the sensor detection module 3 are measured the dynamic and static acceleration of X, Y, three mutually orthogonal directions of Z, and sensor detection module 3 carries out the mutual of data and control information with microcontroller module 4; Control module 4 calculates the angle of inclination of each detection node 1;

Fig. 3 is the inclination angle detection schematic diagram.Fig. 1 stipulated under the initial situation each detection node with the surface water plane as the reference plane, shown in Fig. 3 (a), A _x, A _y, A _zBe respectively the static acceleration that sensor detects in three directions.When car body not during run-off the straight, A _zAll identical with the size and Orientation of acceleration of gravity, and A _x, A _yOn component be zero; When inclination shown in Fig. 3 (b) has occured in car body, the acceleration change of its each axle shown in Fig. 3 (c), A wherein _XoyBe the acceleration of sensor on the xoy plane, its computing method are shown in formula (1):

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

The angle [alpha] that the car body xoy plane that this moment, this detection node calculated and surface water plane X OY tilt is shown in formula (2):

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

Namely the one group of acceleration information that obtains can be processed by the method, obtain the inclination angle between each sub-plane of detection node and the surface water plane.Because ADXL345 3-axis acceleration sensor highest resolution can reach 3.9mg/LSB, so single-sensor can can't detect 1.0 ° angle change.

Step 3, with reference to Fig. 4, a sense cycle of network is processed two stages by data acquisition and data and is formed, in data acquisition phase, main controlled node 2 sends remote frame for respectively each detection node 1, assigns the information instruction with this; Detection node 1 returns to main controlled node 2 with up-to-date angle of inclination information when receiving the remote frame that sends to oneself, intercept or dormant state otherwise be in.For the few network of interstitial content, above-mentioned this polling communication is the data traffic on the control bus effectively, the generation of avoid congestion and collision, and guarantee the reliability of the data and real-time.

Step 4, with reference to Fig. 4, main controlled node 2 collects in the net after the angle of inclination information of all detection node 1, the processing stage of just entering data; For solving the one-sidedness of single-sensor data description, eliminate the noise in its detection information, main controlled node is taken turns data to one of collection and is carried out fusion treatment, thereby obtains the optimal estimation value of the car body travel gradient in this sense cycle; Carry out as follows the processing stage of data: (one) rejects invalid detection node data by the blunder error scalping method, eliminates the random disturbance in the measurement of dip angle; (2) by the remaining measured value of least square fitting, as the observed reading input of Kalman filter; (3) in conjunction with the optimal estimation result of a upper sense cycle, utilize Kalman filter to finish interior road grade optimal estimation value prediction of this cycle, i.e. high-precision vehicle-mounted road grade value.Kalman filtering has guaranteed that the detection data merge the optimal estimation value with least-mean-square-error criterion.

The described blunder error scalping method of step 4, its specific implementation method is: establish total n detection node in the fieldbus networks,

The angle measurement of the detection node i that the expression main controlled node is collected k sense cycle, then the set of k interior all detection node angle measurement compositions of cycle is

The ordering that pair set A carries out from small to large obtains set B={ x ₁(k), x ₂(k), x ₃(k) ..., x _n(k) }.Set C after blunder error is rejected is so calculated by formula (3)～formula (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 wherein _MBe the median of set B, DS is the quartile dispersion, x _UAnd x _DBe respectively its upper quartile and lower quartile, MED{} is the computing of asking the set median.The measured value that satisfies set C is effectively, otherwise is error amount, should reject.

The described least square fitting residue of step 4 measured value, its specific implementation method is: after rejecting invalid sensing data, carry out match by the data among least square method (Least Square Method) the pair set C, obtain the constantly observed reading of k, shown in (8), wherein LSM{} is the computing by the least square fitting collective data.

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

The described Kalman's optimal estimation of step 4 value prediction, its specific implementation method is: consider that the measurement that aligns in the inclination of vehicle angle of travelling is the process of a gradual change, be that the angle that k records constantly should be constantly relevant with k-1, the present invention adopts the optimal estimation value of Kalman filtering method estimated angle.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 k estimated value constantly; Y (k) is the constantly observed reading of (k sense cycle) of k, and its value is calculated by step (two) least square fitting; Parameter Φ is system matrix, and H has characterized k moment estimated value with respect to k-1 rate of change constantly; W (k) and V (k) are respectively k process noise and measurement noise constantly.For formula (9) and (10) described discrete linear systems, Kalman filtering is in conjunction with the optimal estimation combination of k-1 sense cycle, through type (11)～(14) calculate k sense cycle inclination angle the optimal estimation value.

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)

Wherein Q and R are respectively the covariance of W (k) and V (k), K _g(k) be k Kalman filtering gain constantly, X (k|k) characterizes the constantly optimal result of k, X (k|k-1) characterized the status predication value in a upper moment, and P (k|k) and P (k|k-1) are respectively X (k|k) and covariance corresponding to X (k|k-1).In the present embodiment, get F=H=1, X (0)=1, P (0)=0; Because the 3-axis acceleration sensor selected is operated under the full resolution pattern, the sensitivity variations of this moment is 3.9mg/LSB, namely measures the covariance R=3.9 of noise V (k) * 10 ^-3If the process noise Gaussian distributed in order to obtain more rational prediction effect, needs to guarantee Q and R at an order of magnitude, this paper gets Q=1 * 10 ^-3

Claims (5)

1. a high precision vehicle-mounted road grade pick-up unit is characterized in that, comprises being distributed in car body detection node (1) everywhere, passes through the CAN bus communication between detection node (1) and the main controlled node (2); Detection node (1) plane of living in and chassis of vehicle body plane parallel, detection node (1) comprise sensor detection module (3), microcontroller module (4), CAN communication module (5) and the power management module (6) of stable voltage supply are provided for each module; Sensor detection module (3) adopts three number of axle word acceleration transducers; CAN communication module (5) is comprised of bus control chip (7) and driving chip (8); Bus control chip (7) links to each other with microcontroller module (4) by spi bus, finishes the mutual of data and control information; Carry out bus type by twisted-pair shielded wire between each detection node and connect, 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 1, by the side-play amount of three axles in configuration related register initialization three number of axle word acceleration transducers, finishing just, the primary dip value resets;

Three number of axle word acceleration transducers in step 2, the sensor detection module (3) are measured the dynamic and static acceleration of X, Y, three mutually orthogonal directions of Z, and sensor detection module (3) carries out the mutual of data and control information with microcontroller module (4); Control module (4) calculates the angle of inclination of each detection node (1);

Step 3, main controlled node (2) send remote frame for respectively each detection node (1), assign the information instruction with this; Detection node (1) returns to main controlled node (2) with up-to-date angle of inclination information when receiving the remote frame that sends to oneself, intercept or dormant state otherwise be in;

After step 4, main controlled node (2) collect the angle of inclination information of all detection node (1) in the net, just carry out data and process; Data are processed and carried out as follows: (one) rejects invalid detection node data by the blunder error scalping method, eliminates the random disturbance in the measurement of dip angle; (2) by the remaining measured value of least square fitting, as the observed reading input of Kalman filter; (3) in conjunction with the optimal estimation result of a upper sense cycle, utilize Kalman filter to finish interior road grade optimal estimation value prediction of this cycle, i.e. high-precision vehicle-mounted road grade value.

3. detection method according to claim 2 is characterized in that, the described blunder error scalping method of step 4, and its specific implementation method is: establish total n detection node in the fieldbus networks, The angle measurement of the detection node i that the expression main controlled node is collected k sense cycle, then the set of k interior all detection node angle measurement compositions of cycle is

The ordering that pair set A carries out from small to large obtains set B={ x ₁(k), x ₂(k), x ₃(k) ..., x _n(k) }, the set C after blunder error is rejected is so calculated by formula (3)～formula (7):

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 wherein _MBe the median of set B, DS is the quartile dispersion, x _UAnd x _DBe respectively its upper quartile and lower quartile, MED{} is the computing of asking the set median.The measured value that satisfies set C is effectively, otherwise is error amount, should reject.

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

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

5. detection method according to claim 2 is characterized in that, the described Kalman's optimal estimation of step 4 value prediction, and its specific implementation method is: 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 k estimated value constantly; Y (k) is the constantly observed reading of (k sense cycle) of k, and its value is calculated by step (two) least square fitting; Parameter F is system matrix, and H has characterized k moment estimated value with respect to k-1 rate of change constantly; W (k) and V (k) are respectively k process noise and measurement noise constantly; For formula (9) and (10) described discrete linear systems, Kalman filtering is in conjunction with the optimal estimation result of k-1 sense cycle, and the optimal estimation value at k sense cycle inclination angle is calculated in through type (11)～(14):

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

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 are respectively the covariance of W (k) and V (k), K _g(k) be k Kalman filtering gain constantly, X (k|k) characterizes the constantly optimal result of k, X (k|k-1) characterized the status predication value in a upper moment, and P (k|k) and P (k|k-1) are respectively X (k|k) and covariance corresponding to X (k|k-1).

Images