CN104635233A - Method for estimating and classifying motion states of front object based on vehicle-mounted millimeter wave radar - Google Patents

Abstract

The invention discloses a method for estimating and classifying motion states of a front object based on a vehicle-mounted millimeter wave radar. The method is characterized by comprising the following steps of on the basis of the lateral speed information of the limited motion of the front object which is directly measured by the vehicle-mounted millimeter wave radar, establishing a motion equation of the front object under a ground coordinate system, utilizing a self-adaptive Kalman filter estimation algorithm, and accurately estimating the motion state of the front object in real time; according to the motion information of the front object, classifying according to the specified motion state division speed threshold value and the motion state conversion rule. By adopting the technical scheme, the method has the advantages that compared with the prior art, the estimation accuracy of the motion state is greatly improved, the strict front object motion state conversion condition is specified, and the classification accuracy is guaranteed.

Description

Based on objects in front state estimation and the sorting technique of vehicle-mounted millimeter wave radar

Technical field

The present patent application belongs to Radar Technology field, relates to objects in front state estimation and classification, can be used for advanced driver assistance system.

Background technology

In recent years, advanced driver assistance system ADAS has become the study hotspot of automotive safety technology.It mainly comprises adaptive cruise control system, Lane Departure Warning System, front collision early warning system etc. at present.Based on advanced driver assistance system and the advanced information sensing technology such as radar and computer vision, improve driver comfort and the vehicle safety of driver.

Millimetre-wave radar is widely used in driver assistance system, for measuring the target such as vehicle, barrier in the medium and long distance of front.For continuous wave millimetre-wave radar, its measuring principle substantially: transmitter produces continuous high frequency persistent wave, and its frequency carries out cyclical variation in time.Radar wave propagation to target again through target reflection return antenna during this period of time in, now the frequency of transmitter compares echo frequency change, therefore mixer output become there is difference frequency voltage.This difference frequency voltage is directly relevant to the relative distance between radar and objects ahead.And ought relative velocity not be 0 between the two, due to Doppler effect, echo frequency and transmitter frequency also have frequency-splitting on the basis of the aforementioned difference frequency caused by relative distance to be changed, and this difference frequency is directly relevant with both relative velocities.But the direct measurement of relative distance and relative velocity is based on vehicle-mounted millimeter wave radar fix system, and the objects in front motion state directly measured cannot be directly used in drive assist system, therefore needs to estimate real-time and accurately objects in front motion state.

Vehicle condition parameter estimation is widely used in the middle of all kinds of control system of automobile already.Early stage at automobile stability control system (Electronic Stability Program, ESP) in research, utilize the information of vehicles such as the wheel speed of low cost, yaw velocity, recycling Kalman Filter Estimation algorithm, the information of vehicles that automobile side slip angle, coefficient of road adhesion etc. are difficult to directly measure or measure cost higher is estimated, and for stabilitrak.Except using classic card Kalman Filtering algorithm for estimating, also introduce the state estimation algorithm such as particle filter, adaptive Kalman filter.Therefore, on the basis of the objects in front movement state information that the application directly measures at radar, utilize adaptive Kalman filter algorithm for estimating to carry out accurately estimating in real time to the motion state of objects in front based on terrestrial coordinate.

Existing radar objects in front classification is classify according to its attribute mostly, as being divided into wheeled vehicle, endless-track vehicle, pedestrian, trees etc.

Summary of the invention

Target of the present invention is the deficiency overcoming above-mentioned prior art, proposes a kind of objects in front state estimation based on vehicle-mounted millimeter wave radar and sorting technique, to improve estimation and the classification accuracy of objects in front motion state.

The technical scheme that above-mentioned first object of the present invention is achieved is: based on the objects in front estimation method of motion state of vehicle-mounted millimeter wave radar, it is characterized in that: based on the side velocity information of the limited objects in front motion that vehicle-mounted millimeter wave radar is directly measured, set up the equation of motion of objects in front under earth coordinates, utilize adaptive Kalman filter algorithm for estimating, estimate objects in front motion state real-time and accurately.

Further, comprise the steps:

I, by the equation of motion of vehicle-mounted millimeter wave radar map objects in front under earth coordinates be:

$\left\{\begin{array}{c}{x}_{o\stackrel{\·\·}{b}\mathrm{jR}}\left(t\right)=[a-a_v]\\ x_{o\stackrel{\·}{b}\mathrm{jR}}\left(t\right)=[\stackrel{\·}{x}\left(0\right)-{\stackrel{\·}{x}}_v\left(0\right)]+[a-a_v]t\\ x_{\mathrm{objR}}\left(t\right)=[x\left(0\right)-x_R\left(0\right)]+[\stackrel{\·}{x}\left(0\right)t-{\stackrel{\·}{x}}_v\left(0\right)t]+[\frac{1}{2}{\mathrm{at}}^2-\frac{1}{2}{a}_v{t}^2],\end{array}\right.$

In above formula, for the acceleration of objects in front, for the speed of objects in front, x _{obj_R}t distance that () is objects in front;

The equation of motion of II, sign objects in front:

$\stackrel{\·}{x}\left(t\right)=\mathrm{diag}[\mathrm{\Λ},\mathrm{\Λ}]x\left(t\right)+\mathrm{diag}[B,B]w\left(t\right),$

$\mathrm{\Λ}=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right],B=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right],$

Wherein Λ is object moving state system matrix in single coordinate axis; B is process noise matrix; W (t)=[w _x(t), w _y(t)] ^t, w _x(t) ~ N (0, σ _wx ²), w _y(t) ~ N (0, σ _wy ²) be separate random white noise process;

The discrete time model of objects in front side lengthwise movement equation is:

x _k+1＝diag[Φ，Φ]x _k+diag[G，G]w _k

The observation equation of objects in front motion state is:

z(t)＝Cx(t)+v(t)

$C=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\end{array}\right],$

Wherein, z (t) is observing matrix; C is output state matrix; V (t)=[v _x(t), v _x(t), v _y(t)] ^t, v (t) ~ N (0, R) is white Gaussian noise process;

Objects in front motion state observation equation discrete time model is:

Z _k=Cx _k+ v _k, wherein, z _kfor observation vector; v _kfor Gaussian sequence;

III, the state equation being wave filter with the discrete time equation of objects in front motion state equation, with the observation equation that the discrete time equation of objects in front motion state observation equation is wave filter, adopt adaptive Kalman filter algorithm for estimating, carry out accurately estimating in real time to the motion state of objects in front, adaptive Kalman filter algorithm for estimating comprises prediction, correction and noise and estimates three processes, and detailed process is as follows:

One, forecasting process:

Status predication equation: wherein, the state vector that x (k) is the k moment and measurement vector, A is systematic state transfer matrix, the average that q (k) is system noise;

Error covariance predictive equation: p (k+1|k)=Ap (k|k) A ^t+ Q (k), wherein, P is prediction covariance matrix;

Intermediate variable: $\begin{array}{c}\mathrm{\ϵ}(k+1)=y(k+1)-H\hat{x}(k+1|k)-\hat{r}\left(k\right)\\ d\left(k\right)-(1-b)/\left({1-b}^{k+1}\right)\end{array},$ Wherein, y (k) is for measuring vector, and H is output state matrix, the average that r (k) is observation noise, and b is forgetting factor;

Two, trimming process:

Gain equation: K (k+1|k)=P (k+1|k) H ^t[HP (k+1|k) H ^t+ R (k)] ^-1,

K _kfor kalman gain matrix,

Filtering equations: x (k+1|k+1)=x (k+1|k)+K (k+1) ε (k+1),

Error covariance renewal equation: P (k+1|k+1)=[I-K (k+1) H] P (k+1|k),

Three, noise estimation procedure:

Average and the auto-covariance matrix estimate equation of noise are:

$\begin{array}{c}\hat{q}(k+1)=[1-d\left(k\right)]\hat{q}\left(k\right)+d\left(k\right)\×[\hat{x}(k+1|k+1)-A\hat{x}\left(k\right|k)]\\ \hat{Q}(k+1)=[1-d\left(k\right)]\hat{Q}\left(k\right)+d\left(k\right)[K(k+1)\mathrm{\ϵ}(k+1){\mathrm{\ϵ}}^T(k+1)\×K^T(k+1)+P(k+1|k+1)-\mathrm{AP}\left(k\right|k)A^T]\\ \hat{r}(k+1)=[1-d\left(k\right)\hat{r}\left(k\right)+d\left(k\right)]\×[y(k+1)-H\stackrel{}{\hat{x}(k+1|k)]}\\ \hat{R}(k+1)=[1-d\left(k\right)]\hat{R}\left(k\right)+d\left(k\right)\×[\mathrm{\ϵ}(k+1){\mathrm{\ϵ}}^T(k+1)-\mathrm{HP}(k+1|k)H^T]\end{array}$

Wherein, Q _kfor auto-covariance matrix.

The technical scheme that above-mentioned second object of the present invention is achieved is: on the basis of objects in front state estimation, according to objects in front movable information, divide threshold speed and motion state transformation rule according to specified motion state to classify, described objects in front motion state is divided into unfiled, static, motion in the same way, move toward one another, stopping but moving in the same way before and to stop but move toward one another is several before according to existing and historical movement state is specific:

IV, by being defined as follows threshold speed, objects in front motion state to be classified:

V _t-when the objects in front speed of a motor vehicle is equal to or higher than this threshold value, objects in front is considered to move in the same way, and this threshold value is dynamically updated in each controlled circulation by following formula:

V _t=V _minmoving+ V _ego* k ₁+ a _ego* k ₂, wherein, V _minmovingfor this vehicle speed lower time this car considered to be in the threshold speed of transport condition, V _egofor this vehicle speed of current time, a _egofor this car of current time acceleration, k ₁and k ₂for needs debug determined weights by real vehicle;

V _x-when the objects in front speed of a motor vehicle is equal to or less than this threshold value, objects in front is considered to move toward one another, and this threshold is set to static parameter, and currency is-3m/s.

When the objects in front speed of a motor vehicle is at V _twith V _xbetween time, its motion state is that further judgements needs that are static or that stop carrying out according to the historical movement state of this objects in front;

V, to objects in front motion state classification and motion state between conversion formulate respective rule, State Transferring can only be carried out by this rule between each state:

I), unfiled → static, move in the same way, move toward one another, if objects in front speed is in scope and objects in front data can by stably measured in three circulations, then can realize the conversion of this motion state;

Ii), move in the same way → stop but moving in the same way before; Move toward one another → stopping but moving in the same way before, in continuous two circulations of objects in front speed all near 0, then can realize the conversion of this motion state;

Iii), static, stop but moving in the same way before, stop but before move toward one another → in the same way move, move toward one another, continuous three circulation objects in front speed are greater than 0, then can realize the conversion of this motion state.

The application implementation of objects in front state estimation of the present invention and sorting technique, its effect comparing to prior art outstanding is: the accuracy substantially increasing state estimation, and specify strict objects in front motion state switch condition, ensure that classification accuracy.

Accompanying drawing explanation

Fig. 1 is for the present invention is based on vehicle-mounted millimeter wave radar objects in front state estimation and classification process block diagram.

Fig. 2 is adaptive Kalman filter algorithm for estimating schematic diagram of the present invention.

Fig. 3 is objects in front motion state transformation rule schematic diagram of the present invention.

Embodiment

The application not based on its build-in attribute, is directly divided into some classes based on its motion state to the classification of objects in front.Obtainable objects in front motion state comprises side fore-and-aft distance, side longitudinal velocity, side longitudinal acceleration, position angle etc.Consider control system arithmetic speed and may occur measuring error, the application selects the longitudinal velocity of objects in front and arranges corresponding speed threshold value to classify.Meanwhile, formulate strict objects in front motion state switch condition, ensure the accuracy of classification.

Below just accompanying drawing in conjunction with the embodiments, is described in further detail the specific embodiment of the present invention, is easier to understand, grasp to make technical solution of the present invention.

First the motion state model of objects in front is set up.The objects in front of vehicle-mounted millimeter wave radar mainly comprises vehicle, pedestrian, trees etc.These targets generally without movement in vertical direction or its movement in vertical direction speed minimum.Therefore, its movement in vertical direction ignored by the motion state model set up in the present invention, only pays close attention to the motion of objects in front at surface level.Therefore state estimation is just reduced to the state estimation of objects in front in earth coordinates lower horizontal plane.When not considering the deformation of vehicle-mounted millimeter wave radar supports, can think that vehicle-mounted millimeter wave radar and this car are fixed together.Therefore, first determine that the equation of motion of vehicle-mounted millimeter wave radar coordinate system is relative to the earth:

$\left\{\begin{array}{c}\stackrel{\·\·}{x_R}\left(t\right)=a_v\\ \stackrel{\·}{x_R}\left(t\right)=\stackrel{\·}{x_v}\left(0\right)+a_{v}t\\ x_R\left(t\right)=x_R\left(0\right)+\stackrel{\·}{x_v}\left(0\right)t+\frac{1}{2}{a}_v{t}^2\end{array}\right.---\left(1\right)$

In above formula, for the acceleration of vehicle-mounted millimeter wave radar, a _vfor the acceleration of this car, for the speed of vehicle-mounted millimeter wave radar, for the initial velocity of this car, x _rt () is the mounting distance of vehicle-mounted millimeter wave radar.

The equation of motion of objects ahead under earth coordinates is:

$\left\{\begin{array}{c}\stackrel{\·\·}{x}\left(t\right)=a\\ \stackrel{\·}{x}\left(t\right)=\stackrel{\·}{x}\left(0\right)+\mathrm{at}\\ x\left(t\right)=x\left(0\right)+\stackrel{\·}{x}\left(0\right)t+\frac{1}{2}{\mathrm{at}}^2\end{array}\right.---\left(2\right)$

In above formula, for the acceleration of objects in front, a is the acceleration figure of objects in front, for the speed of objects in front, the distance that x (t) is objects in front.

Going out the equation of motion of objects in front under vehicle-mounted millimeter wave radar motion coordinate system by above-mentioned equation inference is:

$\left\{\begin{array}{c}{x}_{o\stackrel{\·\·}{b}\mathrm{jR}}\left(t\right)=[a-a_v]\\ x_{o\stackrel{\·}{b}\mathrm{jR}}\left(t\right)=[\stackrel{\·}{x}\left(0\right)-{\stackrel{\·}{x}}_v\left(0\right)]+[a-a_v]t\\ x_{\mathrm{objR}}\left(t\right)=[x\left(0\right)-x_R\left(0\right)]+[\stackrel{\·}{x}\left(0\right)t-{\stackrel{\·}{x}}_v\left(0\right)t]+[\frac{1}{2}{\mathrm{at}}^2-\frac{1}{2}{a}_v{t}^2]\end{array}\right.---\left(3\right)$

In above formula, for the acceleration of objects in front, for the speed of objects in front, x _{obj_R}t distance that () is objects in front.

Adopt describe the motion state of objects in front, comprise its side fore-and-aft distance, velocity and accelerations etc.The side lengthwise movement equation of objects ahead can be expressed as:

$\stackrel{\·}{x}\left(t\right)=\mathrm{diag}[\mathrm{\Λ},\mathrm{\Λ}]x\left(t\right)+\mathrm{diag}[B,B]w\left(t\right)$

$\mathrm{\Λ}=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right],B=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]---\left(4\right)$

Wherein Λ is object moving state system matrix in single coordinate axis; B is process noise matrix;

W (t)=[w _x(t), w _y(t)] ^t, w _x(t) N (0, σ _wx ²), w _y(t) N (0, σ _wy ²) be separate random white noise process.

The discrete time model of objects in front side lengthwise movement equation is:

x _k+1＝diag[Φ，Φ]x _k+diag[G，G]w _k(5)

The observation equation of objects in front motion state is:

z(t)＝Cx(t)+v(t)

$C=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\end{array}\right]---\left(6\right)$

Wherein, z (t) is observing matrix; C is output state matrix; V (t)=[v _x(t), v _x(t), v _y(t)] ^t, v (t) ~ N (0, R) is white Gaussian noise process.

Objects in front motion state observation equation discrete time model is:

z _k＝Cx _k+v _k(7)

Wherein, z _kfor observation vector; v _kfor Gaussian sequence.

For set up objects in front motion state equation, consider that vehicle-mounted millimeter wave radar is difficult to pre-determine in the statistical property of interior various onboard sensors simultaneously, therefore adopt adaptive Kalman filter algorithm to carry out accurately estimating in real time to objects in front motion state.

Adaptive Kalman filter algorithm comprises prediction, rectification and noise and estimates three processes.

With reference to Fig. 2, in forecasting process, based on the motion state of current time, prior estimate can be carried out to the motion state of subsequent time:

Status predication equation:

$\hat{x}(k+1|k)=A\hat{x}\left(\hat{k}|k\right)+\hat{q}\left(k\right|k)---\left(8\right)$

Wherein, the state vector that x (k) is the k moment and measurement vector, A is systematic state transfer matrix, the average that q (k) is system noise.

Error covariance predictive equation:

p(k+1|k)＝Ap(k|k)A ^T+Q(k) (9)

Wherein, P is prediction covariance matrix.

Intermediate variable:

$\begin{array}{c}\mathrm{\ϵ}(k+1)=y(k+1)-H\hat{x}(k+1|k)-\hat{r}\left(k\right)\\ d\left(k\right)-(1-b)/\left({1-b}^{k+1}\right)\end{array}---\left(10\right)$

Wherein, y (k) is for measuring vector, and H is output state matrix, the average that r (k) is observation noise, and b is forgetting factor.

In correcting process, observed motion state and the motion state pre-estimated are combined, obtain Posterior estimator:

Gain equation:

K(k+1|k)＝P(k+1|k)H ^T[HP(k+1|k)H ^T+R(k)] ^-1(11)

K _kfor kalman gain matrix.

Filtering equations:

x(k+1|k+1)＝x(k+1|k)+K(k+1)ε(k+1) (12)

Error covariance renewal equation:

P(k+1|k+1)＝[I-K(k+1)H]P(k+1|k) (13)

In noise estimation procedure, utilize and estimate that residual sequence is estimated to revise observation noise and system noise covariance matrix, realize accurately estimating in real time objects in front motion state:

The average of noise and auto-covariance matrix estimate equation:

$\begin{array}{c}\hat{q}(k+1)=[1-d\left(k\right)]\hat{q}\left(k\right)+d\left(k\right)\×[\hat{x}(k+1|k+1)-A\hat{x}\left(k\right|k)]\\ \hat{Q}(k+1)=[1-d\left(k\right)]\hat{Q}\left(k\right)+d\left(k\right)[K(k+1)\mathrm{\ϵ}(k+1){\mathrm{\ϵ}}^T(k+1)\×K^T(k+1)+P(k+1|k+1)-\mathrm{AP}\left(k\right|k)A^T]\\ \hat{r}(k+1)=[1-d\left(k\right)\hat{r}\left(k\right)+d\left(k\right)]\×[y(k+1)-H\stackrel{}{\hat{x}(k+1|k)]}\\ \hat{R}(k+1)=[1-d\left(k\right)]\hat{R}\left(k\right)+d\left(k\right)\×[\mathrm{\ϵ}(k+1){\mathrm{\ϵ}}^T(k+1)-\mathrm{HP}(k+1|k)H^T]\end{array}---\left(14\right)$

Wherein, Q _kfor auto-covariance matrix.

According to the equation of motion of aforementioned objects in front under earth coordinates and observation equation, the state equation that formula (5) is Kalman filter, the measurement equation that formula (7) is Kalman filter.

The input information of wave filter comprises the direct metrical information of vehicle-mounted millimeter wave radar, comprises the relative distance of objects in front, position angle and relative velocity:

z _k＝[x，v _r，y] ^T(15)

Have in the objects in front motion state obtained estimated by adaptive Kalman filter, choose the longitudinal velocity of objects in front, classify for objects in front motion state.

First two threshold speeds are calculated.

V _t-when the objects in front speed of a motor vehicle is equal to or higher than this threshold value, objects in front is considered to move in the same way.This threshold value is dynamically updated in each controlled circulation by following formula:

V _t＝V _minmoving+V _ego*k ₁+a _ego*k ₂(16)

Wherein, V _minmovingfor this vehicle speed lower time this car by task motion threshold speed, V _egofor this vehicle speed of current time, a _egofor this car of current time acceleration, k ₁and k ₂for needs debug determined weights by real vehicle.

V _x-when the objects in front speed of a motor vehicle is equal to or less than this threshold value, objects in front is considered to move toward one another.This threshold value is set to static parameter, and currency is-3m/s.

When the objects in front speed of a motor vehicle is between these two threshold speeds, its motion state may be static or stop.Further judgement needs the historical movement state according to this objects in front.

Therefore, in order to objects in front is classified more accurately, need to formulate respective rule to the conversion between the classification of objects in front motion state and motion state.Specifically, as shown in Figure 3, when objects in front motion state meets following condition, the classification of corresponding motion state can be realized:

Before objects in front, motion state is unfiledly namely do not have enough available measurement data, simultaneously continuous three its data of circulation can by stably measured time, if its speed is at V _twith V _xbetween, its motion state meeting convert to static; If its speed is less than or equal to V _x, its motion state can be converted to and travel in opposite directions; If its speed is more than or equal to V _t, its motion state can be converted to and travel in the same way.

Before objects in front, motion state is for move in the same way, in continuous two circulations of objects in front speed simultaneously all near 0 or at V _twith V _xbetween time, its motion state can be converted to stop but moving in the same way before.

Before objects in front, motion state is move toward one another, in continuous two circulations of objects in front speed simultaneously all near 0 or at V _twith V _xbetween time, its motion state can be converted to and stop but move toward one another before.

Before objects in front motion state be static, stop but motion or move toward one another before stopping in the same way before, simultaneously continuous three circulation objects in front speed are greater than V _t, its motion state can be converted to and move in the same way.

Before objects in front motion state be static, stop but motion or move toward one another before stopping in the same way before, simultaneously continuous three circulation objects in front speed are less than V _x, its motion state can be converted to move toward one another.

In sum, be the detailed description to the specific embodiment of the invention, this case protection domain is not constituted any limitation.The technical method that all employing equivalents or equivalence are replaced and formed, all drops within rights protection scope of the present invention.

Claims (3)

1. based on the objects in front estimation method of motion state of vehicle-mounted millimeter wave radar, it is characterized in that: based on the side velocity information of the limited objects in front motion that vehicle-mounted millimeter wave radar is directly measured, set up the equation of motion of objects in front under earth coordinates, utilize adaptive Kalman filter algorithm for estimating, estimate objects in front motion state real-time and accurately.

2., according to claim 1 based on the objects in front estimation method of motion state of vehicle-mounted millimeter wave radar, it is characterized in that comprising the steps:

I, by the equation of motion of vehicle-mounted millimeter wave radar map objects in front under earth coordinates be:

$\left\{\begin{array}{c}{x}_{o\stackrel{\·\·}{b}{j}_R}\left(t\right)=[a-a_v]\\ x_{o\stackrel{\·}{b}{j}_R}\left(t\right)=[\stackrel{\·}{x}\left(0\right)-{\stackrel{\·}{x}}_v\left(0\right)]+[a-a_v]t\\ x_{\mathrm{ob}{j}_R}\left(t\right)=[x\left(0\right)-x_R\left(0\right)]+[\stackrel{\·}{x}\left(0\right)t-{\stackrel{\·}{x}}_v\left(0\right)t]+[\frac{1}{2}{\mathrm{at}}^2-\frac{1}{2}{a}_v{t}^2\end{array}\right.,$

In above formula, for the acceleration of objects in front, for the speed of objects in front, x _{obj_R}t distance that () is objects in front;

The equation of motion of II, sign objects in front:

$\stackrel{\·}{x}\left(t\right)=\mathrm{diag}[\mathrm{\Λ},\mathrm{\Λ}]x\left(t\right)+\mathrm{diag}[B,B]w\left(t\right),$

$\mathrm{\Λ}=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right],B=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right],$

Wherein Λ is object moving state system matrix in single coordinate axis; B is process noise matrix;

W (t)=[w _x(t), w _y(t)] ^t, w _x(t) ~ N (0, σ _wx ²), w _y(t) ~ N (0, σ _wy ²) be separate random white noise process;

The discrete time model of objects in front side lengthwise movement equation is:

x _k+1＝diag[Φ，Φ]x _k+diag[G，G]w _k

The observation equation of objects in front motion state is:

z(t)＝Cx(t)+v(t)

$C=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\end{array}\right],$

Wherein, z (t) is observing matrix; C is output state matrix; V (t)=[v _x(t), v _x(t), v _y(t)] ^t, v (t) ~ N (0, R) is white Gaussian noise process;

Objects in front motion state observation equation discrete time model is:

Z _k=Cx _k+ v _k, wherein, z _kfor observation vector; v _kfor Gaussian sequence;

III, the state equation being wave filter with the discrete time equation of objects in front motion state equation, with the observation equation that the discrete time equation of objects in front motion state observation equation is wave filter, adopt adaptive Kalman filter algorithm for estimating, carry out accurately estimating in real time to the motion state of objects in front, adaptive Kalman filter algorithm for estimating comprises prediction, correction and noise and estimates three processes, and detailed process is as follows:

One, forecasting process:

Status predication equation: wherein, the state vector that x (k) is the k moment and measurement vector, A is systematic state transfer matrix, the average that q (k) is system noise;

Error covariance predictive equation: p (k+1|k)=Ap (k|k) A ^t+ Q (k), wherein, P is prediction covariance matrix;

$\mathrm{\ϵ}(k+1)=y(k+1)-H\hat{x}(k+1|k)-\hat{r}\left(k\right)$

Intermediate variable: d (k)=(1-b)/(1-b ^k+1), wherein, y (k) is for measuring vector, and H is output state matrix, the average that r (k) is observation noise, and b is forgetting factor;

Two, trimming process:

Gain equation: K (k+1|k)=P (k+1|k) H ^t[HP (k+1|k) H ^t+ R (k)] ^-1,

K _kfor kalman gain matrix,

Filtering equations: x (k+1|k+1)=x (k+1|k)+K (k+1) ε (k+1),

Error covariance renewal equation: P (k+1|k+1)=[I-K (k+1) H] P (k+1|k),

Three, noise estimation procedure:

Average and the auto-covariance matrix estimate equation of noise are:

$\hat{q}(k+1)=[1-d\left(k\right)]\hat{q}\left(k\right)+d\left(k\right)\×[\hat{x}(k+1|k+1)-A\hat{x}\left(k\right|k)]$

$\hat{Q}(k+1)=[1-d\left(k\right)]\hat{Q}\left(k\right)+d\left(k\right)[K(k+1)\mathrm{\ϵ}(k+1){\mathrm{\ϵ}}^T(k+1)\×K^T(k+1)+P(k+1|k+1)-\mathrm{AP}\left(k\right|k)A^T]$

$\hat{r}(k+1)=[1-d\left(k\right)\hat{r}\left(k\right)+d\left(k\right)]\×[y(k+1)-H\hat{x}(k+1|k)]$

$\hat{R}(k+1)=[1-d\left(k\right)]\hat{R}\left(k\right)+d\left(k\right)\×[\mathrm{\ϵ}(k+1){\mathrm{\ϵ}}^T(k+1)-\mathrm{HP}(k+1|k)H^T]$

Wherein, Q _kfor auto-covariance matrix.

3. based on the objects in front motion state sorting technique of vehicle-mounted millimeter wave radar, it is characterized in that: on the basis of objects in front state estimation, according to objects in front movable information, divide threshold speed and motion state transformation rule according to specified motion state to classify, described objects in front motion state is divided into unfiled, static, motion in the same way, move toward one another, stopping but moving in the same way before and to stop but move toward one another is several before according to existing and historical movement state is specific:

IV, by being defined as follows threshold speed, objects in front motion state to be classified:

V _t-when the objects in front speed of a motor vehicle is equal to or higher than this threshold value, objects in front is considered to move in the same way, and this threshold value is dynamically updated in each controlled circulation by following formula:

V _t=V _minmoving+ V _ego* k ₁+ a _ego* k ₂, wherein, V _minmovingfor this vehicle speed lower time this car considered to be in the threshold speed of transport condition, V _egofor this vehicle speed of current time, a _egofor this car of current time acceleration, k ₁and k ₂for needs debug determined weights by real vehicle;

V _x-when the objects in front speed of a motor vehicle is equal to or less than this threshold value, objects in front is considered to move toward one another, and this threshold is set to static parameter, and currency is-3m/s.

When the objects in front speed of a motor vehicle is at V _twith V _xbetween time, its motion state is that further judgements needs that are static or that stop carrying out according to the historical movement state of this objects in front;

V, to objects in front motion state classification and motion state between conversion formulate respective rule, State Transferring can only be carried out by this rule between each state:

I), unfiled → static, move in the same way, move toward one another, if objects in front speed is in scope and objects in front data can by stably measured in three circulations, then can realize the conversion of this motion state;

Ii), move in the same way → stop but moving in the same way before; Move toward one another → stopping but moving in the same way before, in continuous two circulations of objects in front speed all near 0, then can realize the conversion of this motion state;

Iii), static, stop but moving in the same way before, stop but before move toward one another → in the same way move, move toward one another, continuous three circulation objects in front speed are greater than 0, then can realize the conversion of this motion state.