JP4270386B2 - Moving body moving amount calculation device - Google Patents

Description

  The present invention relates to a moving body moving amount calculation device for measuring the speed of a host vehicle in an environment where a moving body and a stationary object coexist.

  In the guidance of a moving body such as an automobile, a method for detecting a road plane area that can be traveled or detecting an obstacle present in the travel environment includes a method using laser radar, ultrasonic waves, millimeter wave radar, and the like, It can be roughly divided into methods using images.

  In the detection method using laser radar or millimeter wave radar, there is a problem that the apparatus is generally expensive and sufficient spatial resolution cannot be obtained. In addition, the detection method using ultrasonic waves has a problem in that it is difficult to measure far and the spatial resolution is low.

  On the other hand, the method using an image can be divided into a method using a single eye and a method using a compound eye. Conventionally, most of the methods using an image use a single eye, that is, an image obtained from one viewpoint, and are mainly used in a developed environment such as an expressway, and white lines (for example, separation lines) And the center line) are detected to detect the travel area. However, in general roads and parking lots where the presence of white lines etc. is not guaranteed, and the road surface colors and patterns vary, only the density pattern obtained from the monocular image can be used to stabilize the driveable area and obstacles. It is difficult to distinguish.

  On the other hand, in the detection method using compound eyes, that is, a stereo image, since the three-dimensional structure of the environment can be used in principle, there is a possibility that the driving environment can be recognized more stably. In particular, since the travelable area can be regarded as almost a plane in the space, a projective transformation matrix between images is calculated, and one image is two-dimensionally projected and superimposed on the other image using this projective transformation matrix. A method for detecting whether or not the image is a planar region or an obstacle from the overlapping state of images has been proposed (see Patent Document 1 and Non-Patent Document 1).

  By the way, in a moving body such as a vehicle, it is generally performed to measure the speed of the own vehicle, but various complicated operation control has become possible with the recent digitization of control devices. However, in order to perform more advanced control, it is desired to measure the speed of the host vehicle with higher accuracy.

On the other hand, according to the prior art, the speed measurement of the vehicle, although utilizing speed sensors and a yaw rate sensor or the like, greatly affected by slip or sensor error wheel in these sensors, a low measurement accuracy.

  From this point of view, a movement amount estimation device that detects the speed of a vehicle after converting an image captured by a camera into a video pattern on a road plane has been proposed (for example, see Patent Document 2).

JP 2000-293893 A JP 2003-178309 A Heung-Yung Shum, Richard Szeliski, “Panoramic Image Mosaics” Technical Report, 1997, RM -TAR-97-23, Microsoft Research (MSR-TR-97-23, Microsoft Research)

  However, in Patent Document 2, in order to measure the motion of the host vehicle using an image, it is a precondition that the shooting target is stationary. Therefore, when a moving object is included in the image, or when a stationary area in the image cannot be determined, it is difficult to measure the speed of the host vehicle, and the calculation load is heavy.

  The present invention has been made in consideration of such problems, and provides a moving body moving amount calculation apparatus capable of accurately measuring speed even in an environment where a moving body and a stationary object are mixed. Objective.

A moving body movement amount calculating apparatus according to the present invention includes a plurality of imaging means for imaging the periphery of a moving body,
A projection transformation matrix estimation means for dynamically estimating a projection transformation matrix for a road plane of an image by the plurality of imaging means;
First road plane area extraction means for extracting a road plane area from images by the plurality of imaging means;
Plane parameter calculation means for calculating a position and orientation parameter of the camera with respect to the road plane by decomposing the projection conversion matrix estimated by the projection conversion matrix estimation means;
Virtual projection plane image creation means for generating a virtual projection plane image when the road plane is viewed from above using the position and orientation parameters;
Second road plane area extracting means for extracting a road plane area in the virtual projection plane image;
Characterized in that it comprises a movement amount calculating means for calculating an amount of movement between the different times that the road plane area of the virtual projection plane in the image extracted by the second road plane area extraction means (claim 1 Described invention).

As described above, the plane attitude parameter is calculated from the projective transformation matrix, and the virtual projection plane image is created using the calculated plane attitude parameter, and the movement amount of the plane area is calculated. Therefore, the movement amount can be calculated with higher accuracy. Also, by calculating the amount of movement between times using only the plane area of the virtual projection plane image, it is possible to accurately measure the speed even in an environment where a moving object and a stationary object coexist. In addition, by using only the plane area, it is possible to calculate by excluding a height object or the like, so that it is possible to reduce an error in speed detection based on the height.

Furthermore, the moving body movement amount calculating device according to the present invention includes a plurality of imaging means for imaging the periphery of the moving body,
Road plane area extraction means for extracting a road plane area from images by the plurality of imaging means;
Moreover, you may make it provide the moving body speed calculation means which calculates the speed of a moving body using the moving amount between the different time calculated by the said moving amount calculation means (invention of Claim 2 ). Thereby, since the movement amount of the moving body is calculated using the calculated movement amount with a small error, it is possible to calculate the speed more accurately.

An object extracting means for extracting an object from images obtained from the plurality of imaging means;
A relative speed calculating means for calculating a relative speed of the object with respect to the moving body;
Absolute speed calculating means for calculating the absolute speed of the object using the speed of the moving body by the moving body speed calculating means and the relative speed of the object by the relative speed calculating means;
A determination unit that determines whether the object is a stationary object or a moving object using a calculation result obtained by the absolute velocity calculation unit may be provided (the invention according to claim 3 ).

  By calculating the absolute speed of the object using the accurate own vehicle speed, it is possible to more accurately determine whether or not the object is a stationary object.

Further, the movement amount calculating means, different said virtual projection plane images between time performs matching processing based on the gradient method, may be calculated the movement amount from the shift amount determined (claim 4, wherein Invention).

  According to the moving body moving amount calculating apparatus according to the present invention, since the moving amount between the times is calculated using only the plane area of the virtual projection plane image, the moving object moving amount calculating apparatus accurately even under the environment where the moving body and the stationary object coexist. Speed measurement is possible. In addition, by using only the planar area, it is possible to calculate by excluding a height object or the like, so that the speed detection error can be reduced. Furthermore, since the amount of movement is calculated from a virtual projection plane image parallel to the road plane, the amount of movement can be calculated correctly without being affected by changes in the posture of the vehicle or vibrations.

  FIG. 1 shows a vehicle 10 according to an embodiment to which a moving body movement amount calculating apparatus of the present invention is applied. A reference camera 12 and a reference camera 14 which are a pair of imaging means for capturing an image including a road plane area where the vehicle 10 travels are fixedly arranged on the left and right above the windshield of the vehicle 10. The reference camera 12 and the reference camera 14 are stereo cameras including a CCD camera or the like in which a common imaging area is set, and these may be a road plane area and an obstacle by processing captured images. An arithmetic processing unit 16 that detects a certain object is connected. In the following description, an image captured by the standard camera 12 is referred to as a standard image, and an image captured by the reference camera 14 is referred to as a reference image.

  FIG. 2 shows a functional configuration block of the arithmetic processing unit 16. The arithmetic processing unit 16 includes an image processing unit 18 that processes a standard image captured by the standard camera 12, an image processing unit 20 that processes a reference image captured by the reference camera 14, and a standard captured at a predetermined imaging time interval. And an image storage unit 22 for storing images and reference images.

  The arithmetic processing device 16 includes an arithmetic control unit 24. The arithmetic control unit 24 includes an image storage unit 22, a parameter storage unit 26, a projective transformation matrix calculation unit 28, a plane parameter calculation unit 30, and a plane region extraction unit 32. , An object detection unit 34, an object relative speed calculation unit 36, a virtual projection plane image generation unit 38, a host vehicle speed calculation unit 40, an object absolute speed calculation unit 42, and a display processing unit 44 are connected.

  The parameter storage unit 26 stores an initial value parameter necessary for the calculation and each parameter calculated during the previous calculation or calculation. The projective transformation matrix calculating unit 28 calculates a projective transformation matrix for performing two-dimensional projective transformation from a road plane area in the reference image to a road plane area in the standard image. The plane parameter calculation unit 30 calculates the normal vector of the road plane and the distance from the optical center of the reference camera 12 to the road plane as plane parameters. The plane area extraction unit 32 extracts a road plane area in which the vehicle 10 can travel using the base image and the reference image obtained by projective transformation using the projective transformation matrix. The object detection unit 34 detects an object that exists in an area outside the road plane area. The object relative speed calculation unit 36 calculates a relative speed vector of the extracted object with respect to the vehicle 10. The virtual projection plane image generation unit 38 generates a virtual projection plane (VPP) image (VPP: Virtual Projection Plane) obtained by projecting the extracted road plane area onto a virtual plane parallel to the actual road plane. The own vehicle speed calculation unit 40 calculates the own vehicle speed vector of the vehicle 10 on the VPP image. The object absolute speed calculation unit 42 calculates the absolute speed vector of each object from the relative speed vector of the object with respect to the vehicle 10 and the own vehicle speed vector of the vehicle 10. The display processing unit 44 performs a process of displaying each calculation result on the display, and notifies a warning or the like as necessary based on the calculation result.

  The vehicle 10 and the arithmetic processing device 16 to which the moving body moving amount calculating device of the present invention is applied are basically configured as described above. Next, based on the flowchart shown in FIG. The overall flow of processing by the device 16 will be described.

  First, a standard image and a reference image including a road plane area where the vehicle 10 travels are captured at predetermined time intervals by the standard camera 12 and the reference camera 14, and predetermined image processing is performed in the image processing units 18 and 20. Then, it is stored in the image storage unit 22 as image information for each time (step S0).

  Next, the projective transformation matrix calculation unit 28 uses the initial value parameters stored in the parameter storage unit 26 to perform the two-dimensional projective transformation from the road plane area in the reference image to the road plane area in the reference image. A matrix is dynamically calculated (step S1).

  The plane area extraction unit 32 extracts a road plane area from the reference image using the projective transformation matrix calculated in step S1 (step S2).

  The plane parameter calculator 30 calculates the normal vector of the road plane and the distance from the optical center of the reference camera 12 to the road plane as plane parameters (step S3).

  Next, the object detection unit 34 may convert the road plane area extracted in step S2 onto the road surface using the plane parameter calculated in step S3, and may become an obstacle other than its own vehicle 10. The object is detected by detecting the object or performing stereo measurement on an area other than the road plane area extracted in step S2 (step S4).

  The object relative speed calculation unit 36 calculates a relative speed vector of each object with respect to the vehicle 10 from the position change of each extracted object for each time (step S5).

  Further, the virtual projection plane image generation unit 38 generates a virtual projection plane (VPP) image that is an image obtained by viewing the road surface from above using the normal vector that is the inclination of the road plane calculated in step S3 ( Step S6).

  Then, the host vehicle speed calculation unit 40 calculates the host vehicle speed vector of the vehicle 10 from the position at each time of the VPP image generated in step S6 (step S7).

  Further, the object absolute speed calculation unit 42 calculates the absolute speed vector of each object using the relative speed vector of the object calculated in step S5 and the own vehicle speed vector of the vehicle 10 calculated in step S7 (step S7). S8).

  The display processing unit 44 displays the road plane area extracted as described above, an object that may be an obstacle, the vehicle 10, the absolute speed of the object, and the like on the display. Based on the relative positional relationship and the speed relationship, a warning or the like can be notified as necessary.

  Next, this embodiment will be described in detail step by step.

  First, the principle of road plane region extraction and two-dimensional projective transformation used in the present invention will be described. In the following description, the homogeneous coordinates are expressed by adding a symbol “^” to a symbol in a sentence, and expressed by adding a symbol “˜” above a symbol in a mathematical expression. In addition, a projective-transformed image is represented by adding a symbol “′” to a symbol in a sentence, and represented by adding a symbol “˜” above a symbol in a mathematical expression.

As shown in FIG. 4, when an observation point M1 on a plane (also referred to as a road plane) in the space is projected onto the standard image I _b and the reference image I _r , homogeneous coordinates on the standard image I _b the m ^ _b, the homogeneous coordinates on the reference image I _r When m ^ _r, the homogeneous coordinates m ^ _b, m ^ _r, as shown in equation (1-1), two-dimensional projective transformation It is well known that

（「〜」は、定数倍の不定性を許して等しいものとする。）

("~" Shall be equal by allowing a constant multiple of indefiniteness.)

  Here, H is a 3 × 3 projective transformation matrix. In FIG. 4, O is the optical center of the reference camera 12, O 'is the optical center of the reference camera 14, R is a rotation matrix from the reference camera coordinate system to the reference camera coordinate system, and t is referenced in the reference camera coordinate system. A translation vector from the optical center of the camera 14 toward the optical center of the reference camera 12, d is a distance between the optical center O of the reference camera 12 and the plane Π, and n is a normal vector of the plane Π.

If it can be considered that the area where the road can travel can be considered to be almost a plane in the space, the reference image I _r is subjected to an appropriate projective transformation to match the reference image I _b with respect to the plane portion. An image can be obtained. On the other hand, an object that is not on a plane does not match the image when projective transformation is performed, so that the area of the object can be determined not to be a travelable area or an area that can be an obstacle. Therefore, by calculating the spread of the plane, it is possible to detect a region where the vehicle can travel and an object that becomes an obstacle.

<Step S0>
Image information relating to the reference image I _b and the reference image I _r is acquired.

In order to take an image using the reference camera 12 and the reference camera 14, and to remove differences in brightness between the images, the image processing units 18 and 20 apply a LOG (Laplacian of Gaussian) fill to each image. Further, since the image to which the LOG filter is applied has a low contrast, a histogram flattening process is further performed. The base image I _b and the reference image I _r subjected to these processes and the original image are stored in the image storage unit 22.

<Step S1>
The projection transformation matrix calculation unit 28 dynamically estimates the projection transformation matrix H for the road surface using the standard image I _b and the reference image I _r stored in the image storage unit 22.

That is, the projection transformation matrix H that changes the projection transformation matrix H by a small amount in a certain region R ^I in the reference image I _b and minimizes the evaluation function E (H) of the following equation (1-2) is repeated. Estimate by optimization.

Note that I _b (m _b ) and I _r (Hm _b ) represent the luminance values of the base image I _b and the reference image I _{r at} the homogeneous coordinates m _b and Hm _b .

The above estimation requires a projective transformation matrix H as an initial value appropriately close to a true value and a region R ^I (hereinafter referred to as “calculation region” in the present invention). Therefore, when utilizing the sequence information, by the procedures described below, determining the projective transformation matrix H and calculated area R ^I.

That is, the projection transformation matrix H _S between the base image I _b and the reference image I _{r at} the same time, the projection transformation matrix H _m between two base images I _b acquired at successive times, and the calculation for the base image I _b A region R ^I is obtained. Then, when estimating the projective transformation matrix H (H _s , H _m ) at a certain time t, the estimation result up to the previous time (t−1) is used.

First, a projective transformation matrix H _m (t) between successive reference images I _b (t−1) and I _b (t) is estimated by iterative optimization based on equation (1-2). At this time, as the initial value of the projective transformation matrix H _m (t), the projective transformation matrix H _m estimated before time a (t-1), also as an initial value of the calculation area R ^I is the reference image I _The calculation region R ^I (t−1) obtained at the previous time with respect to _b (t−1) can be used.

Next, by using the obtained projective transformation matrix H _m (t), the predicted value of the calculation region R ^I (t) at the current time is converted by converting the calculation region R ^I (t−1) of the previous time. Ask.

The projection transformation matrix H _S (t) between the reference image I _b and the reference image I _r has the projection transformation matrix H _S (t−1) estimated at the previous time as an initial value, and the projection transformation matrix H _m (t). The calculation region R ^I (t) obtained from the above is used for estimation by iterative optimization based on the equation (1-2).

Thus, when in the continuous estimation using series images, using well the projective transformation matrix H as an initial value close to the true value and the calculation region R ^I, the reference image at time t I _b and the reference image I _r It is possible to stably estimate a dynamic projection transformation matrix H _S (hereinafter referred to as a projection transformation matrix H).

  Next, a method for estimating the projection transformation matrix H by iterative optimization (concentration gradient method) using the expression (1-2) will be described in detail.

  In the density gradient method, one of the two images is changed by a small amount, and the evaluation function representing the degree of overlap (density difference) of the images is minimized to estimate the amount of change in the projective transformation matrix H where the images overlap most. It is a method to do.

_{Assuming that} the luminance value of the reference image I _{r at} the coordinate x (x, y) is I _r (x), the reference image I _r ′ subjected to projective transformation by an appropriate projective transformation matrix H is _expressed by the following equation (1-3): It is represented by Incidentally, "-" on the reference image I _r denotes an image after being projective transformation, "~" on the x represents the homogeneous coordinates of the coordinates x (x, y).

  The projective transformation matrix H is a matrix having 8 degrees of freedom (one parameter is fixed) having 3 × 3 parameters as elements.

（「〜」は、定数倍の不定性を許して等しいものとする。）の関係が得られる。なお、Ｉは３×３の単位行列、Ｄ_Xは射影変換行列Ｈの各パラメータの微小変化量をパラメータとする３×３の微小変化行列とする。 When the eight parameters of the projective transformation matrix H are changed by a minute amount, the coordinate x (x, y) on the reference image I _r ′ obtained by the projective transformation changes to the coordinate x ″ (x ″, y ″). Then

("~" Is assumed to be equal by allowing indefiniteness of a constant multiple). Note that I is a 3 × 3 unit matrix, and D _X is a 3 × 3 minute change matrix whose parameter is the minute change amount of each parameter of the projective transformation matrix H.

（「ｉ」は、画素番号を表す。）
と表すことができる。なお、ｄ_Xは、微小変化行列Ｄ_Xのパラメータをベクトルで表したものである。 When the evaluation function E (H) in the expression (1-2) is rewritten using the relationship of the expressions (1-3) and (1-4), the projective transformation of the reference image I _r ′ and the base image I _b The evaluation function E (H) indicating the overlapping state of the projection transformation matrix H with the parameter of the minute change matrix D _X as a variable

(“I” represents a pixel number.)
It can be expressed as. Here, d _X represents the parameter of the minute change matrix D _X as a vector.

（「≒」は、（１−５）式の一次近似の範囲で等しいものとする。）
となる。 When the equation (1-5) is Taylor-expanded and linearly approximated, the evaluation function E (d _X ) is

(“≈” is assumed to be equal within the range of the linear approximation of equation (1-5).)
It becomes.

を解くことにより求めることができる。 The vector d _X that minimizes the evaluation function E (d _X ) of the expression (1-6) is obtained by differentiating the evaluation function E (d _X ) by the vector d _X and setting the differential value to 0.

Can be obtained by solving

In this case, g _i (luminance gradient) and e _i (luminance difference) are calculated from the base image I _b and the projective transformed reference image I _r ′, and the Jacobian J _dXi is calculated from the coordinates x (x, y). it can. Therefore, the vector d _X can be obtained from the expression (1-7) using the least square method.

Using the vector d _X obtained as described above, that is, the minute change matrix D _X , the projective transformation matrix H is updated according to the following equation (1-8). Note that I is a 3 × 3 unit matrix.

Then, the process of calculating the value of the evaluation function E (d _X ) of the expression (1-6) is repeated again using the projective transformation matrix H obtained by the expression (1-8) to evaluate the evaluation function E (d _X ). The projection transformation matrix H when the value of converges below a predetermined value is defined as a desired projection transformation matrix H.

<Step S2 (first road plane area extracting means)>
In the plane area extraction unit 32, a road plane area in which the vehicle 10 can travel is extracted using the projective transformation matrix H estimated in step S1. This extraction process will be described with reference to FIG.

The projection transformation matrix H estimated in step S1 is applied to the reference image I _r that has been subjected to the LOG filter processing and the histogram flattening processing in step S0, and a projection transformation image I _r ′ is generated.

Next, a difference image (I _r ′ −I _b ) between the reference image I _b subjected to the LOG filter process and the histogram flattening process and the generated projective transformation image I _r ′ is obtained in step S0.

In this case, since the point existing on the plane is accurately projected on the reference image I _b , the luminance value in the difference image (I _r ′ −I _b ) becomes small. On the other hand, the luminance value in the difference image (I _r ′ −I _b ) at a point that is not on the plane increases. Therefore, the road plane area Π _f can be extracted by setting a predetermined threshold value and binarizing the difference image (I _r ′ −I _b ).

<Step S3>
In the plane parameter calculation unit 30, the normal vector n, which is the plane parameter of the plane 法, and the distance d from the optical center O of the reference camera 12 to the plane Π are obtained using the projection transformation matrix H estimated in step S1.

と表される。なお、ｋは、０でない係数であって、画像から得られた射影変換行列Ｈに定数倍の自由度が存在することを表す。内部パラメータＡ_b、Ａ_rが既知であるとすると、次の（３−２）式が成立する。 In the relationship shown in FIG. 4, the rotation matrix R from the reference camera coordinate system to the reference camera coordinate system, the translation vector t from the optical center of the reference camera 14 to the optical center of the reference camera 12, the inside of the reference camera 12 and the reference camera 14 Using the parameters A _b and A _r , the projective transformation matrix H is

It is expressed. Note that k is a coefficient that is not 0, and represents that the projective transformation matrix H obtained from the image has a degree of freedom that is a constant multiple. If the internal parameters A _b and A _r are known, the following equation (3-2) is established.

と書き替えることができる。なお、

である。 Therefore, the singular value decomposition is performed on the projective transformation matrix H ′. U and V are orthonormal matrices, Σ is a diagonal matrix whose elements are singular values σ _{1 to} σ ₃ (σ ₁ ≧ σ ₂ ≧ σ ₃ > 0) of the projective transformation matrix H ′, and the projective transformation matrix H ′ is ,

Can be rewritten. In addition,

It is.

となる。 From the expressions (3-2) and (3-3),

It becomes.

とすると、次の（３−７）式が得られる。 here,

Then, the following expression (3-7) is obtained.

とすると、（３−７）式から得られる３つのベクトル方程式と、ｎ、ｎ′が単位ベクトル、Ｖが正規直交ベクトル、Ｒ′がノルム変化のない回転行列であることから、次の（３−９）式が得られる。 Introducing base vectors (e ₁ , e ₂ , e ₃ ) of the reference camera coordinate system,

Then, since three vector equations obtained from the equation (3-7), n and n ′ are unit vectors, V is an orthonormal vector, and R ′ is a rotation matrix having no norm change, the following (3 -9) is obtained.

となる。 Considering the equation (3-9) as simultaneous equations of n ₁ ′ ² , n ₂ ′ ² and n ₃ ′ ² , the determinant is 0 to have a non-zero solution.

It becomes.

Therefore, the singular values σ _{1 to} σ _{3 of} the projective transformation matrix H ′ are
(I) When there are no multiple solutions (σ ₁ ≠ σ ₂ ≠ σ ₃ )
(II) When there are multiple solutions (σ ₁ = σ ₂ ≠ σ ₃ or σ ₁ ≠ σ ₂ = σ ₃ )
(III) Case with triple solution (σ ₁ = σ ₂ = σ ₃ )
Think separately. In the case of (I) and (II), k = ± σ ₁ or k = ±
When σ ₃ , it contradicts that n ′ calculated from the expression (3-9) is a unit vector. Therefore, in any case, k = ± σ ₂ is obtained. Further, in the case of k = −σ ₂ , the two cameras (the base camera 12 and the reference camera 14) are arranged so as to sandwich the plane, so that k is determined as σ ₂ . Note that the triple solution (III) is excluded because the optical centers of the two reference cameras 12 and the reference camera 14 coincide with each other and only rotate.

として求まる。 In the case of (I) and (II), from the formula (3-9), n ′ is

It is obtained as

を得る。すなわち、Ｒ′は、ｅ₂の回りの回転行列である。したがって、次の（３−１３）、（３−１４）、（３−１５）式を得る。 Since n ₂ ′ = 0 when σ ₁ ≠ σ ₂ ≠ σ ₃ ,

Get. That is, R ′ is a rotation matrix around e ₂ . Therefore, the following equations (3-13), (3-14), and (3-15) are obtained.

となり、

となる。 In the case of (II), as σ ₁ = σ ₂ (or σ ₂ = σ ₃ ), from the equation (3-13),

And

It becomes.

Here, there are four solution candidates due to the difference in the sign of ε ₁ and ε _{2 in} the equation (3-11). Of these four candidates, n ′ that satisfies the condition “a point on the plane appears in the image” is selected to narrow down to two candidates. Further, a condition is uniquely determined by adding a condition that the positional relationship between the base camera 12 and the reference camera 14 does not change during photographing.

  As described above, R ′, t ′ / d, and n ′ calculated by the equations (3-11), (3-13), (3-15), or (3-18) are expressed as (3- By substituting into the equation (6), the rotation matrix R, t / d and normal vector n constituting the plane parameter are obtained. Since the length | t | (distance between the base camera 12 and the reference camera 14) of the translation vector t can be obtained in advance from a design value or an actual measurement value, it is a plane parameter from the obtained t / d. The distance d can be obtained.

  In the plane parameter calculation method (step S3) described above, it is assumed that the rotation matrix R and the translation vector t between the base camera 12 and the reference camera 14 fluctuate, but these are fixed with respect to the vehicle 10. If so, the plane parameter can be quickly calculated by the following method using the rotation matrix R and the translation vector t obtained in advance.

  In this case, the rotation matrix R and the translation vector t can be obtained with high accuracy in an ideal environment in which the vehicle 10 is stopped after the base camera 12 and the reference camera 14 are fixed to the vehicle 10 and then the vehicle 10 is stopped. It is possible to ask for it. Then, when the plane parameter is obtained from the plane projection transformation matrix H calculated at any time such as during traveling, the plane projection transformation is used because the highly accurate rotation matrix R and translation vector t obtained in an ideal environment are used. Even if the matrix H includes an error, it can be calculated without deteriorating the accuracy of the plane parameter.

として求める。並進ベクトルｔの長さ｜ｔ｜（基準カメラ１２及び参照カメラ１４間の距離）は、設計値や実測値から予め求めることができるため、（３−１９）式の単位ベクトルｔ_eに｜ｔ｜を掛けることで、並進ベクトルｔが求められる。 Therefore, first, in a state where the vehicle 10 is stopped, the plane camera 12 and the reference camera 14 capture an image of the plane eyelid, and for example, projective transformation matrix H is estimated by the process of step S1. Then, determine a unit vector t _e of the rotation matrix R and translation vector t with the calculation method of the plane parameters described above. That is, the rotation matrix R is obtained using the equations (3-6), (3-13), and (3-14). Furthermore, (3-6) and (3-15) below, or obtains the t / d using (3-18) wherein the t / d is normalized, the unit vector t _e of the translation vector t ,

Asking. The length of the translation vector t | t | (the distance between the base camera 12 and the reference camera 14), it is possible to determine in advance from the design value and the actual measurement values, (3-19) where the unit vector t _e of | t By multiplying |, the translation vector t is obtained.

  Here, in order to be able to estimate the projective transformation matrix H with high accuracy in the process of step S1, it is preferable to prepare a plane plane having a sufficiently wide area with a pattern that makes it easy to evaluate the degree of image overlap. Further, the rotation matrix R and the translation vector t are obtained by the above method while changing the direction of the plane Π, and the results are averaged to obtain the rotation matrix R and the translation vector t with higher accuracy.

The rotation matrix R and the translation vector t calculated in this way are stored in the parameter storage unit 26. The translation vector t may be stored separately as a unit vector t _e and a vector magnitude | t |. Instead of calculating the rotation matrix R and the translation vector t as described above, the rotation matrix R and the translation vector t are calculated using another calibration method, or are obtained by directly measuring the positional relationship between the reference camera 12 and the reference camera 14. You can also.

  Next, a dynamic projective transformation matrix H is calculated at an arbitrary time such as when the vehicle 10 is traveling (step S1).

の関係が得られる。法線ベクトルｎは単位ベクトル、｜ｔ｜／ｄはスカラであるので、（３−２０）式で算出したベクトルを正規化することで、平面Πの法線ベクトルｎを求めることができる。 (3-2)

The relationship is obtained. Since the normal vector n is a unit vector and | t | / d is a scalar, the normal vector n of the plane plane can be obtained by normalizing the vector calculated by the equation (3-20).

  Further, since the length | t | of the translation vector t is known from the design value and the actual measurement value, the distance d from the optical center of the reference camera 12 to the plane plane is calculated from the following equation (3-21). be able to.

  As described above, by using the rotation matrix R and the translation vector t obtained in advance, the normal vector n and the distance d can be quickly calculated from the projective transformation matrix H estimated by an arbitrary method.

  Further, when the rotation matrix R and the translation vector t between the reference camera 12 and the reference camera 14 are obtained in advance, the projective transformation matrix H and the plane parameters (normal vector n, distance d) are represented by these known parameters. It can be obtained quickly and with high accuracy.

と展開することができる。なお、射影変換行列Ｈは、構成するパラメータから算出されるため、定数倍の不定性を示す係数ｋを１とすることができる。この場合、射影変換行列Ｈの第１項は、並進ベクトルｔが既知であるため定数であり、第２項はｎ／ｄの変数となる。したがって、第２項を推定することで、射影変換行列Ｈを求めることができる。 That is, when rewriting equation (3-1), the projective transformation matrix H is

And can be expanded. Since the projective transformation matrix H is calculated from the constituent parameters, the coefficient k indicating indefiniteness of a constant multiple can be set to 1. In this case, the first term of the projective transformation matrix H is a constant because the translation vector t is known, and the second term is an n / d variable. Therefore, the projection transformation matrix H can be obtained by estimating the second term.

である。なお、ｎ″＝ｎ／ｄとし、その微小変化量をδｎ″とする。 Therefore, let D be the minute change matrix of the projective transformation matrix H when n / d is changed by a small amount.

It is. Note that n ″ = n / d and the minute change amount is δn ″.

（「≒」は、（１−５）式の一次近似の範囲で等しいものとする。）
となる。 When the minute change matrix D is rewritten to the minute change amount δn ″ of n / d, and the evaluation function E (δn ″) corresponding to the expression (1-5) is Taylor-expanded and linearly approximated,

(“≈” is assumed to be equal within the range of the linear approximation of equation (1-5).)
It becomes.

から求めることができる。 The minute change amount δn ″ that minimizes the evaluation function E (δn ″) in the equation (1′-3) is obtained by differentiating the evaluation function E (δn ″) with the minute change amount δn ″ and setting the differential value to zero. In this case, g _i (luminance gradient) and e _i (luminance difference) are calculated from the base image I _b and the reference image I _r , and the Jacobian J _dXi is represented by coordinates x ( x, y), and the Jacobian Jδ _n ″ can be calculated from the internal parameters A _b and A _r of the reference camera 12 and the reference camera 14 and the translation vector t. Therefore, the minute change amount δn ″ is calculated using the least square method.

Can be obtained from

  From the minute change amount δn ″ obtained as described above, the minute change matrix D of the projective transformation matrix H is obtained using the equation (1′-2), and the projective transformation matrix H is expressed by the following equation (1′-5). Update according to

  Further, the minute change amount δn ″ is added to n ″ (= n / d) obtained in the previous calculation, and is updated according to the following equation (1′-6).

  Then, the evaluation function E (δn ″) of the equation (1′-3) is used again by using the projective transformation matrix H obtained by the equation (1′-5) and the n ″ obtained by the equation (1′-6). The process of calculating the value of is repeated, and the projection transformation matrix H when the value of the evaluation function E (δn ″) converges to a predetermined value or less is set as a desired projection transformation matrix H.

として求めることができる。 Further, from n ″ when the projective transformation matrix H converges, the normal vector n and the distance d which are plane parameters are:

Can be obtained as

  Thus, by using the rotation matrix R and the translation vector t obtained in advance, the projection transformation matrix H can be estimated quickly and with high accuracy, and the normal vector n and the distance d are immediately calculated from the projection transformation matrix H. Can do.

<Step S4>
Step S4 is a process for detecting an object, and includes a subroutine process of steps S41 to S46 shown in FIG.

  In step S41, the three-dimensional region is measured in stereo by the base camera 12 and the reference camera 14, and the world coordinates Cw of each edge point of the object are obtained.

  In step S42, object candidate points are extracted based on the height from the plane based on the height coordinate Yw (see FIG. 7) of the world coordinates Cw. Specifically, as shown in FIG. 7, candidate points (Tmin ≦ Yw <Tmax) present in a space whose height from the plane is Tmin (for example, 20 cm) or more and less than Tmax (for example, 2.5 m). ). As a result, at least a part of the pedestrian bridge provided on the road, the entrance of the tunnel, the signboard, etc. is excluded, and only parts that can be related to the traveling of the vehicle 10 such as other vehicles and guardrails can be easily extracted. . Thereby, the candidate point Pa of FIG. 7 is extracted, and the candidate point Pb is excluded.

  Further, candidate points may be extracted by limiting the Xw direction, which is the vehicle width direction, and the Zw direction, which is the front-rear direction, within a predetermined range. As a result, it is possible to prevent unnecessary voting processing from being performed on a portion where the vehicle 10 is not likely to pass through, thereby reducing the amount of calculation. In this case, for example, as shown in FIG. 8, the x direction may be limited to −10 to 10 m, and the z direction may be limited to a range of 0 to 40 m.

  In step S43, each candidate point of the object is projected in units of cells (cells) of a certain size provided on the voting surface 50 shown in FIG. 8 according to the Xw and Zw coordinates. The voting plane 50 is represented as a coordinate plane of coordinates Xw and coordinates Zw (see FIG. 16) that are two orthogonal axes in the world coordinate system. At this time, when a plurality of candidate points are projected onto the cell of the same object, the overlap number is recorded. In this way, the process of projecting candidate points while considering the overlapping number is called voting, and the overlapping number is called the number of votes. In the voting process, points less than Tmin and points greater than or equal to Tmax are excluded in advance in step S42, so the number of votes of points corresponding to footbridges, tunnel entrances, signboards, etc. is 0 or a sufficiently small value. Become. The voting surface 50 shown in FIGS. 8 and 9 is a schematic diagram for explanation, and is not associated with other FIG. 5 and the like.

  In step S44, a clustering process is performed for grouping the candidate points voted on the voting surface 50 into adjacent groups. A group divided by the clustering process is called a cluster. As the clustering process, the nearest distance method can be cited, and the width (coordinate Xw) and the depth (coordinate Zw) are preferably used as the feature quantities used in the shortest distance method. According to such clustering processing, a model of a detection object is not necessary, and objects of various shapes can be detected.

  By the clustering process in step S44, for example, as shown in FIG. 9, three clusters 52a, 52b and 52c are obtained on the voting surface 50.

  Note that the expansion / contraction process may be performed on the cluster after performing the clustering process for the purpose of removing noise due to a correspondence search error (matching error) in stereo measurement. If there is a matching mistake when measuring an object in stereo, an error occurs in the measured three-dimensional position, so that the measured object shape has irregularities (noise), and the cluster shape may differ due to this noise. . In the expansion / contraction process, the uneven portions are connected by expansion and returned to the original size by contraction, so that it is possible to obtain a shape that suppresses the influence of noise.

  In step S45, it is determined whether each cluster 52a, 52b, 52c divided by the clustering process is an object or noise, and the cluster due to noise is removed.

  Specifically, first, the total number of votes is obtained for each of the obtained clusters 52a, 52b and 52c. That is, the total number of votes for the cluster 52a is 38, the total number of votes for the cluster 52b is 21, and the total number of votes for the cluster 52c is 3. Further, the threshold Thz based on the value Zg of the coordinate Zw that is the depth of the center of gravity g on the area in each of the clusters 52a, 52b and 52c is compared with the total number of votes, and the total number of votes is equal to or less than the threshold Thz. Is removed as noise. The threshold Thz is expressed by the following equation (4-1).

  Here, Kg is a coefficient, and is, for example, the minimum total number of votes when an object is present 1 m ahead. Equation (4-1) is set based on the phenomenon that the area of the object projected in the image becomes smaller in inverse proportion to the square of the distance.

  By such processing in step S45, for example, the cluster 52c having a small total number of votes is removed as noise. Also, objects with a small total number of votes, such as footbridges, tunnel entrances, and signboards, are also removed as noise.

  The voting surface from which the noise has been removed is shown as data closer to the actual in accordance with the example of FIG. 5, as shown in FIG. 10, and this voting surface 50 is recorded in a predetermined storage unit. As is apparent from FIG. 10, the voting surface 50 includes a cluster 54a corresponding to a truck whose lane is changed relatively near ahead, a cluster 54b corresponding to a passenger car traveling ahead of the truck, and a guardrail. A corresponding cluster 54c is extracted. In addition, the voting surface 50 schematically shows the coordinates Xw, the coordinates Zw, and the vehicle 10 that is the host vehicle for the designer's confirmation.

  In step S46, the remaining clusters 54a to 54c are recognized as objects, and the corresponding object position areas are cut out and stored in a predetermined storage unit.

  The object extraction process in step S4 (see FIG. 3) includes such steps S41 to S46. Each step will be described in more detail below.

  Step S41 includes a subroutine process of steps S411 to S417 shown in FIG.

Step S411 in (reference image feature point extracting means), it performs a Sobel filtering process for extracting edges and feature points from the reference image I _b. This Sobel filter process is a filter process for image contour extraction, which is a process of multiplying the neighboring pixels for each pixel by a predetermined coefficient, and summing the results, and in the vertical and horizontal directions. This processing is performed using two coefficient matrices.

  That is, an object whose surface is specularly reflected, such as an automobile, changes in reflected light depending on the viewing angle, and therefore the brightness differs even if the same point is observed in the left and right images. For this reason, region-based matching between the left and right images may not be successful, and stereo distance measurement may be inaccurate. Therefore, the distance is measured by performing stereo matching only for the edge portion without performing stereo matching for the textureless portion or the specularly reflected portion. Therefore, first, Sobel filtering is performed on the input image to extract edges and feature points, and then binarized to create an edge image 60 shown in FIG. As is apparent from FIG. 12, the edge image 60 at this time also extracts a portion unrelated to the object detection process such as a white line on the road surface.

  In FIG. 12, the edge image 60 is schematically shown for convenience of explanation, and actually, image data as shown in FIG. 13 is obtained.

In step S412, since the edge on the road plane such as a white line is not a detection target, the portion of the plane area Π _f (see FIG. 5) obtained by the road plane area extraction process (step S2) is removed from the edge image 60. Process. Specifically, a binary exclusion process is performed on the edge image 60 and the planar region Π _f to obtain a corrected edge image 62 shown in FIG. At this time, it is clear that there is no detection target in the upper predetermined width region, the lower bonnet region, or the like in the edge image 60, and furthermore, these regions have fixed positions on the edge image 60. These portions are also removed by a predetermined area designating means.

  Even if the plane portion cannot be completely removed from the edge image 60, the processing is limited to the space through which the vehicle 10 passes as will be described later, so that it is not erroneously detected as a detection target.

In step S413, it sets the epipolar line EP as a search range on the reference image I _r corresponding to each edge point remaining on the corrected edge image 62 (see FIG. 15). Epipolar line EP as a straight line including the points corresponding to the reference image I _r of the edge points on the corrected edge image 62, the internal parameters of the reference camera 14, the internal parameters of the reference camera 12, the reference camera 14 and the reference camera 12 Is calculated based on the rotation matrix R and the translation vector t, which will be described in detail later.

In step S414, a reference plane region Π _fr on the reference image I _r is obtained by multiplying the plane region 逆_f obtained by the road plane region extraction process (step S2) by the inverse matrix of the projective transformation matrix to obtain the reference image I _r. Project above.

In step S415, performs a process of removing part of the reference plane area [pi _fr from the epipolar line EP on the reference image I _r, epipolar line EP is limited to non-planar region.

In step S416, the small area in the reference image I _b corresponding to the edge point, it performs area-based matching process on the epipolar line EP in the reference image I _r. This region-based matching process creates a small matching window centered on the edge point in the base image I _b and sequentially calculates the similarity between the matching window and the small window on the epipolar line EP in the reference image I _r . This is the process to be performed. A point having the highest similarity is obtained as a corresponding point (window) of the edge point by the region-based matching process.

At this time, first, region-based matching processing is performed from the base image I _b to the reference image I _r , the calculated similarity is compared with a threshold value, and when it is determined that the matching is successful, this time, from the reference image I _r It performs region based matching to the reference image I _b, if they match the terms of the previous reference image I _b, it is determined that the matching is established. By performing such region-based matching, matching errors due to repeated patterns can be suppressed. The threshold value used here may be determined by providing a first threshold value for the maximum similarity value and a second threshold value based on the difference between the maximum similarity value and the second maximum similarity value.

For the calculation of the similarity, for example, a normalized cross correlation function (ZNCC) can be used as the similarity function. Since the normalized cross-correlation function calculates the pattern similarity, even if there is an overall luminance offset shift between the base image I _b and the reference image I _r , a matching error is unlikely to occur. The normalized cross correlation function is obtained by the following equation (4-2).

Here, I _b and I _r indicate a standard image and a reference image, respectively, and R indicates a window area. X indicates the position of the pixel in the window. U and v represent the standard positions of the standard image and reference image windows, respectively.

Further, based on the types of the reference image I _b and the reference image I _r , the required calculation speed, and the like, other similarity functions (SSD (Square Residual Sum), NSSD, SAD (Absolute Value Sum), etc.) are used. Also good.

Region-based matching process in step S416, only the edge points of the portion where the plane area [pi _f is removed with respect to the reference image I _b is processed, whereas, with respect to the reference image I _r reference plane of the epipolar line EP Only the part from which the region Π _fr is removed is processed. Therefore, the calculation load is greatly reduced, and a point in the planar area is not detected. In addition, since the calculation load is reduced, the hardware for calculation can be reduced.

In step S417 (first object position calculating means), based on the established criteria image I _b on the point (window) and the point on the reference image I _r of region-based matching (window), the principle of triangulation The camera coordinate Cc of the corresponding point is calculated by the stereo measurement based on it.

In step S418, the camera coordinates Cc are converted into world coordinates Cw based on the vehicle 10 as the vehicle. Specifically, as shown in FIG. 16, the camera coordinate system is represented as orthogonal coordinates Xc, Yc, Zc including the optical center of the reference camera 12 as the origin and the optical axis as the coordinate axis Zc. In the world coordinate system, the coordinate axis Yw is represented in the normal vector n direction, and the coordinate axes Xw, Zw are represented as an orthogonal coordinate system Xw, Yw, Zw provided on a plane surface. The coordinate axis Zw is the vehicle front-rear direction, and the origin is the specific position of the vehicle (a position on the plane) used as a reference when determining the object position with respect to the vehicle. Further, a vector from the origin of the world coordinate system to the origin of the camera coordinate system is expressed as [twx, twy, twz] ^t = tw (twy is equal to the distance d). When each parameter is expressed in this way, the world coordinate Cw is expressed by the following equation (4-3).

Here, when the normal vector n is expressed by the equation (4-4), the rotation matrix R _w is expressed by the equation (4-5).

  In order to simplify the description, in the flowchart of FIG. 11, the processing of step S413 to step S418 is shown in series, but in practice, step S413 to step S413 for each edge point represented in the corrected edge image 62. Step S418 is executed to obtain the corresponding world coordinates Cw.

  In this way, each edge point on the corrected edge image 62 can be represented as a depth image 64 as shown in FIG. 17, for example, based on the coordinate axis Zw that is the depth. In the depth image 64, a point having a size corresponding to the value of the coordinate axis Zw (or the distance from the origin of the world coordinate system) is shown. Each point may be represented by a gradation-like point whose color changes according to the value of the coordinate axis Zw. The depth image 64 is provided for checking the operation of the designer and may not be provided in practice. However, the world coordinates Cw necessary for generating the depth image 64 are recorded in a predetermined storage unit. Keep it.

As described above, according to the process of step S41 (step S411～S418), based on the reference image I _b and the reference image I _r, it is possible to determine the world coordinates Cw for each object edge points. At this time, since the process for the portion excluding the plane area [pi _f, the reduction of computational load is achieved. Among these, steps S413 to S416 can be referred to as reference image corresponding unit search means.

<Step S5>
Step S5 is a process for calculating the relative speed of the object, and includes a subroutine process of steps S51 to S55 shown in FIG.

In step S51, it calculates the overlap ratio Rate of the object region from the reference image _{I b} in successive times.

  In step S52, the center-of-gravity movement distance Lg of the object at successive times is calculated.

  In step S53 (same object determination means), the same object is determined by threshold determination based on the overlap ratio Rate and the gravity center moving distance Lg.

In step S54, it obtains a position vector P _t recalculated to determine the position of the object with high precision.

In step S55, on the basis of the position vector P _t, and calculates the object relative velocity vector V _obst-rel.

Step S5 (see FIG. 3) is composed of such steps S51 to S55. Each step will be described in more detail below. In the description of the steps S51 to S55, among the object detected, but the processing for the reference image I tracks in _b 70 (see FIG. 5) will be exemplified, passenger 72 Ya is the other detected object Similar processing is performed for the guard rail 74. Step S51 includes a subroutine process of steps S511 to S517 shown in FIG.

In step S511, the template 80a stored in step S517 at the previous time is called from a predetermined storage area. Here, the template 80a is an image in which the image of the reference image _Ib of the track 70 corresponding to the cluster 54a on the voting surface 50 (see FIG. 20) is specified and recorded, for example, a height of 1.5 m from the plane. It is recorded as the area.

Conceptually, as shown in FIG. 20, the point P0 is a point on the voting plane 50, and is a point that scans from the optical center O at a constant angle and hits the cluster 54a. World coordinates Cw can be calculated based on the point P0. Point Pr has the same width position and depth position as point P0, and is a point on the road plane. Since the point Pr is a point on the road plane, the height is zero. The point Px has the same width position and depth position as the point P0 and has a height of 1.5 m from the road plane ridge. Next, it converts into the camera coordinate Cc using the points Pr and Px. After conversion to the camera coordinate Cc, it may be converted into coordinates on the reference image I _b with a perspective projection matrix. In this way, the template 80a corresponding to the cluster 54a at the previous time is obtained and recorded. Similarly, the template 80b and the template 80c shown in FIG. 21 are also recorded for the cluster 54b of the previous time corresponding to the passenger car 72 and the cluster 54c of the previous time corresponding to the guardrail 74.

  Further, since the height of each object is obtained in step S418, the heights of the templates 80a, 80b, and 80c may be set according to the obtained object height.

In step S512, it reads out the reference image I _b acquired at step S0 in the main routine processing of the current time. Here, it is assumed that an original image before performing processing such as LOG filter and histogram flattening is used.

In step S513 (template matching unit), with each template 80a, template matching is performed based on the texture on the reference image I _b acquired to the current time, moving each template 80a to the highest point of the similarity. As a specific method of template matching, there is a matching using the above-described ZNCC (4-2) formula, SSD, SAD or the like.

  In step S514, the cluster 54a obtained at the current time is called based on the data recorded in step S46.

  In step S515 (object position area calculating means), an object position area 82a corresponding to the called cluster 54a is obtained. The object position area 82a is obtained by the procedure described in step S511. The object position area 82a may be determined based on the three-dimensional spatial position of each edge point in the depth image 64 as shown in FIG. 17 in addition to the method based on the planar cluster 54a. Good.

  In step S516 (overlap ratio calculating means), the overlap ratio Rate between the template 80a moved in step S513 and the object position area 82a obtained in step S515 is calculated by the following equation (5-1).

Here, R _t-1 is the area of the moved template 80a, and R _t is the corresponding object position area 82a. Further, ∩ is an operator that indicates the overlapping portion of the regions, and min (R _t−1 , R _t ) is a function that selects _one of R _t−1 and R _t having a small area.

According to equation (5-1), Rate = 1 when the moved template 80a and the object position area 82a completely match, and Rate = 0 when there is no matching portion. Further, conceptually, as shown in FIG. 21, the ratio of overlapping portions R _t-1 ∩R _t against R _t indicating the R _t-1 and the object position area showing the template on the reference image I _b Will be asked.

  When the overlapping rate Rate is equal to or greater than a predetermined threshold Tha (0 <Tha <1), the corresponding cluster on the voting surface 50 at the previous time and the cluster on the voting surface 50 at the current time are based on the same object. Temporarily determined and tracked. Tracking is a process of associating clusters obtained at different times, and thereby the motion of the object can be detected.

  In step S517, the object position area 82a obtained in step S515 is recorded in a predetermined storage unit as a new template 80a. This new template 80a is called in step S511 in the next process as described above.

  Thus, in step S51 (steps S511 to S517), template matching is performed based on the template 80a, and double tracking processing is performed to obtain the object position area 82a from the cluster 54a of the voting surface 50. The reliability of tracking processing is increased by obtaining the overlapping rate Rate of these results. Actually, the tracking association obtained in step S51 is finally confirmed in step S53.

  Next, the processing content of step S52 is demonstrated. Step S52 includes a subroutine process of steps S521 to S524 shown in FIG.

  In step S521, the cluster 54a obtained at the current time is called based on the data recorded in step S46.

In step S522 (object center-of-gravity position calculating means), the center of gravity g _T at the current time of the cluster 54a is obtained and recorded on the voting surface 50 as shown in FIG.

In step S523, the center of gravity g _O of the previous time of the cluster 54a recorded when step S533 was executed at the previous time is read.

In step S524 (object center-of-gravity movement amount calculation means), as shown in FIG. 10, the center-of-gravity movement distance Lg between the center of gravity g _O and the center of gravity g _T of the cluster 54a is calculated.

As described above, in step S52 (steps S521 to S524), the center-of-gravity moving distance Lg between the center of gravity g _{O at} the previous time and the center of gravity g _{T at the} current time of the cluster 54a is obtained, which is reasonable as a short-time moving distance. It is possible to associate clusters of things. Actually, the tracking association obtained in step S52 is finally confirmed in the next step S53.

  Next, the processing content of step S53 will be described. Step S53 includes the subroutine processing of steps S531 to S534 shown in FIG.

  In step S531, for each corresponding object, the overlap ratio Rate obtained in step S516 is compared with the threshold value Tha. If Rate ≧ Tha, the process proceeds to step S532, and if Rate <Tha, the process proceeds to step S534.

  In step S532, the gravity center moving distance Lg obtained in step S524 is compared with the threshold Thb for each corresponding object. If Lg ≦ Thb, the process proceeds to step S533, and if Lg> Thb, the process proceeds to step S534. This threshold value Thb is set as a fixed value as a reasonable distance that the object can move in the processing calculation period in consideration of the traveling speed of the host vehicle and the traveling speed of the oncoming vehicle.

In step S533, the two associated objects are confirmed and recorded as the same object and recorded. Further, the center of gravity g _o is replaced as g _o ← g _T and stored in a predetermined storage unit.

  On the other hand, in step S534, the two associated objects are recognized as being different objects. In this case, a registration process for recognizing an object that has not been associated as a new object that has entered the imaging range based on the context of the image or a predetermined deletion process as an object that has left the imaging range is performed. Do.

  As described above, in step S53 (steps S531 to S534), it is confirmed whether or not the two objects associated in the processing up to step S52 are the same object based on the overlapping rate Rate and the gravity center moving distance Lg. Can be performed more reliably.

  In addition, although this step S531-S534 was demonstrated as what is performed with respect to two objects temporarily matched beforehand, it is performed brute force with respect to each object of the previous time detection, and each object of the present time detection. The association may be performed.

Further, the threshold value Thb used in step S532 has been described as a fixed value, but the threshold value Th (Zg) as a function that changes based on the distance Zg in the coordinate Zw direction from the vehicle 10 to the center of gravity g _O is determined as a threshold value. You may use for. In this case, if the distance in the coordinate Zw direction to the center of gravity g _{O of} an object is Zg1, the threshold value is expressed as Th (Zg1). Similarly, the threshold Th (| V _V |) as a function that changes based on the vehicle speed | V _V | may be used for threshold determination.

  Next, the processing content of step S54 and step S55 is demonstrated. Step S54 includes steps S541 to S544 illustrated in FIG. 24, and step S55 includes steps S551 to S553 illustrated in FIG.

In step S541 (target point determination means), it identifies the attention point Pc of the object in the reference image I _b. This attention point Pc is set in the region of the template 80a moved by template matching in step S513, and for example, if it is specified as a characteristic point such as the center point of the template 80a or the corner of the object, the attention point Pc is always on the object. It is specified at substantially the same point.

In step S542, it sets the epipolar line EP that is corresponding to the attention point Pc on the reference image I _r. At this time, since the approximate distance to the track 70 can be calculated by using the cluster 54a, the epipolar line EP can be set as short as necessary based on this.

A method for calculating the approximate distance of the attention point Pc using the cluster will be described. First, the Zw coordinate is obtained from the center of gravity g _T (see FIG. 10) of the cluster 54a. This is temporarily set as the Zw coordinate of the target point Pc, and the corresponding Xw coordinate is calculated by substituting it into the following equation (5-2) together with the image coordinates ( _upc , v _pc ) of the target point Pc. Here, λ represents a constant multiple degree of freedom.

  A straight line passing through the position (Xw, Zw) on the voting surface 50 thus obtained and the position (twx, twz) of the reference camera is set, and the point of the cluster 54a is searched along the straight line, and is closest to the vehicle 10 The Zw coordinate of the point may be the approximate distance of the point of interest Pc.

In step S543, with respect to a point on the reference image I _b which correspond to similar attention point Pc and the step S416, it performs area-based matching process on the epipolar line EP in the reference image I _r.

In step S544 (second object position calculating unit), similarly to the step S417, on the basis of the point on the established reference image I _b and reference point on the image I _r matching, corresponding points by stereo measurement Camera coordinates Cc are calculated. Further, as in step S418, the obtained camera coordinates Cc are converted into world coordinates Cw. World coordinates Cw is the position of the determined object, as shown in FIG. 25, represented as a two-dimensional position vector P _t on the voting plane 50.

Next, in step S551 in FIG. 26, the position vector P _t−1 detected at the previous time of the object is read out. This position vector P _t-1 is recorded in a predetermined storage unit in step S553 at the time of previous time processing.

In step S552 (object movement vector calculation means), the relative velocity vector (movement vector) V _obst-rel of the object is calculated by the following equation (5-3).

Here, Δt is an operation cycle of processing, and a relative velocity vector V _obst-rel is obtained as a change in the position vector P _t per unit time.

In step S553, in order to provide the next processing, and records in the storage unit the position vector P _t by substituting the P _t-1 ← P _t. Position vector P _t to be recorded here, with respect to object position area 82a which stores as a new template 80a in step S517, it is obtained by updating performed similarly to step S541～S544.

According to the processing in steps S54 and S55, since the epipolar line EP is set sufficiently short, the frequency of occurrence of a matching error in the region-based matching in step S543 is sufficiently low, and the object distance is further increased. It can be calculated accurately. In addition, since the point of interest Pc is always set at substantially the same point on the object, the relative velocity vector V _obst-rel indicating the relative movement of the object with time including the movement in the width direction (coordinate Xw direction) is accurately _calculated. Can be calculated.

As a result, vectors Vr1, Vr2, and Vr3, which are relative velocity vectors V _obst-rel of the truck 70, the passenger car 72, and the guard rail 74, are displayed as vectors as shown in FIG. As is clear from FIG. 25, it is determined from the vector Vr1 that the truck 70 has changed its course in the left direction, and from the vector Vr2, the passenger car 72 travels forward at a slightly higher speed than the vehicle 10. It is determined that Further, it is determined from the vector Vr3 that the guard rail 74, which is a stationary object, is moving in the direction opposite to the traveling direction of the vehicle 10. In addition, the own vehicle speed vector V _V in FIG. 25 is obtained in steps S6 and S7.

Next, an epipolar line setting method will be described. To a stereo 3-dimensional measurement is required to determine the position of the observation point M1 reflected in the position and the reference image I _r of the observation points M1 appearing in the reference image I _b, respectively. As shown in FIG. 27, the observation point M1 is projected to a point m _b on the base image I _b, the point m _b existing on the straight line Le passing through the optical center O of the observation point M1 and the reference camera 12. To look for points on the straight line Le, can find observation points M1, linear Le projected onto the reference image I _r becomes epipolar line EP. That is, the reference image I _b observation point projected on the M1 and the reference image I _r to give the support, so that the epipolar on line EP in a reference image I _r may be searched. Incidentally, the point M0 and M2 in FIG. 27 is a point on the line Le A in other respects to be projected to the same point m _b.

  The relational expression of this epipolar constraint is obtained. A relationship of Xr = R · Xb + t is established between the reference camera coordinate Xr and the standard camera coordinate Xb using the rotation matrix R and the translation vector t.

となる。 At this time, the normalized image coordinates X ^ ba of the standard camera 12, the normalized image coordinates X ^ ra of the reference camera 14, and the translation vector t exist on the same plane. That is, since the scalar triple product is zero,

It becomes.

と表すことにする。ただし、ｔ１、ｔ２、ｔ３は並進ベクトルｔの要素を示す。ここで、正規化画像座標と画像座標との関係より、ｍ＾_b＝Ａ_bＸ＾ｂａ、ｍ＾_r＝Ａ_rＸ＾ｒａを代入して整理すると、基礎行列（Ｆｕｎｄａｍｅｎｔａｌ Ｍａｔｒｉｘ）Ｆは次の（５−６）式及び（５−７）式で表される。 However,

It will be expressed as Here, t1, t2, and t3 indicate elements of the translation vector t. Here, the relationship between the normalized image coordinates and image _{coordinates, m ^ b = A b X} ^ ba, and rearranging by substituting _{m ^ r = A r X ^} ra, the fundamental matrix (Fundamental Matrix) F following (5-6) and (5-7).

となり、参照画像Ｉ_r中における観測点Ｍ１に対応したエピポーララインＥＰとなる。ここで、「’」は参照画像であることを示し、ｌ₁’，ｌ₂’，ｌ₃’は参照画像のエピポーララインＥＰのパラメータを示す。また、ｕ’、ｖ’は参照画像の画像座標を示す。 (5-7) by equation when the position observation points M1 space is projected in the reference image I _b was m, 'When = Fm ^, m ^' l than l '= 0, the reference image I _r In

Next, the epipolar line EP corresponding to the observation points M1 in the reference image I _r. Here, “′” indicates a reference image, and l ₁ ′, l ₂ ′, and l ₃ ′ indicate parameters of the epipolar line EP of the reference image. U ′ and v ′ indicate image coordinates of the reference image.

  When the approximate position of the observation point M1 is known, the epipolar line EP can be set short by limiting the length of the straight line Le.

となり、観測点Ｍ１における基準画像Ｉ_bにおけるエピポーララインＥＰとなる。なお、このエピポーララインＥＰの説明で用いたパラメータｕ及びｕ’は、後述するベクトルｕ及びｕ’とは異なるものである。 Assuming that the position projected on the reference image I _r is m ′, if l = F ^t m ′, since l ^t m = 0, in the reference image I _r ,

Thus, the epipolar line EP in the reference image I _{b at} the observation point M1 is obtained. The parameters u and u ′ used in the description of the epipolar line EP are different from the vectors u and u ′ described later.

<Step S6>
This step S6 is a process for generating a VPP image in which the road plane plane is virtually viewed from above in order to obtain the own vehicle speed vector V _V (see FIG. 25) of the vehicle 10 as the own vehicle. It consists of the subroutine processing of step S61 to step S63 shown in FIG. That is, in step S61, a conversion formula _HVPP for converting the reference image _Ib into a VPP image (hereinafter also referred to as VPP conversion) is calculated. Thereafter, in step S62, the reference image I _b is converted into a VPP image I _V using the obtained conversion formula H _VPP and recorded (see FIG. 33). Further, in step S63, the plane area [pi _f and records the VPP transformed using the same transformation equation H _VPP (see FIG. 34). Hereafter, each step S61-S63 is demonstrated in detail.

A virtual camera that captures the VPP image _IV is assumed, and this is called a virtual camera for the sake of explanation.

First, in step S611 in FIG. 29, the inverse matrix A _b ⁻¹ of the internal parameter A _b of the reference camera 12 is read. The internal parameter _Ab is expressed by the following equation (6-1).

Here, α ₁ , β ₁ , s ₁ , C _X1 and C _Y1 are constants determined by calibration of the reference camera 12.

  In step S612, a normal vector n and a distance d (see FIG. 16), which are position and orientation parameters with respect to the road plane ridge, are read.

In step S613, using the normal vector n and the distance d, a rotation matrix R _n that matches the vector e _oz (see FIG. 30), which is the optical axis of the reference camera 12, with the normal vector n is _expressed by the following (6-2). ) To (6-5).

These formulas (6-2) to (6-5) will be described with reference to FIG. 30 as well. A portion surrounded by vertical double lines in the formula represents a norm, and a vector e _oz represents a camera coordinate system. The unit vector of the coordinate Zc (refer FIG. 16) which is an optical axis in FIG. Further, c _n is a unit vector perpendicular to the normal vector n and the vector e _oz , and the angle θ _n is an angle formed between the normal vector n and the vector e _OZ . Note that vectors e ′ _OX , e ′ _OY, and e ′ _OZ in FIG. 30 are orthogonal unit vectors indicating the virtual camera coordinate system, and a chain line indicates the front-rear direction of the vehicle 10.

In step S614, the projection distance, scale, and image center are specified, and the internal parameter A _VPP of the virtual camera when the road plane plane is viewed from above is set. The internal parameter A _VPP is a matrix for converting the image on the virtual camera coordinates onto the image coordinates, and is represented by the following equation (6-6).

Here, α _VPP = d _VPP · k _X−VPP and β _VPP = d _VPP · k _Y−VPP . D _vpp is a projection distance [m], and k _x−vpp and k _y−vpp are scales [pixel / m] in the X coordinate direction and the Y coordinate direction of the VPP image _IV . Further, C _x-vpp and C _y-vpp are the X coordinate and Y coordinate of the VPP image I _V center.

When the distance d, which is a road plane parameter, is designated as the projection distance by the internal parameter A _VPP , the projected image is projected onto the detected road plane Π.

In step S615, a rotation matrix R _V that is a conversion formula for rotating the image by an angle θ _V and an angle −φ is calculated in order to make the vehicle longitudinal direction coincide with the vertical axis of the VPP image. That is, a unit vector e _OZ in the optical axis direction of the reference camera 12 is set as a vector u viewed from the VPP camera coordinate system based on the equation of FIG. 30 and (6-7), and the vector u is set as e ′ in the virtual camera coordinate system. A vector u ′ orthogonally projected onto the _OX , e ′ _OY plane is set based on the equation (6-8).

When the vectors u and u ′ are defined in this way, the angle θ _V is an angle formed by the vector u ′ and the virtual camera coordinates e ′ _oy and is expressed by Expression (6-9).

Further, the angle φ is an angle formed with the vector e _OZ that is the camera optical axis with respect to the vehicle longitudinal direction, and is obtained in advance. The rotation matrix R _{V to be obtained} is expressed by the following equation (6-10).

In step S616, a conversion formula H _VPP for VPP conversion is calculated based on A _b ⁻¹ , R _n , R _V and A _VPP . The conversion formula H _VPP is expressed by the following formula (6-11). Note that R _VPP = R _V R _n .

That is, in the conversion formula H _VPP , a process equivalent to converting the reference image I _b sequentially with the matrices A _b ⁻¹ , R _n , R _V , and A _VPP is performed. Actually, conversion can be performed at one time with one conversion formula H _VPP , but each matrix A _b ⁻¹ , R _n , R _V , and A _VPP can be easily understood conceptually. The conversion by will be described.

First, when the reference image I _b (see FIG. 5) is converted by the inverse matrix A _b ⁻¹ , conversion from the image coordinates to the camera coordinate system is performed, and the camera coordinate system reference image I _bc shown in FIG. can get.

Next, when this camera coordinate system reference image I _{bc is} converted by the rotation matrix Rn, the camera coordinate system reference image I _bc is rotated by an angle θn and becomes parallel to the road plane ridge and the distant portion is enlarged. A first converted image I _V1 is obtained. As shown in FIG. 32, the first converted image I _V1 is obtained as an image inclined by an angle (θ _V −φ) with respect to the vertical axis.

Further, when the first converted image I _{V1 is} converted by the rotation matrix R _V , the first converted image I _V1 is rotated by an angle (θ _V −φ) so that the longitudinal direction of the vehicle coincides with the vertical axis of the VPP image. Thus, the second converted image I _V2 is obtained.

Finally, when the second converted image I _{V2 is} converted by the internal parameter A _VPP of the virtual camera, scale conversion is performed to obtain a VPP image I _V (see FIG. 33) having a desired size.

Such a series of the determinant _{A VPP R V R n A b} -1, transformation equation H _VPP is configured, the reference image I _b VPP image by performing VPP conversion by the conversion equation H _VPP with respect to I _V (see FIG. 33) is obtained. As described above, this VPP conversion is performed in step S62. The obtained VPP image _IV is recorded in the storage unit.

Note that the VPP image I _V is set according to the setting of the internal parameter A _VPP , as shown in FIG. 33, the upper left portion of the image is the origin, the vehicle width direction is the X direction (right direction in FIG. 33), and the vehicle longitudinal direction. Is defined as the Y direction (downward direction in FIG. 33).

As can be understood from FIG. 33, the VPP image _IV is an image as seen from a high distance above the road plane ridge, and the width of the road is projected to be substantially constant. Furthermore, the track 70 and passenger vehicle 72 on base image I _b is the image upper portion is projected distort to expand.

Thereafter, in step S63, VPP conversion is performed on the plane area Π _f (see FIG. 5) by the conversion formula H _VPP , and the VPP plane image I _Vf that is the plane area Π _f viewed from above (see FIG. 34). ) To record. This VPP plane image I _Vf indicates the area of the road plane plane in the VPP image I _V. The obtained VPP plane image I _Vf is stored in the storage unit. This VPP plane image I _Vf has the same shape as the road plane, and in the example shown in FIG. 34, there are non-horizontal structures such as other trucks 70 and roadside portions ahead, and planes that are not at the same level as the road plane. The substantially V-shaped shape is excluded.

<Step S7 (moving body speed calculating means)>
Step S7 is a process for obtaining the vehicle speed vector V _V (see FIG. 25) of the vehicle 10 based on the VPP image I _V and the VPP plane image I _Vf obtained in step S6, and is shown in FIG. It consists of the subroutine process of step S71-step S77. In the following description, the VPP image I _V and the VPP plane image I _Vf obtained by the processing at the current time are represented as a VPP image I _V (t) and a VPP plane image I _Vf (t), and are obtained at the previous time. These are distinguished from each other by representing them as a VPP image I _V (t−1) and a VPP plane image I _Vf (t−1). In the world coordinate system, the front-rear direction of the vehicle 10 is defined as the coordinate Zw (see FIG. 10). As described above, in the VPP image _IV , the front-rear direction is defined as the Y-axis. In the description relating to S7, the parameter corresponding to the front-rear direction of the vehicle 10 is represented with the suffix “Y” or “y”.

In step S71, the VPP image I _V (t−1), the VPP plane image I _Vf (t−1), and the image matching movement amounts ΔX ₀ , ΔY ₀ , Δθ _yaw-0 at the time of previous time processing are called. The image matching movement amounts ΔX ₀ , ΔY ₀ , Δθ _yaw-0 are the parameters recorded in step S 77 during the previous time processing, and are the VPP image I _V (t) and the VPP image I _V (t−1) at the previous time. ) Represents the amount of deviation due to matching.

In step S72, a search range for performing matching between the VPP image I _V (t) and the VPP image I _V (t−1) is set. Specifically, ΔX = ΔX _{MIN to} ΔX _MAX and ΔY = ΔY _{MIN to} ΔY _MAX , Δθ _yaw = Δθ are ranges having a predetermined width centered on the image matching movement amounts ΔX ₀ , ΔY ₀ , Δθ _{yaw-0. yaw−MIN to} Δθ _yaw−MAX is set. In step S73, VPP image matching is performed by relatively moving the VPP image I _V (t) and the VPP image I _V (t−1) within the search range set in step S72. Since the search range is set to a range based on the image matching movement amounts ΔX ₀ , ΔY ₀ , and Δθ _yaw-0 , the time required for VPP image matching is short. In addition, since the own vehicle speed vector V _V does not change abruptly, the location where the similarity is maximum is not shifted from the search range centered on the image matching movement amounts ΔX ₀ , ΔY ₀ , Δθ _yaw-0. Absent. Detailed processing of VPP image matching in step S73 will be described later (see FIG. 36).

In step S74, referring to the result of VPP image matching obtained in step S73, a location where the similarity between the VPP image I _V (t) and the VPP image I _V (t−1) is maximized (NSSD to be described later). (Location where ΔX, ΔY, Δθ _yaw ) is minimum), and the amount of shift between the VPP image I _V (t) and the VPP image I _V (t−1) at that location is expressed as a movement amount move (ΔX _f , ΔY _f , Δθ _yaw−f ). Here, ΔX _f , ΔY _f, and Δθ _yaw−f are values of ΔX, ΔY, and Δθ _{yaw at} a location where the similarity between the VPP image I _V (t) and the VPP image I _V (t−1) is maximum. is there.

In step S75, the magnitude of the vehicle speed vector V _V of the vehicle 10 is obtained from the following equations (7-1) to (7-3) based on the movement amount move (ΔX _f , ΔY _f ).

Here, Vx and Vz are the speeds of the vehicle 10 in the width direction and the vehicle front-rear direction, and d _vpp , α _VPP, and β _VPP are coefficients and numerical values defined in the above equation (6-6). Further, as described above, Δt is a calculation cycle. In formulas (7-2) and (7-3), the speed in the longitudinal direction of the vehicle is Vz, and the suffix “y” is used instead of the suffix “y”. This is because the coordinates of the VPP image are returned to the world coordinate system.

Since the size of the own vehicle speed vector V _V is expressed as the length of the hypotenuse formed by the two-way speed components orthogonal to each other as is well known, equation (7-3) is established.

(7-3) by formula and (7-4) below, the vehicle velocity vector V _V magnitude and angle theta _YAW vehicle since the obtained velocity vector V _V is determined, figure the free-wheel velocity vector V _V 25. The angle θ _YAW can also be used as the rotation angle of the vehicle 10.

By the way, in order to obtain the rotation angle of the vehicle 10 more precisely, the VPP image I _V (t−1) is not only translated in the ΔX and ΔY directions but also moved including rotation. It is advisable to calculate the similarity. In this case, ΔX = ΔX _{MIN to} ΔX _MAX , ΔY = ΔY _{MIN to} ΔY _MAX, and ω = Δθ _{MIN to} Δθ _MAX as rotation angles are ranges having a predetermined width centered on the image matching movement amounts ΔX ₀ and ΔY _0. Set and move with 3 degrees of freedom.

In step S76, the vehicle speed vector V _V and the angle θ _YAW are recorded. Further, in order to use ΔX _f , ΔY _f , Δθ _yaw−f obtained in step S74 as image matching movement amounts ΔX ₀ , ΔY ₀ , Δθ _yaw-0 at the next processing, ΔX ₀ ← ΔX _f , ΔY ₀ ← Record after substituting ΔY _f , Δθ _yaw-0 ← Δθ _yaw−f .

Next, the first processing method for VPP image matching in step S73 will be described. This first processing method includes the processing of steps S731 to S735 in FIG. The processing of this step S731~S735 is, ΔX = ΔX _MIN ~ΔX _MAX as set search range in the step S72, the and _{ΔY = ΔY MIN ~ΔY MAX, Δθ} yaw = Δθ yaw-MIN ~Δθ yaw- _Although the processing is executed individually for the _MAX region, only the processing for one time will be described by omitting the description of the repetitive control.

First, in step S731, the VPP plane image I _Vf (t−1) is rotated by Δθ _yaw set at the current time and moved in the directions of ΔX and ΔY. (See “S731” in FIG. 37). The center of rotation is the position of the optical center (C _x−vpp , C _y−vpp ) in the VPP image.

In step S732 (second road plane area extracting means), the common area R (ΔX, ΔY, Δθ) between the VPP plane image I _Vf (t) and the VPP plane image I _Vf (t−1) moved in step S731. _yaw ) is obtained (see "S732" in FIG. 37).

In step S733, the VPP image I _V (t−1) is rotated by Δθ _yaw set at that time and moved in the directions of ΔX and ΔY (see “S733” in FIG. 37).

In step S734, based on the common region R (ΔX, ΔY, Δθ _yaw ), the VPP image I _V (t), and the moved VPP image I _V (t−1), the VPP image is expressed by the following equation (7-4). NSSD is calculated as a match.

Here, xi is the coordinate of the pixel included in the common area _{R (ΔX, ΔY, Δθ yaw} ), N is the total number of pixels contained in the common area _{R (ΔX, ΔY, Δθ yaw} ), I V (T-1) move (ΔX, ΔY) indicates an image obtained by rotating the VPP image I _V (t−1) by Δθ _yaw in step S734 and moving it by ΔX and ΔY.

By this equation (7-4), the square sum of the luminance difference for each pixel between the VPP image I _V (t) and the VPP image I _V (t−1) rotated by Δθ _yaw and moved by ΔX and ΔY is obtained. As a normalized value, NSSD (Δx, Δy, Δθ _yaw ) is recorded as the similarity for each movement amount move (Δx, Δy, Δθ _yaw ) in the next step S735. Since NSSD (Δx, Δy, Δθ _yaw ) is normalized, mutual comparison is possible regardless of the size of the common region R (ΔX, ΔY, Δθ _yaw ).

According to the processing in step S7, robustness is high because no corresponding point search is performed. In addition, since a VPP image parallel to the plane is generated based on the position and orientation parameters of the plane, the vibration resistance is high without being affected by changes in the vehicle attitude. Actually, since there are some patterns and features such as a ground pattern even if the road plane is irregular, the own vehicle speed vector V _V can be obtained based on such a part.

Furthermore, since VPP image matching is performed in the common region R (ΔX, ΔY, Δθ _yaw ) based on the planar region Π _f , extra portions such as other vehicles and roadside portions are deleted, and high-precision and high-speed calculation is performed. Is carried out.

  Further, as the similarity in VPP image matching, the above ZNCC or the like may be used instead of NSSD.

  Next, the second processing method of VPP image matching in step S73 will be described. This second processing method is basically the same method as the density gradient method described in Step S1 and Non-Patent Document 1, and changes the moving rotation matrix M corresponding to the projective transformation matrix H by a minute amount. However, the two images are converged so that they overlap most.

First, a moving rotation matrix M as an initial value appropriately close to the true value is set, and the VPP image I _V (t−1) is moved and rotated by the following equation (7-5). Here, I _r (x) is a luminance value at the coordinate x (x, y) of the VPP image I _V (t−1). The moving rotation matrix M is a 3 × 3 matrix. As the initial value of the moving rotation matrix M, the value calculated in the previous time step may be used.

（「〜」は、変換された後の輝度値を表す。）

("~" Represents the luminance value after conversion.)

の関係が得られる。なお、Ｉは３×３の単位行列であり、Ｄは移動回転行列Ｍの各パラメータの微小変化量をパラメータとする３×３の微小変化行列であり次の（７−７）式で表される。 When each parameter of the moving rotation matrix M is changed by a minute amount, the coordinate x (x, y) on the converted VPP image I _V (t−1) changes to the coordinate x ″ (x ″, y ″). Assuming

The relationship is obtained. Note that I is a 3 × 3 unit matrix, and D is a 3 × 3 minute change matrix whose parameter is the minute change amount of each parameter of the moving rotation matrix M, and is expressed by the following equation (7-7). The

Here, ω is a minute rotation angle, and dx and dy are minute movement amounts in the x and y directions. Also, ω, dx, and dy are collectively expressed as one parameter Θ. Using this parameter Θ as an argument, the overlap between the VPP image I _V (t−1) and the VPP image I _V (t) that has been subjected to translation and rotation conversion using the relationship of equations (7-5) and (7-6). The evaluation function E (Θ) indicating the state is expressed by the following equation (7-8).

（「ｉ」は、画素番号を表す。）
と表すことができる。ここで、Ｒ_planeは、ＶＰＰ平面画像Ｉ_Vf（ｔ−１）、Ｉ_Vf（ｔ）における道路平面Πに相当する領域である。

(“I” represents a pixel number.)
It can be expressed as. Here, R _plane is an area corresponding to the road plane _plane in the VPP plane images I _Vf (t−1) and I _Vf (t).

The expression (7-8) is equivalent to the expression (1-5), and the base image I _b and the VPP image I _V (t), the reference image I _r and the VPP image I _V (t−1), The matrix Dx and the matrix D correspond to each other.

Therefore, g _i (luminance gradient), e _i (luminance difference), and Jacobian JΘ _i can be calculated by the same method as described above, and the minute change matrix D can be obtained using the least square method.

  Using the minute change matrix D obtained as described above, the moving rotation matrix M is updated according to the following equation (7-9).

Then, the process of calculating the value of the evaluation function E (Θ) of the equation (7-8) is repeated using the moving rotation matrix M obtained by the equation (7-9). In this iterative process, the moving rotation matrix M is updated as M ← M (I + D) using the minute change matrix D obtained by solving the least square method. Δθ _yaw , ΔX, and ΔY can be obtained from the respective components of M when the value of the evaluation function E (Θ) converges below a predetermined value using Equation (7-10).

In such a second method of processing step S73, the first processing method in the obtained [Delta] [theta] _yaw, by using the second processing method [Delta] X, the ΔY as the initial value, more precisely [Delta] [theta] _yaw, [Delta] X , ΔY can be obtained.

<Step S8>
Step S8 is a process for determining whether the detected object is a stationary object or a moving object, and includes a subroutine process of steps S81 to S84 shown in FIG.

  In step S81, vectors Vr1, Vr2 and Vr3, which are the relative velocities of the respective objects obtained in step S55, are read out. These vectors are indicated by arrows in FIG.

In step S82, the vehicle speed vector V _V of the vehicle 10 obtained in step S7 is read. The own vehicle speed vector V _V of the vehicle 10 is shown with the origin in FIG.

In step S83, the absolute velocity vector V _ABS N (N = 1, 2, 3,...) Of each detected object is calculated. The absolute speed V _ABS N is obtained by vector addition as V _ABS N = V _r N + V _V ′. However, V _V ′ is a speed component included in the relative speed V _r N due to the motion of the host vehicle. FIG. 39 shows the absolute velocity vector V _ABS N obtained for each object. As is apparent from FIG. 39, V _ABS 3≈0 for the guard rail 74 which is a stationary object.

In step S84, based on the absolute speed V _ABS N, it is checked whether each detected object is a stationary object or a moving object. Specifically, the magnitude of the absolute velocity vector | V _ABS N | of each object is compared with the threshold Th _ABS , and when Th _ABS ≧ | V _ABS N |, the target object is a stationary object, When | V _ABS N |> Th _ABS , it is determined that the object is a moving object. Thus, the guard rail 74 with V _ABS 3≈0 is determined as a stationary object, and the truck 70 and the passenger car 72 are identified as moving objects.

  Thus, according to the process of step S8, the absolute velocity of the detected object can be obtained with high accuracy, and it can be accurately determined whether or not the object is a stationary object. Information on this determination result can be provided to, for example, a predetermined approach alerting means for the vehicle 10 and the object.

In addition, according to the processing of steps S7 and S8, the amount of movement is calculated using only the plane region Π _f of the VPP image I _V , so that it can be accurately detected even in an environment where moving objects and stationary objects coexist. A vehicle speed vector V _V can be obtained.

Furthermore, for example, when it is determined that an object at a certain height (distance) such as a road surface is imaged and speed measurement is performed based on the road surface, an object having a height with respect to the road surface is imaged. While errors in performing the speed detection based on the height of the speed measurement is known to occur as a reference, according to the processing of step S7 and S8, a height by using only the plane area [pi _f Since the calculation can be performed by excluding the object, an error based on the height does not occur.

  Further, according to the processing of steps S7 and S8, since the distance to the plane is also detected, even when a plane having a height relative to the road surface such as a sidewalk occupies most of the image, the height is taken into consideration. Speed can be calculated.

The plane area Π _f required in the present embodiment is an area corresponding to a substantially horizontal road plane す る on which the vehicle 10 travels. Faces are excluded.

  In the above description, the base camera 12 and the reference camera 14 are provided. However, more cameras may be provided as necessary.

It is explanatory drawing of the vehicle to which the moving body moving amount | distance calculation apparatus of this invention is applied. It is a functional block diagram of the arithmetic processing unit mounted in the vehicle shown in FIG. It is a flowchart explaining the flow of the whole process in the arithmetic processing unit shown in FIG. It is a schematic diagram for demonstrating two-dimensional projective transformation between a pair of imaging means. It is explanatory drawing of the procedure which extracts a plane area | region. It is a flowchart which shows the processing content of an object extraction process. It is a schematic diagram which shows a mode that a candidate point is extracted based on the restriction | limiting of height. It is a schematic view showing the state of the voting processing. It is a schematic diagram which shows the mode of the clustering process performed on a voting surface. It is a figure which shows a voting surface. It is a flowchart which shows the processing content which calculates | requires the world coordinate of each edge point of an object. It is a figure which shows the edge image from which the edge was extracted. It is actual image data of the edge image from which the edge was extracted. It is a figure which shows the correction edge image from which the plane area | region was removed. It is a figure which shows the reference image in which the epipolar line was set. It is a schematic diagram which shows the relationship between a world coordinate system and a camera coordinate system. It is a figure which shows a depth image. It is a flowchart which shows the processing content which calculates the relative speed of an object. It is a flowchart which shows the processing content which investigates the overlap condition of an object area | region. It is a schematic diagram which shows the processing content which creates a template based on a cluster It is a figure which shows the reference | standard image in which the template was set. It is a flowchart which shows the processing content which calculates a gravity center movement distance. It is a flowchart which shows the processing content of an object identity determination. It is a flowchart which shows the processing content which recalculates the position of the same object. It is a figure which shows the voting surface on which the relative velocity of the object was recorded. It is a flowchart which shows the processing content which calculates the relative speed of an object. It is a schematic diagram for demonstrating the process which sets an epipolar line. It is a flowchart which shows the processing content for producing | generating a VPP image. It is a flowchart which shows the processing content which calculates a VPP image conversion type | formula. It is a coordinate diagram which shows the relationship between a normal vector, a camera coordinate system, and the optical axis of a reference | standard camera. It is a figure for demonstrating the process which converts a camera coordinate system reference | standard image so that it may become parallel to a road plane. It is a schematic diagram which shows a mode that the 1st conversion image is rotationally converted. It is a schematic diagram which shows a mode that a reference | standard image is VPP-converted. It is a schematic diagram which shows a mode that a road plane area | region is VPP-converted. It is a flowchart which shows the processing content which calculates the own vehicle speed. It is a flowchart which shows the processing content of VPP image matching. It is a schematic block diagram which shows the processing content of VPP image matching. It is a schematic block diagram which shows the processing content for calculating the absolute speed of an object. It is a figure which shows the voting surface where the absolute velocity of the object was recorded.

Explanation of symbols

DESCRIPTION OF SYMBOLS 10 ... Vehicle 12 ... Base camera 14 ... Reference camera 16 ... Arithmetic processor 18, 20 ... Image processing part 22 ... Image storage part 24 ... Calculation control part 26 ... Parameter storage part 28 ... Projection transformation matrix calculation part 30 ... Plane parameter calculation Unit 32 ... plane area extraction unit 34 ... object detection unit 36 ... object relative speed calculation unit 38 ... virtual projection plane image generation unit 40 ... own vehicle speed calculation unit 42 ... object absolute speed calculation unit 44 ... display processing unit 50 ... voting plane 52 a - 52 c, 54a to 54c ... cluster 60 ... edge image 62 ... corrected edge image 80 a - 80 c ... template 82a ... object position area A _b, A _r ... internal parameter Cc ... camera coordinate Cw ... world coordinate EP ... epipolar line H ... Projection transformation matrix I _b ... base image I _r ... reference image Π ... plane d ... distance n ... normal vector t ... translation vector

Claims (4)

A plurality of imaging means for imaging the periphery of the moving body;
A projection transformation matrix estimation means for dynamically estimating a projection transformation matrix for a road plane of an image by the plurality of imaging means;
First road plane area extraction means for extracting a road plane area from images by the plurality of imaging means;
Plane parameter calculation means for calculating a position and orientation parameter of the road plane relative to the camera by decomposing the projection transformation matrix estimated by the projection transformation matrix estimation means;
Virtual projection plane image creating means for generating a virtual projection plane image of the road viewed from above using the position and orientation parameters;
Second road plane area extracting means for extracting a road plane area in the virtual projection plane image;
A movement amount calculation means for calculating a movement amount between different times of the road plane area in the virtual projection plane image extracted by the second road plane area extraction means;
A moving body moving amount calculating apparatus comprising: In the mobile movement amount calculating device according to claim 1 Symbol placement,
A moving body moving amount calculating apparatus comprising moving body speed calculating means for calculating a moving body speed using moving amounts between different times calculated by the moving amount calculating means. In the moving body movement amount calculation apparatus according to claim 2 ,
Object extraction means for extracting an object region from images obtained from the plurality of imaging means;
A relative speed calculating means for calculating a relative speed of the object with respect to the moving body;
Absolute speed calculating means for calculating the absolute speed of the object using the speed of the moving body by the moving body speed calculating means and the relative speed of the object by the relative speed calculating means;
Discriminating means for discriminating whether the object is a stationary object or a moving object using the calculation result by the absolute velocity calculating means;
Mobile movement amount calculating device, characterized in that it comprises a. In the moving body movement amount calculation apparatus according to any one of claims 1 to 3 ,
The moving amount calculating unit is characterized in that the virtual projection plane images at different times are subjected to matching processing based on a gradient method, and the moving amount is calculated from the obtained shift amount.

Images