CN103353299B - 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 vehicle-mounted road grade detection device of high precision and method

Technical field

The present invention relates to onboard navigation system and intelligent automobile electronic technology field, particularly the vehicle-mounted road grade detection device of a kind of high precision 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 driver, and it can also be helped to make safe and reliable control strategy.The gradient accurately detecting 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 current vehicle speed differential, or method acquisition car body acceleration and the angle of inclination information such as A/D sampling are carried out to the simulating signal of traditional two axle acceleration sensors, all there is defect and the deficiencies such as accuracy of detection is low, the scope of application is narrower in them; (2) use single-sensor node to carry out Data Detection, this mode data describe unilateral, and are subject to the impact of the factors such as temperature, voltage, electromagnetism, cause data reliability poor.Therefore, current pick-up unit cannot meet the actual demand of high precision onboard navigation system.

Summary of the invention

In order to overcome the defect of above-mentioned prior art, the object of the present invention is to provide the vehicle-mounted road grade detection device of a kind of high precision and method, utilize three axle digital acceleration sensor design detection node, gather and calculate tilting of car body angle, then sending data to main controlled node by CAN; The detection data of main controlled node to multiple sensor nodes of diverse location carry out use processing, obtain the optimal estimation value of tilting of car body angle, to realize the high precision test to road grade.

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

The vehicle-mounted road grade detection device of a kind of high precision, is comprised and is distributed in car body detection node 1 everywhere, communicated between detection node 1 with main controlled node 2 by CAN; Residing for detection node 1, plane is parallel with chassis of vehicle body plane, and detection node 1 comprises sensor detection module 3, micro-control module 4, CAN 5 and provides the power management module 6 of stable voltage 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.

Based on the detection method of said apparatus, 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;

Blunder error scalping method described in step 4, 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=\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)

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.The measured value meeting set C is effective, otherwise is error amount, should reject.

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)

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\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 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).

The present invention is designed tilt detection nodes by low cost, low-power consumption, high-resolution three axle digital acceleration sensors, and is laid detection node respectively at car body diverse location on hardware, gathers respective obliquity information; The imperfect observed quantity that these many same type of sensor nodes be distributed on diverse location provide on software, combine according to Optimality Criterias such as blunder error rejecting, least square fitting and Kalman filterings successively, produce and the consistance of the car body travel gradient is explained and description, to eliminate between many sensor informations may redundancy and contradiction, reduce uncertain, thus obtain the estimation of accurate road grade.Present invention achieves the high precision test to the car body travel gradient, communicated by CAN between detection node with main controlled node meanwhile, ensure that reliability and the promptness of data, be applicable to being applied to onboard navigation system and intelligent automobile electronic applications.

Accompanying drawing illustrates:

Fig. 1 is the vehicle-mounted road grade detection device structural drawing of high precision.

Fig. 2 is detection node functional block diagram.

Fig. 3 is angle Cleaning Principle figure.

Fig. 4 is fieldbus networks communication process schematic diagram.

Embodiment

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

See figures.1.and.2, the vehicle-mounted road grade detection device of a kind of high precision, is comprised main controlled node 2 and is distributed in car body detection node 1 everywhere, being communicated between detection node 1 with main controlled node 2 by CAN; Residing for detection node 1, plane is parallel with chassis of vehicle body plane, and detection node 1 comprises sensor detection module 3, micro-control module 4, CAN 5 and power management module 6; Sensor detection module 3 adopts three axle digital acceleration sensors; Micro-control module 4 have employed the super low-power consumption MSP430F149 single-chip microcomputer of TI company; CAN 5 is made up of bus marco chip 7 and driving chip 8; Bus marco chip 7 selects the MCP2510 of Microchip company, and this chip performance is stable, low in energy consumption, supports CAN V2.0B technical manual completely; MCP2510 is connected with micro controller module 4 by spi bus, completes the mutual of data and control information; CAN driving chip selects the TJA1050 of Philips company, it and ISO11898 standard are completely compatible, maximum transmission rate can reach 1Mbps, and bus at least can mount 110 nodes, bus type connection is carried out by twisted-pair shielded wire between each detection node, and at CANH and the CANL two ends of head and the tail node the terminal build-out resistor of 120 Ω in parallel, to absorb the returning echo in bus; Power management module 6 provides stable voltage supply for each module;

Sensor detection module 3 adopts three axle digital acceleration sensor 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), the dynamic and static acceleration in X, Y, Z tri-mutually orthogonal directions can be measured, ADXL345 carries out the mutual of data and control information by spi bus and micro controller module 4 equally, by the side-play amount of configuration related register initialization three axles, reset initial tilt value.

Based on the detection method of above-mentioned detection device, comprise the following steps:

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;

Fig. 3 is inclination angle detection schematic diagram.Fig. 1 under having specified initial situation each detection node with surface water plane as a reference plane, as shown in Fig. 3 (a), A _x, A _y, A _zbe respectively the static acceleration that sensor detects in three directions.When the non-run-off the straight of car body, A _zall identical with the size and Orientation of acceleration of gravity, and A _x, A _yon component be zero; When car body there occurs the inclination as shown in Fig. 3 (b), the acceleration change of its each axle as shown in Fig. 3 (c), wherein A _xoyfor the acceleration of sensor in xoy plane, its computing method are as shown in formula (1):

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

Now the angle [alpha] that tilts of the car body xoy plane that calculates of this detection node and surface water plane X OY is as shown in formula (2):

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

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

Step 3, reference Fig. 4, a sense cycle of network was made up of data acquisition and two stages of data processing, and in data acquisition phase, main controlled node 2 sends remote frame to respectively each detection node 1, assigns 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.For the network that interstitial content is few, above-mentioned this polling communication can data traffic effectively on control bus, avoids generation that is congested and collision, and ensures reliability and the real-time of data.

Step 4, reference Fig. 4, main controlled node 2 just enters data processing stage after collecting the angle of inclination information of all detection node 1 in net; For solving the one-sidedness that single-sensor data describe, eliminate the noise in its Detection Information, main controlled node is taken turns data to collect one and is carried out fusion treatment, thus obtains the optimal estimation value of the car body travel gradient in this sense cycle; Data processing stage carries 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.Kalman filtering ensure that detecting data merges optimal estimation value with least-mean-square-error criterion.

Blunder error scalping method described in step 4, 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) }.Set C so after blunder error rejecting is 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)

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.The measured value meeting set C is effective, otherwise is error amount, should reject.

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)

Kalman's optimal estimation value prediction described in step 4, its specific implementation method is: consider that aligning in the measurement travelling inclination of vehicle angle is the process of a gradual change, namely the angle that records should be relevant to the k-1 moment k moment, and 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 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 combines in conjunction with the optimal estimation of kth-1 sense cycle, through type (11) ~ (14) calculate a kth sense cycle inclination angle 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 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).In the present embodiment, get F=H=1, X (0)=1, P (0)=0; Under the 3-axis acceleration sensor selected is operated in full resolution pattern, sensitivity variations is now 3.9mg/LSB, i.e. covariance R=3.9 × 10 of measurement noises V (k) ^-3; If process noise Gaussian distributed, in order to obtain more rational prediction effect, need ensure that Q and R is at an order of magnitude, gets Q=1 × 10 herein ^-3.

Claims (5)

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=\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)

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\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 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).