JP5652239B2 - Motion estimation apparatus and program - Google Patents

Description

  The present invention relates to a motion estimation device and program, and more particularly, to a motion estimation device and program for estimating motion of a moving object.

  2. Description of the Related Art Conventionally, an image processing apparatus that accurately identifies a three-dimensional shape of an object existing outdoors is known (Patent Document 1). In this image processing apparatus, a point of the same part is specified between images taken at a plurality of points. In order to correctly detect the same point, position / posture information between images is acquired by the sensing means, and epipolar constraint conditions based on the obtained position / posture information are used. That is, the same point is searched for on the epipolar line determined by the obtained position / posture information.

  There is also known a vehicle image processing apparatus that suppresses miscorrespondence between feature points between images and calculates the own vehicle motion and the three-dimensional structure (Patent Document 2). In this vehicular image processing apparatus, in order to suppress erroneous correspondence of feature points between images, motion is estimated from sensor information, and 2 for each feature point detected in the first image based on the estimated motion. The position in the second image is predicted, the search area is limited to the area centered on the position, and the position in the second image corresponding to the feature point is detected.

JP 2006-234703 A JP 2010-85240 A

  In the technique described in the above-mentioned Patent Document 1, a sensor that can detect position / posture information between images without using an image and corresponding points satisfying epipolar constraint conditions determined from the detected position / posture information By searching for only, miscorrespondence of points is prevented. However, in order to realize this, there is a need for a sensor that can accurately measure all the components of six degrees of freedom of rotation and translation, and such a sensor is not usually mounted in a general vehicle. There is.

  In addition, in the technique described in Patent Document 2 described above, miscorresponding to points is suppressed by limiting the search area for feature points based on motion information obtained from sensor information or past own vehicle motion information. . Sensors that measure vehicle motion such as vehicle speed, acceleration, and rotation angle are required, but generally no sensor that can measure all motion components is installed. Without such a sensor, if a movement such as a sudden pitch change that cannot be estimated from past vehicle movement information that is difficult to measure with a general vehicle-mounted sensor occurs correctly, There is a problem that a search area cannot be set and correct feature point correspondence cannot be obtained. In addition, when a wide search area is set in order to cope with such a rapid pitch fluctuation, the effect of suppressing a false response is reduced.

  The present invention has been made to solve the above-described problems, and provides a motion estimation apparatus and program capable of accurately estimating the motion of a moving body without using a sensor that measures all motion components. The purpose is to do.

  In order to achieve the above object, the motion estimation apparatus according to the present invention supports a correspondence between the first image and the second image from each of the first image and the second image obtained by imaging the outside of the moving body. Search means for searching for the feature point, motion acquisition means for acquiring a motion representing a change in position and posture of the moving body when each of the first image and the second image is captured, and the first 3D position acquisition means for acquiring 3D positions of a plurality of feature points on one image, and 3D position acquisition means based on the movement of the moving body acquired by the movement acquisition means. Position prediction means for predicting the position on the second image of the plurality of feature points obtained from the second image on the second image in the plurality of sets of the corresponding feature points searched by the search means Multiple feature point locations The moving body when each of the first image and the second image is captured based on the difference from the positions of the plurality of feature points on the second image predicted by the position predicting means. Vertical rotation motion estimation means for estimating the vertical rotation motion representing the vertical change of the posture of the user, the plurality of corresponding feature points searched by the search means, and the first predicted by the position prediction means Each of the first image and the second image is captured based on the positions of a plurality of feature points on the second image and the vertical rotational motion of the movable body estimated by the vertical rotational motion estimation means. Motion estimation means for estimating the motion of the moving body at the time.

  The program according to the present invention allows a computer to search for a feature point corresponding to the first image and the second image from each of the first image and the second image obtained by imaging the outside of the moving body. Means, motion acquisition means for acquiring a motion representing a change in position and posture of the moving body when each of the first image and the second image is captured, and a plurality of feature points on the first image The three-dimensional position acquisition means for acquiring the three-dimensional position, and the plurality of feature points from which the three-dimensional position is acquired by the three-dimensional position acquisition means based on the motion of the moving body acquired by the motion acquisition means Position prediction means for predicting a position on the second image, positions of a plurality of feature points on the second image in a plurality of sets of the corresponding feature points searched by the search means, Position predictor The vertical direction of the posture of the moving body when each of the first image and the second image is captured based on the difference from the positions of the plurality of feature points on the second image predicted by Vertical rotation motion estimation means for estimating the vertical rotation motion representing the change of the above, a plurality of sets of the corresponding feature points searched by the search means, and on the second image predicted by the position prediction means The movement when each of the first image and the second image is captured based on the positions of a plurality of feature points and the vertical rotational motion of the movable body estimated by the vertical rotational motion estimation means. This is a program for functioning as motion estimation means for estimating body motion.

  According to the present invention, the search unit searches the corresponding feature points between the first image and the second image from each of the first image and the second image obtained by imaging the outside of the moving body. The motion acquisition means acquires a motion representing a change in position and posture of the moving body when each of the first image and the second image is captured.

  Then, the three-dimensional position acquisition unit acquires the three-dimensional positions of the plurality of feature points on the first image. The position predicting unit predicts the positions on the second image of the plurality of feature points whose three-dimensional positions are acquired by the three-dimensional position acquiring unit based on the movement of the moving body acquired by the movement acquiring unit.

  Then, the position of the plurality of feature points on the second image in the plurality of sets of corresponding feature points searched by the search means by the vertical rotation motion estimation means, and the second image predicted by the position prediction means Based on the difference from the positions of the plurality of feature points, a vertical rotation motion representing a vertical change in the posture of the moving body when each of the first image and the second image is captured is estimated. The motion estimation unit estimates the plurality of pairs of corresponding feature points searched by the search unit, the positions of the plurality of feature points on the second image predicted by the position prediction unit, and the vertical rotation motion estimation unit. Based on the vertical rotation motion of the moving body, the motion of the moving body when each of the first image and the second image is captured is estimated.

  As described above, the vertical rotational motion of the mobile object is estimated based on the difference between the acquired motion of the mobile object and the position of the feature point on the second image predicted based on the three-dimensional position of the feature point. Then, by estimating the motion of the moving body, it is possible to accurately estimate the motion of the moving body without using a sensor that measures all motion components.

  The motion estimation apparatus according to the present invention includes a plurality of sets of corresponding feature points searched by the search unit, a position of the plurality of feature points on the second image predicted by the position prediction unit, and a vertical rotation motion estimation. A plurality of sets of corresponding feature points searched by the search means based on the vertical rotational movement of the moving body estimated by the means, corresponding to the feature points on the area representing the stationary object, and And a reliability calculation means for calculating the reliability indicating the correctness of the correspondence between the feature points, wherein the motion estimation means includes a plurality of sets of corresponding feature points searched by the search means and a plurality of corresponding feature points. The motion of the moving body can be estimated on the basis of the reliability calculated for the set of the above.

  The motion estimation apparatus according to the present invention includes the positions of a plurality of feature points on the second image in the plurality of sets of corresponding feature points searched by the search means, and the second image predicted by the position prediction means. The positions of the plurality of feature points are changed according to the vertical rotational motion of the moving body estimated by the vertical rotational motion estimation means, and all the pairs of corresponding feature points searched by the search means Among them, a selection unit that selects a plurality of sets of corresponding feature points is further included, and the motion estimation unit estimates the motion of the moving body based on the plurality of sets of corresponding feature points selected by the selection unit. Can be.

  The motion estimation device further includes motion measurement means for measuring the motion of the moving body, and the motion acquisition means is configured to perform the first image and the second image based on the motion of the mobile body measured by the motion measurement means. It is possible to acquire the motion of the moving body when each of these is imaged.

  The motion acquisition means estimates the motion of the mobile body when each of the first image and the second image is captured based on the motion of the mobile body estimated in the past by the motion estimation means. be able to.

  In addition, the three-dimensional position acquisition unit is configured to search for the motion of the moving object when the previous image and the first image captured before the first image, which are estimated by the motion estimation unit, are captured. The three-dimensional position of each feature point on the first image can be calculated based on the previous image retrieved by the means and each set of feature points corresponding to the first image.

  The motion estimation device further includes road area specifying means for specifying a road area on which the moving body travels on the first image, and the three-dimensional position acquisition means images the road and the outside of the moving body that are obtained in advance. The three-dimensional position of each feature point in the road area on the first image specified by the road area specifying means can be calculated based on the positional relationship with the imaging means.

  As described above, according to the motion estimation apparatus and program of the present invention, the position of the feature point on the second image predicted based on the acquired motion of the moving object and the three-dimensional position of the feature point Based on the difference between the two, the movement of the moving body is estimated accurately by estimating the vertical rotational movement of the moving body and estimating the movement of the moving body, without using a sensor that measures all movement components. The effect of being able to be obtained.

It is a block diagram which shows the motion estimation apparatus which concerns on the 1st Embodiment of this invention. (A) The figure which shows the position on the 1st image of a corresponding point, (B) The figure which shows the position on the 2nd image of a corresponding point, and the position of the predicted feature point. It is a figure for demonstrating a difference flow. It is a figure for demonstrating the relationship between the extracted feature point and a motion of a moving body. It is a flowchart which shows the content of the motion estimation process routine in the motion estimation apparatus which concerns on the 1st Embodiment of this invention. It is a flowchart which shows the content of the pitch angle fluctuation | variation estimation processing routine in the movement estimation apparatus which concerns on the 1st Embodiment of this invention. It is a flowchart which shows the content of the estimation process routine in the movement estimation apparatus which concerns on the 1st Embodiment of this invention. It is a block diagram which shows the motion estimation apparatus which concerns on the 2nd Embodiment of this invention. It is a flowchart which shows the content of the motion estimation process routine in the motion estimation apparatus which concerns on the 2nd Embodiment of this invention. It is a block diagram which shows the motion estimation apparatus which concerns on the 3rd Embodiment of this invention.

  Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. The case where the present invention is applied to a motion estimation device mounted on a vehicle will be described as an example.

  As shown in FIG. 1, the motion estimation device 10 according to the first embodiment includes an imaging device 12 including a monocular camera that captures an image ahead of the host vehicle, and the vehicle speed as the motion of the host vehicle. The vehicle speed sensor 14 to be measured, the yaw angular velocity sensor 16 to measure the yaw angular velocity, the image captured by the imaging device 12, the vehicle speed measured by the vehicle speed sensor 14, and the yaw angular velocity sensor 16 are measured as the movement of the host vehicle. And a computer 18 that estimates the motion of the host vehicle based on the yaw angular velocity and outputs it to an external device (not shown). The vehicle speed sensor 14 and the yaw angular velocity sensor 16 are examples of motion measuring means.

  In the present embodiment, a case where a monocular camera of the imaging device 12 is installed in front of the host vehicle and an image in front of the host vehicle is acquired will be described as an example. However, if an image outside the host vehicle is captured. For example, a monocular camera may be installed behind the host vehicle, and the rear of the host vehicle may be imaged. The monocular camera may be a normal camera with a field angle of about 40 degrees, a wide-angle camera, or an omnidirectional camera. Further, the type of the wavelength of the image to be captured is not limited. There is no limitation on the installation location, the angle of view, the wavelength, and the number of installations as long as the momentum can be estimated appropriately.

  The yaw angular velocity sensor 16 is configured using, for example, a gyro sensor.

  The computer 18 estimates the three-axis angular velocity and the component indicating the translation direction of the host vehicle as the motion of the host vehicle. The computer 18 includes a CPU, a RAM, and a ROM that stores a program for executing a later-described motion estimation processing routine, and is functionally configured as follows. The computer 18 acquires a plurality of images captured by the imaging device 12, and a feature point extraction that extracts a plurality of feature points that are easy to track on the image from the plurality of images acquired by the image acquisition unit 20. A corresponding point search unit 24 that searches for corresponding feature points (hereinafter also referred to as corresponding points) between the two images from the feature points in each of the two images obtained by the feature point extracting unit 22; It is equipped with.

  The feature point extraction unit 22 extracts feature points from two images captured at different times obtained from the imaging device 12. A feature point refers to a point that can be distinguished from surrounding points and that makes it easy to obtain a correspondence between different images. The feature points are automatically extracted by using a method (for example, the Tomasi-Kanade method or the Harris operator) that detects pixels in which the gradient value of the change in shading increases two-dimensionally. The number of feature points may be approximately 30 to 500 points for all images, but may be fixed or variable depending on the situation. The method is not limited to the above as long as it is an appropriate extraction method.

  In the method using the Harris operator, feature points are extracted as described below. First, the matrix M is calculated by the following equation (1), where the luminance of the point (u, v) of the image is I (u, v).

However, I _u and I _v represent horizontal and vertical differentiation, respectively, and G _σ represents smoothing by a Gaussian distribution with a standard deviation σ.

Then, using the eigenvalues λ ₁ and λ ₂ of the matrix M calculated by the above equation (1), the corner strength is calculated by the following equation (2).

  However, k is a preset constant, and a value of 0.04 to 0.06 is generally used. In the method using the Harris operator, a point at which the corner intensity is equal to or greater than a threshold value is selected, and the selected point is extracted as a feature point.

  The corresponding point search unit 24 associates the two images with respect to the feature points extracted from each of the two images by the feature point extraction unit 22 and searches for the corresponding points between the two images. The association of the feature points between the images is performed by obtaining a pair in which the correlation value of the image pattern around the feature point is maximized and the correlation value is equal to or greater than a threshold value. For example, when using correlation by SSD (Sum of squared differentials), the correlation value between the feature point f of the first image and the feature point g of the second image is larger as the value calculated by the following equation (3) is smaller. Set to be a value.

However, the image coordinates of the feature points are f = (f _x , f _y ), g = (g _x , g _y ), and I (x, y) and J (x, y) are the first image, It represents the luminance at the coordinates (x, y) of the second image. Further, w is a variable that determines the size of the peripheral region for calculating the correlation. In the calculation of the correlation value, a method using a luminance gradient instead of luminance, or a correlation based on a characteristic amount such as a histogram in the gradient direction may be used.

  Further, the computer 18 is based on a previous estimation value by a motion information acquisition unit 26 that acquires the motion of the host vehicle and a motion estimation unit 36 described later, based on outputs from the vehicle speed sensor 14 and the yaw angular velocity sensor 16. A three-dimensional position calculation unit 28 for calculating a three-dimensional position of a feature point on one image, a feature point extracted by the feature point extraction unit 22, a motion of the host vehicle calculated by the motion information acquisition unit 26, And a corresponding point predicting unit 30 for predicting the position of the feature point in another image corresponding to the feature point of one image based on the three-dimensional position of the feature point calculated by the three-dimensional position calculating unit 28. ing. The three-dimensional position calculation unit 28 is an example of a three-dimensional position acquisition unit, and the corresponding point prediction unit 30 is an example of a position prediction unit.

  The motion information acquisition unit 26, based on the outputs from the vehicle speed sensor 14 and the yaw angular velocity sensor 16, rotates components with three degrees of freedom in a three-dimensional space while the first image and the second image are captured. A motion of the host vehicle is calculated that represents (pitch angle, yaw angle, roll angle) and a parallel movement component (left-right direction, up-down direction, front-rear direction) with three degrees of freedom. Note that each component of the movement of the host vehicle is represented by the camera coordinate system of the camera when the first image is taken. The component that cannot be measured by the mounted sensor is set to zero. In the present embodiment, since the vehicle speed sensor 14 and the yaw angular velocity sensor 16 are mounted, the rotation component of the pitch angle and the roll angle and the parallel movement in the vertical direction are not measured. Is output.

  The three-dimensional position calculation unit 28 determines the motion of the host vehicle estimated at the previous time by the motion estimation unit 36 (to be described later) (the first image at the present time and the image captured immediately before (the first image at the previous time). Between the first image and the second image) and the corresponding feature points retrieved at the previous time by the corresponding point retrieval unit 24 (the first image at the present time and the image captured immediately before that) 3D position of the feature point on the first image is calculated according to the principle of triangulation.

  The corresponding point predicting unit 30 is a motion of the host vehicle between the first image and the second image output from the motion information acquiring unit 26 and a feature on the first image output from the three-dimensional position calculating unit 28. Using the three-dimensional position of the point, the position on the second image where each feature point of the first image extracted by the feature point extraction unit 22 appears is predicted. The relative position / orientation in the second image with respect to the first image is represented by the rotation matrix R and the translation vector t, and the three-dimensional position of the feature point f of the first image is (X, In the case of (Y, Z), the coordinates (^ x ', ^ y') of the corresponding point in the second image are calculated according to the following equation (4).

  K is a calibration matrix of the imaging apparatus and is expressed by the following equation.

_However, f x, _{f y} represents the x-direction, the focal length of the y direction of the image pickup _device, a _(c x, _{c y)} is the image coordinates of the image center of the image pickup device.

  Further, the computer 18 uses the first image and the second image based on the corresponding points searched by the corresponding point search unit 24 and the positions of the feature points on the second image predicted by the corresponding point prediction unit 30. Pitch angle variation estimation unit 32 for estimating the pitch angle variation amount generated between images, corresponding points retrieved by corresponding point retrieval unit 24, pitch angle variation amount estimated by pitch angle variation estimation unit 32, and correspondence Based on the position of the feature point on the second image predicted by the point prediction unit 30, a reliability calculation unit 34 that calculates the reliability for each corresponding point, and each corresponding point searched by the corresponding point search unit 24 And a motion estimation unit 36 that estimates the motion of the host vehicle based on the reliability of each corresponding point calculated by the reliability calculation unit 34. Note that the pitch angle variation estimation unit 32 is an example of vertical rotation motion estimation means, and the pitch angle variation is an example of vertical rotation motion.

  The pitch angle fluctuation estimation unit 32 estimates the pitch angle fluctuation amount as described below.

First, the image coordinates in the first image and the second image of each corresponding point output from the corresponding point search unit 24, and the second of each feature point output from the corresponding point prediction unit 30. Using the predicted image coordinates in the image as an input, a difference flow d _i is calculated for each feature point. As shown in FIGS. 2A and 2B, the image coordinates in the first image of the i-th corresponding point are (x _i , y _i ), and the image in the second image of the i-th corresponding point. When the coordinates are (x ′ _i , y ′ _i ) and the predicted image coordinates in the second image obtained from the corresponding point prediction unit 30 are (^ x ′ _i , ^ y ′ _i ), the difference flow d _i = (D _ix , d _iy ) is calculated according to the following equation (5).

The solid line vector shown in FIG. 3 is the difference flow d _i = (d _ix , d _iy ), and the two vectors shown by broken lines are the vectors (x ′ _i −x _i , y ′ _i −y _i ) and a vector (^ x ′ _i −x _i , ^ y ′ _i −y _i ) indicating a flow based on the predicted feature points. As described above, the difference flow indicates a motion vector of a difference between the flow based on the searched corresponding point and the flow based on the predicted feature point.

As shown in FIG. 3, the difference flow d _i includes the optical flow obtained from the corresponding points detected by the corresponding point search unit 24 and the feature points on the second image predicted by the corresponding point prediction unit 30. It represents the difference from the optical flow obtained from the position, and represents the difference between the motion information output from the motion information acquisition unit 26 and the actual motion. When there is a large pitch angle variation between images and it is not included in the motion information from the motion information acquisition unit 26, this difference flow can be considered as a flow mainly caused by the pitch angle variation.

  Therefore, in the present embodiment, the pitch angle variation estimation unit 32 estimates a pitch angle variation component that is not included in the motion information from the motion information acquisition unit 26 based on the difference flow of each feature point.

The reliability calculation unit 34, for the i-th corresponding point (i = 1,..., N) output from the corresponding point search unit 24, features on the region representing a stationary object according to the following equation (6). A reliability c _i that is a correspondence between points and indicates the correctness of the correspondence between feature points is calculated.

However, the image coordinates in the first image of the i-th corresponding point are (x _i , y _i ), the image coordinates in the second image of the i-th corresponding point are (x _i ′, y _i ′), The image coordinates of the i-th feature point obtained from the corresponding point prediction unit 30 in the second image are (^ x _i ', ^ y _i '). Further, w is the pitch angle variation estimated by the pitch angle variation estimation unit 32, and k is a preset constant.

  The motion estimation unit 36 uses the reliability of each corresponding point calculated by the reliability calculation unit 34 to change the position and orientation between the first image and the second image, and the first image and the first image. This is estimated as the movement of the host vehicle when the second image is captured.

  When calculating the change in position and orientation when two images are captured from the coordinates of the corresponding feature points in the two images, the change in the position and orientation to be calculated is the first as shown in FIG. This is a six-degree-of-freedom motion composed of a rotation matrix R and a translation vector t from an image to a second image.

Here, a calculation method of the rotation matrix R and the translation vector t from the first image to the second image will be described. For the first image coordinates of the corresponding point of the n points in the image I _i and image coordinates I _i of the corresponding point of n points in the second image _'(n ≧ 8), correspondence is correct errors of corresponding points Otherwise, there is a 3 × 3 matrix F that satisfies the following expression (7) as a matrix indicating the change in position and orientation.

However, I _i = (u _i , v _i , 1) ^T , I _i ′ = (u _i ′, v _i ′, 1) ^T , and the image coordinates (u _i , v _i ) in the first image The image coordinates of the point in the second image corresponding to the point are (u _i ′, v _i ′).

Here, the matrix F satisfying the equation (7) has a constant multiple indefiniteness. That is, when F _s satisfies the above expression (7), αF _s also satisfies the above expression (7) (where α is a real number). Therefore, the matrix F can be expressed as the following equation (8).

  Further, the following expression (9) is obtained from the above expressions (7) and (8).

Here, if there are eight or more pairs of corresponding points I _i and I _i ′, at least eight of the above formula (9) can be obtained, so that eight variables f _{11 to} f ₃₂ can be obtained.

In addition, when the correspondence of the feature points is obtained in the real image, the corresponding image coordinates include an error, and some of the feature points may have an erroneous correspondence. (10) evaluation value Z _r which is calculated by the formula seeks matrix F such that maximum.

Where I _i is a homogeneous coordinate vector representing the image coordinates of the i-th corresponding point in the first image, and I _i ′ represents the image coordinates of the i-th corresponding point in the second image. It is a homogeneous coordinate vector. Also, c _i is the reliability of the i-th corresponding point, δ (x) is a function that takes 1 if x is true, and 0 otherwise, d (a, b) represents the distance between a and b, θ Is a preset threshold value. n is the number of corresponding points searched by the corresponding point search unit 24.

  Further, when the calibration matrix K of the imaging device 12 is known, the rotation matrix R and the translation vector according to the following expressions (11) and (12) based on the matrix F specified as described above. t is calculated and output as a motion (change in position and orientation) of the imaging device 12 when each of the first image and the second image is captured.

  By the above method, the motion estimation unit 36, based on the image coordinates of a plurality of sets of corresponding feature points in the first image and the second image, and the reliability of each corresponding point, The motion of the host vehicle between the two images is calculated.

  Next, the operation of the motion estimation apparatus 10 according to the first embodiment will be described. In addition, the case where the motion of the own vehicle is estimated when the own vehicle carrying the motion estimation device 10 is traveling will be described as an example.

  The motion estimation processing routine shown in FIG. 5 is repeatedly executed in the motion estimation device 10 each time the front of the host vehicle is imaged by the imaging device 12 at predetermined time intervals and captured by the imaging device 12. In step 100, a first image (for example, an image captured immediately before the latest image) and a second image (for example, the latest image) captured in front of the host vehicle at different timings are acquired. At the same time, in step 102, the vehicle speed measured by the vehicle speed sensor 14 and the yaw angular velocity measured by the yaw angular velocity sensor 16 are acquired while the first image and the second image are captured, and the first image and the second image are obtained. The motion of the host vehicle during the time of capturing the image is calculated.

  In step 104, a predetermined number of feature points are extracted from the first image and the second image. In step 106, each feature point in the first image extracted in step 104 is tracked in the second image, and each corresponding feature point is searched from the second image.

  In the next step 108, the motion of the host vehicle estimated in the motion estimation processing routine executed last time corresponds to the two searched images (the first image of this time and the image captured immediately before). And feature points to be acquired. In step 110, the three-dimensional position of each feature point in the first image is calculated based on the movement of the host vehicle acquired in step 108 and the correspondence between the feature points.

  In step 112, each feature point of the first image is calculated based on the motion of the host vehicle calculated in step 102 and the three-dimensional position of each feature point in the first image calculated in step 110. The position of the feature point on the second image corresponding to is predicted.

  In step 114, based on the corresponding points searched in step 106 and the position of each feature point on the second image predicted in step 112, the first image and the second image The pitch angle fluctuation amount of the own vehicle during the imaging is estimated.

  In the next step 116, each corresponding point searched in step 106, the position of each feature point on the second image predicted in step 112, and the pitch angle of the host vehicle estimated in step 114 above. The reliability of each corresponding point is calculated based on the fluctuation amount.

  In step 118, based on the corresponding points retrieved in step 106 and the reliability of the corresponding points calculated in step 116, the change in the position and orientation of the imaging device 12 between the two imaging times. That is, the motion (the amount of movement in the XYZ-axis direction and the amount of rotation based on the XYZ-axis) of the host vehicle on which the imaging device 12 is mounted is estimated and output to an external device, and the motion estimation processing routine Exit.

  The above motion estimation processing routine is repeatedly executed, and the motion of each host vehicle between images repeatedly captured by the imaging device 12 is estimated.

  The step 114 is realized by a pitch angle variation estimation processing routine shown in FIG.

First, in step 120, as the difference flow d _i of each corresponding point, the position of the characteristic point on the second image of the corresponding points is searched in step 106, it predicted in step 112, the second image The difference from the position of the corresponding feature point above is calculated.

In step 122, an initial value 0 is set for the estimated pitch angle fluctuation p, and an initial value ∞ is set for the minimum deviation _emin . In step 124, an initial value 1 is set to a variable r indicating the number of repetitions, and in step 126, an initial value 1 is set to a variable s indicating the number of selections.

In step 128, one corresponding point is randomly selected from the corresponding points searched in step 106. In step 130, the pitch angle variation is calculated based on the difference flow d _i calculated in step 120 for the selected corresponding point. Here, the x-axis component d _ix and y-axis component d _iy differential flow d _i, respectively the pitch angle variation _w x, a _{w y,} determined by solving the following equation (13).

However, represents a (x _i, y _i) is the image coordinates of the first image of the selected corresponding _points, f x, _{f y} are the x direction, the focal length in the y-direction.

Then, in step 132, _w x calculated in step 130, _w x / _{w y} based on _{w y} is, determines whether within the allowable range. The allowable range is a preset range such as 0.9 to 1.1 with 1 as the center. If the difference flow is a flow caused by pitch angle fluctuation, the calculated w _x and w _y are the same. If w _x / w _y is within the allowable range, the process proceeds to step 138.

On the other hand, if w _x / w _y is out of the allowable range, the process proceeds to step 134 in order to redo the selection of the corresponding point, and the selection count s is incremented by one. In step 136, it is determined whether or not the selection count s is equal to or less than the maximum selection count S. If the number of selections s is S or less, the process returns to step 128. On the other hand, if the number of selections s is greater than S, it is determined that a large pitch angle variation has not occurred, and the difference flow has a small influence of the pitch angle variation, and the process proceeds to step 154.

In step 138, set the pitch angle variation w w _x, the average of w _y, at step 140, for all the corresponding points to calculate the flow to be calculated from the pitch angle variation. Here, when the pitch angle is w, the flow V _i = (v _ix , where the image coordinates in the first image are calculated from the variation of the pitch angle of the i-th corresponding point of (x _i , y _i ). v _iy ) is calculated by the following equation (14).

  In step 142, for each corresponding point, the deviation between the flow calculated in step 140 and the difference flow calculated in step 120 is calculated according to the following equation (15).

In the next step 144, the corresponding points are arranged in descending order or ascending order of the deviations corresponding to the corresponding points calculated in step 142, and the deviation of the corresponding points arranged in the middle is obtained as the median e of deviations. In step 146, it is determined whether or not the median e of deviation is smaller than the minimum deviation e _min . If the median e of deviation is equal to or greater than the minimum deviation amount e _min , the process proceeds to step 150. On the other hand, if the median e of the deviation is less than the minimum deviation amount e _min , the estimated pitch angle fluctuation p is updated to w and the minimum deviation amount e _min is updated to e in step 148.

  In step 150, the number of repetitions r is increased by 1. In step 152, it is determined whether the number of repetitions r is equal to or less than the maximum number of repetitions R. If the number of repetitions r is less than or equal to R, the process returns to step 126. If the number of repetitions r is greater than R, in step 154, the estimated pitch angle fluctuation p is output as an estimated value of the pitch angle fluctuation amount.

  The step 118 is realized by an estimation processing routine shown in FIG.

  First, in step 160, a variable r indicating the number of repetitions is set to an initial value 1, and in step 162, eight corresponding points are randomly selected from the corresponding points searched in step 106.

In step 164, the elements F _{11 to} f ₃₂ of the matrix F are calculated based on the combination of the eight corresponding points selected in step 162 to generate the matrix F _r . In the next step 166, the evaluation value Z _r is calculated according to the above equation (10) based on the corresponding points searched in step 106 and the matrix F _r generated in step 164. In step 168, the repeat count r is incremented by 1. In step 170, it is determined whether the repeat count r is equal to or less than the maximum repeat count T. If the number of repetitions r is less than or equal to T, the process returns to step 162. On the other hand, if the number of repetitions r is greater than T, the process proceeds to step 172.

In step 172, identifies the matrix Fr evaluation value _{Z r} calculated in step 166 is maximum. In step 174, based on the matrix Fr specified in step 172, the movement of the host vehicle between the times when the first image and the second image are captured according to the equations (11) and (12). Is calculated, and the estimation processing routine is terminated.

  As described above, according to the motion estimation apparatus according to the first embodiment, the second image predicted based on the motion of the host vehicle estimated using the Kalman filter and the three-dimensional position of the feature point. Based on the difference between the position of the upper feature point and the position of the searched corresponding point on the second image, the pitch angle fluctuation of the host vehicle is estimated, the reliability of the corresponding point is calculated, By estimating the motion of the host vehicle using the reliability, the motion of the host vehicle can be accurately estimated without using a sensor that measures all motion components.

  In addition, even when sudden pitch fluctuations occur during traveling, the vehicle's own vehicle is accurately detected based on the correspondence of the feature points on the stationary object without being affected by other moving objects such as other vehicles. Motion can be estimated.

  When using multiple images to estimate the motion that occurred between image capture times, extract characteristic points such as corners from the images, associate those points between images, and use the correspondence between the points Find changes in position and orientation between images. In this case, when searching for corresponding points in a real image, it is inevitable that erroneously corresponding points are included, and there are also corresponding points on moving objects such as other vehicles. If motion (position / posture change) is estimated using corresponding corresponding points or corresponding points on a moving object, an estimation error may increase.

  Since the movement of each point on the image that occurs between images taken at different times by the on-board camera is determined by the three-dimensional position of each point and the movement of the vehicle, the approximate three-dimensional position of each point and the approximate If the vehicle motion is known, the position of the corresponding point between images can be predicted. The three-dimensional position of each point, that is, the three-dimensional structure information of the surrounding environment, can be calculated from the movement of the host vehicle at the previous time and the correspondence between the feature points. In addition, the motion information of the host vehicle can be acquired from an in-vehicle sensor such as a vehicle speed sensor or a yaw angular velocity sensor. Therefore, when these pieces of information are obtained, it is possible to predict the movement of a point that occurs between images with respect to a stationary object. Therefore, a corresponding point that shows a movement different from the predicted movement is a corresponding point on a moving object or a corresponding point. It is possible to determine that the possibility is high, and it is possible to prevent deterioration in accuracy of motion estimation.

  However, when the vehicle is running, there may be a sudden up / down rotational movement, that is, pitch angle fluctuations due to minute irregularities on the road, etc., and this pitch angle fluctuation should be measured with a normal on-board sensor. I can't. Moreover, since it is a fluctuation | variation which generate | occur | produces rapidly, it is difficult to estimate from the motion estimated by the previous time. Because this pitch angle variation has a large effect on the movement of points on the image, if you predict the movement of points between images from the motion information for which the rotation component of the pitch angle variation is not obtained, what is the actual movement of the points? to differ greatly. Therefore, it is impossible to correctly determine the corresponding point on the moving point or the corresponding point on the moving object.

  Therefore, in the present embodiment, the reliability of the corresponding point is estimated after estimating only the rotation component of the pitch angle variation from the difference between the predicted point motion without the pitch angle variation and the corresponding corresponding point motion. Is calculated. As a result, even when sudden pitch fluctuations occur, it is possible to correctly determine whether the corresponding point is a miscorresponding point or a corresponding point on a moving object, and estimate the motion of the host vehicle with high accuracy. Can do.

  Next, a second embodiment will be described. In addition, about the part which becomes the structure similar to 1st Embodiment, the same code | symbol is attached | subjected and description is abbreviate | omitted.

  In the second embodiment, based on the vehicle motion estimated up to the previous time, the current motion information is acquired, and the feature point 3 is based on the positional relationship between the imaging device and the road. The difference from the first embodiment is that the dimension position is calculated.

  As shown in FIG. 8, the motion estimation apparatus 210 according to the second embodiment includes an imaging device 12, a GPS sensor 214 that measures the position of the host vehicle, an image captured by the imaging device 12, and a GPS sensor. And a computer 218 that estimates the motion of the host vehicle based on the position of the host vehicle measured by 214 and outputs the estimated motion to an external device (not shown).

  The computer 218 includes an image acquisition unit 20, a feature point extraction unit 22, a corresponding point search unit 24, a map information storage unit 222, a road area specification unit 224, a motion information acquisition unit 226, and a three-dimensional position calculation unit. 228, a corresponding point prediction unit 30, a pitch angle variation estimation unit 32, a reliability calculation unit 34, and a motion estimation unit 36.

  The map information storage unit 222 stores map information in which road information such as the number of lanes and road width at each point on the road is stored. The road area specifying unit 224 acquires road information at the current location of the host vehicle according to the position of the host vehicle measured by the GPS sensor 214, and an image captured by the imaging device 12 based on the acquired road information. Identify the upper road area.

  The three-dimensional position calculation unit 228 calculates each feature point in the road area identified by the road area identification unit 224 based on the three-dimensional positional relationship between the imaging device 12 and the road on which the host vehicle is obtained in advance. A three-dimensional position is calculated.

  The motion information acquisition unit 226 acquires the motion of the host vehicle estimated up to the previous time by the motion estimation unit 36, and uses the Kalman filter, for example, to determine the motion of the host vehicle at the present time (first image and second image). Estimate the movement of the vehicle in between.

  The corresponding point prediction unit 30 includes the feature points extracted by the feature point extraction unit 22, the motion of the host vehicle calculated by the motion information acquisition unit 226, and the feature points in the road region calculated by the three-dimensional position calculation unit 228. Based on the three-dimensional position, the position of the feature point in the second image corresponding to the feature point of the first image for which the three-dimensional position is calculated is predicted.

  A motion estimation processing routine according to the second embodiment will be described with reference to FIG. In addition, about the process similar to 1st Embodiment, the same code | symbol is attached | subjected and detailed description is abbreviate | omitted.

  First, in step 100, the first image and the second image captured by the imaging device 12 are acquired. In step 250, the motion of the host vehicle estimated by the motion estimation processing routine up to the previous time is acquired. The motion of the host vehicle is estimated while the first image and the second image are captured.

  In step 104, a predetermined number of feature points are extracted from the first image and the second image, respectively. In step 106, each feature point in the first image is handled from the second image. Each feature point to be searched is retrieved.

  In the next step 252, the position of the host vehicle measured by the GPS sensor 214 is acquired, and road information at the position of the host vehicle is acquired. In step 254, the road area on the first image is specified based on the road information acquired in step 252. In step 256, the three-dimensional position of the feature point in the road area specified in step 254 is calculated based on the positional relationship between the imaging device 12 and the road obtained in advance.

  In step 112, each feature point of the first image is based on the motion of the host vehicle estimated in step 250 and the three-dimensional position of each feature point in the first image calculated in step 256. The position of the feature point on the second image corresponding to is predicted.

  In step 114, the pitch angle fluctuation amount of the host vehicle while the first image and the second image are captured is estimated, and in the next step 116, the reliability of each corresponding point is calculated. In step 118, the motion of the host vehicle is estimated and output to the external device, and the motion estimation processing routine is terminated.

  As described above, with regard to the three-dimensional position of each point, that is, the three-dimensional structure information of the surrounding environment, an approximate structure such as the position of the road surface with respect to the camera mounted on the vehicle can be predicted. Also, the motion information of the host vehicle can be estimated from the motion of the host vehicle up to the previous time.

  Next, a third embodiment will be described. In addition, about the part which becomes the structure similar to 1st Embodiment, the same code | symbol is attached | subjected and description is abbreviate | omitted.

  The third embodiment is mainly different from the first embodiment in that the vehicle motion is estimated without calculating the reliability for each corresponding point.

  As illustrated in FIG. 10, the computer 318 of the motion estimation apparatus 310 according to the third embodiment includes an image acquisition unit 20, a feature point extraction unit 22, a corresponding point search unit 24, and a motion information acquisition unit 26. A three-dimensional position calculation unit 28, a corresponding point prediction unit 30, a pitch angle variation estimation unit 32, and a motion estimation unit 336 are provided. The motion estimation unit 336 is an example of a selection unit and a motion estimation unit.

  The motion estimation unit 336 includes the corresponding points searched by the corresponding point search unit 24, the pitch angle fluctuation amount estimated by the pitch angle fluctuation estimation unit 32, and the feature on the second image predicted by the corresponding point prediction unit 30. Based on the position of the point, the movement of the host vehicle is estimated as described below.

  First, among the corresponding points searched by the corresponding point search unit 24, the image coordinates of the searched corresponding points on the second image and the prediction on the second image obtained from the corresponding point prediction unit 30. Corresponding points whose distance from the image coordinates obtained by adding the pitch angle variation obtained by the pitch angle variation estimation unit 32 to the image coordinates are equal to or smaller than a threshold value are selected.

  Here, the distance between the image coordinates of the retrieved corresponding point on the second image and the image coordinates obtained by adding the pitch variation to the predicted image coordinates is calculated by the following equation (16).

  And the motion of the own vehicle when the 1st image and the 2nd image are imaged is estimated using the selected corresponding point.

At this time, in order to specify the matrix F _r that maximizes the evaluation value, the reliability of the corresponding point is not used, but the evaluation value Z _r calculated by the following equation (17) is used.

  Where n is the number of selected corresponding points.

  In addition, about the other structure and process of the motion estimation apparatus which concern on 3rd Embodiment, since it is the same as that of 1st Embodiment, description is abbreviate | omitted.

  In this way, by selecting the corresponding point on the correct stationary object using the estimated pitch angle variation, even if a sudden pitch variation occurs during traveling, other moving objects such as other vehicles The motion of the host vehicle can be estimated with high accuracy without being affected.

  In the first to second embodiments, the case where the reliability for the corresponding point is calculated according to the above equation (6) has been described as an example. However, the present invention is not limited to this. The reliability for the corresponding points may be calculated using another function in which the reliability decreases as the distance calculated by the equation (16) increases.

  In the first embodiment and the third embodiment, the case where the vehicle speed sensor and the yaw angular velocity sensor are used has been described as an example. However, the present invention is not limited to this, and other sensors are used. Thus, motion information other than the pitch angle component may be measured.

  The program of the present invention may be provided by being stored in a storage medium.

10, 210, 310 Motion estimation device 12 Imaging device 14 Vehicle speed sensor 16 Yaw angular velocity sensor 18, 218, 318 Computer 22 Feature point extraction unit 24 Corresponding point search unit 26, 226 Motion information acquisition unit 28, 228 3D position calculation unit 30 Corresponding point prediction unit 32 Pitch angle variation estimation unit 34 Reliability calculation unit 36, 336 Motion estimation unit 214 GPS sensor 222 Map information storage unit 224 Road area specifying unit

Claims (8)

Search means for searching corresponding feature points between the first image and the second image from each of the first image and the second image obtained by imaging the outside of the moving body;
Motion acquisition means for acquiring a motion representing a change in position and posture of the moving body when each of the first image and the second image is captured;
Three-dimensional position acquisition means for acquiring three-dimensional positions of a plurality of feature points on the first image;
A position for predicting the position on the second image of the plurality of feature points from which the three-dimensional position is acquired by the three-dimensional position acquisition unit based on the movement of the moving body acquired by the movement acquisition unit Prediction means,
The positions of the plurality of feature points on the second image in the plurality of sets of the corresponding feature points searched by the search means, and the plurality of features on the second image predicted by the position prediction means. Based on the difference from the position of the point, the vertical rotational motion estimation that estimates the vertical rotational motion representing the vertical change in the posture of the moving body when each of the first image and the second image is captured. Means,
The plurality of sets of corresponding feature points searched by the search means, the positions of the plurality of feature points on the second image predicted by the position prediction means, and estimated by the vertical rotation motion estimation means. Motion estimation means for estimating the motion of the mobile body when each of the first image and the second image is captured based on the vertical rotational motion of the mobile body;
A motion estimation device including: The plurality of sets of corresponding feature points searched by the search means, the positions of the plurality of feature points on the second image predicted by the position prediction means, and estimated by the vertical rotation motion estimation means. Further, each of the plurality of sets of corresponding feature points searched by the search means based on the vertical rotational movement of the moving body is a correspondence of the feature points on the area representing the stationary object, and A reliability calculation means for calculating a reliability indicating the correctness of the correspondence of the feature points;
The motion estimation unit is configured to determine the motion of the moving object based on the plurality of sets of corresponding feature points searched by the search unit and the reliability calculated for the plurality of sets of corresponding feature points. The motion estimation apparatus according to claim 1, wherein The positions of the plurality of feature points on the second image in the plurality of sets of the corresponding feature points searched by the search means, and the plurality of features on the second image predicted by the position prediction means. Based on the positions of the points changed by the amount of the vertical rotational motion of the movable body estimated by the vertical rotational motion estimation means, all the sets of the corresponding feature points searched by the search means A selection means for selecting a plurality of sets of the corresponding feature points,
The motion estimation device according to claim 1, wherein the motion estimation unit estimates the motion of the moving body based on the plurality of sets of the corresponding feature points selected by the selection unit. It further includes a motion measuring means for measuring the motion of the moving body,
The motion acquisition unit acquires the motion of the moving body when each of the first image and the second image is captured based on the motion of the moving body measured by the motion measuring unit. The motion estimation apparatus according to any one of claims 1 to 3.   The motion acquisition means estimates the motion of the moving body when each of the first image and the second image is captured based on the motion of the mobile body estimated in the past by the motion estimation means. The motion estimation device according to any one of claims 1 to 3.   The three-dimensional position acquisition means is estimated by the motion estimation means, and the motion of the moving body when each of the previous image and the first image captured before the first image is captured, The three-dimensional position of each feature point on the first image is calculated based on each set of feature points corresponding to the previous image and the first image searched by the search unit. The motion estimation apparatus according to claim 5. A road area specifying means for specifying a road area on which the moving body travels on the first image;
The three-dimensional position acquisition means is a road area on the first image specified by the road area specification means based on a positional relationship between a road determined in advance and an imaging means for imaging the outside of the moving body. The motion estimation apparatus according to claim 1, wherein a three-dimensional position of each feature point is calculated. Computer
Retrieval means for retrieving a corresponding feature point between the first image and the second image from each of the first image and the second image obtained by imaging the outside of the moving body;
Motion acquisition means for acquiring a motion representing a change in position and posture of the moving body when each of the first image and the second image is captured;
Three-dimensional position acquisition means for acquiring three-dimensional positions of a plurality of feature points on the first image;
A position for predicting the position on the second image of the plurality of feature points from which the three-dimensional position is acquired by the three-dimensional position acquisition unit based on the movement of the moving body acquired by the movement acquisition unit Prediction means,
The positions of the plurality of feature points on the second image in the plurality of sets of the corresponding feature points searched by the search means, and the plurality of features on the second image predicted by the position prediction means. Based on the difference from the position of the point, the vertical rotational motion estimation that estimates the vertical rotational motion representing the vertical change in the posture of the moving body when each of the first image and the second image is captured. And a plurality of sets of the corresponding feature points searched by the search means, positions of the plurality of feature points on the second image predicted by the position prediction means, and the vertical rotation motion estimation means Based on the up-and-down rotational movement of the moving body estimated by the above, the functioning as a motion estimation means for estimating the movement of the moving body when each of the first image and the second image is captured is performed. Of the program.

Images