CN1299125C - Vehicle forward target detecting method - Google Patents

Abstract

The present invention relates to a method for detecting target objects in front of a vehicle, which belongs to the technical field of sensing and processing vehicle traveling information. The present invention is characterized in that a four-order kalman filtering method is used for filtering and processing the measurement data of the collected target objects of the vehicle, and a smooth and accurate target object distance and relative speed information can be obtained. A method for the continuity measurement and judgment of a target is further provided, and lost radar data and data from the other target object are correspondingly processed.

Description

Method for detecting object in front of vehicle

Technical Field

A method for detecting a target object in front of a vehicle belongs to the technical field of vehicle driving information perception and processing.

Background

The vehicle driving information sensing and processing technology is one of the key technologies of modern automobile control and safety, the existing measuring technology is divided into a microwave measuring system and a laser measuring system according to the application medium of the measuring technology, and the laser radar system is widely regarded for high measuring speed, high precision and long measuring distance. The design of an optical scanning type laser ranging radar is introduced in document 1 (Tujia Dawei: "driving environment sensing research for vehicle collision avoidance control", China mechanical engineering, Vol.10, No. 6, 6 months 1999), which can realize scanning ranging in a wide range, the system can measure the distance and azimuth information of a front target object, the measurement error is not effectively processed, the obtained relative speed fluctuation is extremely large, and the system cannot be used in practice.

Disclosure of Invention

The invention aims to provide a method for detecting a target object in front of a vehicle, aiming at the defects of the prior art, the method carries out filtering processing on the obtained distance and azimuth angle information of the target object, can obtain smoother and more accurate distance and relative speed information of the target object, and further provides a method for judging the measurement continuity of the target, and can judge whether the obtained continuous measurement data come from the same target object.

The invention provides a method for detecting a target object in front of a vehicle, which is characterized by sequentially comprising the following steps controlled and executed by a vehicle processor:

1) initialization:

given radar measurement system range measurement noise variance σ_r ²Angle measurement noise variance σ_θ ²System noise variance Q_uThe initial value X of the system state vector_0eInitial value P of covariance matrix of system state estimation error_0e；

2) Measuring the distance D of the target_nAnd azimuth angle A_nWherein, the subscript n represents the number of measurements and calculations, and its initial value is 1; the current measured values are indicated by the corresponding amounts with the subscript n;

3) calculating the object in the vehicleCurrent observed value Z of distance in driving direction, i.e. x direction_n：

Z_n＝D_ncosA_n

4) Calculating the noise variance of the current x-direction distance measurement:

${\mathrm{\σ}}_x^2={\mathrm{\σ}}_r^2\·{\cos }^2{A}_n+{D_n}^2\·{\mathrm{\σ}}_{\mathrm{\θ}}^2{\sin }^2{A}_n$

5) and carrying out filtering calculation according to the following fourth-order Kalman filtering recursion formula:

X_ny＝F·X_(n-1)e

P_ny＝F·P_(n-1)e·F′+G·Q_u·G′

$K_n=P_{\mathrm{ny}}\·H^{\′}\·{(H\·P_{\mathrm{ny}}\·H^{\′}+{\mathrm{\σ}}_x^2)}^{-1}$

P_ne＝(I-K_n·H)·P_ny

X_ne＝X_ny+K_n·(Z_n-H·X_ny)

wherein, $X_{\mathrm{ne}}^{\′}=[x_n,{\stackrel{\·}{x}}_n,{\stackrel{\·\·}{x}}_n,{\stackrel{\·\·\·}{x}}_n]$ representing a dynamic optimal estimate of the current object information,' is the matrix transposed symbol, x_nRepresents an optimal estimate of the x-direction distance value of the object,an optimal estimate representing the relative velocity between the vehicle and the target object; x_nyFor one-step forecast estimation of target object information, F is a system matrix:

$F=\left[\begin{array}{cccc}1& T& T^2/2& T^3/6\\ 0& 1& T& T^2/2\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]$

t is system sampling time; p_neIs the current system state estimation error covariance matrix, G is the system error impact matrix,

$G=\left[\begin{array}{c}0\\ 0\\ 0\\ T\end{array}\right]$

Q_uis the system noise variance; p_nyPredicting an estimation error covariance matrix for the system state in one step; k_nIs a Kalman filter gain matrix; h is an observation matrix, and H ═ 1000](ii) a I is a unit array;

6) store P_ne、X_neFor continued recursive computation and output; store P_ny、X_nyA value of (d) for measurement continuity judgment calculation;

7) and (3) judging the continuity of measurement:

7.1) calculation of a one-step predictive estimate of the observed value Z_ny：

Z_ny＝H·X_ny

7.2) with X_nyThe corresponding variance matrix S:

$S=H\·P_{\mathrm{ny}}\·H^{\′}+{\mathrm{\σ}}_x^2$

7.3) defining the observation factors: d²＝v′·S^-1·v

Wherein: v ═ Z_n-Z_ny

7.4) according to d²Judging whether the probability that the latest measured value and the last measured value come from the same target is equal to or exceeds a threshold value or not, if so, judging that the target object information obtained through filtering estimation is accurate, outputting the current distance, relative speed and azimuth angle, and returning to step 2) for next measurement;

if the number of times of the target object measurement is less than the threshold value, the radar is indicated to lose the target object measurement information, or the measurement value comes from another target object, and whether the number of times of the target object measurement is less than the threshold value continuously exceeds a preset value e needs to be further judged:

if the target object state does not exceed the preset value e, assuming that the target object motion state is kept unchanged in the period of time when the target object measurement information is lost, and obtaining the calculation method of the target object state according to the Kalman filtering assumption as follows:

$x_n=x_{(n-1)}+{\stackrel{\·}{x}}_{(n-1)}T+\frac{1}{2}{\stackrel{\·\·}{x}}_{(n-1)}{T}^2+\frac{1}{6}{\stackrel{\·\·\·}{x}}_{(n-1)}{T}^3$

${\stackrel{\·}{x}}_n={\stackrel{\·}{x}}_{(n-1)}+{\stackrel{\·\·}{x}}_{(n-1)}T+\frac{1}{2}{\stackrel{\·\·\·}{x}}_{(n-1)}{T}^2$

${\stackrel{\·\·}{x}}_n={\stackrel{\·\·}{x}}_{(n-1)}+{\stackrel{\·\·\·}{x}}_{(n-1)}T$

${\stackrel{\·\·\·}{x}}_n={\stackrel{\·\·\·}{x}}_{(n-1)}$

outputting the current target object in the x directionOptimal estimation of the directional distance x_nRelative velocity of

Is estimated optimally, and the azimuth angle A_n(ii) a Returning to the step 2) to perform next measurement;

if the value exceeds a preset value e, a new target object is considered to appear, the latest measurement target object is set as a tracking target object, the measurement information of the tracking target object is taken as the initial state value of the target object, namely the measurement value of the X-direction distance of the latest target object is taken as X_neIn x_nValue of (A), X_neSetting other state values to 0, and setting P_neSetting as a unit array, returning to the step 2) and starting to track and measure a new target object.

In the above step 7.4), the basis d²The threshold value of the probability value of the latest measured value and the last measured value from the same target is judged to be 95 percent according to the formula d²< 3.841 it is determined whether the latest measurement value and the last measurement value are from the same target.

Experiments prove that the method can obtain smoother and more accurate target object distance and relative speed information; the method can carry out corresponding processing aiming at the judgment result of the target measurement continuity, ensures the continuity of target object information extraction and achieves the expected effect.

Description of the drawings:

FIG. 1 is a functional block diagram of a system implementing the present method;

FIG. 2 is a schematic diagram of the measurement of azimuth angle of an object;

FIG. 3 is a vehicle coordinate system; wherein 301-vehicle equipped with laser radar detection system, 302-target object, x-vehicle forward direction, y-vehicle forward vertical direction, D-target object distance, A-target object azimuth angle;

FIG. 4 is a flow chart of a method of detecting an object ahead of a vehicle;

FIG. 5 is a diagram of a filtering algorithm recursion process;

FIG. 6 is a graph of experimental data I; where 6(a) is distance data, 601-local enlargement of experimental data; 6(b) is relative velocity data; and 6(c) is azimuth data.

FIG. 7 is a graph of experimental data II; where 7(a) is distance data, 7(b) is relative velocity data, and 7(c) is measurement continuity observation factor data.

Detailed Description

The embodiments of the present invention will be described with reference to the accompanying drawings.

As shown in fig. 1, the method can be implemented using a scanning lidar system designed by the university of shanghai, tujia, 1999 (see tujia dimension: "driving environment sensing research for vehicle collision avoidance control", china mechanical engineering, volume 10, phase 6, month 6 1999). The system adopts a phase method for distance measurement, a laser diode emits a laser beam modulated by sine amplitude through injection type current modulation, an emission light group is changed into a beam shape meeting the requirement, horizontal scanning is realized by a scanning rotating mirror, and the laser beam is emitted to a space and irradiates a front vehicle and an obstacle. The light reflected by the target is converged on the avalanche tube from the scanning rotating mirror through the receiving optical group to be converted into an electric signal, the signal enters the phase discriminator through the automatic gain control circuit with the low-noise amplification function to obtain the phase difference of the received laser relative to the emitted laser, the phase discriminator transmits the phase difference to the processor through the plug-in board, and the distance of the target is calculated in the processor. The calculation formula of the object distance D is as follows:

$D=\frac{c\&CenterDot;\mathrm{\&Delta;\&phi;}}{2\mathrm{\&pi;f}}---\left(1\right)$

where Δ Φ is the phase delay of the reflected laser beam relative to the emitted laser beam, c is the propagation speed of light in air, and f is the light intensity modulation frequency.

The one-dimensional position detector in fig. 1 is used to measure the angle of the scanning beam and further determine the azimuth angle of the target object. The principle of calculation of the object azimuth is shown in fig. 2. The light ray 204 reflected from the object 201 is converged by the scanning mirror 203 to form a light spot, and then strikes the one-dimensional position detector 205, and the one-dimensional position detector 205 includes a photosensitive element capable of detecting the distance from the light spot to the central axis 202, which is shown as x₁And (4) showing. The distance between the installation position of the one-dimensional position detector 205 and the rotation axis of the scanning turning mirror 203 is represented by L, and from the geometrical relationship shown in fig. 2, the calculation formula of the azimuth angle of the target object can be obtained as follows:

$A=\mathrm{arctg}\left(\frac{L}{x_1}\right)---\left(2\right)$

the proposed method is implemented in a processor as shown in fig. 1. Fig. 3 is a vehicle travel coordinate system, where the x direction is the direction in which the vehicle 301 travels, the y direction is the direction perpendicular to the direction in which the vehicle 301 travels, the target is 302, the target distance is D, and the target azimuth is a. The specific flow of the method is shown in FIG. 4.

As shown in FIG. 4, after the algorithm starts to run, the system initialization is first performed, and the distance measurement noise variance σ of the radar measurement system is given_r ²(derived statistically from radar measurements, a typical radar measurement system may be 1 meter²) Angle measurement noise variance σ_θ ²(derived statistically from radar measurements, a typical radar measurement system may take 1 degree²) System noise variance Q_u(for the vehicle application environment, it is taken as 0.5m/s⁴) System stateInitial value X of vector_0e(the initial value of the system state vector can be given arbitrarily and the zero vector is generally given by the convergence of the algorithm), and the initial value P of the system state estimation error covariance matrix_0e(the initial value of the covariance matrix of the estimation error of the system state can be given arbitrarily by the convergence of the algorithm, and a unit matrix is generally given).

After the system initialization is finished, obtaining the distance and the azimuth angle information of the target object by using the formulas (1) and (2), sending the information into a Kalman filtering program for filtering, and obtaining the smooth distance and the relative speed information of the target object through filtering, wherein the method comprises the following steps:

1) calculating the current observed value Z of the distance of the target object in the driving direction (x direction) of the vehicle_n：

Z_n＝D_ncosA_n (3)

Wherein, the subscript n represents the number of times of measurement and calculation, and the initial value thereof is 1; the current measured values being indicated by the corresponding amounts plus the subscript n, e.g. D_nIs a current measurement of the distance of the target object, A_nIs a current measurement of the azimuth of the target object; the last measurement being indicated by the corresponding amount plus the subscript (n-1), e.g. Z_(n-1)Representing the last measurement of the x-direction distance of the target. The subscript n for the variables below has the same meaning as described above.

2) Calculating the noise variance of the current x-direction distance measurement:

${\mathrm{\&sigma;}}_x^2={\mathrm{\&sigma;}}_r^2\&CenterDot;{\cos }^2{A}_n+{D_n}^2\&CenterDot;{\mathrm{\&sigma;}}_{\mathrm{\&theta;}}^2{\sin }^2{A}_n---\left(4\right)$

3) and carrying out filtering calculation according to the following fourth-order Kalman filtering recursion formula:

X_ny＝F·X_(n-1)e (5)

P_ny＝F·P_(n-1)e·F′+G·Q_u·G′ (6)

$K_n=P_{\mathrm{ny}}\&CenterDot;H^{\&prime;}\&CenterDot;{(H\&CenterDot;P_{\mathrm{ny}}\&CenterDot;H^{\&prime;}+{\mathrm{\&sigma;}}_x^2)}^{-1}---\left(7\right)$

P_ne＝(I-K_n·H)·P_ny (8)

X_ne＝X_ny+K_n·(Z_n-H·X_ny) (9)

wherein:

$X_{\mathrm{ne}}^{\&prime;}=[x_n,{\stackrel{\&CenterDot;}{x}}_n,{\stackrel{\&CenterDot;\&CenterDot;}{x}}_n,{\stackrel{\&CenterDot;\&CenterDot;\&CenterDot;}{x}}_n]---\left(10\right)$

for dynamic optimal estimation of current object information (' is matrix transposed notation, x)_nRepresents an optimal estimate of the x-direction distance value of the object,

optimal estimate representing the relative velocity between the vehicle and the target object), X_nyFor one-step forecast estimation of target object information, F is a system matrix:

$F=\left[\begin{array}{cccc}1& T& T^2/2& T^3/6\\ 0& 1& T& T^2/2\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]---\left(11\right)$

t is system sampling time; p_neIs the current system state estimation error covariance matrix, G is the system error impact matrix,

$G=\left[\begin{array}{c}0\\ 0\\ 0\\ T\end{array}\right]---\left(11\right)$

Q_uis the system noise variance; p_nyPredicting an estimation error covariance matrix for the system state in one step; k_nIs a Kalman filter gain matrix; h is an observation matrix, and H ═ 1000](ii) a I is a unit array;

4) obtaining the distance x of the target object in the x direction according to the recursion formula_nAnd relative velocityTo estimate the optimum.

5) Store P_ne、X_neFor continued recursive computation and output; store P_ny、X_nyIs used for the calculation of the measurement continuity judgment.

The filtering process described above is explained as follows:

considering the actual vehicle motion condition, assuming that the target vehicle runs at a constant acceleration in the x direction and is interfered by a white noise acceleration, and taking the system state vector as shown in the equation (10), the discrete x-direction system model is expressed as:

X_n+1＝F·X_n+G·u_n (13)

Z_nis the current measurement value of the x-direction distance of the target object, T is the sampling time of the measurement system, G is selected by considering the effect of multiple integration, namely, the influence on other states is realized by layer-by-layer integration under the assumption that the system noise only directly influences the change of the acceleration of the vehicleNo direct effect is shown. System noise u_nHas a variance of Q_u。

The system x-direction measurement model is as follows:

Z_n＝H·X_n+v_n (14)

wherein Z_nCalculated from the formula (3).

H＝[1 0 0 0 ] (15)

v_nThe variance of the x-direction distance measurement noise for the nth measurement is calculated as follows:

${\mathrm{\&sigma;}}_x^2={\mathrm{\&sigma;}}_r^2\&CenterDot;{\cos }^2\mathrm{\&theta;}+r^2\&CenterDot;{\mathrm{\&sigma;}}_{\mathrm{\&theta;}}^2{\sin }^2\mathrm{\&theta;}---\left(16\right)$

wherein sigma_r ²Is the variance, σ, of the distance measurement error_θ ²Is the variance of the azimuth angle measurement error.

The Kalman (Kalman) filter formula is shown below:

X_ne＝X_ny+K_n·(Z_n-H·X_ny) (17)

wherein, X_neFor dynamic optimal estimation of current object information, X_nyFor one-step predictive estimation of target information, Z_nIs the current observed value of the x-direction distance of the target object, K_nIs a Kalman (Kalman) filter gain matrix.

According to the optimal estimation theory, the system recursion determining process is as follows:

X_ny＝F·X_(n-1)e (18)

P_ny＝F·P_(n-1)e·F′+G·Q_u·G′ (19)

$K_n=P_{\mathrm{ny}}\&CenterDot;H^{\&prime;}\&CenterDot;{(H\&CenterDot;P_{\mathrm{ny}}\&CenterDot;H^{\&prime;}+{\mathrm{\&sigma;}}_x^2)}^{-1}---\left(20\right)$

P_ne＝(I-K_n·H)·P_ny (21)

wherein, P_neIs a covariance matrix of current system state estimation errors, P_nyIs a covariance matrix of one-step prediction estimation errors of the system state. Thus, given a system sampling time T, a random acceleration variance Q_uRadar measurement noise variance σ_r ²And σ_θ ²The system initial value P_0eAnd X_0eBy performing Kalman (Kalman) filter estimation of the radar target information according to the above equations (17) to (21), accurate and smooth target distance and relative velocity information can be obtained. The filtering algorithm recursion process is shown in fig. 5.

The invention designs a target measurement continuity judging method, which can continuously track and measure the same target object, remove the interference to the whole filtering process caused by the loss of individual data of the radar, and prevent the interference of factors such as road environment and the like to the system, and comprises the following specific steps:

6) one-step predictive estimation Z of computed observations_ny：Z_ny＝H·X_ny (22)

7) And X_nyCorresponding variance matrix

$S:S=H\&CenterDot;P_{\mathrm{ny}}\&CenterDot;H^{\&prime;}+{\mathrm{\&sigma;}}_x^2---\left(23\right)$

8) Defining an observation factor: d²＝v′·S^-1·v (24)

Wherein: v ═ Z_n-Z_ny (25)

9) Since the measurement error follows a normal distribution, d²Obey degree of freedom n_mX of²Distribution of where n is_mFor the number of information of the object to be measured, e.g. in the present invention, in the state variable X_nIn (1), only the x-direction distance information of the target object can be measured, so n _m1. If the significance level is 5%, for a single observation system, x is²The decision criteria for hypothesis testing can be obtained by a distribution look-up tableComprises the following steps:

d²≤3.841 (26)

if the new measurement result makes the formula (26) true, the probability that the latest measurement value and the last measurement value come from the same target can be judged to be more than or equal to 95%, the target object information obtained through filtering estimation can be considered to be accurate, and the result is normally output; if the new measured value fails to satisfy the formula (26), it is determined that the probability that the new measured value is not from the tracked target or that the radar has lost the target object measurement information is 95%, and the target vehicle information obtained by the filtering estimation at this time is considered inaccurate on the assumption that a small probability event is an impossible event, and cannot be used as the optimal estimation of the target vehicle information.

The loss of the target object measurement information by the radar due to the vehicle pitching and swinging is only temporary, and in order to maintain the continuity of the target object information output result, according to the kalman filtering assumption, it can be considered that the target object motion state is maintained in the period of time when the target object measurement information is lost, and the calculation method for obtaining the target object state is shown as formula (27).

$x_n=x_{(n-1)}+{\stackrel{\&CenterDot;}{x}}_{(n-1)}T+\frac{1}{2}{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{(n-1)}{T}^2+\frac{1}{6}{\stackrel{\&CenterDot;\&CenterDot;\&CenterDot;}{x}}_{(n-1)}{T}^3$

${\stackrel{\&CenterDot;}{x}}_n={\stackrel{\&CenterDot;}{x}}_{(n-1)}+{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{(n-1)}T+\frac{1}{2}{\stackrel{\&CenterDot;\&CenterDot;\&CenterDot;}{x}}_{(n-1)}{T}^2---\left(27\right)$

${\stackrel{\&CenterDot;\&CenterDot;}{x}}_n={\stackrel{\&CenterDot;\&CenterDot;}{x}}_{(n-1)}+{\stackrel{\&CenterDot;\&CenterDot;\&CenterDot;}{x}}_{(n-1)}T$

${\stackrel{\&CenterDot;\&CenterDot;\&CenterDot;}{x}}_n={\stackrel{\&CenterDot;\&CenterDot;\&CenterDot;}{x}}_{(n-1)}$

And returning to the step 1).

If the number of times of measurement that cannot satisfy the formula (26) continuously exceeds a predetermined value e (the value of e is determined according to the motion property of the target object and the sampling frequency of the measurement system, according to the present invention, for the vehicle-mounted environment, e is taken as the number of times of measurement within 1 second, i.e., e is taken as 10), which indicates that the above-mentioned failure is not caused by the temporary loss of the measurement information of the target vehicle by the radar, but a new target vehicle appears, and at this time, the latest measurement target object is set as the tracking target object, and the measurement information thereof is taken as the initial state value of the target object (the measurement value of the X-direction distance of the latest target object is taken as X_neIn x_nValue of (A), X_neMiddle and other state values are set to 0), P is set_neSetting the unit array as a unit array, and starting tracking measurement on a new target object.

FIG. 6 is a set of experimental data for a practical vehicle experiment using the method of the present invention. Fig. 6(a) is distance data of an actual vehicle experiment, and for comparison, raw measurement values before filtering and radar output values after filtering are respectively shown in the figure; 601 is a partial magnified view of experimental data; FIG. 6(b) is the real vehicle experimental data of relative velocity, wherein the real value of relative velocity is measured by the microwave velocimeter; 6(c) is azimuth angle real vehicle experimental data obtained by the invention, and the unit is degree.

As can be seen from fig. 6, the kalman filtering algorithm provided by the present invention can remove the influence of the measurement noise and the system noise on the measurement result, and obtain accurate and real-time target distance and relative velocity information.

Fig. 7 shows another measurement result of a real vehicle experiment performed by the method of the present invention, in which a temporary loss of radar measurement data occurs due to the bumpy road surface. Fig. 7(a) is a comparison between radar raw measurement data and radar output data after signal processing; FIG. 7(b) is an experimental result of relative velocity; fig. 7(c) is a value of an observation factor used for measurement continuity judgment in the course of an actual vehicle experiment.

As can be seen from fig. 7(a), although there is a short loss of raw data of radar measurement, the radar output data maintains good continuity. As can be seen from fig. 7(b), the relative velocity value output by the radar apparatus is not affected by the temporary loss of radar data. As can be seen from fig. 7(c), the target measurement continuity determination method of the present invention can find the loss of the target measurement data by the radar in time. Fig. 7 shows that the target object measurement continuity determination method designed by the present invention can effectively remove the influence caused by the temporary loss of radar data, and verifies the beneficial effects of the present invention.

Claims (2)

1. The method for detecting the object in front of the vehicle is characterized by comprising the following steps executed by a vehicle processor in sequence:

1) initialization:

given radar measurement system range measurement noise variance σ_r ²Angle measurement noise variance σ_θ ²System noise variance Q_uThe initial value X of the system state vector_0eInitial value P of covariance matrix of system state estimation error_0e；

2) Measuring the distance D of the target_nAnd orientationAngle A_nWherein, the subscript n represents the number of measurements and calculations, and its initial value is 1; the current measured values are indicated by the corresponding amounts with the subscript n;

3) calculating the current observed value Z of the distance of the target object in the driving direction of the vehicle, namely the x direction_n：

Z_n＝D_ncosA_n

4) Calculating the noise variance of the current x-direction distance measurement:

${\mathrm{\&sigma;}}_x^2={\mathrm{\&sigma;}}_r^2{\&CenterDot;\cos }^2{A}_n+{D_n}^2\&CenterDot;{\mathrm{\&sigma;}}_{\mathrm{\&theta;}}^2{\sin }^2{A}_n$

5) and carrying out filtering calculation according to the following fourth-order Kalman filtering recursion formula:

$X_{\mathrm{ny}}=F\&CenterDot;X_{(n-1)e}$

$P_{\mathrm{ny}}=F\&CenterDot;P_{(n-1)e}\&CenterDot;F^{\&prime;}+G\&CenterDot;Q_u\&CenterDot;G^{\&prime;}$

$K_n=P_{\mathrm{ny}}\&CenterDot;H^{\&prime;}\&CenterDot;{(H\&CenterDot;P_{\mathrm{ny}}\&CenterDot;H^{\&prime;}+{\mathrm{\&sigma;}}_x^2)}^{-1}$

P_ne＝(I-K_n·H)·P_ny

X_ne＝X_ny+K_n·(Z_n-H·X_ny)

wherein, $X_{\mathrm{ne}}^{\&prime;}=[x_n,{\stackrel{\&CenterDot;}{x}}_n,{\stackrel{\&CenterDot;\&CenterDot;}{x}}_n,{\stackrel{\&CenterDot;\&CenterDot;\&CenterDot;}{x}}_n]$ the dynamic optimal estimate representing the current target information is a matrix transposed symbol, x_nRepresents an optimal estimate of the x-direction distance value of the object,

an optimal estimate representing the relative velocity between the vehicle and the target object; x_nyFor one-step forecast estimation of target object information, F is a system matrix:

$F=\left[\begin{array}{cccc}1& T& T^2/2& T^3/6\\ 0& 1& T& T^2/2\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]$

t is system sampling time; p_neIs the current system state estimation error covariance matrix, G is the system error impact matrix,

$G=\left[\begin{array}{c}0\\ 0\\ 0\\ T\end{array}\right]$

Q_uis the system noise variance; p_nyPredicting an estimation error covariance matrix for the system state in one step; k_nIs a Kalman filter gain matrix; h is an observation matrix, and H ═ 1000](ii) a I is a unit array;

6) store P_ne、X_neFor continued recursive computation and output; store P_ny、X_nyA value of (d) for measurement continuity judgment calculation;

7) and (3) judging the continuity of measurement:

7.1) calculation of a one-step predictive estimate of the observed value Z_ny：

Z_ny＝H·X_ny

7.2) with X_nyThe corresponding variance matrix S:

$S=H\&CenterDot;P_{\mathrm{ny}}\&CenterDot;H^{\&prime;}+{\mathrm{\&sigma;}}_x^2$

7.3) defining the observation factors: d²＝v′·S^-1·v

Wherein: v ═ Z_n-Z_ny

7.4) according to d²Judging whether the probability that the latest measured value and the last measured value come from the same target is equal to or exceeds a threshold value or not, if so, judging that the target object information obtained through filtering estimation is accurate, outputting the current distance, relative speed and azimuth angle, and returning to step 2) for next measurement;

if the number of times of the target object measurement is less than the threshold value, the radar is indicated to lose the target object measurement information, or the measurement value comes from another target object, and whether the number of times of the target object measurement is less than the threshold value continuously exceeds a preset value e needs to be further judged:

if the target object state does not exceed the preset value e, assuming that the target object motion state is kept unchanged in the period of time when the target object measurement information is lost, and obtaining the calculation method of the target object state according to the Kalman filtering assumption as follows:

$x_n=x_{(n-1)}+{\stackrel{\&CenterDot;}{x}}_{(n-1)}T+\frac{1}{2}{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{(n-1)}{T}^2+\frac{1}{6}{\stackrel{\&CenterDot;\&CenterDot;\&CenterDot;}{x}}_{(n-1)}{T}^3$

${\stackrel{\&CenterDot;}{x}}_n={\stackrel{\&CenterDot;}{x}}_{(n-1)}+{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{(n-1)}T+\frac{1}{2}{\stackrel{\&CenterDot;\&CenterDot;\&CenterDot;}{x}}_{(n-1)}{T}^2$

${\stackrel{\&CenterDot;\&CenterDot;}{x}}_n={\stackrel{\&CenterDot;\&CenterDot;}{x}}_{(n-1)}+{\stackrel{\&CenterDot;\&CenterDot;\&CenterDot;}{x}}_{(n-1)}T$

${\stackrel{\&CenterDot;\&CenterDot;\&CenterDot;}{x}}_n={\stackrel{\&CenterDot;\&CenterDot;\&CenterDot;}{x}}_{(n-1)}$

outputting the optimal estimation x of the distance of the current target object in the x direction_nRelative velocity of

Is estimated optimally, and the azimuth angle A_n(ii) a Returning to the step 2) to perform next measurement;

if the value exceeds a preset value e, a new target object is considered to appear, the latest measurement target object is set as a tracking target object, the measurement information of the tracking target object is taken as the initial state value of the target object, namely the measurement value of the X-direction distance of the latest target object is taken as X_neIn x_nValue of (A), X_neSetting other state values to 0, and setting P_neSetting as a unit array, returning to the step 2) and starting to track and measure a new target object.

2. The method for detecting an object ahead of a vehicle according to claim 1, wherein in the step 7.4), the criterion d is set²The threshold value of the probability value of the latest measured value and the last measured value from the same target is judged to be 95 percent according to the formula d²< 3.841 it is determined whether the latest measurement value and the last measurement value are from the same target.

Images