CN102999759A - Light stream based vehicle motion state estimating method - Google Patents

Abstract

The invention discloses a light stream based vehicle motion state estimating method which is applicable to estimating motion of vehicles running of flat bituminous pavement at low speed in the road traffic environment. The light stream based vehicle motion state estimating method includes mounting a high-precision overlook monocular video camera at the center of a rear axle of a vehicle, and acquiring video camera parameters by means of calibration algorithm; preprocessing acquired image sequence by histogram equalization so as to highlight angular point characteristics of the bituminous pavement, and reducing adverse affection caused by pavement conditions and light variation; detecting the angular point characteristics of the pavement in real time by adopting efficient Harris angular point detection algorithm; performing angular point matching tracking of a front frame and a rear frame according to the Lucas-Kanade light stream algorithm, further optimizing matched angular points by RANSAC (random sample consensus) algorithm and acquiring more accurate light stream information; and finally, restructuring real-time motion parameters of the vehicle such as longitudinal velocity, transverse velocity and side slip angle under a vehicle carrier coordinate system, and accordingly, realizing high-precision vehicle ground motion state estimation.

Description

A kind of state of motion of vehicle method of estimation of optical flow-based

Technical field

The present invention relates to a kind of method of estimation state of motion of vehicle of optical flow-based, belong to autonomous driving and auxiliary driving field.

Background technology

State estimation in the Vehicle Driving Cycle process, to realize one of autonomous driving and auxiliary gordian technique of driving, be intended to determine the important state variables such as longitudinal velocity, transverse velocity and side slip angle of vehicle under the transport condition, to vehicle anti-lock brake system (ABS), vehicle electric stability program (ESP), the active safety control systems such as four-wheel steering stabilizing control system (4WS) have great actual application value.These active safety control system can work depend on real-time, the Obtaining Accurate of vehicle-state parameter to a great extent.

At present, in the automobile active safety field, the method for estimation of the motion state of vehicle mainly can be divided three classes.One class is indirect measurement, namely utilizes the output information of existing onboard sensor such as inertial navigation system INS, wheel speed sensors etc., calculates to obtain the correlation behavior of vehicle by simple mathematics.But As time goes on the deviation of sensor can progressively accumulate, and causes the estimated accuracy of vehicle-state to reduce.Equations of The Second Kind is direct measurement, namely utilize high-precision equipment such as high precision global position system GPS etc. directly to obtain the motion state parameters of vehicle, this method measuring accuracy is high, but expensive, and GPS in tunnel that signal is blocked, the effective output information of stereo region, bearing accuracy also can reduce.The 3rd class is based on the method for auto model, namely carry out kinematics or Dynamic Modeling by the motion process to vehicle, simultaneously with corresponding onboard sensor (such as wheel speed sensors, gyroscope, accelerometer etc.) information as observation information, and then utilize suitable filtering algorithm for estimating to realize estimation to motoring condition.This method need to be to vehicle whole even each tire carry out respectively modeling, unpunctual when these models or model parameter, evaluated error is larger.

Summary of the invention

In order to overcome the limitation of said method, for with 0 ~ 15(km/hour) low speed driving is at the vehicle of smooth bituminous pavement, the present invention proposes the state of motion of vehicle measuring method of the optical flow-based that a kind of cost is low, real-time good, adaptive capacity to environment is strong, precision is higher.

To achieve these goals, adopt following technical scheme:

Step 1: in the vehicle rear axle center high precision monocular-camera is installed, its optical axis is downward perpendicular to vehicle operating plane and directed towards ground, and 20 ~ 40 centimetres of vertical heights are used for obtaining the pavement image sequence of variation.Definition vehicle carrier coordinate system, the initial point o of vehicle carrier coordinate system is positioned at vehicle barycenter place, and the x axle is along the longitudinal axis of vehicle and consistent with vehicle forward direction, and the z axle is downward perpendicular to vehicle operating plane and directed towards ground, and it is definite that the y axle press the right-handed helix rule.Definition pavement image coordinate system, the initial point o of pavement image coordinate system _pBe the summit in the image lower left corner, longitudinal axis u is parallel to vehicle carrier x axle, and transverse axis v is parallel to vehicle carrier y axle.

Step 2: adopt classical camera calibration method to carry out camera calibration, to obtain the inner parameter of video camera: optical axis is at the vertical projection point of the plane of delineation coordinate (o at image coordinate system _u, o _v), u axle normal vector α on the image, v axle normal vector β on the image, verticality γ and the external parameter of u, v coordinate axis: video camera is with respect to 3 * 3 rotation matrix r of world coordinate system _Cam, video camera is with respect to 3 * 1 translation vector t of world coordinate system _Cam

Step 3: initialisation image sequence pointer i makes i=1.

Step 4: get i frame pavement image and i+1 frame pavement image, respectively i frame pavement image and i+1 frame pavement image are carried out the image pre-service, described image pre-service comprises: grey level histogram function and the equalization processing of computed image.

The grey level histogram function of described image adopts following methods to calculate:

At first with image gray processing, and with the gray-scale value normalization of all pixels of image, calculate the grey level histogram function of this image:

$\mathrm{pro}\left(r_k\right)=\frac{n_k}{n},k=\mathrm{0,1,2}...\mathrm{255,0}\≤r_k\≤1---\left(1\right)$

Wherein, n is total number of image pixels, r _kBe k gray level, n _kGray level is r in the image for this reason _kNumber of pixels, pro (r _k) expression gray level r _kThe probability that occurs.In rectangular coordinate system, r _kWith pro (r _k) graph of a relation be grey level histogram.

Described equalization processing adopts the gray accumulation distribution function of image:

$s_k=\underset{l=0}{\overset{k}{\mathrm{\Σ}}}\mathrm{pro}\left(r_l\right),0\≤r_l\≤1,l=\mathrm{0,1,2}...k,k=\mathrm{0,1,2},...,255---\left(2\right)$

Wherein, s _kBe k gray level cumulative distribution function, r _lBe l gray level.Gray level after the conversion

r′ _k＝INT[(L-1)s _k+0.5]，k＝0,1,2,…,255 (3)

Wherein, L=256, INT are bracket function, r ' _kR _kGray level after conversion.

Step 5: the pretreated i two field picture of image is carried out Harris Harris Corner Detection: Harris Haris Corner Detection comprises: calculate grey scale change amount and the angle point response function of every bit in the i two field picture, and carry out angle point and judge.

Following methods is adopted in the calculating of described grey scale change amount:

E(u ₀,v ₀)＝∑[I(u ₀+Δu,v ₀+Δv)-I(u ₀,v ₀)] ²，(u ₀+Δu,v ₀+Δv)∈w(u ₀,v ₀,r _ad) (4)

Wherein, E (u ₀, v ₀) be any point (u in the i two field picture ₀, v ₀) the grey scale change amount, Δ u, Δ v are respectively point (u ₀, v ₀) along u axle, the axial displacement of v, I (u ₀, v ₀) be point (u ₀, v ₀) gray-scale value, I (u ₀+ Δ u, v ₀+ Δ v) is point (u ₀+ Δ u, v ₀+ Δ v) gray-scale value, window function w (u ₀, v ₀, r _Ad)={ (u, v) | (u-u ₀) ²+ (v-v ₀) ²≤ r _Ad ²With any point (u ₀, v ₀) centered by, r _AdA circular window for radius.To I (u ₀+ Δ u, v ₀+ Δ v) carry out Taylor Taylor series expansion, ignore high-order term and get:

I(u ₀+Δu,v ₀+Δv)≈I(u ₀,v ₀)+Δu·I _u(u ₀,v ₀)+Δv·I _v(u ₀,v ₀)(5)

Wherein, $I_u(u_0,v_0)=\frac{\∂I(u_0,v_0)}{\∂u},$ $I_v(u_0,v_0)=\frac{\∂I(u_0,v_0)}{\∂v}$ Respectively I (u ₀, v ₀) in u axle, the axial gradient of v.Can be got by above two formulas:

$E(u_0,v_0)=[\mathrm{\Δu},\mathrm{\Δv}]M\left[\begin{array}{c}\mathrm{\Δu}\\ \mathrm{\Δv}\end{array}\right],M=\mathrm{\Σ}\left[\begin{array}{cc}{I^2}_u(u_0,v_0)& I_u(u_0,v_0)I_v(u_0,v_0)\\ I_u(u_0,v_0)I_v(u_0,v_0)& {I^2}_v(u_0,v_0)\end{array}\right]---\left(6\right)$

Wherein, matrix M is any point (u ₀, v ₀) autocorrelation matrix of video in window on every side, symmetrical matrix M is carried out the similarity diagonalization offspring enters relational expression (6):

$E(u_0,v_0)=R^{-1}\left(\begin{array}{cc}{\mathrm{\λ}}_1& 0\\ 0& {\mathrm{\λ}}_2\end{array}\right)---\left(7\right)$

Wherein, R is twiddle factor, R ^-1Be the inverse matrix of R, λ ₁, λ ₂Eigenwert for the M battle array.

Described angle point response function CRF adopts following method to obtain:

CRF(u ₀,v ₀)＝det(M)-η·[trace(M)] ² (8)

Wherein, CRF (u ₀, v ₀) be any point (u ₀, v ₀) the angle point response function, det (M)=λ ₁λ ₂, the determinant of expression Metzler matrix, trace (M)=λ ₁+ λ ₂, the mark of representing matrix M, scale factor η are empirical value, η=0.04.

Described angle point is judged the employing following methods:

When angle point response function CRF surpasses threshold value, think that this point is exactly angle point, and obtain the pixel coordinate of angle point that threshold value gets 18.

Step 6: utilizing Lucas-Ke is that moral Lucas-Kanade optical flow method coupling is followed the tracks of Harris Harris algorithm at the coordinate of detected all angle points of i two field picture in the i+1 two field picture: the arbitrary angle point G in the i two field picture is (u at the coordinate of i two field picture _i, v _i), Lucas-Ke of described angle point G is that the tracking of moral Lucas-Kanade optical flow method coupling may further comprise the steps:

At first, set up equation of constraint between i frame and the i+1 two field picture:

I(u _i+1,v _i+1,t _i+1)＝I(u _i+Δu _i,v _i+Δv _i,t _i+Δt)＝I(u _i,v _i,t _i) (9)

Wherein, t _i, t _I+1Dividing is the moment of taking i frame and i+1 two field picture, time interval Δ t=t _I+1-t _i, Δ t is fixed value, the representative value of Δ t has 1/30(second), 1/25(second), 1/15(second), Δ u _i, Δ v _iRespectively the displacement of angle point G u axle, v axle within the Δ t time, (u _i, v _i) be that angle point G is at the coordinate of i two field picture, (u _I+1, v _I+1) be angle point G at the coordinate of i+1 two field picture, I (u _i, v _i, t _i) be t _iThe gray-scale value of moment angle point G, I (u _I+1, v _I+1, t _I+1) be t _I+1The gray-scale value of moment angle point G.

Then, I (u _i+ Δ u _i, v _i+ Δ v _i, t _i+ Δ t) carry out Taylor Taylor series expansion, ignore high-order term and get:

I(u _i+Δu _i,v _i+Δv _i,t _i+Δt)≈I(u _i,v _i,t _i)+I _u·Δu _i+I _v·Δv _i+I _t·Δt (10)

Wherein,

Gradation of image u direction of principal axis partial derivative,

Gradation of image v direction of principal axis partial derivative,

That gradation of image is to the partial derivative of time t.

Can be got by relational expression (9), (10):

$I_u\·\frac{\mathrm{\Δ}{u}_i}{\mathrm{\Δt}}+I_v\·\frac{\mathrm{\Δ}{v}_i}{\mathrm{\Δt}}+I_t=0---\left(11\right)$

Then, set up t _iNear the system equation of 5 * 5 neighborhood territory pixels the moment angle point G, set up following system of equations:

Hc＝g (12)

Wherein, $H=\left[\begin{array}{cc}{I}_u(u_{i,1},v_{i,1},t_i),& I_v(u_{i,1},v_{i,1},t_i)\\ I_u(u_{i,2},v_{i,2},t_i),& I_v(u_{i,2},v_{i,2},t_i)\\ .& .\\ .& .\\ .& .\\ I_u(u_{i,25},v_{i,25},t_i),& I_v(u_{i,25},v_{i,25},t_i)\end{array}\right],$ $c=\left[\begin{array}{c}\frac{\mathrm{\Δ}{u}_i}{\mathrm{\Δt}}\\ \frac{\mathrm{\Δ}{v}_i}{\mathrm{\Δt}}\end{array}\right],$ $g=\left[\begin{array}{c}-I_t(u_{i,1},v_{i,1},t_i)\\ -I_t(u_{i,2},v_{i,2},t_i)\\ .\\ .\\ .\\ -I_t(u_{i,25},v_{i,25},t_i)\end{array}\right],$

25 * 2 the matrix that H is comprised of the shade of gray of 5 * 5 neighborhood territory pixels, c is 2 * 1 the column vector that light stream forms by the displacement between adjacent two two field pictures, 25 * 1 the column vector that g is comprised of gray scale partial derivative in time.

At last, adopt least square method to ask the least square solution of system equation, the i.e. motion of central point G.

Transposition on equation (12) both sides with premultiplication H battle array:

H ^THc＝H ^Tg (13)

Wherein, H ^TIt is the transposition of H battle array.The light stream of the i.e. front and back frame formation of the solution of equation is as follows:

$\left[\begin{array}{c}\frac{\mathrm{\Δ}{u}_i}{\mathrm{\Δt}}\\ \frac{\mathrm{\Δ}{v}_i}{\mathrm{\Δt}}\end{array}\right]={\left(H^{T}H\right)}^{-1}{H}^{T}g---\left(14\right)$

Wherein, (H ^TH) ^-1It is matrix H ^TThe inverse matrix of H.Angle point G is at the coordinate (u of i+1 two field picture _I+1, v _I+1) adopt following formula to obtain:

$\left[\begin{array}{c}{u}_{i+1}\\ v_{i+1}\end{array}\right]=\left[\begin{array}{c}\frac{\mathrm{\Δ}{u}_i}{\mathrm{\Δt}}\\ \frac{\mathrm{\Δ}{v}_i}{\mathrm{\Δt}}\end{array}\right]\mathrm{\Δt}+\left[\begin{array}{c}{u}_i\\ v_i\end{array}\right]---\left(15\right)$

Step 7: all coupling angle points that utilize the consistent RANSAC algorithm optimization of stochastic sampling step 6 to obtain:

At first, set up the model Q of the projective transformation matrix between i frame and i+1 two field picture:

ξU _i+1＝QU _i (16)

Wherein, $U_{i+1}=\left[\begin{array}{c}{u}_{i+1}\\ v_{i+1}\\ 1\end{array}\right],$ $U_i=\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right],$ $Q=\left[\begin{array}{ccc}{q}_0& q_1& q_2\\ q_3& q_4& q_5\\ q_6& q_7& q_8\end{array}\right],$ U _i, U _I+1Be respectively the homogeneous vectors that arbitrary coupling angle point G in all coupling angle points that step 6 obtains consists of at the coordinate of i frame and i+1 two field picture, Q is the projective transformation matrix between i frame and i+1 two field picture, and ξ is scale factor, gets ξ=1; Q wherein ₀, q ₁, q ₃, q ₄The matrix that consists of $\left[\begin{array}{cc}{q}_0& q_1\\ q_3& q_4\end{array}\right]$ Expression is carried out convergent-divergent, rotation, symmetry, wrong contact transformation, q to image ₆, q ₇Vector [the q that consists of ₆q ₇] represent image is carried out translation transformation, q ₂, q ₅The vector that consists of $\left[\begin{array}{c}{q}_2\\ q_5\end{array}\right]$ Expression is carried out projective transformation, q to image ₈Expression is carried out stretching to image.Select 4 pairs of coupling angle points to draw 8 linear equations, by self constraint condition of projective transformation battle array Q, form 9 equations and namely estimate Q, the step of estimation again:

(I) initialization iterations variable m=1, and establish the iterations maximal value

(II) chooses 4 pairs of coupling angle points at random in all coupling angle points that step 6 obtains, as a sample set S _m, the sample complementary set after the extraction is

With sample set S _mIn some substitution relational expression (16) try to achieve initial Q _m

(III) utilizes complementary set

In the U that consists of at the coordinate of i frame of arbitrary coupling angle point _iWith the Q that is obtained by (II) _mObtain error function:

$F={d^2}_{\mathrm{Euc}}(U_{i+1},Q_m{U}_i)+{d^2}_{\mathrm{Euc}}(U_i,Q_m^{-1}{U}_{i+1})---\left(17\right)$

Wherein,

Q _mInverse matrix, d _Euc(U _I+1, Q _mU _i) expression point U _I+1With a Q _mU _iIn three-dimensional Euclidean distance,

Expression point U _iWith the point

In three-dimensional Euclidean distance; Error threshold gets 3, and error function F is interior point less than the match point of this threshold value, and point set in point adds in this, otherwise be exterior point;

(IV) interior point set and sample set S _mForm consistent collection If

In in count more than or equal to 60 percent of coupling angle point sum, then think to have obtained correct projective transformation matrix, and will

As sample set substitution relational expression (16), adopt least square method to recomputate New model Q ' _mAs the correction model of projective transformation matrix, if

In in count less than 60 percent of coupling angle point sum, then algorithm failure makes Q _m=0;

(V) if

Make m=m+1, continue to carry out successively (II) to (V); If

The iterative process of then sampling finishes; After sampling, obtain the new model set

In choose the consistent collection of sample number maximum

As the matching angle point set after optimizing, the corresponding model Q ' that calculates _MaxAs final transformation matrix Q, algorithm finishes.

Step 8: the matching angle point set after the optimization that employing frame difference method treatment step 7 obtains, calculate the motion state of vehicle: the coupling angle point behind the consistent RANSAC algorithm optimization of extraction stochastic sampling is done the poor pixel displacement that obtains at the coordinate of i frame, i+1 two field picture with the matching angle point coordinate in i frame, the i+1 two field picture.In conjunction with the camera interior and exterior parameter that is obtained by step 2, calculate the quantity of state of vehicle under the vehicle carrier coordinate system.

Described frame difference method is calculated state of motion of vehicle, and the concrete grammar of employing is as follows:

1. the matching angle point set sample size of establishing behind the consistent RANSAC algorithm optimization of stochastic sampling is

Initialization sample number variable j makes j=1;

2. it is poor that after will optimizing j coupling angle point done at the coordinate of i+1 frame and i two field picture, gets j coupling angle point after the optimization at shooting i frame and the pixel displacement Δ p (u in the i+1 two field picture time interval _{I, j}, v _{I, j}), wherein, u _{I, j}, v _{I, j}Respectively u axle, the v axial coordinate of the coupling angle point of j behind the consistent RANSAC algorithm optimization of stochastic sampling in the i two field picture in the i two field picture;

3. the camera parameter demarcated of integrating step 2 is with pixel displacement Δ p (u _{I, j}, v _{I, j}) carrying out the coordinate inverse, j match point after being optimized taken i frame and the physical displacement in the i+1 two field picture time interval;

4. video camera is taken the i two field picture within the time interval of taking the i+1 two field picture, each parameter of the one group of travel condition of vehicle that is calculated by j match point after optimizing:

$v_{i,j}=\frac{\mathrm{\ΔP}(x_{i,j},y_{i,j})}{\mathrm{\Δt}}=\frac{P(x_{i+1,j},y_{i+1,j})-P(x_{i,j},y_{i,j})}{t_{i+1}-t_i}$

$v_{{\stackrel{\→}{x}}_{i,j}}=\frac{\mathrm{\Δ}{x}_{i,j}}{\mathrm{\Δt}}=\frac{x_{i+1,j}-x_{i,j}}{t_{i+1}-t_i}---\left(18\right)$

$v_{{\stackrel{\→}{y}}_{i,j}}=\frac{\mathrm{\Δ}{y}_{i,j}}{\mathrm{\Δt}}=\frac{y_{i+1,j}-y_{i,j}}{t_{i+1}-t_i}$

Wherein, arctan () represents arctan function, t _i, t _I+1Dividing is the moment of taking i frame, i+1 two field picture, P (x _{I, j}, y _{I, j}), P (x _{I+1, j}, y _{I+1, j}) be respectively by j the t that the coupling angle point is obtained after optimizing _iThe moment and t _I+1The physical coordinates of moment vehicle, Δ P (x _{I, j}, y _{I, j}) be by j after optimizing mate that angle point obtains from t _iThe time be carved into t _I+1The physical displacement of moment vehicle, x _{I, j}, x _{I+1, j}Respectively by j the t that the coupling angle point is obtained after optimizing _iThe moment and t _I+1The ordinate of orthogonal axes value of moment vehicle, y _{I, j}, y _{I+1, j}Respectively by j the t that the coupling angle point is obtained after optimizing _iThe moment and t _I+1The transverse axis coordinate figure of moment vehicle, Δ x _{I, j}, Δ y _{I, j}Be respectively by j after optimizing mate that angle point obtains from t _iThe time be carved into t _I+1The constantly longitudinal axis, the transverse axis displacement of vehicle, v _{I, j}Be by j after optimizing mate that angle point obtains from t _iThe time be carved into t _I+1The instantaneous velocity of moment vehicle,

Be respectively by j after optimizing mate that angle point obtains from t _iThe time be carved into t _I+1The longitudinal axis, the X direction speed of moment vehicle;

If 5.

Then make j=j+1, continue to carry out successively above-mentioned 2. arriving 5.; If

Then iterative process finishes.

Above-mentioned frame difference method is obtained

The group vehicle movement parameter do on average, and with average result as vehicle from t _iThe time be carved into t _I+1Kinematic parameter constantly:

$v_i=\frac{\underset{j=1}{\overset{\hat{j}}{\mathrm{\Σ}}}{v}_{i,j}}{\hat{j}}$ $v_{{\stackrel{\→}{x}}_i}=\frac{\underset{j=1}{\overset{\hat{j}}{\mathrm{\Σ}}}{v}_{{\stackrel{\→}{x}}_{i,j}}}{\hat{j}}$ (19)

$v_{{\stackrel{\→}{y}}_i}=\frac{\underset{j=1}{\overset{\hat{j}}{\mathrm{\Σ}}}{v}_{{\stackrel{\→}{y}}_{i,j}}}{\hat{j}}$ ${\mathrm{\β}}_i=\arctan \left(\frac{v_{{\stackrel{\→}{y}}_i}}{v_{{\stackrel{\→}{x}}_i}}\right)$

Wherein, v _iFrom t _iThe time be carved into t _I+1The instantaneous velocity of moment vehicle,

Respectively from t _iThe time be carved into t _I+1The longitudinal axis, the X direction speed of moment vehicle, β _iFrom t _iThe time be carved into t _I+1The side slip angle of moment vehicle.

Step 9: the pavement image that will obtain in chronological order successively repeating step 3 to step 9 recursion calculates, even image sequence pointer i=i+1, repeating step 3 to step 9 is calculated, and obtains vehicle at each longitudinal velocity, transverse velocity and side slip angle constantly.

Beneficial effect

1. the state of motion of vehicle method of estimation of the optical flow-based that proposes of the present invention has adopted efficient angular-point detection method, matching tracking method and the consistent optimized algorithm of stochastic sampling etc., has that real-time is good, measuring accuracy is than advantages of higher.

2. the image pre-processing method that adopts of the state of motion of vehicle method of estimation of the optical flow-based that proposes of the present invention has reduced the adverse effect that road conditions and illumination variation are brought, and has strengthened the ambient adaptability of algorithm.

3. the state of motion of vehicle method of estimation of the optical flow-based of the present invention's proposition can more accurately record the important state variables such as vertical speed of a motor vehicle, the horizontal speed of a motor vehicle and side slip angle, and these state parameters can be used as the input signal of the systems such as vehicle active safety, integrated positioning and detection of obstacles.

Description of drawings

Fig. 1 is process flow diagram of the present invention;

Fig. 2 is the present frame original image that video camera obtains in the test;

Fig. 3 is the pretreated current frame image of image in the test;

Fig. 4 is the angle point that Harris Harris angular-point detection method detects at current frame image;

Fig. 5 is Lucas-Ke corners Matching figure that to be moral Lucas-Kanade corners Matching method trace at current frame image and next frame image;

Fig. 6 is current frame image behind the consistent RANSAC algorithm optimization of stochastic sampling and the corners Matching figure of next frame image.

Embodiment

State estimation in the Vehicle Driving Cycle process, to realize one of autonomous driving and auxiliary gordian technique of driving, be intended to determine the important state variables such as longitudinal velocity, transverse velocity and side slip angle of vehicle under the transport condition, to vehicle anti-lock brake system (ABS), vehicle electric stability program (ESP), the active safety control systems such as four-wheel steering stabilizing control system (4WS) have great actual application value.These active safety control system can work depend on real-time, the Obtaining Accurate of vehicle-state parameter to a great extent.

At present, in the automobile active safety field, the method for estimation of the motion state of vehicle mainly can be divided three classes.One class is indirect measurement, namely utilizes the output information of existing onboard sensor such as inertial navigation system INS, wheel speed sensors etc., calculates to obtain the correlation behavior of vehicle by simple mathematics.But As time goes on the deviation of sensor can progressively accumulate, and causes the estimated accuracy of vehicle-state to reduce.Equations of The Second Kind is direct measurement, namely utilize high-precision equipment such as high precision global position system GPS etc. directly to obtain the motion state parameters of vehicle, this method measuring accuracy is high, but expensive, and GPS in tunnel that signal is blocked, the effective output information of stereo region, bearing accuracy also can reduce.The 3rd class is based on the method for auto model, namely carry out kinematics or Dynamic Modeling by the motion process to vehicle, simultaneously with corresponding onboard sensor (such as wheel speed sensors, gyroscope, accelerometer etc.) information as observation information, and then utilize suitable filtering algorithm for estimating to realize estimation to motoring condition.This method need to be to vehicle whole even each tire carry out respectively modeling, unpunctual when these models or model parameter, evaluated error is larger.

The present invention is directed under the road traffic environment with 0 ~ 15(km/hour) travel at the vehicle of smooth bituminous pavement, propose a kind of state of motion of vehicle measuring method of optical flow-based, the method has that cost is low, real-time good, adaptive capacity to environment is strong, precision is than advantages of higher.Do following reasonable assumption:

The bituminous pavement level of approximation of (1) travelling;

(2) think that vehicle is straight-line within the time interval of taking the consecutive frame image;

(3) think that light intensity is constant within the time interval of taking the consecutive frame image.

In conjunction with process flow diagram shown in Figure 1, thinking of the present invention is described further:

Step 1: in the vehicle rear axle center high precision monocular-camera is installed, its optical axis is downward perpendicular to vehicle operating plane and directed towards ground, and 20 ~ 40 centimetres of vertical heights are used for obtaining the pavement image sequence of variation.Definition vehicle carrier coordinate system, the initial point o of vehicle carrier coordinate system is positioned at vehicle barycenter place, and the x axle is along the longitudinal axis of vehicle and consistent with vehicle forward direction, and the z axle is downward perpendicular to vehicle operating plane and directed towards ground, and it is definite that the y axle press the right-handed helix rule.Definition pavement image coordinate system, the initial point o of pavement image coordinate system _pBe the summit in the image lower left corner, longitudinal axis u is parallel to vehicle carrier x axle, and transverse axis v is parallel to vehicle carrier y axle.

Step 2: video camera is demarcated, to obtain the inside and outside parameter of video camera.For the real-time that improves whole algorithm flow and guarantee stated accuracy, that the present invention quotes is classical camera calibration method [but list of references: Z Zhang.Aflexible new technique for camera calibration[J] the .IEEE Transactions on Pattern Analysis andMachine Intelligence of Zhang Zhengyou, 2000,22 (11): 1330-1334].The classical calibration algorithm of Zhang Zhengyou by to scaling board different directions (more than three times) complete taking pictures repeatedly, need not know the mode of motion of scaling board, just can realize the demarcation to video camera.

According to actual needs, the used scaling board of the present invention is to utilize plotting apparatus to draw 4 * 4 at an A4 blank sheet of paper evenly to distribute and equirotal black squares, and it is sticked to makes on the duroplasts thin plate.The black squares length of side is 30mm, and spacing 20mm, view picture scaling board have 64 square vertices (being angle point).For simplicity, at timing signal, world coordinate system OXYZ directly is based upon on the scaling board, initial point O is positioned on the square vertices in the lower left corner of scaling board, and X, Y-axis are parallel to respectively foursquare two limits, determine Z axis by right hand rule.The Z axis coordinate of such 64 angle points is 0(for easy, and the Z axis coordinate of point is ignored), X, Y coordinate also can be determined easily.Any point of the resulting X-Y scheme picture point of video camera coordinate in image coordinate system is p (u, v), and the relation table of putting on the scaling board between P (x, y) and its subpoint p (u, v) is shown:

$a\left[\begin{array}{c}\mathrm{<figure-callout\; id="1"\; label="u\; v"\; filenames="HDA00002368243800011.png"\; state="\{\{state\}\}">u</figure-callout>}& \\ v\\ 1\end{array}\right]=\mathrm{aA}[{\mathrm{<figure-callout\; id="1"\; label="r\; cam"\; filenames="HDA00002368243800011.png"\; state="\{\{state\}\}">r</figure-callout>}}_{{\mathrm{cam}}_{}},r_{{\mathrm{cam}}_2},r_{{\mathrm{cam}}_3},t_{\mathrm{cam}}]\left[\begin{array}{c}x\\ y\\ 0\\ 1\end{array}\right]=\mathrm{aA}[{\mathrm{<figure-callout\; id="1"\; label="r\; cam"\; filenames="HDA00002368243800011.png"\; state="\{\{state\}\}">r</figure-callout>}}_{{\mathrm{cam}}_{}},r_{{\mathrm{cam}}_2},t_{\mathrm{cam}}]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]---\left(1\right)$

Wherein, a is any dimension scale, and the present invention is without convergent-divergent, thus a=1, [u, v, 1] ^TBeing the augmented matrix that p (u, v) consists of, is the spot projection homogeneous vectors that the coordinate of corresponding point consists of to the plane of delineation on the scaling board plane, [x, y, 0,1] ^TThe augmented matrix of P (x, y), [x, y, 1] ^TThe homogeneous vectors that scaling board Plane-point P (x, y) consists of, That video camera is with respect to 3 * 3 rotation matrixs of world coordinate system, t _CamBe video camera with respect to 3 * 1 translation vectors of world coordinate system, $A=\left[\begin{array}{ccc}\mathrm{\&alpha;}& \mathrm{\&gamma;}& o_u\\ 0& \mathrm{\&beta;}& o_v\\ 0& 0& 1\end{array}\right]$ Matrix is the intrinsic parameters of the camera matrix, because the optical axis of video camera is perpendicular to ground, so (o _u, o _v) be optical axis at the vertical projection point of the plane of delineation coordinate at image coordinate system, α, β are the normal vectors of image u axle, v axle, γ is the verticality of two coordinate axis.

According to the character of rotation matrix, know that the mould value of the rotation matrix different lines orthogonal quadrature of vector and column vector all is 1, namely

Among the present invention, subscript ^-1All inverse operations of representing matrix, the transposition of the equal representing matrix of subscript T; Every width of cloth image can obtain following two basic constraint conditions to Intrinsic Matrix:

b ₁ ^T(A ^-1) ^TA ^-1b ₂＝0 (2)

b ₁ ^T(A ^-1) ^TA ^-1b ₁＝b ₂ ^T(A ^-1) ^TA ^-1b ₂ (3)

Order $B=\left[\begin{array}{ccc}{b}_{11}& b_{12}& b_{13}\\ b_{21}& b_{22}& b_{23}\\ b_{31}& b_{32}& b_{33}\end{array}\right]=[{\mathrm{<figure-callout\; id="1"\; label="b"\; filenames="HDA00002368243800011.png"\; state="\{\{state\}\}">b</figure-callout>}}_1,b_2,b_3]=\mathrm{aA}[{\mathrm{<figure-callout\; id="1"\; label="r\; cam"\; filenames="HDA00002368243800011.png"\; state="\{\{state\}\}">r</figure-callout>}}_{{\mathrm{cam}}_{}},r_{{\mathrm{cam}}_2},t_{\mathrm{cam}}],$ B is 3 * 3 projection matrix, makes 3 * 3 matrixes:

$D={\left(A^{-1}\right)}^T{A}^{-1}=\left[\begin{array}{ccc}{d}_{11}& d_{12}& d_{13}\\ d_{12}& d_{22}& d_{23}\\ d_{13}& d_{23}& d_{33}\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{{\mathrm{\&alpha;}}^2}& -\frac{\mathrm{\&gamma;}}{{\mathrm{\&alpha;}}^2\mathrm{\&beta;}}& \frac{v_0\mathrm{\&gamma;}-u_0\mathrm{\&beta;}}{{\mathrm{\&alpha;}}^2\mathrm{\&beta;}}\\ -\frac{\mathrm{\&gamma;}}{{\mathrm{\&alpha;}}^2\mathrm{\&beta;}}& \frac{{\mathrm{\&gamma;}}^2}{{\mathrm{\&alpha;}}^2{\mathrm{\&beta;}}^2}+\frac{1}{{\mathrm{\&beta;}}^2}& -\frac{\mathrm{\&gamma;}(v_0\mathrm{\&gamma;}-u_0\mathrm{\&beta;})}{{\mathrm{\&alpha;}}^2{\mathrm{\&beta;}}^2}-\frac{v_0}{{\mathrm{\&beta;}}^2}\\ \frac{v_0\mathrm{\&gamma;}-u_0\mathrm{\&beta;}}{{\mathrm{\&alpha;}}^2\mathrm{\&beta;}}& -\frac{\mathrm{\&gamma;}(v_0\mathrm{\&gamma;}-u_0\mathrm{\&beta;})}{{\mathrm{\&alpha;}}^2{\mathrm{\&beta;}}^2}-\frac{v_0}{{\mathrm{\&beta;}}^2}& \frac{{(v_0\mathrm{\&gamma;}-u_0\mathrm{\&beta;})}^2}{{\mathrm{\&alpha;}}^2{\mathrm{\&beta;}}^2}+\frac{v_0^2}{{\mathrm{\&beta;}}^2}+1\end{array}\right]$

Because of D ^T=D is symmetrical matrix, then can be transformed into 6 dimensional vectors that an element rearranges

Form is as follows:

$\tilde{D}={[d_{11},d_{12},d_{22},d_{13},d_{23},d_{33}]}^T---\left(4\right)$

Then the product representation of rotation matrix the 1st row, the 2nd column vector is as follows:

$b_1^T{\left(A^{-1}\right)}^T{A}^{-1}{\mathrm{<figure-callout\; id="1"\; label="b"\; filenames="HDA00002368243800011.png"\; state="\{\{state\}\}">b</figure-callout>}}_1=b_1^{\mathrm{<figure-callout\; id="1"\; label="T\; D\; b"\; filenames="HDA00002368243800011.png"\; state="\{\{state\}\}">T</figure-callout>}}D{b}_{}{}_1=v_{11}^T\tilde{D}$

$b_1^T{\left(A^{-1}\right)}^T{A}^{-1}{b}_2=b_1^{t}D{b}_2=v_{12}^T\tilde{D}---\left(5\right)$

$b_2^T{\left(A^{-1}\right)}^T{A}^{-1}{b}_2=b_2^{T}D{b}_2=v_{22}^T\tilde{D}$

v ₁₁=[b ₁₁b ₁₁, b ₁₁b ₁₂+ b ₁₂b ₁₁, b ₁₂b ₁₂, b ₁₃b ₁₁+ b ₁₁b ₁₃, b ₁₃b ₁₂+ b ₁₂b ₁₃, b ₁₃b ₁₃] ^TWherein, v ₁₂=[b ₁₁b ₂₁, b ₁₁b ₂₂+ b ₁₂b ₂₁, b ₁₂b ₂₂, b ₁₃b ₂₁+ b ₁₁b ₂₃, b ₁₃b ₂₂+ b ₁₂b ₂₃, b ₁₃b ₂₃] ^T, last intrinsic parameter

v ₂₂=[b ₂₁b ₂₁, b ₂₁b ₂₂+ b ₂₂b ₂₁, b ₂₂b ₂₂, b ₂₃b ₂₁+ b ₂₁b ₂₃, b ₂₃b ₂₂+ b ₂₂b ₂₃, b ₂₃b ₂₃] ^TEquation of constraint (2), (3) are equivalent to:

$\left[\begin{array}{c}{v}_{12}^T\\ {(v_{11}-v_{22})}^T\end{array}\right]\tilde{D}=0---\left(6\right)$

Wherein, $\left[\begin{array}{c}{v}_{12}^T\\ {(v_{11}-v_{22})}^T\end{array}\right]$ Matrix is 2 * 6 matrixes, and namely every pictures can be set up 2 system of equations, and 6 unknown numbers then need 3 pictures to find the solution the D matrix at least, can obtain camera intrinsic parameter battle array A.After Intrinsic Matrix A determines, the corresponding external parameter of every width of cloth scaling board image:

${\mathrm{<figure-callout\; id="1"\; label="r\; cam"\; filenames="HDA00002368243800011.png"\; state="\{\{state\}\}">r</figure-callout>}}_{{\mathrm{cam}}_{}}=A^{-1}{\mathrm{<figure-callout\; id="1"\; label="b"\; filenames="HDA00002368243800011.png"\; state="\{\{state\}\}">b</figure-callout>}}_1/\left|\right|A^{-1}{\mathrm{<figure-callout\; id="1"\; label="b"\; filenames="HDA00002368243800011.png"\; state="\{\{state\}\}">b</figure-callout>}}_1\left|\right|$ $r_{{\mathrm{cam}}_2}=A^{-1}{b}_2/\left|\right|A^{-1}{\mathrm{<figure-callout\; id="1"\; label="b"\; filenames="HDA00002368243800011.png"\; state="\{\{state\}\}">b</figure-callout>}}_1\left|\right|$ (7)

$r_{{\mathrm{cam}}_3}={\mathrm{<figure-callout\; id="1"\; label="r\; cam"\; filenames="HDA00002368243800011.png"\; state="\{\{state\}\}">r</figure-callout>}}_{{\mathrm{cam}}_{}}\&times;r_{{\mathrm{cam}}_2}$ t _cam＝A ^-1b ₃/||A ^-1b ₁||

Wherein, || A ^-1b ₁|| refer to matrix A ^-1b ₁The mould value, In * the vectorial multiplication cross computing of expression;

Because the intrinsic parameter of video camera is determined by inner structure, irrelevant with external factor, therefore first under good indoor environment, take several (〉=3) preferably scaling board image, with inside and outside parameter obtained above as initial value and adopt the maximal possibility estimation criterion that these parameters are optimized processing.Result after the optimization is as final inside and outside parameter.

Step 3: in order to process respectively in chronological order image sequence, define image sequence pointer i, and make i=1.

Step 4: get i frame pavement image and i+1 frame pavement image, respectively i frame pavement image and i+1 frame pavement image are carried out the image pre-service.Fig. 2 knows that by Fig. 2 the original image angle point is not obvious, characteristic information is single for the present frame on the Vehicle Driving Cycle road surface that video camera in the test obtains is i frame original image.In order to highlight the bituminous pavement Corner Feature, reduce the adverse effect that road conditions and illumination variation are brought, the sequence image that obtains is carried out image enhancement processing.Histogram equalization is the important algorithm of image enhancement technique, have a wide range of applications in fields such as compression of images, image segmentation and image recognitions, can change targetedly histogrammic intensity profile situation, be distributed in whole tonal range with making uniform gray level, and the histogram equalization techniques processing speed is fast, satisfies the requirement of processing in real time.

Described histogram equalization comprises: grey level histogram function and the equalization processing of computed image.

The grey level histogram function of described image adopts following methods to calculate:

At first with image gray processing, and with the gray-scale value normalization of all pixels of image, calculate the grey level histogram function of this image:

$\mathrm{pro}\left(r_k\right)=\frac{n_k}{n},k=\mathrm{0,1,2}...\mathrm{255,0}\&le;r_k\&le;1---\left(8\right)$

Wherein, n is total number of image pixels, r _kBe k gray level, n _kGray level is r in the image for this reason _kNumber of pixels, pro (r _k) expression gray level r _kThe probability that occurs; In rectangular coordinate system, r _kWith pro (r _k) graph of a relation be grey level histogram.This grey level histogram has reflected the statistical relationship between each gray level and its frequency of occurrences in the image, has described average light and shade and the contrast situation of entire image.

The gray accumulation distribution function of described equalization processing employing image is mapped to wider, a better unified distribution with the intensity profile of image:

$s_k=\underset{l=0}{\overset{k}{\mathrm{\&Sigma;}}}\mathrm{pro}\left(r_l\right),0\&le;r_l\&le;1,l=\mathrm{0,1,2}...k,k=\mathrm{0,1,2},...,255---\left(9\right)$

Wherein, s _kBe k gray level cumulative distribution function, r _lBe l gray level; Gray level after the conversion:

r′ _k＝INT[(L-1)s _k+0.5]，k＝0,1,2,…,255 (10)

Wherein, L=256, INT are bracket function, r ' _kR _kGray level after conversion.The pretreated effect of the original pavement image of present frame in the test is compared with Fig. 2 as shown in Figure 3, and the contrast of image strengthens, and Corner Feature is more obvious, more is conducive to follow-up Corner Detection.

Step 5: the pretreated i two field picture of image is carried out Harris Harris Corner Detection, and to utilize Lucas-Ke be that moral Lucas-Kanade optical flow method coupling is followed the tracks of Harris Harris algorithm at the coordinate of detected all angle points of i two field picture in the i+1 two field picture.

Angle point claims that again unique point is a kind of important local feature of image, can reflect many information of image outline, also is the key factor that impact detects effect.The most frequently used angular-point detection method has Harris Harris Corner Detection Algorithm, Susan SUSAN algorithm, Tuscany Canny operator Corner Detection etc. at present.Wherein Susan SUSAN algorithm anti-noise ability is strong, blurs but work as image target edge, and angle point extracts out of true; Tuscany Canny has the advantage that signal to noise ratio (S/N ratio) is large, accuracy of detection is high, but easily loses edge details; Harris Harris algorithm has unchangeability to translation and the rotation of gradation of image, and processing speed is fast.

It is exactly " light stream " that the displacement association that the angle point that detects forms between the two continuous frames image converts velocity correlation to, it is the instantaneous velocity of the pixel motion of space motion object on the observation imaging surface, can the time domain by the pixel intensity data in the image sequence change and correlativity be determined separately " motion " of location of pixels the sports ground that can not directly obtain with approximate treatment from sequence image.

It is moral Lucas-Kanade matching algorithm etc. that optical flow method tracking and matching method commonly used has the dense light stream matching method of Huo En-Shu Sike Horn-Schunck, eigentransformation method SIFT, Lucas-Ke that yardstick is constant.In all multi-methods, Huo En-dense optical flow method of Shu Sike Horn-Schunck belongs to the computing method of global optical flow, can obtain accurate light stream and estimate, but real-time is relatively poor; Though the eigentransformation method SIFT algorithm that yardstick is constant has plurality of advantages such as rotation, yardstick convergent-divergent, brightness variation remain unchanged, algorithm is complicated, and processing speed far can not satisfy the requirement of real-time estimation vehicle movement parameter; Lucas-Ke is that moral Lucas-Kanade algorithm is one of important method of asking sparse optical flow, and this algorithm only needs the angle point local message of wicket on every side, so speed is applicable to brightness and changes not quite the occasion that interframe movement is little and coherent.

In sum, Harris Harris Corner Detection Algorithm and Lucas-Ke be moral Lucas-Kanade matching algorithm can satisfy under the road traffic environment with 0 ~ 15(km/hour) low speed driving real-time and the accuracy requirement of measuring at the vehicle movement parameter of smooth bituminous pavement, the present invention adopts Harris Haris algorithm to seek the angle point of i two field picture then, utilizing Lucas-Ke is the coordinate that moral Lucas-Kanade light stream matching algorithm is followed the tracks of angle point Corresponding matching point in the i+1 two field picture of i frame, and then obtains over the ground motion state parameters of vehicle.

If certain a bit towards either direction the acute variation that less skew can both cause gray scale occurs, Harris Harris algorithm just thinks that this point is angle point, and the gray scale function of image at any point (u ₀, v ₀) locate along one party to the grey scale change amount be defined as:

E(u ₀,v ₀)＝∑[I(u ₀+Δu,v ₀+Δv)-I(u ₀,v ₀)] ²,(u ₀+Δu,v ₀+Δv)∈w(u ₀,v ₀,r _ad) (11)

Wherein, E (u ₀, v ₀) be any point (u in the i two field picture ₀, v ₀) the grey scale change amount, Δ u, Δ v are respectively that pixel is along u axle, the axial displacement of v, I (u ₀, v ₀) be point (u ₀, v ₀) gray-scale value, I (u ₀+ Δ u, v ₀+ Δ v) is point (u ₀+ Δ u, v ₀+ Δ v) gray-scale value, window function w (u ₀, v ₀, r _Ad)={ (u, v) | (u-u ₀) ²+ (v-v ₀) ²≤ r _Ad ²With any point (u ₀, v ₀) centered by, r _AdA circular window for radius; To I (u ₀+ Δ u, v ₀+ Δ v) carry out Taylor Taylor series expansion, ignore high-order term and get:

I(u ₀+Δu,v ₀+Δv)≈I(u ₀,v ₀)+Δu·I _u(u ₀,v ₀)+Δv·I _v(u ₀,v ₀) (12)

Wherein, $I_u(u_0,v_0)=\frac{\&PartialD;I(u_0,v_0)}{\&PartialD;u},$ $I_v(u_0,v_0)=\frac{\&PartialD;I(u_0,v_0)}{\&PartialD;v}$ Respectively I (u ₀, v ₀) in u axle, the axial gradient of v; Can be got by above two formulas:

$E(u_0,v_0)=[\mathrm{\&Delta;u},\mathrm{\&Delta;v}]M\left[\begin{array}{c}\mathrm{\&Delta;u}\\ \mathrm{\&Delta;v}\end{array}\right],M=\mathrm{\&Sigma;}\left[\begin{array}{cc}{I^2}_u(u_0,v_0)& I_u(u_0,v_0)I_v(u_0,v_0)\\ I_u(u_0,v_0)I_v(u_0,v_0)& {I^2}_v(u_0,v_0)\end{array}\right]---\left(13\right)$

Wherein, matrix M is any point (u ₀, v ₀) autocorrelation matrix of video in window on every side, for a width of cloth picture, autocorrelation function has characterized the intensity of variation of topography's gray scale, processes the offspring and enters relational expression (13) so symmetrical matrix M is carried out similarity diagonalization:

$E(u_0,v_0)=R^{-1}\left(\begin{array}{cc}{\mathrm{\&lambda;}}_1& 0\\ 0& {\mathrm{\&lambda;}}_2\end{array}\right)---\left(14\right)$

Wherein, R is twiddle factor, R ^-1Be the inverse matrix of R, λ ₁, λ ₂Be the eigenwert of M battle array, reflected the imaging surface curvature of 2 major axes orientations of angle point.When 2 eigenwerts that and if only if are all large, all will cause violent grey scale change along the skew of any direction, the angle point response function CRF that satisfies Harris Harris definition is expressed as:

Described angle point response function CRF adopts following method to obtain:

CRF(u ₀,v ₀)＝det(M)-η·[trace(M)] ² (15)

Wherein, CRF (u ₀, v ₀) be any point (u ₀, v ₀) the angle point response function, det (M)=λ ₁λ ₂, the determinant of expression Metzler matrix, trace (M)=λ ₁+ λ ₂, the mark of representing matrix M, scale factor η are empirical value, η=0.04.When the CRF of point surpasses given threshold value (usually getting 18), think that this point is exactly angle point, and obtain the pixel coordinate of angle point.Harris Haris algorithm is red mark point in angle point that current frame image detects such as Fig. 4.

Then, utilizing Lucas-Ke is that moral Lucas-Kanade optical flow method coupling is followed the tracks of Harris Harris at the coordinate of detected all angle points of i two field picture in the i+1 two field picture.Lucas-Ke is that moral Lucas-Kanade is a kind of light stream algorithm for estimating of two frame differences, and this algorithm is based on following three hypothesis:

(1) brightness constancy.The pixel of target remains unchanged when adjacent interframe movement in appearance in the image scene.Its brightness does not change when being followed the tracks of frame by frame for gray level image hypothesis pixel;

(2) Time Continuous or motion are " little motions ".The ratio that the time that refers in the practical application changes relative image motion is enough little, and target is just smaller in the motion of interframe like this;

(3) space is consistent.In scene on the same surface contiguous point have similar motion, the projection on the plane of delineation is also in adjacent domain.

The arbitrary angle point G of Harris Harris algorithm in detected all angle points of i two field picture is (u at the coordinate of i two field picture _i, v _i), Lucas-Ke of described angle point G is that the tracking of moral Lucas-Kanade optical flow method coupling may further comprise the steps:

At first, set up equation of constraint between i frame and the i+1 two field picture:

I(u _i+1,v _i+1,t _i+1)＝I(u _i+Δu _i,v _i+Δv _i,t _i+Δt)＝I(u _i,v _i,t _i) (16)

Wherein, t _i, t _I+1Dividing is the moment of taking i frame and i+1 two field picture, time interval Δ t=t _I+1-t _i, the time interval is relevant with the image update frequency of obtaining, the representative value of Δ t has 1/30(second), 1/25(second), 1/15(second) etc., Δ u _i, Δ v _iRespectively that angle point G is from t _iThe time be carved into t _I+1U axle constantly, the displacement of v axle, (u _i, v _i) be that angle point G is at the coordinate of i two field picture, (u _I+1, v _I+1) be angle point G at the coordinate of i+1 two field picture, I (u _i, v _i, t _i) be t _iThe gray-scale value of moment angle point G, I (u _I+1, v _I+1, t _I+1) be t _I+1The gray-scale value of moment angle point G.

Then, I (u _i+ Δ u _i, v _i+ Δ v _i, t _i+ Δ t) carry out Taylor Taylor series expansion, ignore high-order term and get:

I(u _i+Δu _i,v _i+Δv _i,t _i+Δt)≈I(u _i,v _i,t _i)+I _u·Δu _i+I _v·Δv _i+I _t·Δt (17)

Wherein,

Gradation of image u direction of principal axis partial derivative,

Gradation of image v direction of principal axis partial derivative,

That gradation of image is to the partial derivative of time t;

Can be got by relational expression (16), (17):

$I_u\&CenterDot;\frac{\mathrm{\&Delta;}{u}_i}{\mathrm{\&Delta;t}}+I_v\&CenterDot;\frac{\mathrm{\&Delta;}{v}_i}{\mathrm{\&Delta;t}}+I_t=0---\left(18\right)$

Then, set up t _iNear the system equation of 5 * 5 neighborhood territory pixels the moment angle point G, set up following system equation group:

Hc＝g (19)

Wherein, $H=\left[\begin{array}{cc}{I}_u(u_{i,1},v_{i,1},t_i),& I_v(u_{i,1},v_{i,1},t_i)\\ I_u(u_{i,2},v_{i,2},t_i),& I_v(u_{i,2},v_{i,2},t_i)\\ .& .\\ .& .\\ .& .\\ I_u(u_{i,25},v_{i,25},t_i),& I_v(u_{i,25},v_{i,25},t_i)\end{array}\right],c=\left[\begin{array}{c}\frac{\mathrm{\&Delta;}{u}_i}{\mathrm{\&Delta;t}}\\ \frac{\mathrm{\&Delta;}{v}_i}{\mathrm{\&Delta;t}}\end{array}\right],g=\left[\begin{array}{c}-I_t(u_{i,1},v_{i,1},t_i)\\ -I_t(u_{i,2},v_{i,2},t_i)\\ .\\ .\\ .\\ -I_t(u_{i,25},v_{i,25},t_i)\end{array}\right],$

25 * 2 the matrix that H is comprised of the shade of gray of 5 * 5 neighborhood territory pixels of angle point G, c is 2 * 1 the column vector that light stream forms by the displacement between adjacent two two field pictures, 25 * 1 the column vector that g is comprised of gray scale partial derivative in time;

At last, adopt least square method to ask the least square solution of system equation, the i.e. motion of central point G;

Transposition on equation (19) both sides with premultiplication H battle array:

H ^THc＝H ^Tg (20)

The light stream of the i.e. front and back frame formation of the solution of equation is as follows:

$\left[\begin{array}{c}\frac{\mathrm{\&Delta;}{u}_i}{\mathrm{\&Delta;t}}\\ \frac{\mathrm{\&Delta;}{v}_i}{\mathrm{\&Delta;t}}\end{array}\right]={\left(H^{T}H\right)}^{-1}{H}^{T}g---\left(21\right)$

Angle point G is at the coordinate (u of i+1 two field picture _I+1, v _I+1) adopt following formula to obtain:

$\left[\begin{array}{c}{u}_{i+1}\\ v_{i+1}\end{array}\right]=\left[\begin{array}{c}\frac{\mathrm{\&Delta;}{u}_i}{\mathrm{\&Delta;t}}\\ \frac{\mathrm{\&Delta;}{v}_i}{\mathrm{\&Delta;t}}\end{array}\right]\mathrm{\&Delta;t}+\left[\begin{array}{c}{u}_i\\ v_i\end{array}\right]---\left(22\right)$

Lucas-Ke be moral Lucas-Kanade algorithm tracking effect as shown in Figure 5, the first half of Fig. 5 is the present frame pavement image, the latter half is the next frame pavement image, the point that red straight line links to each other be exactly Lucas-Ke be the coupling angle point of moral Lucas-Kanade algorithm keeps track.

Step 6: utilize the angle point that mates in the consistent RANSAC algorithm optimization of the stochastic sampling step 5.Corners Matching is finished the end that can not indicate algorithm, because process Lucas-Ke is after moral Lucas-Kanade algorithm carries out the angle point initial matching, mainly exist two impacts to mate the factors of correctness and precision: the initial matching that is on the one hand angle point can not guarantee fully that resulting angle point is correct, is from the corner location of image extraction and not exclusively accurate on the other hand.So the present invention has introduced the larger angle point of matching error in the consistent RANSAC algorithm of the stochastic sampling of the robust filtering step 5, this algorithm is the sample data collection that comprises abnormal data according to a group, calculates the mathematical model parameter of data, obtains the effective sample data.By practical situation of the present invention, the effective sample data are " the interior point " after the optimization, can utilize the inherent restriction relation of matching angle point set to reject wrong and the larger coupling angle point of error, eliminate " exterior point " thus impact guarantee precision, improve the quality of mating angle point and the robustness of algorithm.The consistent RANSAC algorithm of stochastic sampling specifically adopts following steps:

At first, set up the model Q of the projective transformation matrix between i frame and i+1 two field picture:

ξU _i+1＝QU _i (23)

Wherein, $U_{i+1}=\left[\begin{array}{c}{u}_{i+1}\\ v_{i+1}\\ 1\end{array}\right],$ $U_i=\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right],$ $Q=\left[\begin{array}{ccc}{q}_0& q_1& q_2\\ q_3& q_4& q_5\\ q_6& q_7& q_8\end{array}\right],$ U _i, U _I+1Be respectively arbitrary coupling angle point G in the homogeneous vectors of the coordinate formation of i frame and i+1 two field picture, Q is the projective transformation matrix between i frame and i+1 two field picture, and ξ is scale factor, gets ξ=1; Q wherein ₀, q ₁, q ₃, q ₄The matrix that consists of $\left[\begin{array}{cc}{q}_0& q_1\\ q_3& q_4\end{array}\right]$ Expression is carried out convergent-divergent, rotation, symmetry, wrong contact transformation, q to image ₆, q ₇Vector [the q that consists of ₆q ₇] represent image is carried out translation transformation, q ₂, q ₅The vector that consists of $\left[\begin{array}{c}{q}_2\\ q_5\end{array}\right]$ Expression is carried out projective transformation, q to image ₈Expression is carried out stretching to image; Q has 8 degree of freedom, as long as select 4 pairs of coupling angle points just can draw 8 linear equations, by self constraint condition of projective transformation matrix Q, forms 9 equations and namely estimates Q, estimating step again:

(I) initialization iterations variable m=1, and establish the iterations maximal value

(II) chooses 4 pairs of coupling angle points at random coupling angle point centering, as a sample set S _m, the sample complementary set after the extraction is

With sample set S _mIn some substitution relational expression (23) try to achieve initial Q _m

(III) utilizes complementary set In the U that consists of at the coordinate of i frame of arbitrary coupling angle point _iWith the Q that is obtained by (II) _mObtain error function: $F={d^2}_{\mathrm{Euc}}(U_{i+1},Q_m{U}_i)+{d^2}_{\mathrm{Euc}}(U_i,Q_m^{-1}{U}_{i+1})---\left(24\right)$

Wherein,

Q _mInverse matrix, d _Euc(U _I+1, Q _mU _i) expression point U _I+1With a Q _mU _iIn three-dimensional Euclidean distance,

Expression point U _iWith the point In three-dimensional Euclidean distance; Error threshold gets 3, and the error function value is regarded as interior point less than the match point of this threshold value, and point set in point adds in this, otherwise be exterior point;

(IV) interior point set and sample set S _mForm consistent collection

If

In in count more than or equal to 60 percent of coupling angle point sum, then think to have obtained correct projective transformation matrix, and will

As sample set substitution relational expression (23), adopt least square method to recomputate New model Q ' _mAs the correction model of projective transformation matrix, if In in count less than 60 percent of coupling angle point sum, then algorithm failure makes Q _m=0;

(V) if

Make m=m+1, continue to carry out successively (II) to (V); If

The iterative process of then sampling finishes; After sampling, obtain the new model set In choose the consistent collection of sample number maximum As the matching angle point set after optimizing, the corresponding model Q ' that calculates _MaxAs final transformation matrix Q, algorithm finishes.

Effect behind the consistent RANSAC algorithm optimization of stochastic sampling as shown in Figure 6, red straight line links to each other among Fig. 62 be match point after optimizing, rejected apparent error match point and the larger point of matching error among Fig. 5.So the high reliability of the matching angle point set after optimizing and pin-point accuracy have further improved the precision that vehicle movement parameter is estimated in real time.

Step 7: the matching angle point set after the optimization that employing frame difference method treatment step six obtains, the motion state of calculating vehicle.Coupling angle point behind the consistent RANSAC algorithm optimization of extraction stochastic sampling is done the poor pixel displacement that obtains at the coordinate of i frame, i+1 two field picture with the matching angle point coordinate in i+1 frame, the i two field picture.In conjunction with the camera interior and exterior parameter that is obtained by step 2, calculate the quantity of state of vehicle under the vehicle carrier coordinate system.

Described frame difference method is calculated state of motion of vehicle, and the concrete grammar of employing is as follows:

1. the matching angle point set sample size of establishing behind the consistent RANSAC algorithm optimization of stochastic sampling is

Initialization sample number variable j makes j=1;

2. j the coupling angle point that extracts behind the consistent RANSAC algorithm optimization of stochastic sampling is respectively p (u at the coordinate of i+1 frame and i two field picture _{I+1, j}, v _{I+1, j}) and p (u _{I, j}, v _{I, j}), wherein, u _{I, j}, v _{I, j}Respectively u axle, the v axial coordinate of the coupling angle point of j behind the consistent RANSAC algorithm optimization of stochastic sampling in the i two field picture in the i two field picture, with p (u _I+1, j, v _I+1, j) and p (u _{I, j}, v _{I, j}) homogeneous vectors [u that consists of respectively _{I+1, j}, v _{I+1, j}, 1] ^T, [u _{I, j}, v _{I, j}, 1] ^TSubstitution relational expression (1) is carried out the coordinate inverse:

$\left[\begin{array}{c}{x}_{i+1,j}\\ y_{i+1,\mathrm{<figure-callout\; id="1"\; label="j"\; filenames="HDA00002368243800011.png"\; state="\{\{state\}\}">j</figure-callout>}}\\ 1\end{array}\right]={\left(A\right[{\mathrm{<figure-callout\; id="1"\; label="r\; cam"\; filenames="HDA00002368243800011.png"\; state="\{\{state\}\}">r</figure-callout>}}_{{\mathrm{cam}}_{}},{\mathrm{<figure-callout\; id="1"\; label="r\; cam"\; filenames="HDA00002368243800011.png"\; state="\{\{state\}\}">r</figure-callout>}}_{{\mathrm{cam}}_{}},t_{\mathrm{cam}}\left]\right)}^{-1}\left[\begin{array}{c}{u}_{i+1,j}\\ v_{i+1,\mathrm{<figure-callout\; id="1"\; label="j"\; filenames="HDA00002368243800011.png"\; state="\{\{state\}\}">j</figure-callout>}}\\ 1\end{array}\right],\left[\begin{array}{c}{x}_{i,j}\\ y_{i,\mathrm{<figure-callout\; id="1"\; label="j"\; filenames="HDA00002368243800011.png"\; state="\{\{state\}\}">j</figure-callout>}}\\ 1\end{array}\right]={\left(\right[r_{{\mathrm{<figure-callout\; id="1"\; label="cam"\; filenames="HDA00002368243800011.png"\; state="\{\{state\}\}">cam</figure-callout>}}_1},{\mathrm{<figure-callout\; id="1"\; label="r\; cam"\; filenames="HDA00002368243800011.png"\; state="\{\{state\}\}">r</figure-callout>}}_{{\mathrm{cam}}_{}},t_{\mathrm{cam}}\left]\right)}^{-1}\left[\begin{array}{c}{u}_{i,j}\\ v_{i,j}\\ 1\end{array}\right]---\left(25\right)$

Wherein, [x _{I+1, j}, y _{I+1, j}, 1] ^T, [x _{I, j,}y _{I, j}, 1] ^TRespectively that j coupling angle point after optimizing is in the homogeneous vectors of the physical coordinates formation of i+1 frame and i two field picture;

3. video camera is taken the i two field picture within the time interval of taking the i+1 two field picture, by j after optimizing each parameter of mating one group of travel condition of vehicle that angle point calculates in real time:

$v_{i,j}=\frac{\mathrm{\&Delta;P}(x_{i,j},y_{i,j})}{\mathrm{\&Delta;t}}=\frac{P(x_{i+1,j},y_{i+1,j})-P(x_{i,j},y_{i,j})}{t_{i+1}-t_i}$

$v_{{\stackrel{\&RightArrow;}{x}}_{i,j}}=\frac{\mathrm{\&Delta;}{x}_{i,j}}{\mathrm{\&Delta;t}}=\frac{x_{i+1,j}-x_{i,j}}{t_{i+1}-t_i}---\left(26\right)$

$v_{{\stackrel{\&RightArrow;}{y}}_{i,j}}=\frac{\mathrm{\&Delta;}{y}_{i,j}}{\mathrm{\&Delta;t}}=\frac{y_{i+1,j}-y_{i,j}}{t_{i+1}-t_i}$

Wherein, arctan () represents arctan function, t _i, t _I+1Dividing is the moment of taking i frame, i+1 two field picture, P (x _{I, j}, y _{I, j}), P (x _{I+1, j}, y _{I+1, j}) be respectively by j after optimizing mate that angle point obtains at t _iThe moment and t _I+1The coordinate of moment vehicle, Δ P (x _{I, j}, y _{I, j}) be by j after optimizing mate that angle point obtains from t _iThe time be carved into t _I+1The constantly displacement of vehicle, x _{I, j}, x _{I+1, j}Be respectively by j after optimizing mate that angle point obtains at t _iThe moment and t _I+1The ordinate of orthogonal axes value of moment vehicle, y _{I, j}, y _{I+1, j}Be respectively by j after optimizing mate that angle point obtains at t _iThe moment and t _I+1The transverse axis coordinate figure Δ x of moment vehicle _{I, j}, Δ y _{I, j}Be respectively by j after optimizing mate that angle point obtains from t _iThe time be carved into t _I+1The constantly longitudinal axis, the transverse axis displacement of vehicle, v _{I, j}Be by j after optimizing mate that angle point obtains from t _iThe time be carved into t _I+1The instantaneous velocity of moment vehicle, Be respectively by j after optimizing mate that angle point obtains from t _iThe time be carved into t _I+1The longitudinal axis, the X direction instantaneous velocity of moment vehicle;

If 4.

Then make j=j+1, continue to carry out successively above-mentioned 2. arriving 4.; If

Then iterative process finishes.

Above-mentioned frame difference method is obtained

The group vehicle movement parameter is done on average, and with average result as from t _iThe time be carved into t _I+1The kinematic parameter of moment vehicle:

$v_i=\frac{\underset{j=1}{\overset{\hat{j}}{\mathrm{\&Sigma;}}}{v}_{i,j}}{\hat{j}}$ $v_{{\stackrel{\&RightArrow;}{x}}_i}=\frac{\underset{j=1}{\overset{\hat{j}}{\mathrm{\&Sigma;}}}{v}_{{\stackrel{\&RightArrow;}{x}}_{i,j}}}{\hat{j}}---\left(27\right)$

$v_{{\stackrel{\&RightArrow;}{y}}_i}=\frac{\underset{j=1}{\overset{\hat{j}}{\mathrm{\&Sigma;}}}{v}_{{\stackrel{\&RightArrow;}{y}}_{i,j}}}{\hat{j}}$ ${\mathrm{\&beta;}}_i=\arctan \left(\frac{v_{{\stackrel{\&RightArrow;}{y}}_i}}{v_{{\stackrel{\&RightArrow;}{x}}_i}}\right)$

Wherein, v _iFrom t _iThe time be carved into t _I+1The instantaneous velocity of moment vehicle,

Respectively from t _iThe time be carved into t _I+1Vertical, the transverse velocity of moment vehicle, β _iFrom t _iThe time be carved into t _I+1The side slip angle of moment vehicle;

Step 8: the pavement image that will obtain in chronological order successively repeating step three to step 8 recursion calculates, even image sequence pointer i=i+1, repeating step three to step 8 is calculated, and obtains vehicle at each longitudinal velocity, transverse velocity and side slip angle constantly.

Claims (1)

1. the state of motion of vehicle method of estimation of an optical flow-based is characterized in that, comprises following steps:

Step 1: in the vehicle rear axle center high precision monocular-camera is installed, its optical axis is downward perpendicular to vehicle operating plane and directed towards ground, and 20 ~ 40 centimetres of vertical heights are used for obtaining the pavement image sequence of variation; Definition vehicle carrier coordinate system, the initial point o of vehicle carrier coordinate system is positioned at vehicle barycenter place, and the x axle is along the longitudinal axis of vehicle and consistent with vehicle forward direction, and the z axle is downward perpendicular to vehicle operating plane and directed towards ground, and it is definite that the y axle press the right-handed helix rule; Definition pavement image coordinate system, the initial point o of pavement image coordinate system _pBe the summit in the image lower left corner, longitudinal axis u is parallel to vehicle carrier x axle, and transverse axis v is parallel to vehicle carrier y axle;

Step 2: adopt classical camera calibration method to carry out camera calibration, to obtain the inner parameter of video camera: optical axis is at the vertical projection point of the plane of delineation coordinate (o at image coordinate system _u, o _v), u axle normal vector α on the image, v axle normal vector β on the image, verticality γ and the external parameter of u, v coordinate axis: video camera is with respect to 3 * 3 rotation matrix r of world coordinate system _Cam, video camera is with respect to 3 * 1 translation vector t of world coordinate system _Cam

Step 3: initialisation image sequence pointer i makes i=1;

Step 4: get i frame pavement image and i+1 frame pavement image, respectively i frame pavement image and i+1 frame pavement image are carried out the image pre-service, described image pre-service comprises: grey level histogram function and the equalization processing of computed image;

The grey level histogram function of described image adopts following methods to calculate:

At first with image gray processing, and with the gray-scale value normalization of all pixels of image, calculate the grey level histogram function of this image:

$\mathrm{pro}\left(r_k\right)=\frac{n_k}{n},k=\mathrm{0,1,2}...\mathrm{255,0}\&le;r_k\&le;1---\left(1\right)$

Wherein, n is total number of image pixels, r _kBe k gray level, n _kGray level is r in the image for this reason _kNumber of pixels, pro (r _k) expression gray level r _kThe probability that occurs; In rectangular coordinate system, r _kWith pro (r _k) graph of a relation be grey level histogram;

Described equalization processing adopts the gray accumulation distribution function of image:

$s_k=\underset{l=0}{\overset{k}{\mathrm{\&Sigma;}}}\mathrm{pro}\left(r_l\right),0\&le;r_l\&le;1,l=\mathrm{0,1,2}...k,k=\mathrm{0,1,2},...,255---\left(2\right)$

Wherein, s _kBe k gray level cumulative distribution function, r _lBe l gray level; Gray level after the conversion

r′ _k＝INT[(L-1)s _k+0.5]，k＝0,1,2,…,255 (3)

Wherein, L=256, INT are bracket function, r ' _kR _kGray level after conversion;

Step 5: the pretreated i two field picture of image is carried out Harris Harris Corner Detection: Harris Haris Corner Detection comprises: calculate grey scale change amount and the angle point response function of every bit in the i two field picture, and carry out angle point and judge;

Following methods is adopted in the calculating of described grey scale change amount:

E(u ₀,v ₀)＝∑[I(u ₀+Δu,v ₀+Δv)-I(u ₀,v ₀)] ²，(u ₀+Δu,v ₀+Δv)∈w(u ₀,v ₀,r _ad) (4)

Wherein, E (u ₀, v ₀) be any point (u in the i two field picture ₀, v ₀) the grey scale change amount, Δ u, Δ v are respectively point (u ₀, v ₀) along u axle, the axial displacement of v, I (u ₀, v ₀) be point (u ₀, v ₀) gray-scale value, I (u ₀+ Δ u, v ₀+ Δ v) is point (u ₀+ Δ u, v ₀+ Δ v) gray-scale value, window function w (u ₀, v ₀, r _Ad)={ (u, v) | (u-u ₀) ²+ (v-v ₀) ²≤ r _Ad ²With any point (u ₀, v ₀) centered by, r _AdA circular window for radius; To I (u ₀+ Δ u, v ₀+ Δ v) carry out Taylor Taylor series expansion, ignore high-order term and get:

I(u ₀+Δu,v ₀+Δv)≈I(u ₀,v ₀)+Δu·I _u(u ₀,v ₀)+Δv·I _v(u ₀,v ₀) (5)

Wherein, $I_u(u_0,v_0)=\frac{\&PartialD;I(u_0,v_0)}{\&PartialD;u},$ $I_v(u_0,v_0)=\frac{\&PartialD;I(u_0,v_0)}{\&PartialD;v}$ Respectively I (u ₀, v ₀) in u axle, the axial gradient of v; Can be got by above two formulas:

$E(u_0,v_0)=[\mathrm{\&Delta;u},\mathrm{\&Delta;v}]M\left[\begin{array}{c}\mathrm{\&Delta;u}\\ \mathrm{\&Delta;v}\end{array}\right],M=\mathrm{\&Sigma;}\left[\begin{array}{cc}{I^2}_u(u_0,v_0)& I_u(u_0,v_0)I_v(u_0,v_0)\\ I_u(u_0,v_0)I_v(u_0,v_0)& {I^2}_v(u_0,v_0)\end{array}\right]---\left(6\right)$

Wherein, matrix M is any point (u ₀, v ₀) autocorrelation matrix of video in window on every side, symmetrical matrix M is carried out the similarity diagonalization offspring enters relational expression (6):

$E(u_0,v_0)=R^{-1}\left(\begin{array}{cc}{\mathrm{\&lambda;}}_1& 0\\ 0& {\mathrm{\&lambda;}}_2\end{array}\right)---\left(7\right)$

Wherein, R is twiddle factor, R ^-1Be the inverse matrix of R, λ ₁, λ ₂Eigenwert for the M battle array;

Described angle point response function CRF adopts following method to obtain:

CRF(u ₀,v ₀)＝det(M)-η·[trace(M)] ² (8)

Wherein, CRF (u ₀, v ₀) be any point (u ₀, v ₀) the angle point response function, det (M)=λ ₁λ ₂, the determinant of expression Metzler matrix, trace (M)=λ ₁+ λ ₂, the mark of representing matrix M, scale factor η are empirical value, η=0.04;

Described angle point is judged the employing following methods:

When angle point response function CRF surpasses threshold value, think that this point is exactly angle point, and obtain the pixel coordinate of angle point that threshold value gets 18;

Step 6: utilizing Lucas-Ke is that moral Lucas-Kanade optical flow method coupling is followed the tracks of Harris Harris algorithm at the coordinate of detected all angle points of i two field picture in the i+1 two field picture: the arbitrary angle point G in the i two field picture is (u at the coordinate of i two field picture _i, v _i), Lucas-Ke of described angle point G is that the tracking of moral Lucas-Kanade optical flow method coupling may further comprise the steps:

At first, set up equation of constraint between i frame and the i+1 two field picture:

I(u _i+1,v _i+1,t _i+1)＝I(u _i+Δu _i,v _i+Δv _i,t _i+Δt)＝I(u _i,v _i,t _i) (9)

Wherein, t _i, t _I+1Dividing is the moment of taking i frame and i+1 two field picture, time interval Δ t=t _I+1-t _i, Δ t is fixed value, the representative value of Δ t has 1/30(second), 1/25(second), 1/15(second), Δ u _i, Δ v _iRespectively the displacement of angle point G u axle, v axle within the Δ t time, (u _i, v _i) be that angle point G is at the coordinate of i two field picture, (u _I+1, v _I+1) be angle point G at the coordinate of i+1 two field picture, I (u _i, v _i, t _i) be t _iThe gray-scale value of moment angle point G, I (u _I+1, v _I+1, t _I+1) be t _I+1The gray-scale value of moment angle point G;

Then, I (u _i+ Δ u _i, v _i+ Δ v _i, t _i+ Δ t) carry out Taylor Taylor series expansion, ignore high-order term and get:

I(u _i+Δu _i,v _i+Δv _i,t _i+Δt)≈I(u _i,v _i,t _i)+I _u·Δu _i+I _v·Δv _i+I _t·Δt (10)

Wherein,

Gradation of image u direction of principal axis partial derivative,

Gradation of image v direction of principal axis partial derivative, That gradation of image is to the partial derivative of time t;

Can be got by relational expression (9), (10):

$I_u\&CenterDot;\frac{\mathrm{\&Delta;}{u}_i}{\mathrm{\&Delta;t}}+I_v\&CenterDot;\frac{\mathrm{\&Delta;}{v}_i}{\mathrm{\&Delta;t}}+I_t=0---\left(11\right)$

Then, set up t _iNear the system equation of 5 * 5 neighborhood territory pixels the moment angle point G, set up following system of equations:

Hc＝g (12)

Wherein, $H=\left[\begin{array}{cc}{I}_u(u_{i,1},v_{i,1},t_i),& I_v(u_{i,1},v_{i,1},t_i)\\ I_u(u_{i,2},v_{i,2},t_i),& I_v(u_{i,2},v_{i,2},t_i)\\ .& .\\ .& .\\ .& .\\ I_u(u_{i,25},v_{i,25},t_i),& I_v(u_{i,25},v_{i,25},t_i)\end{array}\right],$ $c=\left[\begin{array}{c}\frac{\mathrm{\&Delta;}{u}_i}{\mathrm{\&Delta;t}}\\ \frac{\mathrm{\&Delta;}{v}_i}{\mathrm{\&Delta;t}}\end{array}\right],$ $g=\left[\begin{array}{c}-I_t(u_{i,1},v_{i,1},t_i)\\ -I_t(u_{i,2},v_{i,2},t_i)\\ .\\ .\\ .\\ -I_t(u_{i,25},v_{i,25},t_i)\end{array}\right],$

25 * 2 the matrix that H is comprised of the shade of gray of 5 * 5 neighborhood territory pixels, c is 2 * 1 the column vector that light stream forms by the displacement between adjacent two two field pictures, 25 * 1 the column vector that g is comprised of gray scale partial derivative in time;

At last, adopt least square method to ask the least square solution of system equation, the i.e. motion of central point G;

Transposition on equation (12) both sides with premultiplication H battle array:

H ^THc＝H ^Tg (13)

Wherein, H ^TIt is the transposition of H battle array; The light stream of the i.e. front and back frame formation of the solution of equation is as follows:

$\left[\begin{array}{c}\frac{\mathrm{\&Delta;}{u}_i}{\mathrm{\&Delta;t}}\\ \frac{\mathrm{\&Delta;}{v}_i}{\mathrm{\&Delta;t}}\end{array}\right]={\left(H^{T}H\right)}^{-1}{H}^{T}g---\left(14\right)$

Wherein, (H ^TH) ^-1It is matrix H ^TThe inverse matrix of H; Angle point G is at the coordinate (u of i+1 two field picture _I+1, v _I+1) adopt following formula to obtain:

$\left[\begin{array}{c}{u}_{i+1}\\ v_{i+1}\end{array}\right]=\left[\begin{array}{c}\frac{\mathrm{\&Delta;}{u}_i}{\mathrm{\&Delta;t}}\\ \frac{\mathrm{\&Delta;}{v}_i}{\mathrm{\&Delta;t}}\end{array}\right]\mathrm{\&Delta;t}+\left[\begin{array}{c}{u}_i\\ v_i\end{array}\right]---\left(15\right)$

Step 7: all coupling angle points that utilize the consistent RANSAC algorithm optimization of stochastic sampling step 6 to obtain:

At first, set up the model Q of the projective transformation matrix between i frame and i+1 two field picture:

ξU _i+1＝QU _i (16)

Wherein, $U_{i+1}=\left[\begin{array}{c}{u}_{i+1}\\ v_{i+1}\\ 1\end{array}\right],$ $U_i=\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right],$ $Q=\left[\begin{array}{ccc}{q}_0& q_1& q_2\\ q_3& q_4& q_5\\ q_6& q_7& q_8\end{array}\right],$ U _i, U _I+1Be respectively the homogeneous vectors that arbitrary coupling angle point G in all coupling angle points that step 6 obtains consists of at the coordinate of i frame and i+1 two field picture, Q is the projective transformation matrix between i frame and i+1 two field picture, and ξ is scale factor, gets ξ=1; Q wherein ₀, q ₁, q ₃, q ₄The matrix that consists of $\left[\begin{array}{cc}{q}_0& q_1\\ q_3& q_4\end{array}\right]$ Expression is carried out convergent-divergent, rotation, symmetry, wrong contact transformation, q to image ₆, q ₇Vector [the q that consists of ₆q ₇] represent image is carried out translation transformation, q ₂, q ₅The vector that consists of $\left[\begin{array}{c}{q}_2\\ q_5\end{array}\right]$ Expression is carried out projective transformation, q to image ₈Expression is carried out stretching to image; Select 4 pairs of coupling angle points to draw 8 linear equations, by self constraint condition of projective transformation battle array Q, form 9 equations and namely estimate Q, the step of estimation again:

(I) initialization iterations variable m=1, and establish the iterations maximal value

(II) chooses 4 pairs of coupling angle points at random in all coupling angle points that step 6 obtains, as a sample set S _m, the sample complementary set after the extraction is

With sample set S _mIn some substitution relational expression (16) try to achieve initial Q _m

(III) utilizes complementary set In the U that consists of at the coordinate of i frame of arbitrary coupling angle point _iWith the Q that is obtained by (II) _mObtain error function:

$F={d^2}_{\mathrm{Euc}}(U_{i+1},Q_m{U}_i)+{d^2}_{\mathrm{Euc}}(U_i,Q_m^{-1}{U}_{i+1})---\left(17\right)$

Wherein,

Q _mInverse matrix, d _Euc(U _I+1, Q _mU _i) expression point U _I+1With a Q _mU _iIn three-dimensional Euclidean distance,

Expression point U _iWith the point

In three-dimensional Euclidean distance; Error threshold gets 3, and error function F is interior point less than the match point of this threshold value, and point set in point adds in this, otherwise be exterior point;

(IV) interior point set and sample set S _mForm consistent collection

If

In in count more than or equal to 60 percent of coupling angle point sum, then think to have obtained correct projective transformation matrix, and will

As sample set substitution relational expression (16), adopt least square method to recomputate New model Q ' _mAs the correction model of projective transformation matrix, if

In in count less than 60 percent of coupling angle point sum, then algorithm failure makes Q _m=0;

(V) if

Make m=m+1, continue to carry out successively (II) to (V); If

The iterative process of then sampling finishes; After sampling, obtain the new model set

In choose the consistent collection of sample number maximum

As the matching angle point set after optimizing, the corresponding model Q ' that calculates _MaxAs final transformation matrix Q, algorithm finishes;

Step 8: the matching angle point set after the optimization that employing frame difference method treatment step 7 obtains, calculate the motion state of vehicle: the coupling angle point behind the consistent RANSAC algorithm optimization of extraction stochastic sampling is done the poor pixel displacement that obtains at the coordinate of i frame, i+1 two field picture with the matching angle point coordinate in i frame, the i+1 two field picture; In conjunction with the camera interior and exterior parameter that is obtained by step 2, calculate the quantity of state of vehicle under the vehicle carrier coordinate system;

Described frame difference method is calculated state of motion of vehicle, and the concrete grammar of employing is as follows:

1. the matching angle point set sample size of establishing behind the consistent RANSAC algorithm optimization of stochastic sampling is

Initialization sample number variable j makes j=1;

2. it is poor that after will optimizing j coupling angle point done at the coordinate of i+1 frame and i two field picture, gets j coupling angle point after the optimization at shooting i frame and the pixel displacement Δ p (u in the i+1 two field picture time interval _{I, j}, v _{I, j}), wherein, u _{I, j}, v _{I, j}Respectively u axle, the v axial coordinate of the coupling angle point of j behind the consistent RANSAC algorithm optimization of stochastic sampling in the i two field picture in the i two field picture;

3. the camera parameter demarcated of integrating step 2 is with pixel displacement Δ p (u _{I, j}, v _{I, j}) carrying out the coordinate inverse, j match point after being optimized taken i frame and the physical displacement in the i+1 two field picture time interval;

4. video camera is taken the i two field picture within the time interval of taking the i+1 two field picture, each parameter of the one group of travel condition of vehicle that is calculated by j match point after optimizing:

$v_{i,j}=\frac{\mathrm{\&Delta;P}(x_{i,j},y_{i,j})}{\mathrm{\&Delta;t}}=\frac{P(x_{i+1,j},y_{i+1,j})-P(x_{i,j},y_{i,j})}{t_{i+1}-t_i}$

$v_{{\stackrel{\&RightArrow;}{x}}_{i,j}}=\frac{\mathrm{\&Delta;}{x}_{i,j}}{\mathrm{\&Delta;t}}=\frac{x_{i+1,j}-x_{i,j}}{t_{i+1}-t_i}---\left(18\right)$

$v_{{\stackrel{\&RightArrow;}{y}}_{i,j}}=\frac{\mathrm{\&Delta;}{y}_{i,j}}{\mathrm{\&Delta;t}}=\frac{y_{i+1,j}-y_{i,j}}{t_{i+1}-t_i}$

Wherein, arctan () represents arctan function, t _i, t _I+1Dividing is the moment of taking i frame, i+1 two field picture, P (x _{I, j}, y _{I, j}), P (x _{I+1, j}, y _{I+1, j}) be respectively by j the t that the coupling angle point is obtained after optimizing _iThe moment and t _I+The physical coordinates of 1 moment vehicle, Δ P (x _{I, j}, y _{I, j}) be by j after optimizing mate that angle point obtains from t _iThe time be carved into t _I+1The physical displacement of moment vehicle, x _{I, j}, x _{I+1, j}Respectively by j the t that the coupling angle point is obtained after optimizing _iThe moment and t _I+1The ordinate of orthogonal axes value of moment vehicle, y _{I, j}, y _{I+1, j}Respectively by j the t that the coupling angle point is obtained after optimizing _iThe moment and t _I+1The transverse axis coordinate figure of moment vehicle, Δ x _{I, j}, Δ y _{I, j}Be respectively by j after optimizing mate that angle point obtains from t _iThe time be carved into t _I+1The constantly longitudinal axis, the transverse axis displacement of vehicle, v _{I, j}Be by j after optimizing mate that angle point obtains from t _iThe time be carved into t _I+1The instantaneous velocity of moment vehicle,

Be respectively by j after optimizing mate that angle point obtains from t _iThe time be carved into t _I+1The longitudinal axis, the X direction speed of moment vehicle;

If 5.

Then make j=j+1, continue to carry out successively above-mentioned 2. arriving 5.; If

Then iterative process finishes;

Above-mentioned frame difference method is obtained

The group vehicle movement parameter do on average, and with average result as vehicle from t _iThe time be carved into t _I+1Kinematic parameter constantly:

$v_i=\frac{\underset{j=1}{\overset{\hat{j}}{\mathrm{\&Sigma;}}}{v}_{i,j}}{\hat{j}}$ $v_{{\stackrel{\&RightArrow;}{x}}_i}=\frac{\underset{j=1}{\overset{\hat{j}}{\mathrm{\&Sigma;}}}{v}_{{\stackrel{\&RightArrow;}{x}}_{i,j}}}{\hat{j}}---\left(19\right)$

$v_{{\stackrel{\&RightArrow;}{y}}_i}=\frac{\underset{j=1}{\overset{\hat{j}}{\mathrm{\&Sigma;}}}{v}_{{\stackrel{\&RightArrow;}{y}}_{i,j}}}{\hat{j}}$ ${\mathrm{\&beta;}}_i=\arctan \left(\frac{v_{{\stackrel{\&RightArrow;}{y}}_i}}{v_{{\stackrel{\&RightArrow;}{x}}_i}}\right)$

Wherein, v _iFrom t _iThe time be carved into t _I+1The instantaneous velocity of moment vehicle,

Respectively from t _iThe time be carved into t _I+1The longitudinal axis, the X direction speed of moment vehicle, β _iFrom t _iThe time be carved into t _I+The side slip angle of 1 moment vehicle;

Step 9: the pavement image that will obtain in chronological order successively repeating step 3 to step 9 recursion calculates, even image sequence pointer i=i+1, repeating step 3 to step 9 is calculated, and obtains vehicle at each longitudinal velocity, transverse velocity and side slip angle constantly.

Images