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

The detection method of vehicle front object

Technical field

The detection method of vehicle front object belongs to perception of vehicle driving information and processing technology field.

Background technology

Perception of vehicle driving information and treatment technology are one of gordian techniquies of Hyundai Motor control and safety, existing measuring technique is divided into microwave measurement system and laser measurement system two classes by its applicating medium, and laser radar system is fast because of measuring speed, precision is high, ranging far is subjected to paying attention to widely.Document 1 (Tu Dawei: " the environment sensing Study of An that is used for collision prevention of vehicle control ", China's mechanical engineering, the 10th the 6th phase of volume, in June, 1999) introduced a kind of design of optical scanning formula range laser radar in, can realize the scanning range finding of relative broad range, this system can record the distance and bearing angle information of the place ahead object, and measuring error is not effectively addressed, the relative velocity fluctuation that obtains greatly can not be used in the reality.

Summary of the invention

The objective of the invention is to, a kind of detection method of vehicle front object is proposed at the deficiencies in the prior art, this method is carried out Filtering Processing with object distance and the azimuth information that obtains, can access more level and smooth accurate object distance and relative velocity, and the measurement continuity determination methods of target has further been proposed, can judge that whether the continuous coverage data that obtained are from same object.

The detection method of vehicle front object proposed by the invention is characterized in that, it contains the following steps of being carried out by the control of vehicle processor successively:

1) initialization:

Given radar measurement system range observation noise variance σ _r ², measurement of angle noise variance σ _θ ², system noise variance Q _u, system state vector initial value X _0e, system state estimation error covariance matrix initial value P _0e

2) the measurement target object distance is from D _nWith position angle A _n, wherein, subscript n is represented the number of times of measurements and calculations, its initial value is 1; Current measured value is represented with respective amount indexing n;

3) calculate object at this car travel direction, the i.e. current observed reading Z of distance on the x direction _n:

Z _n＝D _ncosA _n

4) calculate the noise variance of this x to range observation:

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

5) carrying out filtering by following quadravalence Kalman filtering recursion formula calculates:

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]$ The dynamic optimal estimation of expression current goal thing information, ' be the matrix transpose symbol, x _nRepresent the optimal estimation of object x to distance value, The optimal estimation of relative velocity between expression vehicle-to-target thing; X _NyFor the step forecast of object information estimates that 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 the systematic sampling time; P _NeBe current system state estimation error covariance matrix, G is the systematic error influence matrix,

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

Q _uIt is the system noise variance; P _NyBe system state one step forecast evaluated error covariance matrix; K _nBe the Kalman filtering gain matrix; H is an observing matrix, and H=[1 00 0]; I is a unit matrix;

6) store P _Ne, X _NeValue, the recursion that is used to continue is calculated and output; Store P _Ny, X _NyValue, be used to measure continuity and judge calculate;

7) measure successional judgement:

7.1) a step forecast of calculating observation value estimates Z _Ny:

Z _ny＝H·X _ny

7.2) and X _NyCorresponding variance matrix S:

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

7.3) the definition observation factor: d ²=v ' S ^-1V

Wherein: v=Z _n-Z _Ny

7.4) according to d ²Value judge up-to-date measured value and last time measured value whether equal or exceed threshold value from the probability of same target, as equal or exceed, think that then the object information that obtains by Filtering Estimation is accurately, then export this distance, relative velocity and position angle, and return the 2nd) go on foot and measure next time;

As less than this threshold value, show that then radar loses the object metrical information, or measured value needs further to judge from another target whether the number of times less than above-mentioned threshold value surpasses predetermined value e continuously:

If surpass predetermined value e, suppose then that at this section that the object metrical information is lost the object motion state remains unchanged in the period, according to the Kalman filtering hypothesis, the computing method that obtain the object state as shown in the formula:

$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)}$

Output current goal thing is at the optimal estimation x of the distance of x direction _n, relative velocity

Optimal estimation, and position angle A _nReturn the 2nd) go on foot and measure next time;

If surpass predetermined value e, then thinking has new object to occur, and up-to-date measurement target thing is made as the tracking target thing, and its metrical information is as the state initial value of object, promptly up-to-date object x to the measured value of distance as X _NeMiddle x _nValue, X _NeIn other state values be made as 0, with P _NeBe made as unit matrix, return the 2nd) step begin new object is carried out tracking measurement.

The above-mentioned the 7.4th) in the step, described according to d ²Value judge up-to-date measured value and last time measured value be 95% from the threshold value of the probable value of same target, promptly according to formula d ²≤ 3.841 judge up-to-date measured value and last time measured value whether from same target.

Experiment showed, that this method can obtain level and smooth more accurate object distance and relative velocity; Can carry out respective handling at the successional judged result of target measurement, guarantee the continuity of object information extraction, produce a desired effect.

Description of drawings:

Fig. 1 is the system principle diagram that realizes this method;

Fig. 2 is an object measurement of azimuth schematic diagram;

Fig. 3 is a vehicle axis system; Wherein, 301-is equipped with the vehicle of laser radar detection system, 302-object, x-vehicle forward direction, the y-vehicle vertical direction of advancing, D-object distance, A-object position angle;

Fig. 4 is the process flow diagram of vehicle front object detection method;

Fig. 5 is filtering algorithm recursive process figure;

Fig. 6 is an experimental data figure one; Wherein, 6 (a) are range data, 601-experimental data partial enlarged drawing; 6 (b) are the relative velocity data; 6 (c) are bearing datas.

Fig. 7 is an experimental data figure two; Wherein, 7 (a) are range data, and 7 (b) are the relative velocity data, and 7 (c) measure continuity observation factor data.

Embodiment

Accompanying drawings the specific embodiment of the present invention.

As shown in Figure 1, can adopt Shanghai University to slaughter big dimension and realize this method, (see Tu Dawei: " the environment sensing Study of An that is used for collision prevention of vehicle control ", Chinese mechanical engineering in the scanning type laser radar system of design in 1999, the 10th the 6th phase of volume, in June, 1999).This system adopts ranging phase method, laser diode sends the laser beam of sine amplitude modulation through pouring-in current-modulation, becomes the beam shape that meets the demands by emission light group, realizes horizontal scanning with scanning mirror again, to spatial emission, shine on front vehicles and the barrier.The light of being returned by target reflection converges on the snowslide pipe through receiving the light group from scanning mirror, become electric signal, this signal enters phase detector through the automatic gain control circuit with low noise enlarging function, obtain to receive the phase differential of laser with respect to emission laser, phase detector is passed to processor with this phase differential by plug-in card, calculates the object distance in processor.The computing formula of object distance D is as follows:

$D=\frac{c\·\mathrm{\Δ\φ}}{2\mathrm{\πf}}---\left(1\right)$

Wherein, Δ φ is the phase delay of the laser beam of reflected back with respect to emission of lasering beam, and c is the aerial velocity of propagation of light, and f is the intensity modulation frequency.

Utilize the one dimension position sensor among Fig. 1 to measure the scanning light beam angle, and then the position angle of definite object.Principle is calculated as shown in Figure 2 in the object position angle.From the light 204 of object 201 reflected backs, through the convergence of scanning mirror 203, become a luminous point, beat on one dimension position sensor 205, contain light activated element in the one dimension position sensor 205, can survey the distance of 202 of this luminous points to the axis, among the figure with x ₁Expression.Distance between one dimension position sensor 205 installation sites and scanning mirror 203 rotation axiss represents that with L by geometric relationship shown in Figure 2, it is as follows to obtain object position angle calculating formula:

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

Method proposed by the invention realizes in processor shown in Figure 1.Fig. 3 is the vehicle ' coordinate system, and wherein the x direction is the direction that vehicle 301 travels, and the y direction is and the vertical direction of vehicle 301 travel directions, and object is 302, and the object distance is D, and the object position angle is A.The idiographic flow of this method is seen Fig. 4.

As shown in Figure 4, after algorithm brings into operation, at first carry out system initialization, given radar measurement system range observation noise variance σ _r ²(obtained by the radargrammetry data statistics, general radar measurement system can be taken as 1 meter ²), measurement of angle noise variance σ _θ ²(obtained by the radargrammetry data statistics, general radar measurement system can be taken as 1 degree ²), system noise variance Q _u(, be taken as 0.5m/s for the vehicle applied environment ⁴), system state vector initial value X _0e(by convergence, system state vector initial value can be given arbitrarily, general given null vector), system state estimation error covariance matrix initial value P _0e(by convergence, system state estimation error covariance matrix initial value can be given arbitrarily, general given unit matrix).

After system initialization is finished, utilize formula (1), (2) to obtain object distance and bearing angle information, and these information are sent into the Kalman filtering program carry out Filtering Processing, obtain level and smooth object distance and its relative velocity by Filtering Processing, following steps are arranged:

1) calculates object and go up the current observed reading Z of distance in this car direction (x to) of travelling _n:

Z _n＝D _ncosA _n (3)

Wherein, subscript n is represented the number of times of measurements and calculations, and its initial value is 1; Current measured value is represented with respective amount indexing n, as D _nBe the current measured value of object distance, A _nIt is the azimuthal current measured value of object; Last measured value is with respective amount indexing (n-1) expression, as Z _(n-1)Expression object x is to the last measured value of distance.The implication of the subscript n of following related variable is same as described above.

2) calculate the noise variance of this x to range observation:

${\mathrm{\σ}}_x^2={\mathrm{\σ}}_r^2\·{\cos }^2{A}_n+{D_n}^2\·{\mathrm{\σ}}_{\mathrm{\θ}}^2{\sin }^2{A}_n---\left(4\right)$

3) carrying out filtering by following quadravalence Kalman filtering recursion formula calculates:

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}}\·H^{\′}\·{(H\·P_{\mathrm{ny}}\·H^{\′}+{\mathrm{\σ}}_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}}^{\′}=[x_n,{\stackrel{\·}{x}}_n,{\stackrel{\·\·}{x}}_n,{\stackrel{\·\·\·}{x}}_n]---\left(10\right)$

For the dynamic optimal of current goal thing information estimate (' be the matrix transpose symbol, x _nRepresent the optimal estimation of object x to distance value,

The optimal estimation of relative velocity between expression vehicle-to-target thing), X _NyFor the step forecast of object information estimates that 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 the systematic sampling time; P _NeBe current system state estimation error covariance matrix, G is the systematic error influence matrix,

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

Q _uIt is the system noise variance; P _NyBe system state one step forecast evaluated error covariance matrix; K _nBe the Kalman filtering gain matrix; H is an observing matrix, and H=[1 00 0]; I is a unit matrix;

4) according to above-mentioned recursion formula obtain object the x direction apart from x _nAnd relative velocity Optimal estimation.

5) store P _Ne, X _NeValue, the recursion that is used to continue is calculated and output; Store P _Ny, X _NyValue, be used to measure the calculating that continuity is judged.

Be explained as follows for above-mentioned filtering:

Consider the actual vehicle moving situation, the hypothetical target vehicle to travelling with constant acceleration, is subjected to the interference of white noise acceleration at x, gets the system state vector as the formula (10), and then Li San x is expressed as to system model:

X _n+1＝F·X _n+G·u _n (13)

Z _nBe the current measured value of the x of object to distance, T is the measuring system sampling time, and the selection of G is an effect of considering multiple integral, and promptly the supposing the system noise only directly has influence on the variation of vehicle acceleration, the influence of other states is embodied by integration layer by layer, there is no direct influence.System noise u _nVariance be Q _u

System x to measurement model is:

Z _n＝H·X _n+v _n (14)

Z wherein _nCalculate by (3) formula.

H＝[1?0?0?0?] (15)

v _nBe the x that measures for the n time to the range observation noise, its variance is calculated by following formula:

${\mathrm{\σ}}_x^2={\mathrm{\σ}}_r^2\·{\cos }^2\mathrm{\θ}+r^2\·{\mathrm{\σ}}_{\mathrm{\θ}}^2{\sin }^2\mathrm{\θ}---\left(16\right)$

σ wherein _r ²Be the variance of range observation error, σ _θ ²It is the variance of azimuth measurement error.

Kalman (Kalman) filtering formula is as follows:

X _ne＝X _ny+K _n·(Z _n-H·X _ny) (17)

Wherein, X _NeFor the dynamic optimal of current goal thing information is estimated X _NyFor Z is estimated in the step forecast of object information _nBe the current observed reading of object x to distance, K _nBe Kalman (Kalman) filter gain matrix.

According to the optimal estimation theory, determine that system's recursive 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}}\·H^{\′}\·{(H\·P_{\mathrm{ny}}\·H^{\′}+{\mathrm{\σ}}_x^2)}^{-1}---\left(20\right)$

P _ne＝(I-K _n·H)·P _ny (21)

Wherein, P _NeBe current system state estimation error covariance matrix, P _NyIt is system state one step forecast evaluated error covariance matrix.Like this, given systematic sampling time T, casual acceleration variance Q _u, radargrammetry noise variance σ _r ²And σ _θ ², the initial value P of system _0eAnd X _0e, carry out Kalman (Kalman) Filtering Estimation of radar target thing information according to above-mentioned formula (17)～(21), can obtain accurate, level and smooth object distance and relative velocity.The filtering algorithm recursive process as shown in Figure 5.

Next judge the object continuity of measurement, the running environment more complicated of actual vehicle, the data that can not guarantee radargrammetry are continuously and from same object, the present invention has designed the measurement continuity determination methods of target, can carry out Continuous Tracking to same object measures, remove because the radar individual data is lost the interference to whole filtering that brings, and can prevent the interference of factor such as road environment to system, concrete steps are as follows:

6) Z is estimated in the forecast of one of calculating observation value step _Ny: Z _Ny=HX _Ny(22)

7) and X _NyCorresponding variance matrix

$S:S=H\·P_{\mathrm{ny}}\·H^{\′}+{\mathrm{\σ}}_x^2---\left(23\right)$

8) the definition observation factor: d ²=v ' S ^-1V (24)

Wherein: v=Z _n-Z _Ny(25)

9) owing to the measuring error Normal Distribution, therefore, d ²The obedience degree of freedom is n _mX ²Distribute, wherein n _mBe the number of the object information of energy measurement, for example among the present invention, at state variable X _nIn, only there is object x can measure to range information, so n _m=1.As to get the level of signifiance be 5%, for the system of single observation amount, by x ²The distribution judgment criterion that can obtain test of hypothesis of tabling look-up is:

d ²≤3.841 (26)

If new measurement result is set up formula (26), then can judge up-to-date measured value with last time measured value from the probability of same target more than or equal to 95%, can think that the object information by the Filtering Estimation acquisition is accurately, the result normally exports; Can not satisfy formula (26) as new measured value, judge that then new measured value is not from tracking target or the probability that radar loses the object metrical information to have occurred be 95%, according to small probability event is the hypothesis of impossible event, can think that be inaccurate by the target vehicle information that Filtering Estimation obtains this moment, can not be with this optimal estimation as target vehicle information.

Because jolting and swinging the radar that brings of vehicle is temporary transient to losing of object metrical information, for keeping object information output result's continuity, suppose according to Kalman filtering, can think at this section that the object metrical information is lost in the period, the object motion state remains unchanged, and the computing method that obtain the object state as the formula (27).

$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---\left(27\right)$

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

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

N=n+1 returns the 1st) step.

(value of e is according to the kinetic property of object and the sample frequency decision of measuring system when surpassing a certain predetermined value e as the measurement number of times that can not satisfy formula (26) continuously, in the present invention, for vehicle environment, desirable e is the measurement number of times in 1 second, be e=10), illustrate that this not satisfy be not because radar causes temporarily losing of target vehicle metrical information, but there is new target vehicle to occur, at this moment, up-to-date measurement target thing is made as the tracking target thing, its metrical information as the state initial value of object (up-to-date object x to the measured value of distance as X _NeMiddle x _nValue, X _NeIn other state values be made as 0), with P _NeBe made as unit matrix, begin new object is carried out tracking measurement.

Fig. 6 utilizes method of the present invention to carry out one section experimental data of real vehicle experiment.Wherein, Fig. 6 (a) is the range data of real vehicle experiment, for ease of contrast, has provided preceding original measurement value and the filtered radar output valve of filtering among the figure respectively; The 601st, the experimental data partial enlarged drawing; Fig. 6 (b) is a relative velocity real vehicle experimental data, and relative velocity actual value wherein utilizes the Microwave Velocity instrument to record; 6 (c) are the position angle real vehicle experimental datas that the present invention obtains, and unit is degree.

From Fig. 6 as seen, the Kalman filtering algorithm that utilizes the present invention to carry can be removed measurement noise and system noise to the influence of measurement result, obtains accurate, real-time object distance and relative velocity.

Fig. 7 utilizes method of the present invention to carry out another section measurement result of real vehicle experiment, and temporarily losing of radargrammetry data appearred in jolting of during experiment because road surface.Wherein, Fig. 7 (a) is the contrast of radar raw measurement data and the radar output data after signal Processing; Fig. 7 (b) is the experimental result of relative velocity; Fig. 7 (c) is in the real vehicle experimentation, is used to measure the value of the observation factor that continuity judges.

From Fig. 7 (a) as seen, though there be of short duration losing in the radargrammetry raw data, the radar output data has kept continuity preferably.From Fig. 7 (b) as seen, the relative velocity of the radar installations output influence that not temporarily lost by radar data.From Fig. 7 (c) as seen, utilize object of the present invention to measure the continuity determination methods, can in time find radar losing the object measurement data.Fig. 7 shows, utilizes the designed object of the present invention to measure the continuity determination methods, can effectively remove since radar data temporarily lose the influence that brings, verified beneficial effect of the present invention.

Claims (2)

1, the detection method of vehicle front object is characterized in that, it contains the following steps of being carried out by the control of vehicle processor successively:

1) initialization:

Given radar measurement system range observation noise variance σ _r ², measurement of angle noise variance σ _θ ², system noise variance Q _u, system state vector initial value X _0e, system state estimation error covariance matrix initial value P _0e

2) the measurement target object distance is from D _nWith position angle A _n, wherein, subscript n is represented the number of times of measurements and calculations, its initial value is 1; Current measured value is represented with respective amount indexing n;

3) calculate object at this car travel direction, the i.e. current observed reading Z of distance on the x direction _n:

Z _n＝D _ncosA _n

4) calculate the noise variance of this x to range observation:

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

5) carrying out filtering by following quadravalence Kalman filtering recursion formula calculates:

$X_{\mathrm{ny}}=F\·X_{(n-1)e}$

$P_{\mathrm{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]$ The dynamic optimal of expression current goal thing information is estimated, is the matrix transpose symbol, x _nRepresent the optimal estimation of object x to distance value,

The optimal estimation of relative velocity between expression vehicle-to-target thing; X _NyFor the step forecast of object information estimates that 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 the systematic sampling time; P _NeBe current system state estimation error covariance matrix, G is the systematic error influence matrix,

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

Q _uIt is the system noise variance; P _NyBe system state one step forecast evaluated error covariance matrix; K _nBe the Kalman filtering gain matrix; H is an observing matrix, and H=[1 00 0]; I is a unit matrix;

6) store P _Ne, X _NeValue, the recursion that is used to continue is calculated and output; Store P _Ny, X _NyValue, be used to measure continuity and judge calculate;

7) measure successional judgement:

7.1) a step forecast of calculating observation value estimates Z _Ny:

Z _ny＝H·X _ny

7.2) and X _NyCorresponding variance matrix S:

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

7.3) the definition observation factor: d ²=v ' S ^-1V

Wherein: v=Z _n-Z _Ny

7.4) according to d ²Value judge up-to-date measured value and last time measured value whether equal or exceed threshold value from the probability of same target, as equal or exceed, think that then the object information that obtains by Filtering Estimation is accurately, then export this distance, relative velocity and position angle, and return the 2nd) go on foot and measure next time;

As less than this threshold value, show that then radar loses the object metrical information, or measured value needs further to judge from another target whether the number of times less than above-mentioned threshold value surpasses predetermined value e continuously:

If surpass predetermined value e, suppose then that at this section that the object metrical information is lost the object motion state remains unchanged in the period, according to the Kalman filtering hypothesis, the computing method that obtain the object state as shown in the formula:

$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)}$

Output current goal thing is at the optimal estimation x of the distance of x direction _n, relative velocity

Optimal estimation, and position angle A _nReturn the 2nd) go on foot and measure next time;

If surpass predetermined value e, then thinking has new object to occur, and up-to-date measurement target thing is made as the tracking target thing, and its metrical information is as the state initial value of object, promptly up-to-date object x to the measured value of distance as X _NeMiddle x _nValue, X _NeIn other state values be made as 0, with P _NeBe made as unit matrix, return the 2nd) step begin new object is carried out tracking measurement.

2, the detection method of vehicle front object as claimed in claim 1 is characterized in that, the above-mentioned the 7.4th) in the step, described according to d ²Value judge up-to-date measured value and last time measured value be 95% from the threshold value of the probable value of same target, promptly according to formula d ²≤ 3.841 judge up-to-date measured value and last time measured value whether from same target.

Images