WO2005038710A1 - 移動体動き算出方法および装置、並びにナビゲーションシステム - Google Patents

移動体動き算出方法および装置、並びにナビゲーションシステム Download PDF

Info

Publication number
WO2005038710A1
WO2005038710A1 PCT/JP2004/015677 JP2004015677W WO2005038710A1 WO 2005038710 A1 WO2005038710 A1 WO 2005038710A1 JP 2004015677 W JP2004015677 W JP 2004015677W WO 2005038710 A1 WO2005038710 A1 WO 2005038710A1
Authority
WO
WIPO (PCT)
Prior art keywords
motion
movement
moving body
corresponding points
plane
Prior art date
Application number
PCT/JP2004/015677
Other languages
English (en)
French (fr)
Inventor
Kunio Nobori
Satoshi Sato
Original Assignee
Matsushita Electric Industrial Co., Ltd.
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Matsushita Electric Industrial Co., Ltd. filed Critical Matsushita Electric Industrial Co., Ltd.
Priority to EP04792819A priority Critical patent/EP1580687A4/en
Priority to JP2005514877A priority patent/JP3843119B2/ja
Priority to US10/540,135 priority patent/US7542834B2/en
Publication of WO2005038710A1 publication Critical patent/WO2005038710A1/ja

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G05D1/0253—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting relative motion information from a plurality of images taken successively, e.g. visual odometry, optical flow
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G06T7/55—Depth or shape recovery from multiple images
    • G06T7/579—Depth or shape recovery from multiple images from motion
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/50—Context or environment of the image
    • G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60R—VEHICLES, VEHICLE FITTINGS, OR VEHICLE PARTS, NOT OTHERWISE PROVIDED FOR
    • B60R2300/00—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle
    • B60R2300/30—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle characterised by the type of image processing
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60R—VEHICLES, VEHICLE FITTINGS, OR VEHICLE PARTS, NOT OTHERWISE PROVIDED FOR
    • B60R2300/00—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle
    • B60R2300/30—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle characterised by the type of image processing
    • B60R2300/302—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle characterised by the type of image processing combining image information with GPS information or vehicle data, e.g. vehicle speed, gyro, steering angle data
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60R—VEHICLES, VEHICLE FITTINGS, OR VEHICLE PARTS, NOT OTHERWISE PROVIDED FOR
    • B60R2300/00—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle
    • B60R2300/30—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle characterised by the type of image processing
    • B60R2300/304—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle characterised by the type of image processing using merged images, e.g. merging camera image with stored images
    • B60R2300/305—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle characterised by the type of image processing using merged images, e.g. merging camera image with stored images merging camera image with lines or icons
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60R—VEHICLES, VEHICLE FITTINGS, OR VEHICLE PARTS, NOT OTHERWISE PROVIDED FOR
    • B60R2300/00—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle
    • B60R2300/30—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle characterised by the type of image processing
    • B60R2300/307—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle characterised by the type of image processing virtually distinguishing relevant parts of a scene from the background of the scene
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60R—VEHICLES, VEHICLE FITTINGS, OR VEHICLE PARTS, NOT OTHERWISE PROVIDED FOR
    • B60R2300/00—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle
    • B60R2300/80—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle characterised by the intended use of the viewing arrangement
    • B60R2300/806—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle characterised by the intended use of the viewing arrangement for aiding parking
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60R—VEHICLES, VEHICLE FITTINGS, OR VEHICLE PARTS, NOT OTHERWISE PROVIDED FOR
    • B60R2300/00—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle
    • B60R2300/80—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle characterised by the intended use of the viewing arrangement
    • B60R2300/8086—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle characterised by the intended use of the viewing arrangement for vehicle path indication
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60R—VEHICLES, VEHICLE FITTINGS, OR VEHICLE PARTS, NOT OTHERWISE PROVIDED FOR
    • B60R2300/00—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle
    • B60R2300/80—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle characterised by the intended use of the viewing arrangement
    • B60R2300/8093—Details of viewing arrangements using cameras and displays, specially adapted for use in a vehicle characterised by the intended use of the viewing arrangement for obstacle warning

Definitions

  • the present invention relates to a method and an apparatus for calculating a movement of a moving body, and a navigation system.
  • the present invention relates to a technique for calculating the movement of a moving body, and more particularly to a technique for calculating the movement of the moving body in a three-dimensional space using an image captured by a camera installed on the moving body.
  • the main methods for calculating the movement of a moving object such as a vehicle and a self-propelled robot are methods that use sensors that detect the rotation of wheels and the angle, acceleration, and angular acceleration of steered wheels, and cameras installed on the moving object. O Using the image around the moving object taken in step o
  • the method using a sensor that detects the rotation of the wheels and the angle of the steered wheels has the advantage of being stable because the error is small under certain circumstances such as a flat and dry road surface, and the detection is rarely possible. is there.
  • the error of the movement becomes large in principle.
  • the method using a sensor that detects acceleration or angular acceleration in principle, the movement of a moving object can be calculated even when the road surface and wheels slide or when the road surface is not flat.
  • the optical flow is a vector that connects two points in time that correspond to one point on the subject, that is, points on the image, that is, corresponding points. Since a geometric constraint equation is established between the corresponding point and the camera motion, the camera motion can be calculated when multiple corresponding points satisfy certain conditions.
  • Non-Patent Document 1 proposes a method called a so-called eight-point method, in which force motion is calculated from a set of eight or more stationary corresponding points between two images.
  • 8-point method it is difficult to extract corresponding points with a small error of 8 points or more because errors are included in the calculation results of the corresponding points.
  • the error of the camera motion may be large even if the error of the corresponding point is relatively small from the qualitative point.
  • Non-Patent Literature 2 and Patent Literature 1 a method of calculating camera motion and plane equations from four or more corresponding points, assuming that all corresponding input points are on the same plane Has been proposed.
  • This method is an example of a method of calculating camera motion from fewer than eight corresponding points, assuming that a plurality of corresponding points satisfy a certain constraint.
  • the fact that the number of corresponding points used for calculating the camera motion is small means that when the corresponding points include an error with a certain probability, the effect that the calculated camera motion has a low probability of including an error can be expected. Furthermore, when more than four corresponding points are used, the camera motion is calculated in a manner that minimizes the square error, and the measurement included in the corresponding points is calculated. The effect that the influence of the error can be reduced can be expected.
  • a camera motion with a small error is calculated by applying a Kalman filter that reflects a vehicle behavior and a camera arrangement to a camera motion calculated by selecting a corresponding point on a road surface plane.
  • a method has been proposed. By applying a fill-in process to the calculated camera motion, errors in camera motion calculation can be reduced.
  • the camera movement is limited to three parameters of parallel movement and rotation on a plane, and a geometric constraint equation between the corresponding point and the camera movement is given by:
  • a method has been proposed in which a camera motion is calculated by an exploratory method using an optical constraint equation regarding pixel values between small regions near a corresponding point.
  • Non-patent Document 4 among unknown parameters when calculating camera motion from corresponding points, rotation and translation are calculated in two stages, and unknown parameters obtained in one process are calculated. A method has been proposed to reduce the number of evenings and reduce errors in camera motion calculation.
  • Patent Literature 1 International Patent Publication No. 97/35 1 6 1 Pamphlet
  • Patent Document 2 Japanese Patent Application Laid-Open No. 2000-2006-160
  • Patent Literature 3 Japanese Patent Application Publication No. 2003-51015 27
  • Non-Patent Document 1 A computer algorithm for reconstructing a scene fr om two projections., H.C.Longuet-Higgins, Nature, 293: 133-135, Sept. 1981
  • Non-Patent Document 2 “Image Understanding” Kenichi Kanaya, Morikita Publishing, 1990
  • Non-Patent Document 3 "A Robust Method for Computing Vehicle Ego -motion"
  • Non-Patent Document 4 "Recovery of Ego -Motion Using Image Stabilization"
  • Kanaya's method can calculate the camera motion from four or more corresponding points on a plane, but the stationary motion that exists on the same plane from multiple corresponding points in the image It is necessary to extract corresponding points.
  • the filtering process is effective as a method for reducing the effect of errors included in the camera motion calculation result.
  • the cause of camera movements and errors follow the model assumed in advance.For example, if the vehicle is running at low speed, the effects of unpredictable disturbances such as bumps and bumps on the road surface will increase. However, it is difficult to design Phil Yu itself. For this reason, there is a problem that the error increases when the parameter of the phil evening is not appropriate.
  • Non-Patent Document 4 separates rotation and translation in two steps, so for example, if both components of rotation and translation are included to the extent that they cannot be ignored, In some cases, it is not possible to correctly estimate the stage.
  • the present invention provides a technique for obtaining the movement of a moving body on which a camera is installed using a camera image, using a corresponding point (optical point), and the corresponding point having a large error.
  • An object of the present invention is to make it possible to accurately calculate the movement of a moving object even when the moving object is included.
  • the present invention provides a method for obtaining a plurality of corresponding points from two camera images having different shooting times when obtaining a motion of a moving object on which a camera is installed using images around the moving object taken by the camera. Assuming a predetermined plane in the image, a first motion of the moving object is obtained using the obtained plurality of corresponding points, and the first motion and the plurality of corresponding points are used to obtain the moving object. It seeks a second move.
  • unknown parameters can be reduced by assuming a predetermined plane when obtaining the first motion, and by giving a constraint that some corresponding points exist on the predetermined plane. It is possible to obtain a rough, first error-free motion with respect to the plane. Then, by using the first motion, for example, even when the plane is actually inclined, a second motion with a small error can be obtained.
  • the first motion is calculated by selecting a partial corresponding point composed of m (m is an integer of 3 or more) corresponding points out of a plurality of corresponding points by q (q is an integer of 2 or more) A group is selected, a motion candidate of the moving object is calculated from each of the selected q corresponding partial points using a plane expression representing a plane, and the calculated q motion candidates are evaluated by a predetermined evaluation method. It is preferable that the first movement is specified according to the evaluation result.
  • the first motion of the moving object having a small error can be calculated without being affected by a large error such as an outlier included in the corresponding point.
  • the method includes a two-step motion calculation step, calculates a first motion assuming a plane, and then calculates a second motion using the first motion. Even if the road surface is inclined, the motion of the moving object can be calculated with high accuracy.
  • FIG. 1 is a diagram showing a situation in the first embodiment of the present invention.
  • FIG. 2 is a block diagram showing a configuration according to the first embodiment of the present invention.
  • FIG. 3 is a block diagram showing a detailed configuration of the first motion calculator in FIG. 4 (a) and 4 (b) are examples of images taken in the situation of FIG. 1, and FIG. 4 (c) is a diagram showing corresponding points.
  • FIG. 5 is a diagram showing the relationship between camera movement and image change.
  • FIG. 6 is a graph conceptually showing the distribution of the evaluation value E 3 i of the motion Mj for each corresponding point.
  • FIG. 7 is an example of a composite image on which a predicted trajectory corresponding to the calculated motion is superimposed.
  • Fig. 8 (a) is an example of an actual image from the camera installed in the vehicle, and
  • Fig. 8 (b) is a graph showing the relationship between the moving speed of the vehicle and the corresponding point accuracy rate in the situation of Fig. 8 (a).
  • Fig. 9 (a) is an image example when the vehicle is tilted
  • Fig. 9 (b) is the distribution of the evaluation value Eji of the motion Mj for each corresponding point in the situation of Fig. 9 (a).
  • FIG. 10 (a) shows a situation where another vehicle is present behind the vehicle
  • FIG. 10 (b) is an example of an image taken in the situation of FIG. 10 (a).
  • FIG. 11 is a graph showing the relationship between the ratio (R / Rtrue) of the stationary corresponding point ratio R to the true stationary corresponding point ratio Rtrue and the error EM of the force motion.
  • Figures 12 (a) and (b) are diagrams conceptually showing a search using planar parameters.
  • FIG. 13 shows an example of a composite image on which a caution message or the like is superimposed.
  • Figure 14 is an example of a captured image in an indoor parking lot.
  • FIG. 15 is a diagram showing a representation of the movement of a moving object based on the Atsukaman model.
  • FIG. 16 is a configuration diagram of a navigation system according to the second embodiment of the present invention.
  • Figure 17 is a graph showing the trajectory of the vehicle obtained by experiments in an indoor parking lot.
  • FIG. 18 is a diagram showing another configuration for implementing the present invention. BEST MODE FOR CARRYING OUT THE INVENTION
  • a method for obtaining a motion of a moving object on which a camera is installed by using an image around the moving object photographed by the camera includes a plurality of images from two images having different photographing times.
  • the moving body motion calculation method according to the first aspect, wherein the plane is a road surface, a ceiling surface, or a wall surface.
  • the moving object motion calculating method obtains the first motion using the three corresponding points.
  • the first motion calculating step includes a step of updating a plane formula that defines the plane using the first motion or the second motion obtained in the past.
  • the first motion calculation step includes, among the plurality of corresponding points, a partial corresponding point composed of m (m is an integer of 3 or more) corresponding points, q (q Is an integer of 2 or more.)
  • the first step of selecting pairs and the selection in the first step From the set of q corresponding partial points, a second step of calculating the motion candidate of the moving object using a plane expression representing the plane, and q motion candidates calculated in the second step are respectively described.
  • a moving object motion calculating method which includes a third step of performing the above-described first motion specifying in accordance with a result of the evaluation by a predetermined evaluation method.
  • the first motion calculation step comprises: using the plurality of corresponding points and the first motion or the second motion obtained in the past to calculate a stationary corresponding point. Calculating a stationary corresponding point ratio that is a ratio, wherein the third step is to perform an evaluation for specifying the first motion using the calculated stationary corresponding point ratio.
  • the first motion calculating step includes a step of calculating a predicted value of the movement of the moving body from the first motion or the second motion obtained in the past.
  • the third step provides a moving object motion calculation method according to a fifth aspect, wherein the first motion is specified in consideration of the predicted value.
  • the second motion is evaluated while using the first motion as an initial value while evaluating the plurality of corresponding points by a search method.
  • a moving object motion calculation method according to a first aspect to be obtained is provided.
  • the second motion calculation step performs the evaluation using a stationary corresponding point that is a ratio of a stationary corresponding point. provide.
  • the moving object is a vehicle
  • the second motion calculation step includes a partial corresponding point corresponding to the first motion instead of the first motion instead of the first motion.
  • the plane equation is used as an initial value, and the search method is used to evaluate the plurality of corresponding points while tilting the plane around the axle center close to the camera.
  • a moving object motion calculation method according to a fifth aspect for obtaining a second motion is provided.
  • the moving body has non-steered wheels whose axle direction is fixed, and in the first and second motion calculation steps, a coordinate system for representing the motion of the moving body.
  • the vertical axis of the non-steered wheel so as to be orthogonal to the straight line including the axle,
  • a first aspect of the present invention provides a moving body motion calculation method in which a motion of the moving body on a road surface is obtained as an arc motion about the vertical axis.
  • the moving body motion calculation method according to the 11th aspect, wherein the vertical axis passes through a center position of the non-steered wheel.
  • a device for obtaining a motion using a surrounding image of a moving object on which a camera is installed by using the image of the moving object a plurality of images from two images having different shooting times are provided.
  • a second motion calculation unit for obtaining a second motion indicating the motion of the moving body.
  • a navigation device having a position information acquiring unit for acquiring position information of a moving object, and a moving object motion calculating device of the thirteenth aspect, wherein the position information acquiring unit A navigation system for obtaining a current position of the moving object based on the position information thus obtained and the movement of the moving object obtained by the moving object movement calculating device is provided.
  • a vehicle provided with a camera is taken as an example of a moving object, and the motion of the vehicle is obtained using an image behind the vehicle captured by the camera.
  • FIG. 1 is a diagram showing a situation in the present embodiment.
  • a vehicle 120 as a moving body is provided with a camera 120 at a rear portion thereof so as to photograph the rear area.
  • the camera 120 shoots the area around the rear of the vehicle and outputs a series of image sequences.
  • FIG. 2 is a block diagram showing a configuration including the moving object motion calculation device according to the present embodiment.
  • reference numeral 100 denotes a moving body motion calculation device that obtains the movement of the vehicle 1 from an image captured by the camera 120
  • 121 denotes a mobile body motion calculation based on an image captured by the camera 120.
  • An image synthesizing device 122 generates a synthesized image on which the image is superimposed, and a display 122 displays the synthesized image generated by the image synthesizing device 122.
  • the moving body motion calculation device 100 includes a corresponding point calculation unit 101 for obtaining a plurality of corresponding points (optical flow vectors) between two consecutive images output from the camera 120, and a corresponding point calculation unit 1 Using a plurality of corresponding points output from 0 1, assuming a plane, a first movement calculating section 102 for obtaining a first movement Ma indicating the movement of the vehicle 1, and a corresponding point calculating section 1 Using the plurality of corresponding points output from the first motion calculating unit 102 and the first motion Ma output from the first motion calculating unit 102, a second motion Mb indicating the motion of the vehicle 1 is calculated. And a second motion calculation unit 103 to be obtained.
  • the moving body motion calculating apparatus 100, the image synthesizing apparatus 122, and the display 122 in FIG. 2 are installed in the vehicle 1, for example, but may be provided in a different place from the vehicle 1.
  • FIG. 3 is a block diagram showing a detailed configuration of the first motion calculator 102 in the moving body motion calculator 100 of FIG.
  • reference numeral 1 1 1 denotes a partial corresponding point selecting unit for selecting a plurality of sets of partial corresponding points each including a predetermined number of corresponding points from a plurality of corresponding points output from the corresponding point calculating unit 101
  • 1 1 2 denotes
  • a motion candidate calculation unit that calculates a motion candidate of the vehicle 1 from each of the partial corresponding points of each pair selected by the partial corresponding point selection unit 1 1 1, 1 1 3 is calculated by the motion candidate calculation unit 1 1 2
  • a motion evaluation selection unit that evaluates the motion candidate by a predetermined evaluation method and specifies the first motion Ma according to the evaluation result.
  • 1 14 is a plane equation calculation unit that outputs a plane equation representing a predetermined plane assumed when calculating the first movement Ma
  • 1 15 is a stationary corresponding point ratio which is a ratio of stationary corresponding points.
  • a stationary corresponding point ratio calculation unit that outputs R, and 116 is a moving object motion prediction unit that calculates a predicted value of the moving object motion.
  • the world coordinate system (Xw, Yw, Zw) is stationary in a three-dimensional space
  • the vehicle coordinate system (Xc, Yc, Zc) and the viewpoint coordinate system (X, Y, ⁇ ). ) Is fixed to vehicle 1.
  • View point coordinate system (x, y, z) and vehicle coordinate system The relationship between the coordinate values (xc, yc, zc) can be expressed as (Equation 1) using a matrix C representing the positional relationship between coordinate systems.
  • FIG. 4 (a) and 4 (b) show an example of a series of image sequences output from the force camera 120 when the vehicle 1 moves backward in the situation of FIG. 1, and FIG. 4 (a) shows the sequence at time t_l.
  • the captured image FIG. 4 (b) is an image captured at time t.
  • FIG. 4 (c) is a diagram showing corresponding points (arrows in the figure) which are movements of the same point between the two images at time t-1 and time t.
  • FIG. 5 is a diagram showing a relationship between a three-dimensional movement of the camera 120 and an image change.
  • the stationary point X in the world coordinate system becomes the coordinate value (x, y) in the viewpoint coordinate system.
  • , z) to (x,, y,, z,).
  • (Equation 3) is established between the coordinate values (x, y, z), (x,, y,, z ') and the camera motion M.
  • 5 (b) and 5 (c) are images at time t-1 and t, respectively.
  • point X at viewpoint coordinates (X, ⁇ , z) is at image coordinates (u, v) and time t
  • the point X at the viewpoint coordinates (x,, y,, z,) corresponds to the image coordinates (u,, v,), respectively.
  • Equation 5 Holds.
  • f indicates the focal length of the camera.
  • Equation 9 is obtained. Since two equations (Equation 9) hold for each set of corresponding points, if the plane equation and the focal length f of the camera 120 are known, independent (not collinear) corresponding points (u, If three or more sets of (V), (11 ', V') are given, six variables of the motion M (tX, ty, tz, wx, wy, wz) of the camera 120 can be calculated.
  • the vehicle coordinate system (Xc, Yc, Zc) and the viewpoint coordinate system (X, ⁇ , Z) are assumed to be arranged as shown in Fig. 1, and represent the positional relationship between the vehicle coordinate system and the viewpoint coordinate system.
  • C shall be measured in advance.
  • the focal length f of the camera 120 is also obtained in advance, and its value is obtained by multiplying the focal length in the real coordinate system by a conversion coefficient between the real coordinates and the image coordinates.
  • the corresponding point calculation unit 101 temporarily stores a series of image sequences output from the camera 120, and calculates n corresponding points from two consecutive images in the image sequence (n is an integer of 2 or more). ) To detect.
  • n an integer of 2 or more.
  • the number n of the corresponding points is a predetermined value.
  • the two images for detecting the corresponding points do not necessarily have to be continuous, as long as the imaging times are different.
  • the corresponding points may be detected by, for example, the corresponding point search method of Kanade, Lucas and Tomasi (eg, "Detection and Tracking of Point Features", Carlo Tomasi and Takeo Kanade, Carnegie Mellon University Technical Report, CMU -CS-91-132, April 1991.). This method is widely known Since it is a method, detailed description is omitted here.
  • Kanade Lucas and Tomasi
  • This method is widely known Since it is a method, detailed description is omitted here.
  • the plane equation calculation unit 114 outputs a predetermined plane equation.
  • the plane formula used here is an approximation of the road surface in the situation of FIG. 1 as a plane.
  • the plane formula calculating unit 114 calculates the planar parameters S (a, b) of the previously measured (Formula 6). , c) is output.
  • the plane equation calculation unit 114 also has a function of updating the plane equation. This will be described later.
  • the stationary corresponding point ratio calculation unit 115 outputs the stationary corresponding point ratio R.
  • the stationary corresponding point ratio is preferably a ratio of a corresponding point that is stationary with respect to world coordinates and has a correct correspondence among the n corresponding points calculated by the corresponding point calculator 101. It represents.
  • the correspondence of the corresponding points is correct means that the error of the coordinate values of the corresponding points is equal to or less than a predetermined threshold. Note that it is extremely difficult to accurately obtain the stationary corresponding point ratio R in advance in online processing in real time, and in practice, the stationary corresponding point ratio R is the predicted value of the ratio described above. . .
  • the stationary corresponding point ratio calculation unit 115 outputs a predetermined value, for example, 0.5 as the stationary corresponding point ratio R.
  • the stationary corresponding point ratio R 0.5 means that it is predicted that "the n corresponding points calculated by the corresponding point calculating unit 101 include 50% of the stationary corresponding points having a correct correspondence".
  • Means As the predetermined value, for example, the stationary corresponding point ratio is measured a plurality of times in advance in a running state similar to that in FIG. 1, and the lowest value among the measured values may be used.
  • the stationary corresponding point calculation unit 115 also has a function of newly calculating the stationary corresponding point ratio R. This will be described later.
  • the moving body motion prediction unit 116 has a function of obtaining a predicted value of the motion of the vehicle 1, but here, it is assumed that no particular processing is performed.
  • the partial corresponding point selection unit 111 includes n corresponding points Pi output from the corresponding point calculation unit 101, the plane expression S output from the plane expression calculation unit 114, and the stationary corresponding point ratio calculation unit.
  • R output from 1 15
  • q pairs of partial corresponding points composed of m corresponding points are selected.
  • the number m of corresponding points forming the partial corresponding points is the number of corresponding points required to calculate the movement of the moving object.
  • m 3.
  • m can be greater than 3.
  • the partial corresponding point selection unit 111 calculates the number q of partial corresponding point sets.
  • R r is a road surface corresponding point rate
  • z is a guarantee rate
  • the road surface stationary corresponding point ratio R r is a probability that each corresponding point selected as a partial corresponding point exists on the road surface, is stationary with respect to world coordinates, and has a correct correspondence relationship. Therefore, if there is no correlation between the probability R s that the corresponding point exists on the road surface and the stationary corresponding point rate R of the corresponding point, the stationary corresponding point ratio R is calculated as the probability R s that the corresponding point exists on the road surface.
  • the product of the multiplication is the road stationary point ratio Rr. Also, if the corresponding points existing on the road surface can be accurately selected, and the static corresponding point ratio of the selected corresponding points is R, the road corresponding point ratio R of the selected corresponding points is , Which is equal to the stationary corresponding point ratio R.
  • R r 0.3 is used as the predetermined value.
  • the predetermined value for example, in a running state similar to that in FIG. 1, a value obtained by measuring the probability Hs that the selected corresponding point exists on the road surface and multiplying this by the stationary corresponding point ratio R is used. Good.
  • Guarantee rate z statistically guarantees that there is at least one set of partial corresponding points where all (m) corresponding points out of q sets of partial corresponding points are composed of stationary correct corresponding points on the road surface. It is rate to do.
  • z 0.999 Used.
  • the partial corresponding point selection unit 111 selects q sets of partial corresponding points consisting of different m corresponding points from n corresponding points using the calculated number of pairs q and outputs the selected sets.
  • a corresponding point may be selected at random from all n corresponding points, or a corresponding point satisfying a specific condition may be extracted and then selected at random.
  • the latter is used. That is, after extracting corresponding points in the image area where the plane equation S can exist, q sets of partial corresponding points consisting of m different corresponding points are randomly selected from the extracted corresponding points and output. For example, in FIG. 4 (d), the area where the plane S can exist in the image is limited to the shaded area below the horizontal line.
  • the road surface stationary corresponding point ratio R r when selecting the partial corresponding points after extracting the corresponding points existing in the shaded area is the road stationary corresponding point ratio when selecting randomly from n corresponding points.
  • R r is higher than R r, and as a result, the number q of partial corresponding points can be made smaller.
  • the motion candidate calculation unit 112 calculates q motion candidates from the q sets of partial corresponding points selected by the partial corresponding point selection unit 111 using the plane equation S.
  • the corresponding point in the image P i (u, v, u 5, ⁇ ') is located on the plane equation S (a, b, c) , and, if the camera motion M was assumed that small , (Equation 9) holds.
  • Equation 9 assuming that the corresponding point and the plane equation are known and the camera movement M is unknown, the camera movement M can be calculated if there are three or more corresponding points.
  • the motion evaluation selecting unit 113 selects the q motions calculated by the motion candidate calculating unit 112.
  • the n corresponding points P i output from the corresponding point calculating unit 101 and the stationary corresponding point ratio R output from the stationary corresponding point ratio calculating unit 115 are used. If necessary, the motion prediction output from the moving body motion prediction unit 116 may be used. This will be described later.
  • (Equation 11) means that there are four independent linear equations for the unknown number 4, and the corresponding point P i (ui, vi, ui ', vi,) calculated from the image contains an error. Therefore, the three-dimensional coordinates (xi, yi, zi) are calculated using the least squares method.
  • the camera motion corresponding to is selected as the first motion Ma.
  • the evaluation values E ji are sorted in ascending order, the horizontal axis represents the corresponding point numbers (1,..., n) after sorting, and the vertical axis represents the evaluation value E ji of the camera motion Mj for each corresponding point. Value.
  • the first motion calculation unit 102 calculates the first motion Ma, which is the motion of the vehicle 1, using the n corresponding points Pi on the image at the time t-1 and the time t. And output.
  • the second motion calculation unit 103 uses the first motion Ma output from the first motion calculation unit 102 and the n corresponding points P i output from the corresponding point calculation unit 101 to generate a vehicle.
  • a second movement Mb indicating both movements is calculated.
  • the second motion Mb is calculated by a search method using the first motion Ma as an initial value.
  • the first motion Ma (tX, ty, tz, wx, w, wz) is set as an initial value, and a predetermined minute motion (dtx, dty, dtz, dwx, dwy, dwz) to obtain multiple motion candidates.
  • o 3 to 6 motion candidates
  • second motion calculating section 103 outputs the motion candidate Mb k selected at that time as second motion Mb.
  • the moving body motion calculation apparatus 100 calculates the motion of the vehicle 1 between the time t-1 and the time t. Further, by repeating the same processing for the image sequence output from the camera 120, the movement of the vehicle 1 can be continuously calculated.
  • the image synthesizing device 121 receives the image captured by the camera 120 and the second motion Mb output from the moving object motion calculating device 100, and And generating a composite image on which information regarding the image is superimposed.
  • the display 122 displays the composite image generated by the image composition device 122.
  • FIG. 7 is an example of the displayed composite image, in which a predicted trajectory TR corresponding to the second movement Mb of the vehicle 1 is superimposed on an image captured by the camera 120.
  • the motion of the vehicle can be accurately calculated.
  • Fig. 8 (a) is an example of an image actually taken with a camera installed behind the vehicle. The corresponding points were searched for the moving image captured in the scene shown in Fig. 8 (a) by the method described above.
  • Figure 8 (b) is a graph showing the relationship between the corresponding point correct answer rate and the moving speed of the vehicle at that time. Here, the ratio of the corresponding points where the error on the image coordinates is within one pixel is defined as the corresponding point correct answer rate. In the graph of Fig. 8 (b), the values of time and moving speed are scaled appropriately.
  • the first motion Ma is accurately calculated in the first motion calculation unit 102 even if the corresponding point correct answer rate is greatly reduced. be able to. That is, in the case of the method of this embodiment, if at least one correct partial corresponding point is included in q partial corresponding points, q motion candidates M j Contains at least one correct move. Furthermore, using the stationary corresponding point rate: R corresponding to the correct answer rate of the corresponding point search, q movements M j are evaluated by the (nxR) -th evaluation value from the better evaluation value for the n corresponding points. It is possible to calculate the correct motion Ma by performing evaluation using.
  • the motion of the vehicle can be accurately calculated.
  • the first motion Ma is calculated using a horizontal and flat road surface equation obtained by measurement in a state where the vehicle is stationary as a predetermined plane equation.
  • the road surface such as a hill or a hoistway is inclined, or when the posture of the vehicle changes due to the loading of occupants, luggage, or the operation of the steering wheel, the predetermined flat type measured in advance It no longer matches the road surface plane on which the vehicle is running.
  • Fig. 9 (a) is an example of an image when the car! 3 ⁇ 4 is tilted.
  • the road surface in the image also leans, so that it does not match the predetermined plane formula measured in advance.
  • the motion M j calculated using (Equation 9) includes an error.
  • the evaluation value E ji of the motion M j for the n corresponding points gradually increases as compared with the case where the plane equation is correct, and the evaluation value E j also increases. Becomes. Therefore, even the first motion M a having the smallest evaluation value includes an error.
  • the second motion Mb is calculated using the first motion Ma as an initial value and using a search method based on n XR evaluation values from the smaller one.
  • the evaluation function is smooth between the initial value and the true value (or the best value)
  • the true value can be obtained by an exploratory method.
  • the evaluation value used in the present embodiment satisfies the above condition as long as it is near the true value. Therefore, even if the first motion M a includes an error because the actual road surface no longer matches the predetermined plane formula, the error is calculated by the second motion calculation unit 102. of A small second motion M b can be calculated.
  • the system that monitors the area around the vehicle with a camera has the following features. That is, a camera with a wide viewing angle is installed so that the driver can monitor the surroundings of the vehicle, typically, as shown in Fig. 4, so that images from the road surface near the vehicle to obstacles above the horizon are displayed in the image. Is done.
  • the image of the camera installed in this way can be expected to reflect the road surface in almost all scenes.
  • the motion of the vehicle can be calculated from three corresponding points using (Equation 9).
  • the 'method without assuming a plane formula' requires at least four or more corresponding points in order to obtain a motion (or a motion candidate), and is therefore susceptible to errors in the calculation process.
  • the effect of being able to avoid the effects of errors can be obtained.
  • FIG. 10 (a) is a diagram showing a situation in which another moving vehicle 2 exists behind the own vehicle 1.
  • own vehicle 1 is moving backward (right in the figure), and other vehicle 2 is moving forward (left in the figure).
  • the image shown in Fig. 10 (b) is Taken by 120.
  • the movement of 1 is a movement with a large error.
  • the movement of the host vehicle 1 is calculated by assuming the equation of the road surface plane. For this reason, in the first motion calculation unit 102, the other vehicle separated from the road surface plane
  • the movement Mj of the vehicle 1 obtained from the corresponding point on 2 includes a large error, and the evaluation value Ej increases.
  • the motion obtained from the corresponding point on the other vehicle 2 is not selected in the first motion calculating unit 102, and the motion with respect to the road surface plane obtained from the corresponding point on the road is Movement is selected as Ma. From the above, it can be said that the present embodiment has an effect that the movement of a moving object can be correctly calculated even when another moving object is present in the image.
  • the plane equation calculation unit 114 can also update the plane equation using the first motion Ma obtained by the first motion calculation unit 102 from the past image.
  • the second motion Mb may be used instead of the first motion Ma, or both may be used. You may use it.
  • the calculation may be performed each time the motion calculation is performed a predetermined number of times, or may be performed when a change occurs in the road surface or the vehicle condition.
  • the stationary corresponding point ratio calculating unit 115 includes a plurality of corresponding points output from the corresponding point calculating unit 101 and a first motion calculating unit based on past images. It is also possible to calculate the stationary corresponding point ratio using the first motion Ma calculated in 102.
  • the stationary corresponding point calculation unit 115 calculates the first motion Ma calculated by the first motion calculation unit 102 with respect to n corresponding points as described above (Equation 1). 1) and (Equation 12) are evaluated. Then, a ratio of the n corresponding points whose evaluation value Ei is equal to or smaller than a predetermined threshold is calculated, and this is stored as a new stationary corresponding point ratio. Then, when the next motion calculation is executed, the retained corresponding point ratio is output. Thereafter, by repeating the same processing sequentially, the stationary corresponding point rate is updated.
  • the calculated motion includes an error with respect to the true motion.
  • the relationship between the stationary corresponding point ratio R, the true stationary corresponding point ratio Rtrue, and the camera motion error EM is shown in the graph of FIG.
  • the graph of FIG. 11 schematically shows the common tendency based on the results of experiments performed by the inventors of the present application.
  • the stationary corresponding point ratio R and the true stationary corresponding point ratio Rtrue match (the ratio R / Rtrue is 1).
  • the camera motion error EM becomes the smallest, and the camera motion calculation accuracy decreases even when the stationary corresponding point ratio R becomes larger or smaller than the true stationary corresponding point ratio Rtrue. .
  • the stationary corresponding point ratio R (t-1) calculated using the motion obtained at time t-1 is expected to be close to the true stationary corresponding point ratio Rtrue (t) in the image at time t. You. Therefore, by updating the stationary corresponding point ratio, it is possible to calculate a motion with a small error.
  • the moving object motion was calculated using the stationary corresponding point ratio R (t-1) calculated at time t-1.
  • the stationary corresponding point ratio R (t) obtained from the moving object motion was calculated. If the stationary corresponding point ratio R (t-1) is lower, the moving object motion at time t may be recalculated using the new stationary corresponding point ratio R (t).
  • the second motion Mb may be used instead of the first motion Ma, or both may be used.
  • the moving body motion prediction unit 116 can predict the motion of the vehicle 1 using the first motion Ma obtained in the past by the first motion calculation unit 102, and can output the motion prediction value.
  • the motion evaluation selection unit 113 calculates the first motion Ma in consideration of the motion prediction value output from the moving body motion prediction unit 116.
  • the moving body motion estimating unit 116 calculates that the motion of the vehicle 1 is substantially constant velocity Prediction is performed assuming velocity motion. That is, the first motion Ma calculated at time t-1 is stored and output as the motion prediction value Mr t at time t.
  • the motion prediction value Mr t is also evaluated, and by selecting the first motion Ma, even if a correct motion cannot be obtained from the q partial correspondence points, the vehicle 1 If the motion is close to constant velocity / constant angular velocity motion, the motion prediction value Mr t is selected as the first motion Ma. As a result, it is possible to avoid a case where a large error occurs in the first movement Ma.
  • the motion prediction is performed assuming that the moving body is moving at a constant velocity and a constant angular velocity.
  • the method of the motion prediction is not limited to this.
  • the motion prediction may be performed assuming that the mobile body is performing uniform acceleration / conformal acceleration motion.In this case as well, a case where a large error occurs in the first motion Ma is avoided.
  • the motion prediction of the moving object may use the second motion Mb instead of the first motion Ma, or may use both of them.
  • the number of unknown parameters can be reduced, and therefore, the first motion Ma with a small large error can be obtained. Furthermore, the second motion Mb with a small error can be obtained by using a search method using the first motion Ma as an initial value.
  • (Equation 11) and (Equation 12) are used to evaluate motion candidates.
  • the evaluation value Ej i expressed was used, the evaluation method is not limited to this, and any evaluation value may be used as long as it is a value for evaluating whether the corresponding point matches the camera motion. It doesn't matter.
  • an evaluation value indicating a relationship between a position (motion) between cameras and a corresponding point which is called an epipolar constraint or a basic matrix, may be used.
  • (Equation 13) is a specific example.
  • the (nxR) -th evaluation value from the smaller of the evaluation values Eji for the n corresponding points is selected as the evaluation value Ej.
  • the method of specifying j is not limited to this.
  • the average value of the evaluation values from the smaller evaluation value E ji to the (nxR) -th evaluation value may be used as the evaluation value E j.
  • the method of specifying the evaluation value E j may be changed according to the value of the stationary corresponding point ratio R. For example, if the stationary corresponding point rate R is larger than a predetermined threshold, the (nxR) th evaluation value from the smaller of the evaluation values E ji is set to Ej, while the stationary corresponding point rate R is set to a predetermined threshold. If the evaluation value is smaller than the above, the average of the evaluation values from the smaller one of the evaluation values E ji to the (nxR) -th order may be used as E j.
  • the stationary corresponding point ratio R when the stationary corresponding point ratio: R is relatively small, the influence of errors included in the evaluation values can be suppressed by taking the average of the plurality of evaluation values.
  • the stationary corresponding point ratio: R when the stationary corresponding point ratio: R is relatively high, by using one evaluation value as the evaluation value Ej, the small difference near the (nxR) th is not averaged, and the evaluation value is evaluated. The effect is reflected in Ej.
  • the calculation of the second motion Mb is performed by a search method using the first motion Ma as an initial value.
  • the present invention is not limited to such a method.
  • the first motion calculating unit 102 instead of the first motion Ma, calculates a partial corresponding point and a plane expression from which the first motion Ma is calculated, and the second motion calculating unit 1 Output to 03.
  • the second motion calculating unit 103 uses the partial corresponding points and the plane expression and the n corresponding points output from the corresponding point calculating unit 101 to calculate the second motion Mb by another search method. It may be calculated.
  • the camera motion candidate Mbk (k2 1,..., O) is calculated using (Equation 9).
  • the evaluation value Ebk of the motion candidate Mbk for the n corresponding points is calculated by the above-described method, and the motion candidate Mbk having the minimum evaluation value Ebk is selected. Further, the above-described processing is repeatedly executed using the plane equation Sk corresponding to the selected Mbk as an initial value, and the processing is terminated when the value converges or when the number of repetitions reaches a predetermined number. Then, the motion candidate Mbk selected at that time is output as the second motion Mb.
  • This method recalculates the motion based on the plane formula S close to the actual road surface, when the main cause of the error included in the first motion Ma is the difference between the plane formula S and the actual road surface Therefore, it is thought that motion with small error can be calculated. In addition, when this method is used, the search space can be narrowed, so that there is an advantage that the processing is lightened.
  • c is a scale term, so searching for c may not be effective.
  • the plane equation S (a, b, c) of the viewpoint coordinate system is used as the initial value, and when the search is performed using the plane equation Sk with a small increase / decrease of a certain small change, Fig. 12 As shown in (a), the search uses a plane formula that is unrelated to the actual road surface fluctuation. On the other hand, as shown in FIG.
  • a predetermined minute change for example, when the plane is inclined with respect to the axle center close to the camera 120 (the center of the rear wheel axle in the figure), a predetermined minute change (da, db , dc), the search using the plane equation S k with increased or decreased Since it can be said that the search reflects position fluctuations with the road surface, it is possible to calculate motion with a smaller number of repetitions and to calculate motion with a smaller error.
  • a predetermined minute change for example, in a state similar to FIG. 12, various plane formulas S and their minute changes (da, db, dc) are measured in advance and used. Just fine.
  • the movement locus of the vehicle 1 is superimposed on the image as shown in FIG. 7 using the second movement Mb output from the moving body movement calculation device 100,
  • the usage form of the moving body movement is not limited to this.
  • the image synthesizing apparatus 122 inputs the image coordinates and the three-dimensional position of (nxR) corresponding points from the smaller evaluation value Ei of the first motion Ma. If there are (nxR) corresponding points whose three-dimensional position is on the path of the moving object and is within a predetermined range, a caution as shown in Fig. 13 The message may be superimposed and synthesized, and the corresponding corresponding point may be highlighted by surrounding the frame with a frame.
  • the output of the moving body motion calculating apparatus 100 is supplied to the image synthesizing apparatus 121 and the display 122, but this is performed by the moving body motion calculating apparatus 100. It is not intended to limit the use of the moving object movement or the output thereof, and may be used in combination with any other device.
  • the camera 120 is installed at the rear of the vehicle 1 and photographs the rear of the vehicle.
  • the position and the photographing range of the camera are not limited to this.
  • the camera can be in any position as long as the surroundings are photographed.
  • the plane assumed for the motion calculation is the road surface, but the assumed plane is not limited to the road surface, and the relative position with respect to the moving object may be given in advance. Any plane can be used as long as it can be made.
  • the ceiling surface can also be given a relative position with respect to the moving object in advance, so that the ceiling surface may be assumed to be a flat surface instead of a road surface.
  • the wall surface may be assumed to be a plane instead of the road surface.
  • the vehicle coordinate system may be set so that the vertical axis is orthogonal to the straight line including the axle of the non-steered wheels of the vehicle, and the movement of the vehicle on the road surface may be represented as an arc movement around the vertical axis.
  • FIG. 15 is a diagram showing a representation of the movement of a moving object based on the so-called Atsuri-man model.
  • Fig. 15 for a vehicle whose front wheel is a steered wheel and whose rear wheel is a non-steered wheel whose axle is fixed to the vehicle body, the center of the rotational component on the horizontal plane is placed on the extension line of the axle of the non-steered wheel. I have.
  • a navigation system is configured by combining a moving body motion calculation device configured in the same manner as in the first embodiment with a navigation device.
  • the navigation device here has a function to measure the current position using radio waves from artificial satellites, and a function to display the current position and guide the display to the destination using the current position and map information. Shall have.
  • FIG. 16 is a configuration diagram of a navigation system including the mobile object motion calculation device according to the present embodiment.
  • the same components as those in FIGS. 2 and 3 are denoted by the same reference numerals, and detailed description thereof will be omitted.
  • the navigation device 130 includes a position information acquisition unit 131, a position information calculation unit 132, an image synthesis unit 133, and a map information storage unit 134.
  • the position information acquisition section 131 has a so-called GPS function, and receives radio waves from a plurality of artificial satellites to acquire information on the position of the vehicle. Also, the position information acquisition unit 131 can receive radio waves from the satellite, and if the position information can be acquired by the GPS function (when the GPS function is valid), outputs the acquired position information and outputs the acquired position information from the satellite. If it is not possible to obtain the location information by the GPS function (for example, when the GPS function is disabled) due to the inability to receive radio waves, it is assumed that the location information cannot be obtained. Output the information shown.
  • the position information calculating unit 132 receives the position information from the position information obtaining unit 131, and the moving object motion information (second motion Mb) output from the moving object motion calculating device 100, Calculate the current position of the vehicle.
  • the map information storage unit 134 stores information on roads, parking lots, and maps corresponding to the map coordinates.
  • the image synthesizing unit 133 inputs the current position from the position information calculating unit 132, reads the road and map information corresponding to the current position from the map information storage unit 134, converts it into an image, and outputs it. .
  • the operation of the navigation system in FIG. 16 will be described. Now, it is assumed that the vehicle is outside, is capable of receiving radio waves from satellites, and has the GPS function enabled. At this time, the position information obtaining unit 13 1 outputs the position information obtained by the GPS function, and the position information calculating unit 13 2 uses the position information received from the position information obtaining unit 13 1 as the current position as it is. Output and store temporarily.
  • the image synthesizing unit 133 receives the current position from the position information calculating unit 132, reads out the map information around the current position stored in the map information storage unit 134, converts it into a map image, and outputs it. This map image is displayed on the display 122.
  • the position information calculating section 13 2 receives the moving object motion information input from the moving object motion calculating apparatus 102. ignore.
  • the position information acquisition unit 1331 outputs information indicating that the GPS function is invalid.
  • the position information calculation unit 132 integrates the moving object movement output from the moving object movement calculation device 100 into the current position stored inside, and sets the result as a new current position. Output and temporarily store.
  • the image synthesizing unit 13 3 operates in the same way as when the GPS function is enabled, receives the current position from the position information calculation unit 13 2, and stores a map around the current position stored in the map information storage unit 1 34. The information is read, converted to a map image and output. This map image is displayed on the display 122.
  • the position information calculation unit 13 2 calculates the current position, reads out the parking lot information stored in the map information storage unit 13 4 as necessary, and Send to 4.
  • the plane type calculation unit 114 reads out and outputs the plane type information included in the parking lot information as needed.
  • the position information calculation unit 1332 sends the position information obtained by the position information acquisition unit 131 Is output as the current position.
  • FIG. 17 is a graph showing the trajectory of a vehicle obtained by an experiment using the moving object motion calculation method according to the present invention in an indoor parking lot.
  • the moving body motion calculation method according to the present invention it is possible to calculate the moving body motion using only the camera image installed in the vehicle, and in addition, when the road surface is inclined. Since the error can be suppressed to a small value, it is possible to calculate a movement trajectory with a small error even in an indoor parking lot as shown in FIG.
  • the error of the corresponding point search tends to increase (the corresponding point correct answer rate decreases) as the moving speed of the moving object increases, and the corresponding point If the error becomes too large, the error in the movement of the moving object becomes large.
  • the use of the mobile body motion calculation device 100 is limited to indoors where radio waves from satellites cannot be received, and radio waves from satellites can be received. Outdoors, the location information obtained by the GPS function is used as the current location. This makes it possible to calculate the current position with high accuracy.
  • the method of calculating the current position is selectively switched according to whether or not the position information can be acquired by the GPS function. In both cases, the current position can always be calculated.
  • the switching of the current position calculation method is performed according to whether the current position can be acquired by the GPS function.
  • the calculation method of the current position may be switched according to whether or not the determined current position is near the entrance of the indoor parking lot on the map information.
  • a temporary The defined plane is not limited to the road surface, and may be any plane as long as it can provide a relative position with respect to the moving object in advance.
  • the plane formula when the ceiling is assumed to be a plane is stored in the map information storage unit 134 together with the map information.
  • the position information calculation unit 13 2 reads out the plane formula of the ceiling of the indoor parking lot from the map information storage unit 134, and performs the first movement. The output is output to the plane expression calculation unit 114 of the calculation unit 102.
  • the plane expression calculation unit 1 ⁇ 4 outputs the plane expression. Thereby, the moving body motion calculation apparatus 100 calculates the moving body movement based on the ceiling plane.
  • the moving body motion calculation device 100 selects and uses the plane expression according to the current position, so that It is possible to calculate highly accurate moving body movement.
  • the vehicle has been described as an example of a moving object.
  • the type of the moving object is not limited to a vehicle, and may be, for example, a robot.
  • each processing unit and device may be realized by hardware or may be realized by software.
  • each processing means is realized by software using a computer having a CPU 141, a ROM 144, a RAM 144, and an image input / output function as shown in FIG. Good.
  • the present invention can accurately determine the movement of a moving object on which a camera is installed by using a camera image. Can be used for location presentation services, etc. Is,

Abstract

移動体動き算出装置（１００）において、対応点算出部（１０１）はカメラ１２０によって撮影された画像間の対応点を算出する。第１の動き算出部（１０２）は画像内に所定の平面を仮定して、対応点を用いて移動体の第１の動き（Ｍａ）を算出する。第２の動き算出部（１０３）は第１の動き（Ｍａ）と対応点を用いて、第２の動き（Ｍｂ）を算出する。

Description

明細書 移動体動き算出方法および装置、 並びにナビゲ一シヨンシステム 技術分野
本発明は、 移動体の動きを算出する技術に関し、 特に移動体に設置されたカメ ラで撮影した画像を用いて、 3次元空間での移動体自身の動きを算出する技術に 関するものである。 背景技術
車両ゃ自走ロボットなどの移動体の動きを算出する主な方法として、 車輪の回 転や操舵輪の角度、 加速度、 角加速度などを検出するセンサを用いる方法と、 移 動体に設置されたカメラで撮影した移動体周辺の画像を用いる方法との 2つがあ る o
車輪の回転や操舵輪の角度を検出するセンサを用いる方法は、 平坦で乾いた路 面などの一定の環境下では誤差が小さく、 検出不能となる場合も少ないため、 安 定であるという利点がある。 しかし、 路面と車輪が滑る、 路面が平面ではない、 といった環境下では、 原理的に動きの誤差が大きくなるという欠点がある。 また、 加速度や角加速度を検出するセンサを用いる方法では、 原理的には、 路 面と車輪が滑る場合や路面が平面ではない場合でも、 移動体の動きを算出可能で ある。 しかし、 重力加速度に対して動きや姿勢の変化による加速度や角加速度の 比率が小さい、 センサで発生する雑音ゃドリフトが比較的大きい、 などの理由に より、 特に路面が水平な平面でない場合や移動体が傾いている場合に、 動きの誤 差が大きくなるという欠点がある。
一方、 移動体に設置されたカメラで撮影した移動体周辺の画像を用いる方法の 場合、 路面が平面でない場合や移動体が傾いている場合でも、 原理的には、 移動 体の動きを正しく算出することが可能である。
カメラの動きを、 撮影した動画像を用いて求める代表的な方法として、 ォプテ ィカルフローを用いる方法が提案されている。 オプティカルフローは、 被写体上 のある 1点に対応する、 時間的に連続した 2つの画像上の点すなわち対応点を結 ぶベクトルである。 対応点とカメラ動きとの間には、 幾何学的な拘束式が成り立 つため、 複数の対応点が一定の条件を満たす場合にカメラの動きを算出すること ができる。
例えば、 非特許文献 1では、 2画像間の静止した 8点以上の対応点の組から力 メラ動きを算出する、 いわゆる 8点法と呼ばれる方法が提案されている。 しかし ながら、 この 8点法では、 対応点の算出結果に誤差が含まれるために、 8点以上 の誤差の小さい対応点を抽出することが困難であるという課題や、 カメラ動きの 導出式の不安定性から、 対応点の誤差が比較的小さくてもカメラ動きの誤差が大 きくなることがあるという課題があることが、 広く知られている。
また、 8点法に限らず、 実際に撮影された画像を用いてカメラの動きを算出す る場合、 画像処理の過程で発生する、 はずれ値様の大きな誤差や、 画像中の雑音 や量子化誤差などの計測誤差を 0にするのは、 実質的に不可能といってよい。 そのため、 画像を用いてカメラ動きを算出する方法では、 誤差の影響を抑制す ることを目的として、 さまざまな方法が提案されている。 例えば、 対応点間の拘 束条件を与えることによってより少ない対応点からカメラ動きを算出する方法や 、 カメラ動きの算出過程における未知パラメ夕数を減らしてカメラ動きを算出す る方法、 未知パラメ夕を多段階に算出する方法、 カルマンフィル夕などのフィル 夕を用いる方法、 などがある。
例えば、 非特許文献 2および特許文献 1では、 入力となる対応点は全て同一平 面上に存在するものと仮定した上で、 4点以上の対応点からカメラ動きと平面の 式を算出する方法が提案されている。 この方法は、 複数の対応点がある拘束条件 を満たすものと仮定して、 8点より少ない対応点からカメラ動きを算出する方法 の例である。
カメラ動きの算出に用いる対応点の数が少ないということは、 対応点がある確 率で誤差を含む場合に、 算出したカメラ動きが誤差を含む確率が低くなるという 効果が期待できる。 さらに、 4点より多くの対応点を用いた場合には、 二乗誤差 を最小化する方法でカメラ動きを算出することによって、 対応点に含まれる計測 誤差の影響を低減できるという効果が期待できる。
また例えば、 特許文献 2では、 路面平面の対応点を選択して算出されたカメラ 動きに対して、 車両挙動やカメラ配置を反映したカルマンフィル夕をかけること によって、 誤差の小さいカメラ動きを算出する方法が提案されている。 算出した カメラ動きにフィル夕処理をかけることによって、 カメラ動き算出における誤差 を低減することができる。
また例えば、 特許文献 3および非特許文献 3では、 カメラの動きを平面上の平 行移動と回転の 3パラメ夕に限定した上で、 対応点とカメラ動き間の幾何学的拘 束式と、 対応点の近傍の小領域間の画素値に関する光学的拘束式を用いて、 探索 的な手法によってカメラの動きを算出する方法が提案されている。 カメラの動き を限定して未知パラメ夕を減らすことによって、 カメラ動き算出における誤差を 低減することができる。
また例えば、 非特許文献 4では、 対応点からカメラ動きを算出する際の未知パ ラメ夕のうち、 回転と平行移動とを 2段階に分けて算出することで、 1回の処理 で求める未知パラメ夕を少なくし、 カメラ動き算出における誤差を低減する方法 が提案されている。
(特許文献 1 ) 国際公開第 9 7 / 3 5 1 6 1号パンフレツト
(特許文献 2 ) 特開 2 0 0 1 - 2 6 6 1 6 0号公報
(特許文献 3 ) 特表 2 0 0 3 - 5 1 5 8 2 7号公報
(非特許文献 1 ) "A computer algorithm for reconstructing a scene fr om two projections. " , H. C. Longuet-Higgins, Nature, 293: 133 - 135, Sept. 1981
(非特許文献 2 ) "画像理解"金谷健一、 森北出版、 1 9 9 0年
(非特許文献 3 ) "A Robust Method for Computing Vehicle Ego -motion"
, G.P. Stein, 0. Mano and A. Shashua, IEEE Intelligent Vehicles Symposiu m ( IV2000),Oct. 2000
(非特許文献 4 ) "Recovery of Ego -Motion Using Image Stabilization "
, M. Irani, B. Rousso and S. Peleg, ICVPR, pp454-460, March, 1994 発明の開示
(発明が解決しょうとする課題）
しかしながら、 上述した従来の方法では、 それぞれ次のような問題があった。 まず、 対応点の拘束条件を与えることによって、 より少ない対応点からカメラ 動きを算出する方法では、 多数の対応点の中から、 特定の拘束条件を満たす対応 点を抽出する必要がある。 しかし、 この対応点を抽出する過程における誤りを 0 にすることは困難であり、 誤って抽出された対応点からカメラ動きを算出した場 合には、 カメラ動きの誤差が大きくなるという別の課題がある。 例えば、 金谷の 方法 （非特許文献 2 ) は、 平面上の 4点以上の対応点からカメラの動きを算出可 能であるが、 画像中の複数の対応点から同一平面上に存在する静止した対応点を 抽出する必要がある。 しかし、 移動体に設置したカメラで撮影された画像に適用 する場合には、 同一平面上に存在する静止した対応点を誤りなく抽出するのは困 難である。 このため、 対応点を抽出する過程における誤りによってカメラ動きの 誤差が大きくなつてしまう、 という別の問題が生じる。
また、 フィル夕処理は、 カメラ動きの算出結果に含まれる誤差の影響を低減す る方法として有効である。 しかし、 カメラ動きや誤差の原因が予め仮定したモデ ルに従うことが前提であるため、 例えば、 車両が低速で走行する場合、 路面の凹 凸ゃ段差などの予測困難な外乱の影響が大きくなるため、 フィル夕そのものの設 計が困難である。 このため、 フィル夕のパラメ夕が適当でない場合に、 逆に誤差 が大きくなつてしまうという問題がある。
また、 未知パラメ夕を減らす方法では、 実際のカメラの動きが未知パラメ夕を 減らした関係式と近い場合は、 誤差が小さい動きを算出できる。 しかし、 実際の カメラの動きが未知パラメ夕を減らした関係式から外れた場合、 誤差が大きくな つてしまう。 例えば、 Steinらの方法 （特許文献 3および非特許文献 3 ) では、 力 メラは特定の平面上しか移動しないと仮定して、 平面上の平行移動 2パラメ夕と 平面上の回転 1パラメ夕の計 3パラメタのみを算出している。 このため、 カメラ が傾いた場合や路面が平面でない場合に、 カメラ動きの誤差が大きくなるという 問題がある。
さらに、 未知パラメ夕を 2段階に算出する方法では、 各段階で適切な未知パラ メタが選択されている場合、 有効である。 しかし、 パラメ夕の選択が適当でない 場合、 各段での推定において、 未知パラメタを減らす方法と同様に、 誤差が大き くなる。 例えば、 Iraniらの方法 （非特許文献 4 ) は、 回転と平行移動を分けて 2 段階で求めているため、 例えば、 回転と平行移動のいずれの成分も無視できない 程度に含まれる場合に、 各段の推定を正しく求めることができない場合がある。 前記の問題に鑑み、 本発明は、 カメラが設置された移動体について、 カメラ画 像を用いてその動きを求める技術において、 対応点 （オプティカルフ口一） を用 い、 対応点がたとえ大きな誤差を含む場合であっても、 精度良く、 移動体動きを 算出可能にすることを課題とする。
(課題を解決するための手段）
本発明は、 カメラが設置された移動体について、 カメラによって撮影された移 動体周囲の画像を用いて動きを求める際に、 撮影時刻が異なる 2枚のカメラ画像 から複数の対応点を求め、 カメラ画像内に所定の平面を仮定した上で、 求めた複 数の対応点を用いて、 移動体の第 1の動きを求め、 この第 1の動きと複数の対応 点とを用いて、 移動体の第 2の動きを求めるものである。
本発明によると、 第 1の動きを求める際に、 所定の平面を仮定し、 一部の対応 点が所定の平面上に存在するという拘束条件を与えることによって、 未知パラメ 夕を減らすことができ、 平面に対するおおまかな、 大きな誤差のない第 1の動き を求めることができる。 そして、 この第 1の動きを用いることによって、 例えば 平面が実際には傾いていたような場合であっても、 誤差が小さい第 2の動きを求 めることができる。
また、 前記第 1の動きの算出は、 複数の対応点の中から、 m (mは 3以上の整 数） 個の対応点で構成される部分対応点を q ( qは 2以上の整数） 組選択し、 選 択した q組の部分対応点からそれぞれ、 平面を表す平面式を用いて、 移動体の動 き候補を算出し、 算出した q個の動き候補を所定の評価手法によって評価し、 こ の評価結果に従って、 前記第 1の動きを特定するのが好ましい。
これにより、 対応点に含まれるはずれ値様の大きな誤差に影響されないで、 誤 差の小さい移動体の第 1の動きを算出することができる。 また、 前記第 2の動きの算出は、 第 1の動きを初期値として、 探索手法によつ て、 複数の対応点に対する評価を行いつつ、 第 2の動きを求めるのが好ましい。 これにより、 より精度の高い、 移動体の第 2の動きを算出することができる。
(発明の効果）
本発明によると、 2段階の動き算出ステップを含み、 平面を仮定して第 1の動 きを算出した後に、 この第 1の動きを用いて第 2の動きを算出するので、 例えば 、 移動体と路面が傾いた状況であっても、 移動体の動きを精度良く算出すること ができる。 図面の簡単な説明
図 1は本発明の第 1の実施形態における状況を示す図である。
図 2は本発明の第 1の実施形態に係る構成を示すプロック図である。
図 3は図 2における第 1の動き算出部の詳細な構成を示すブロック図である。 図 4 (a) , (b) は図 1の状況で撮影された画像の例、 図 4 (c) はその対 応点を示す図、
図 5はカメラの動きと画像変化との関係を示す図である。
図 6は各対応点に対する動き Mjの評価値 E 3 iの分布を概念的に示すグラフ である。
図 7は算出した動きに対応する予測軌跡が重畳された合成画像の例である。 図 8 (a) は車両に設置したカメラによる実際の画像の例、 図 8 (b) は図 8 (a) の状況における車両の移動速度と対応点正解率との関係を示すグラフであ る
図 9 (a) は車両が傾いた場合の画像例、 図 9 (b) は図 9 (a) の状況に置 ける各対応点に対する動き Mjの評価値 E j iの分布である。
図 10 (a) は車両後方に他の車両が存在している状況を示す図、 図 10 (b ) は図 10 (a) の状況において撮影された画像の例である。
図 1 1は静止対応点率 Rと真の静止対応点率 Rtrueとの比 （R/Rtrue) と力 メラ動きの誤差 EMとの関係を示すグラフである。 図 1 2 ( a ) ， ( b ) は平面パラメ夕を用いた探索を概念的に示す図である。 図 1 3は注意メッセージなどが重畳された合成画像の例である。
図 1 4は屋内駐車場における撮影画像の例である。
図 1 5はアツカーマンモデルに基づく移動体動きの表現を示す図である。 図 1 6は本発明の第 2の実施形態に係るナビゲ一シヨンシステムの構成図であ る o
図 1 7は屋内駐車場における実験により得られた車両の移動軌跡を示すグラフ である
図 1 8は本発明を実施するための他の構成を示す図である。 発明を実施するための最良の形態
本発明の第 1態様では、 カメラが設置された移動体について、 前記カメラによ つて撮影された移動体周囲の画像を用いて動きを求める方法として、 撮影時刻が 異なる 2枚の前記画像から複数の対応点を求める対応点算出ステップと、 前記画 像内に所定の平面を仮定し、 前記複数の対応点を用いて前記移動体の動きを示す 第 1の動きを求める第 1の動き算出ステップと、 前記第 1の動きと前記複数の対 応点とを用いて、 前記移動体の動きを示す第 2の動きを求める第 2の動き算出ス テツプとを備えたものを提供する。
本発明の第 2態様では、 前記平面は、 路面、 天井面、 または壁面である第 1態 様の移動体動き算出方法を提供する。
本発明の第 3態様では、 前記第 1の動き算出ステップは、 前記対応点を 3個用 いて前記第 1の動きを求めるものである第 1態様の移動体動き算出方法を提供す る。
本発明の第 4態様では、 前記第 1の動き算出ステップは、 過去に求められた前 記第 1の動きまたは前記第 2の動きを用いて、 前記平面を規定する平面式を更新 するステップを含む第 1態様の移動体動き算出方法を提供する。
本発明の第 5態様では、 前記第 1の動き算出ステップは、 前記複数の対応点の 中から、 m (mは 3以上の整数） 個の対応点で構成される部分対応点を q ( qは 2以上の整数） 組選択する第 1のステップと、 前記第 1のステップにおいて選択 した q組の部分対応点から、 それぞれ、 前記平面を表す平面式を用いて、 前記移 動体の動き候補を算出する第 2のステツプと、 前記第 2のステツプにおいて算出 した q個の動き候補を所定の評価手法によって評価し、 この評価結果に従って前 記第 1の動きを特定する第 3のステップとを含む第 1態様の移動体動き算出方法 を提供する。
本発明の第 6態様では、 前記第 1の動き算出ステップは、 前記複数の対応点と 、 過去に求められた前記第 1の動きまたは前記第 2の動きとを用いて、 静止した 対応点の比率である静止対応点率を算出するステップを含み、 前記第 3のステツ プは、 算出された前記静止対応点率を用いて、 前記第 1の動きを特定するための 評価を行うものである第 5態様の移動体動き算出方法を提供する。
本発明の第 7態様では、 前記第 1の動き算出ステップは、 過去に求められた前 記第 1の動きまたは前記第 2の動きから前記移動体の動きの予測値を求めるステ ップを含み、 前記第 3のステップは、 前記予測値を加味して前記第 1の動きを特 定するものである第 5態様の移動体動き算出方法を提供する。
本発明の第 8態様では、 前記第 2の動き算出ステップは、 前記第 1の動きを初 期値として、 探索手法によって、 前記複数の対応点に対する評価を行いつつ、 前 記第 2の動きを求めるものである第 1態様の移動体動き算出方法を提供する。 本発明の第 9態様では、 前記第 2の動き算出ステップは、 静止した対応点の比 率である静止対応点を用いて、 前記評価を行うものである第 8態様の移動体動き 算出方法を提供する。
本発明の第 1 0態様では、 前記移動体は車両であり、 前記第 2の動き算出ステ ヅプは、 前記第 1の動きの代わりに、 この第 1の動きに対応する部分対応点と前 記平面を表す平面式を用い、 前記平面式を初期値として、 探索手法によって、 前 記カメラに近い車軸中心を中心として平面を傾けながら、 前記複数の対応点に対 する評価を行いつつ、 前記第 2の動きを求めるものである第 5態様の移動体動き 算出方法を提供する。
本発明の第 1 1態様では、 前記移動体は車軸の方向が固定である非操舵輪を有 し、 前記第 1および第 2の動き算出ステップにおいて、 前記移動体の動きを表す ための座標系の垂直軸を前記非操舵輪の車軸を含む直線と直交するようにおき、 前記移動体の路面上の動きを前記垂直軸周りの円弧運動として求める第 1態様の 移動体動き算出方法を提供する。
本発明の第 1 2態様では、 前記垂直軸は、 前記非操舵輪の中心位置を通るよう におかれている第 1 1態様の移動体動き算出方法を提供する。
本発明の第 1 3態様では、 カメラが設置された移動体について、 前記カメラに よって撮影された移動体周囲の画像を用いて動きを求める装置として、 撮影時刻 が異なる 2枚の前記画像から複数の対応点を求める対応点算出部と、 前記画像内 に所定の平面を仮定し、 前記対応点算出部によって求められた複数の対応点を用 いて、 前記移動体の動きを示す第 1の動きを求める第 1の動き算出部と、 前記第 1の動き算出部によって求められた第 1の動きと、 前記複数の対応点とを用いて
、 前記移動体の動きを示す第 2の動きを求める第 2の動き算出部とを備えたもの を提供する。
本発明の第 1 4態様では、 移動体の位置情報を取得する位置情報取得部を有す るナビゲーシヨン装置と、 第 1 3態様の移動体動き算出装置とを備え、 前記位置 情報取得部によつて取得された位置情報と、 前記移動体動き算出装置によって求 められた前記移動体の動きとを基にして、 前記移動体の現在位置を求めるナビゲ —シヨンシステムを提供する。
以下、 本発明の実施の形態について、 図面を参照して説明する。
(第 1の実施形態）
本発明の第 1の実施形態では、 移動体として、 カメラが設置された車両を例に とり、 カメラによって撮影された車両後方の画像を用いて、 車両の動きを求める ものとする。
図 1は本実施形態におけ 状況を示す図である。 図 1に示すように、 移動体と しての車両 1には、 その後部に、 後方周辺を撮影するようにカメラ 1 2 0が設置 されている。 カメラ 1 2 0は車両後方周辺を撮影して一連の画像列を出力する。 図 2は本実施形態に係る移動体動き算出装置を含む構成を示すブロック図であ る。 図 2において、 1 0 0はカメラ 1 2 0によって撮影された画像から車両 1の 動きを求める移動体動き算出装置、 1 2 1はカメラ 1 2 0によって撮影された画 像に、 移動体動き算出装置 1 0 0によって求められた車両 1の動きに閧する情報 を重畳した合成画像を生成する画像合成装置、 1 2 2は画像合成装置 1 2 1によ つて生成された合成画像を表示するディスプレイである。
移動体動き算出装置 1 0 0は、 カメラ 1 2 0から出力された連続する 2画像間 から対応点 （オプティカルフローべクトル） を複数個求める対応点算出部 1 0 1 と、 対応点算出部 1 0 1から出力された複数の対応点を用いて、 平面を仮定して 、 車両 1の動きを示す第 1の動き M aを求める第 1の動き算出部 1 0 2と、 対応 点算出部 1 0 1から出力された複数の対応点と、 第 1の動き算出部 1 0 2から出 力された第 1の動き M aとを用いて、 車両 1の動きを示す第 2の動き M bを求め る第 2の動き算出部 1 0 3とを備えている。
図 2の移動体動き算出装置 1 0 0、 画像合成装置 1 2 1およびディスプレイ 1 2 2は、 例えば、 車両 1内に設置されるが、 車両 1とは異なる場所に設けること も可能である。
図 3は図 2の移動体動き算出装置 1 0 0における第 1の動き算出部 1 0 2の詳 細な構成を示すブロック図である。 図 3において、 1 1 1は対応点算出部 1 0 1 から出力された複数の対応点から、 所定数の対応点からなる部分対応点を複数組 選択する部分対応点選択部、 1 1 2は部分対応点選択部 1 1 1によって選択され た各組の部分対応点から、 車両 1の動き候補をそれそれ算出する動き候補算出部 、 1 1 3は動き候補算出部 1 1 2によって算出された動き候補を所定の評価手法 によって評価し、 この評価結果に従って、 第 1の動き M aを特定する動き評価選 択部である。
また、 1 1 4は第 1の動き M aを算出する際に仮定する所定の平面を表す平面 式を出力する平面式算出部、 1 1 5は静止した対応点の比率である静止対応点率 Rを出力する静止対応点率算出部、 1 1 6は移動体動きの予測値を求める移動体 動き予測部である。
ここでは、 図 1の状況において、 車両 1が後進した場合を例にとって、 説明を 行う。
図 1において、 世界座標系 （Xw， Yw， Z w) は 3次元空間中で静止してお り、 車両座標系 （X c， Y c， Z c ) および視点座標系 （X， Y， Ζ ) は車両 1 に固定されているものとする。 視点座標系の座標値 （x， y， z ) と車両座標系 の座標値 （xc, y c, z c) との関係は、 座標系間の位置関係を表す行列 Cを 用いて、 （式 1) のように表すことができる。
(式 1)
( c03、
Figure imgf000013_0001
1 i 世界座標系に対して静止した点が、 車両座標系における車両の動き M cによつ て、 車両座標系の座標値 （xc， yc， z c) から （xc' ， y c ' ， z c 5 ) に移動した場合、 （式 2) の関係式が成り立つ。 さらに、 このときの視点座標系 の動きが Mであり、 同じ点の視点座標系の座標値が （x， y, z) から （χ' ， y， ， ζ， ） に移動したとすると、 （式 3) の関係式が成り立つ。 したがって、 車両 1の動き Mcとカメラ 120の動き Μとの間には （式 4) の関係式が成り立 つ。
(式 2)
Figure imgf000013_0003
(式 3)
Figure imgf000013_0002
(式 4)
Mc = CMC一 以上のことから、 車両座標系と視点座標系の位置関係を示す Cが既知のとき、 車両座標系を基準にした車両 1の動き M cを求めるためには、 視点座標系を基準 にしたカメラ 12ひの動き Mを算出して、 （式 4) で変換すればよい。 カメラ 1 20と車両 1との相対位置が固定されている場合、 カメラ 120の動きと車両 1 の動き （移動体の動き） とは一意に変換できるため、 本願明細書では、 特に明示 がない限り、 区別はしないものとする。
図 4 (a) ， (b) は図 1の状況において、 車両 1が後方に移動した場合に力 メラ 120から出力される一連の画像列の例であり、 図 4 (a) は時刻 t_lに 撮影された画像、 図 4 (b) は時刻 tに撮影された画像である。 また、 図 4 (c ) は、 時刻 t一 1と時刻 tの 2画像間の同一点の動きである対応点 （図中の矢印 ) を示す図である。
図 5はカメラ 120の 3次元的な動きと画像変化との関係を示す図である。 図 5 (a) に示すように、 カメラ 120が時刻 t一 1から時刻 tの間に動き Mで移 動したとき、 世界座標系における静止点 Xが、 視点座標系の座標値 （x， y, z ) から （x， ， y， ， z， ） に移動したものとする。 このとき、 座標値 （x， y ， z)、 （x， ， y， ， z' ) とカメラ動き Mとの間には、 （式 3) が成り立つ 。 図 5 (b) (c) はそれぞれ時刻 t— 1, tにおける画像であり、 時刻 t一 1 において視点座標 （X, γ, z) の点 Xが画像座標 （u, v) に、 時刻 tにおい て視点座標 （x， ， y， ， z， ） の点 Xが画像座標 （u， ， v， ) に、 それぞれ 対応している。 このとき （u, V) と （x， y， z)、 (u' ， v， ；) と （χ' , y， ， ζ' ) の関係と （式 3) を用いると、 （式 5) の関係式が成り立つ。 （ 式 5) において、 fはカメラの焦点距離を示す。
(式 5)
u = /-, v = f - z z
u, fx' f rQ0x + r01y + ra2z + tx ^ v,_ , _ f r10x + rny + r12z + tY
z' r20x + r21y + r22z + tz ' z' r20x + r21y + r^z + tz このとき、 静止点 X (X, y, z ) が （式 6) の平面式で表される平面上に存 在し、 かつ、 カメラ 120の動き M (tx， t y, t z , wx， wy , wz ) が 十分に小さく （式 7) が成り立つとき、 点 Xの画像座標 （U, v)、 （u， ， V ， ) とカメラ動き Mとの間に （式 8) の関係式が成り立つ。 さらに、 （式 8) を カメラ動き Mの要素に着目して整理すると、 （式 9) が得られる。 1組の対応点 ごとに （式 9) の 2つの式が成り立つことから、 平面式とカメラ 120の焦点距 離 fが既知の場合は、 独立した （同一直線上ではない） 対応点 （u, V) , (11 ' , V' ) が 3組以上与えられれば、 カメラ 120の動き M (t X, t y, t z , wx， wy, wz) の 6変数を算出することができる。
(式 6) ax + by + cz =1
(式 7)
Figure imgf000015_0001
(式 8)
(1 + atx )u + (wz + btx )v + (- wy + ctx)f
u 1 1
― (wy + atz )u +—{-wx + btz )v + (1 + ctz )
{-wz + atv)u + (1 + btY)v + (wc + ctY)f
1― Ϊ
― (wy + atz )u +—{-wx + btz )v + (1 + ctz ) (式 9)
Figure imgf000016_0001
以下、 時刻 t— 1から時刻 tの間のカメラ動き Mを算出する場合を例にとって 、 説明を行う。 なお、 上述したように、 カメラの動きと移動体の動きは （式 4) により一意に変換できるため、 カメラ動きが算出できれば、 移動体の動きも実質 的に算出されたことになる。
ここでは、 車両座標系 （Xc， Yc， Z c) および視点座標系 （X, Υ, Z) は図 1のように配置されているものとし、 車両座標系に対する視点座標系の位置 関係を表す Cは、 予め計測されているものとする。 また、 カメラ 120の焦点距 離 fも予め求められており、 その値は、 実座標系での焦点距離に、 実座標と画像 座標との変換係数を乗じたものとする。
対応点算出部 101はカメラ 120から出力された一連の画像列を一時的に保 存し、 画像列のうちの連続した 2枚の画像から、 対応点を n個 （nは 2以上の整 数） 検出する。 ここでは、 時刻 t— 1の画像上の n点の座標 （ui, vi) (i =1， ···， n) について、 対応する、 時刻 tの画像上の座標 （ui， , vi' ) (i = l, ···, n) をそれぞれ検出する。 検出した n点の対応点の情報を、 Pi (ui, vi, ui， ， vi， ） （i = l, ···, n) と表す。 対応点の個数 nは 、 予め定められた値とする。
なお、 対応点を検出する 2枚の画像は、 必ずしも連続している必要はなく、 撮 影時刻が異なっていればよい。
ここでの対応点検出は、 例えば、 Kanade， Lucas and Tomasiの対応点探索方法 によって行えばよい （例えば、 "Detection and Tracking of Point Features " , Carlo Tomasi and Takeo Kanade, Carnegie Mellon University Technical Re port, CMU-CS-91-132, April 1991.を参照） 。 この方法は、 一般に広く知られた 方法であるので、 ここでは詳細な説明を省略する。
第 1の動き算出部 102は、 対応点算出部 101によって検出された n個の対 応点 P i (ui, vi， ui， ， vi， ） （i= l，. ···， n) を用いて、 車両 1 の第 1の動き Maを算出する。 以下、 第 1の動き算出部 102を構成する各処理 手段の動作について説明する。
平面式算出部 114は、 ここでは所定の平面式を出力する。 ここで用いる平面 式は、 図 1の状況における路面を平面として近似したものとする。 例えば、 車両 が静止した状態で、 水平かつ平坦な路面平面の式を予め計測しておき、 平面式算 出部 114は、 予め計測した （式 6) の平面式のパラメ夕 S (a, b, c) を出 力する。 なお、 平面式算出部 114は、 平面式を更新する機能も有する。 これに ついては、 後述する。
静止対応点率算出部 115は、 静止対応点率 Rを出力する。 ここで、 静止対応 点率とは、 好ましくは、 対応点算出部 101によって算出された n個の対応点の うち、 世界座標に対して静止しており、 かつ、 対応関係が正しい対応点の比率を 表すものである。 ここで、 対応点の対応関係が正しいとは、 対応点の座標値の誤 差が所定の閾値以下であることを意味する。 なお、 実時間でのオンライン処理に おいて、 静止対応点率 Rを事前に正確に得ることは極めて困難であり、 実際には 、 静止対応点率 Rとは、 上述した比率の予測値とする。 .
ここでは、 静止対応点率算出部 115は、 静止対応点率 Rとして、 所定の値、 例えば 0. 5を出力する。 静止対応点率 R= 0. 5とは、 「対応点算出部 101 によって算出された n個の対応点に、 静止した、 対応関係の正しい対応点が 50 %含まれる」 と予測していることを意味する。 所定の値としては、 例えば、 図 1 と類似の走行状態において静止対応点率を予め複数回計測し、 計測した中の最低 値を用いればよい。 なお、 静止対応点算出部 115は、 静止対応点率 Rを新たに 算出する機能も有する。 これについては、 後述する。
移動体動き予測部 116は、 車両 1の動きの予測値を求める機能を有するが、 ここでは、 特に処理を行わないものとする。
部分対応点選択部 111は、 対応点算出部 101から出力された n個の対応点 Pi、 平面式算出部 114から出力された平面式 S、 および静止対応点率算出部 1 1 5から出力された静止対応点率: Rを用いて、 m個の対応点で構成される部分 対応点を q組選択する。 ここで、 部分対応点を構成する対応点の個数 mは、 移動 体の動きを算出するために必要となる対応点の数である。 （式 9 ) を用いてカメ ラ動きを算出するためには、 最低 3個の対応点が必要である。 このため、 ここで は、 m= 3とする。 もちろん、 mは 3より大きい値であってもかまわない。 部分対応点選択部 1 1 1は、 まず、 部分対応点の組数 qを算出する。 部分対応 点の組数 qは、 その値が大きいほど、 第 1の動き算出部 1 0 2での処理時間が長 くなり、 逆に qが小さいほど処理時間は小さくなる。 そのため、 例えば処理時間 の制約がある場合は、 その上限から qを決定しても良い。
ここでは、 部分対応点の組数 qを （式 1 0 ) の関係式から求めるものとする。
(式 1 0 )
„ log(l
log(l - Rrm)
(式 1 0 ) において、 R rは路面静止対応点率、 zは保証率である。
路面静止対応点率 R rは、 部分対応点として選択された各対応点が、 路面上に 存在し、 世界座標に対して静止しており、 かつ対応関係が正しい対応点の確率で ある。 したがって、 対応点が路面上に存在する確率 R sと、 対応点の静止対応点 率 Rとの間に相関がない場合、 対応点が路面上に存在する確率 R sに静止対応点 率 Rを掛けたものが路面静止対応点率 R rとなる。 また、 もしも路面上に存在す る対応点を正確に選択することが可能であり、 かつ、 選択した対応点の静止対応 点率が Rである場合、 選択した対応点の路面対応点率 R は、 静止対応点率 Rと 等しくなる。 ここでは、 所定の値として、 例えば、 R r = 0 . 3を用いる。 所定 の値としては、 例えば図 1と類似の走行状態において、 選択後の対応点が路面上 に存在する確率 H sを計測しておき、 これと静止対応点率 Rをかけた値を用いれ ばよい。
保証率 zは、 q組の部分対応点のうち、 全て （m個） の対応点が路面上の静止 した正しい対応点で構成された部分対応点が少なくとも 1組存在することを統計 的に保証する率である。 ここでは、 所定の値として、 例えば、 z = 0 . 9 9 9を 用いる。 言い換えると、 少なくとも 1組の部分対応点が路面上の正しい対応点で 構成されることを、 保証率 Zで保証するためには、 q組の部分対応点を選択すれ ばよい。
(式 1 0 ) において、 qと R rが一定の場合、 部分対応点を構成する対応点数 mが小さいほど保証率 zが大きくなり、 zと R rが一定の場合は、 mが小さいほ ど qが小さくなる。 したがって、 対応点の数 mは小さいほど、 処理時間が短く、 部分対応点の少なくとも 1組が路面上の正しい対応点で構成される可能性が高く なる。 このことと、 （式 9 ) の算出には 3点以上の対応点数が必要であることか ら、 最良の mは 3となる。
次に、 部分対応点選択部 1 1 1は、 算出した組数 qを用いて、 n個の対応点か ら、 異なる m個の対応点からなる部分対応点を q組選択して出力する。 部分対応 点を選択する方法として、 n個の全ての対応点からランダムに選択してもよいし 、 特定の条件を満たす対応点を抽出した上で、 そこからランダムに選択するとし てもよい。 ここでは後者を用いる。 すなわち、 平面式 Sが存在し得る画像領域内 の対応点を抽出した上で、 抽出した対応点から、 ランダムに、 異なる m個の対応 点からなる部分対応点を q組選択して出力する。 例えば、 図 4 ( d ) において、 画像中で平面 Sが存在し得る領域は、 水平線より下の斜線領域に限られることを 示している。 この方法では、 斜線領域に存在する対応点を抽出した上で部分対応 点を選択する場合の路面静止対応点率 R rは、 n個の対応点からランダムに選択 する場合の路面静止対応点率 R rに比べて、 より高い値となり、 その結果、 部分 対応点の組数 qをより小さい値にすることができる。
動き候補算出部 1 1 2は、 部分対応点選択部 1 1 1によって選択された q組の 部分対応点から、 それぞれ、 平面式 Sを用いて、 q個の動き候補を算出する。 上述したように、 画像中の対応点 P i ( u， v , u 5 , ν ' ) が平面式 S ( a , b , c ) 上にあり、 かつ、 カメラの動き Mが小さいと仮定した場合、 （式 9 ) の関係式が成り立つ。 （式 9 ) において、 対応点と平面式が既知、 カメラの動き Mを未知とすると、 対応点が 3個以上あればカメラの動き Mを算出することがで きる。 そこで、 動き候補算出部 1 1 2は、 3個の対応点からなる q組の部分対応 点について、 それぞれ、 （式 9 ) の関係式を用いて、 q個のカメラの動き M j ( 一- j = l, ···, q) を算出し、 これを車両 1の動き候補として出力する。
動き評価選択部 1 13は、 動き候補算出部 1 12によって算出された q個の動
一- き候補 （カメラ動き Mj ( j = 1 , …， q) ) を、 所定の評価手法によって評価 し、 この評価結果に従って、 第 1の動き Maを特定する。 ここでの評価では、 対 応点算出部 101から出力された n個の対応点 P i、 および静止対応点率算出部 1 15から出力された静止対応点率 Rを用いる。 なお、 必要に応じて、 移動体動 き予測部 1 16から出力された動き予測を用いてもよい。 これについては、 後述 する。
ここでは、 まず、 カメラ動き Mjと n個の対応点 P iとの一致する度合いを評 価する。 具体的には、 それそれのカメラ動き Mjについて、 n個の対応点 （u i ， v i, ui， ， vi， ） （i = l， …， n) を （式 1 1 ) に代入し、 n個の対 応点の 3次元座標 (xi, yi, z i) を算出する。 ここで、 （式 1 1 ) は未知 数 4に対して独立した一次式が 4つあること、 および、 画像から算出した対応点 P i (u i, vi, u i' , vi， ) は誤差を含むと考えられることから、 最小 二乗法を用いて 3次元座標 （x i， yi， z i) を算出する。
(式 1 1)
^ -wzx + y + wxz + tY
"'= /-
WYX -W^V + Z + tz WYX -WxV + Z + tz さらに、 算出した 3次元座標 （xi， y i , z i) とカメラ動き Mjを （式 1 1 ) に再び代入して、 新たに対応点 （u i j， V i j， u i j， ， V i j， ） を 算出する。 ここで、 対応点 P i (ui, vi, u i ' ， v i' ) およびカメラ動 き Mjが誤差のない正確な値である場合、 P i (ui， v i, u i' , vi， ） と （u i j， V i j， u i j， ， V i j， ） は一致し、 誤差がある場合は一致し ない。 そこで、 (ui, i , u i ' ， vi， ) と (ui j , v i j , u i j ' 3 v i j ' ) の誤差距離平均 E j iを、 （式 12) から算出する。 E j iの値は 、 カメラ動き Mjと対応点 P iが一致するときは、 0になり、 一致しないときは 、 値が大きくなるという性質をもつことから、 E j iをカメラ動き M jと対応点 P iの一致度合いに関する評価値として用いる。
(式 12)
-Ji - uij + (νι - vij)2 +■yfui'-uij'y + (vi'-vij'
次に、 n個の対応点 P iそれぞれに対するカメラ動き M jの評価値 E j i (i = 1 , …， η) のうち、 評価の良い方すなわち値の小さい方から （nxR)番目 の評価値を、 カメラ動き Mjの評価値 Ejとして選択する。
そして、 q個のカメラ動き Mj (j = l, …， q) の中で、 それそれの評価値 E j (j = l， …， q) のうち評価が最も良い、 すなわち値が最も小さいものに 対応するカメラ動きを、 第 1の動き Maとして選択する。
図 6は n個の対応点に対するある 1つのカメラ動き Mjの評価値 E j i (i = 1， ···， n) の分布を概念的に示すグラフである。 図 6では、 評価値 E j iを小 さい順にソートしており、 横軸はソート後の対応点番号 （1， …， n) 、 縦軸は 各対応点に対するカメラ動き Mjの評価値 E j iの値である。
ここで、 カメラ動き Mjが正しく、 かつ、 n個の対応点のうち （nxR)個が 静止した正しい対応点であるとすると、 理想的には図 6 (a) のように、 小さい 方から （nxR)番目までの評価値 E i jは 0で、 それ以降は次第に値が大きく なる。 このため、 評価値 Ejも 0となる。 しかし、 実際には対応点の計測誤差な どの小さな誤差が存在するため、 （nxR) 番目までの評価値 E i jは必ずしも 0にはならない。
一方、 カメラ動き Mjが正しくない場合は、 図 6 (b) のように、 評価値 E j iの多くが大きな値となり、 評価値 E jも大きな値となる。 このことを言い換え ると、 静止対応点率 Rを前提条件とした場合、 カメラ動き M jの評価値 E jが小 さいということは前提条件を満たすことを意味し、 さらに、 評価値 Ejが最小と なるカメラ動き （すなわち第 1の動き Ma) は、 前提条件を最も良く満たすカメ ラ動きであることを意味する。
このような動作によって、 第 1の動き算出部 102は、 時刻 t— 1と時刻 tの 画像上の n個の対応点 P iを用いて、 車両 1の動きである第 1の動き Maを算出 して出力する。
第 2の動き算出部 103は、 第 1の動き算出部 102から出力された第 1の動 き Maと、 対応点算出部 101から出力された n個の対応点 P iとを用いて、 車 両 1の動きを示す第 2の動き Mbを算出する。 ここでは、 第 1の動き Maを初期 値とした探索的手法によって、 第 2の動き Mbを算出するものとする。
具体的には、 まず、 第 1の動き Ma ( t X, t y, t z， wx, w , wz) を初期値として、 各パラメ一夕に対して所定の微小動き （dtx， dty, dt z， dwx, dwy, dwz) をそれそれ増減して、 複数の動き候補を得る。 こ れにより、 o = 3 ~ 6個の動き候補 Mbk= (tx土 dtx, ty土 dty， t z±dt z, wx土 dwx， wy土 dwy, wz土 dwzj (k=l， ···, o が得られる。
次に、 第 1の動き算出部 102の動き評価選択部 113と同様に、 o個の動き 候補 Mb kについて、 それそれ、 n個の対応点に対する評価値 Ebki (i = l ， ···， n) を算出する。 そして、 小さい方から （nxR) 番目の評価値 Ebki を動き候補 Mb kの評価値 Ebkとし、 さらに、 評価値 E b kが最小となる動き 候補 Mb kを選択する。
次いで、 選択された動き候補 Mb kを初期値として、 上と同様の処理を行う。 これを繰り返し実行し、 値が収束したとき、 または、 繰り返し回数が所定回数に 達したとき、 処理を終了する。 そして、 第 2の動き算出部 103は、 そのとき選 択されている動き候補 Mb kを第 2の動き Mbとして出力する。
このような動作により、 移動体動き算出装置 100は、 時刻 t— 1から時刻 t の間の車両 1の動きを算出する。 また、 カメラ 120から出力された画像列につ いて、 同様の処理を繰り返すことによって、 車両 1の動きを連続して算出するこ とができる。
そして、 画像合成装置 121は、 カメラ 120によって撮影された画像と、 移 動体動き算出装置 100から出力された第 2の動き Mbを入力して、 車両 1の動 きに関する情報が重畳された合成画像を生成する。 ディスプレイ 1 2 2は画像合 成装置 1 2 1によって生成された合成画像を表示する。 図 7は表示された合成画 像の例であり、 車両 1の第 2の動き M bに対応する予測軌跡 T Rが、 カメラ 1 2 0で撮影された画像に重畳されている。
<本実施形態の効果 >
本実施形態によると、 例えば、 対応点探索の誤りが多く、 大きな誤差を含む場 合であっても、 車両の動きを精度良く算出することができる。
対応点算出部 1 0 1の動作説明で触れた Kanade， Lucas and Tomasiの方法は、 高い精度と少ない計算量の両面で優れた対応点探索方法として広く知られている 。 しかし、 そのような優れた方法であっても、 制約の無い環境で撮影された画像 において対応点探索の誤りを 0にすることは、 まず不可能と言ってよい。
図 8 ( a ) は実際に車両の後方にカメラを設置して撮影した画像の例である。 図 8 ( a ) のシーンで撮影した動画像について、 上述した手法で対応点探索を行 つた。 図 8 ( b ) はそのときの対応点正解率と車両の移動速度との関係を示すグ ラフである。 ここでは、 画像座標上の誤差が 1画素以内であった対応点の比率を 対応点正解率としている。 なお、 図 8 ( b ) のグラフでは、 時間と移動速度の値 には、 適当なスケールが掛けられている。
図 8 ( b ) からも分かるように、 実際に撮影された画像を用いた場合、 精度が 良いとされている手法であっても、 対応点正解率が 5 0 %程度にまで低下する場 合がある。 また、 はずれ値の様な大きな誤差を持つ対応点が多数含まれる場合も ある。 本願発明者らは、 このような場合が稀ではないことを、 他の実験結果から も確認している。
そして、 上述したような従来の方法では、 多数の誤りを排除する仕組みを持た ないことから、 例えば、 対応点探索の正解率が大きく低下してしまった場合、 精 度の高い動き算出は、 到底期待できない。
これに対して本実施形態では、 たとえ対応点正解率が大きく低下したような場 合であっても、 第 1の動き算出部 1 0 2において、 第 1の動き M aを精度良く算 出することができる。 すなわち、 本実施形態の方法の場合、 q組の部分対応点の 中に、 正しい部分対応点が少なくとも 1組含まれていれば、 q個の動き候補 M j の中に少なくとも 1つの正しい動きが含まれることになる。 さらに、 対応点探索 の正解率に相当する静止対応点率： Rを用いて、 q個の動き M jを、 n個の対応点 に対する評価値のうち良い方から （n x R) 番目の評価値を用いて評価すること によって、 正しい動き M aを算出することを可能にしている。
また、 本実施形態によると、 例えば路面が傾いている場合であっても、 車両の 動きを精度良く算出することができる。
本実施形態では、 車両が静止した状態で計測して得た水平かつ平坦な路面平面 の式を、 所定の平面式として用いて、 第 1の動き M aを算出するものとした。 し かし、 坂道や昇降路などの路面が傾いている場合や、 乗員や荷物の積載、 または ハンドル操作などによって車両の姿勢が変わった場合は、 予め計測した所定の平 面式は、 実際に車両が走行している路面平面と一致しなくなる。
図 9 ( a) は車! ¾が傾いた場合の画像例である。 車両が傾くと、 画像内の路面 も傾くため、 予め計測した所定の平面式と一致しなくなる。 この場合、 選択され た部分対応点の 3個の対応点が、 路面上にありかつ対応関係が正しいものであつ たとしても、 （式 9 ) を用いて算出された動き M jは誤差を含む。 このため、 図 9 ( b ) に示すように、 n個の対応点に対する動き M jの評価値 E j iは、 平面 式が正しい場合に比べて少しずつ大きな値となり、 評価値 E jも大きな値となる 。 したがって、 評価値が最も小さい第 1の動き M aであっても、 誤差を含むこと になる。
ここで、 評価値が図 9 ( b ) のようになる場合に、 従来技術であるはずれ値除 去方法を適用し、 評価値が小さい一部の対応点を抽出して、 （式 9 ) を用いた最小 二乗法によって動きを算出したとしても、 誤差を小さくすることは困難である。 これに対して本実施形態では、 第 1の動き M aを初期値として、 小さい方から n X R個の評価値を基準とした探索的手法を用いて、 第 2の動き M bを算出する 。 一般に、 初期値から真の値 （または最良の値） までの間で、 その評価関数が滑 らかであれば、 探索的手法によって真の値を得ることが可能である。 そして、 本 実施形態で用いた評価値は、 真の値の近傍であれば上記条件を満たす。 このため 、 たとえ、 実際の路面が所定の平面式と一致しなくなつたために第 1の動き M a が誤差を含んでいる場合であっても、 第 2の動き算出部 1 0 2によって、 誤差の 小さい第 2の動き M bを算出することができる。
また、 本実施形態において、 平面式を仮定することの妥当性と効果について、 実際の車両の走行環境において発生する問題を基にして、 説明する。
車両周辺をカメラで監視するシステムには、 次のような特徴がある。 すなわち 、 運転者が車両周辺を監視できるように、 典型的には図 4のように、 車両近傍の 路面から水平線より上の障害物までが画像に映るように、 広視野角のカメラが設 置される。 このように設置されたカメラの画像には、 ほとんど全てのシーンにお いて路面が映ることが期待できる。 さらに、 仮に路面が連続した平面ではない場 合であっても、 多くの被写体が路面平面の延長上に存在する。 例えば水平線近傍 の遠方の被写体は、 実際には路面に接していなくても、 路面と同一の平面式上に 存在すると仮定することができる。 このようなケースが多いことが、 本願発明者 らによる実験でも確認できた。 このことから、 車両周辺を監視するシステムにお いて、 路面を表す平面式を仮定して動きを求める方法は、 妥当といえる。
また、 平面式を仮定した場合、 （式 9 ) を用いて、 3個の対応点から車両の動 きを算出できる。 これに対して、 平面式を仮定しない'方法では、 動き （または動 き候補） を求めるために、 少なくとも 4個以上の対応点が必要となり、 このため 、 算出過程において誤差の影響を受けやすい。 これに対して、 本実施形態のよう に平面式を仮定することによって、 誤差の影響を回避できるという効果が得られ る
さらに、 単眼カメラの画像を用いてカメラの動きを算出する場合、 スケール項 を求めるために、 画像中に大きさが既知の被写体が必要となる。 本実施形態では 、 平面式を仮定することによって、 画像中に大きさが既知の被写体が存在しなく ても、 スケールを考慮したカメラ動きを算出できるという効果もある。
さらに、 本実施形態のように平面式を仮定することによって、 画像中に他の移 動体が存在する場合であっても、 移動体動きを正しく算出できるという効果も得 られる。
図 1 0 ( a ) は自車両 1の後方に他の移動車両 2が存在している状況を示す図 である。 図 1 0 ( a ) では、 自車両 1は後方 （図中右） に、 他車両 2は前方 （図 中左） にそれぞれ移動している。 このとき、 図 1 0 ( b ) のような画像がカメラ 120によって撮影される。
ここで、 対応点が、 路面と他車両 2上のそれぞれに、 50%ずつ算出されたと する （対応関係の誤りはないものとする） 。 このような状況に、 従来の方法を適 用すると、 自車両 1について、 路面に対する動きと、 他車両 2に対する動きとの
2つが算出される可能性があるため、 これら 2つの動きを判別する手段が別途必 要になる。 さらに 2つの動きを判別する手段において誤りが発生すれば、 自車両
1の動きは誤差の大きな動きとなる。
これに対して、 本実施例では、 路面平面の式を仮定して自車両 1の動きを算出 する。 このため、 第 1の動き算出部 102において、 路面平面から離れた他車両
2上の対応点から得られた自車両 1の動き Mjは、 大きな誤差を含むこととなり 、 その評価値 E jは大きくなる。 この結果、 第 1の動き算出部 102において、 他車両 2上の対応点から得られた動きが選択されることはなく、 路面上の対応点 から得られた路面平面に対する動きが、 第 1の動き Maとして選択される。 以上 のことから、 本実施形態は、 画像中に他の移動物が存在する場合であっても、 移 動体の動きを正しく算出できるという効果があると言える。
以上説明したように、 平面式を仮定して動きを算出することによって、 実際の 車両の走行環境で発生する、 対応点探索過程で発生する誤差、 路面や車両の傾き 、 他の移動物の存在などといった問題に対しても、 移動体動きを精度良く算出す ることができる、 という効果が得られる。
<平面式の更新 >
平面式算出部 1 14は、 過去の画像から第 1の動き算出部 102によって求め られた第 1の動き Maを用いて、 平面式を更新することも可能である。
例えば、 平面式算出部 1 14は、 時刻 t = lにおいて、 予め得られた所定の初 期平面式 （a0, b O, c 0) を出力する。 第 1の動き算出部 102は、 この初 期平面式 （a0， b O, c 0) を用いて時刻 t = 0と t = l間の第 1の動き Ma を求める。 次に、 時刻 t = 2において、 平面式算出部 1 14は、 時刻 t = 0と t =1間の第 1の動き Maを用いて、 移動体が初期平面式 （a0， b O, c 0) に 対して第 1の動き Maだけ動いたものとして、 新たな平面式 （a 1， b l， c l ) を算出して出力する。 これ以降、 同様の処理を順次繰り返すことによって、 平 面式は更新されていく。
このように、 過去に算出した移動体動きから新たに平面式を算出し、 次の動き 算出に用いることによって、 より正確な平面の位置に基づいた動き算出を実行す ることができる。 したがって、 移動体動きをより精度良く算出することができる なお、 平面式の更新には、 第 1の動き M aに代えて、 第 2の動き M bを用いて もよいし、 またその両方を用いてもかまわない。
また、 平面式の更新は、 必ずしも、 動き算出を実行するたびに行う必要はない 。 例えば、 動き算出を所定回数実行する毎に行ってもよいし、 路面や車両の状況 に変化が生じたときに行うようにしてもよい。
<静止対応点率の更新 >
静止対応点率算出部 1 1 5は、 所定の静止対応点率を出力する代わりに、 対応 点算出部 1 0 1から出力された複数の対応点と、 過去の画像から第 1の動き算出 部 1 0 2によって算出された第 1の動き M aとを用いて、 静止対応点率を算出す ることも可能である。
例えば、 静止対応点算出部 1 1 5は、 第 1の動き算出部 1 0 2によって算出さ れた第 1の動き M aについて、 n個の対応点に対して、 上述したような （式 1 1 ) および （式 1 2 ) を用いた評価を行う。 そして、 n個の対応点のうち評価値 E iが所定の閾値以下であるものの比率を算出し、 これを新たな静止対応点率とし て保持する。 そして、 次の動き算出を実行するときに、 保持していた静止対応点 率を出力する。 これ以降、 同様の処理を順次繰り返すことによって、 静止対応点 率は更新されていく。
ここで、 動き算出に用いられた静止対応点率が真の静止対応点率と異なる場合 、 算出される動きは、 真の動きに対する誤差を含むことになる。 この静止対応点 率 Rと真の静止対応点率 Rtrueおよびカメラ動きの誤差 E Mとの関係を図 1 1の グラフに示す。 図 1 1のグラフは本願発明者らが行った実験の結果から、 その共 通の傾向を模式的に表したものである。 図 1 1のグラフからも分かるように、 静 止対応点率 Rと真の静止対応点率 Rtrueがー致しているとき （比 R/Rtrueが 1 のとき） 、 カメラ動きの誤差 EMが最も小さくなり、 静止対応点率 Rが真の静止 対応点率 Rtrueに対して大きくなつた場合でも小さくなつた場合でも、 カメラ動 きの算出精度は低下する。
一方、 動き算出の繰り返しの周期が比較的短い場合、 車両周辺の画像の変化は 小さく、 したがって、 静止対応点率の変化も小さいと考えられる。 このため、 時 刻 t一 1において得られた動きを用いて算出した静止対応点率 R (t- 1) は、 時刻 tの画像における真の静止対応点率 Rtrue (t) に近いと予想される。 した がって、 静止対応点率を更新していくことによって、 誤差の小さい動きを算出す ることが可能になる。
なお、 時刻 tにおいて、 時刻 t— 1で算出した静止対応点率 R (t- 1) を用 いて移動体動きを算出した結果、 この移動体動きから求めた静止対応点率 R (t ) が静止対応点率 R (t- 1) を下回った場合は、 時刻 tの移動体動きを、 新た な静止対応点率 R (t) を用いて再計算するようにしてもよい。
図 11からわかるように、 静止対応点率 Rが真の静止対応点率 Rt rueより も小さい場合は、 カメラ動きの誤差はあまり大きくならないのに対して、 静止対 応点率 Rが真の静止対応点率 Rt rueよりも大きい場合は、 カメラ動きの誤差 は極端に大きくなる。 したがって、 R (t) <R (t- 1) の場合、 静止対応点 率 R (t - 1) を用いて算出したカメラ動きは大きな誤差を含んでいる可能性が ある。 このことから、 R (t ) <R (t - 1) の場合に、 新たに得られた R (t ) を用いて移動体動きを再計算することによって、 より精度の高い動き算出が可 能になる。
なお、 静止対応点率の更新には、 第 1の動き Maに代えて、 第 2の動き Mbを 用いてもよいし、 またその両方を用いてもかまわない。
<移動体動きの予測値の利用 >
移動体動き予測部 116は、 第 1の動き算出部 102によって過去に求められ た第 1の動き Maを用いて、 車両 1の動きを予測し、 その動き予測値を出力する ことができる。 この場合、 動き評価選択部 113は、 移動体動き予測部 116か ら出力された動き予測値を加味して、 第 1の動き Maを算出する。
ここでは例えば、 移動体動き予測部 116は、 車両 1の動きはほぼ等速 ·等角 速度運動であると仮定して、 予測を行うものとする。 すなわち、 時刻 t— 1に算 出された第 1の動き Maを保存しておき、 時刻 tの動き予測値 Mr tとして出力 する。 動き評価選択部 113は、 動き候補算出部 112から入力した q個のカメ ラ動き Mj (j = l, ···, q) とともに、 動き予測値 Mrtも併せた （q+1) 個のカメラ動きに対して、 上述したように （式 11) (式 12) を用いて評価を 行う。 そして、 最も評価値の良いものを、 時刻 tにおける第 1の動き Maとして 選択する。
上述したように、 部分対応点の組数 qと保証率 zとの間には （式 10) の関係 があり、 静止対応点率 R< 1、 かつ、 qが有限である限り、 保証率 zは 1未満と なる。 つまり、 q組の部分対応点から第 1の動き Maを求める場合、 ある確率で 、 誤差が大きい動きが算出される。
これに対して、 動き予測値 Mr tも併せて評価を行い、 第 1の動き Maを選択 することによって、 q組の部分対応点からは正しい動きが得られないような場合 でも、 車両 1の動きが等速 ·等角速度運動に近い場合は、 動き予測値 Mr tが第 1の動き Maとして選択される。 この結果、 第 1の動き Maに大きな誤差が生じ るケースを回避することができる。
なお、 ここでは、 移動体が等速 ·等角速度運動していると仮定して動き予測を 行うものとしたが、 動き予測の方法はこれに限られるものではない。 例えば、 移 動体が等加速 ·等角加速度運動をしていると仮定して動き予測を行ってもよく、 この場合でも上と同様に、 第 1の動き Maに大きな誤差が生じるケースを回避す ることができる。
なお、 移動体の動き予測には、 第 1の動き Maに代えて、 第 2の動き Mbを用 いてもよいし、 またその両方を用いてもかまわない。
以上のように本実施形態によると、 対応点が存在する所定の平面を仮定するこ とによって、 未知パラメ夕を減らすことができることから、 大きな誤差の少ない 第 1の動き Maを求めることができる。 さらに、 第 1の動き Maを初期値とした 探索的手法を用いることによって、 誤差の小さい第 2の動き Mbを求めることが できる。
なお、 本実施形態では、 動き候補の評価のために、 （式 11) と （式 12) に 表された評価値 Ej iを用いたが、 評価手法はこれに限定されるものではなく、 対応点がカメラ動きに一致するか否かを評価する値であればどのような評価値を 用いてもかまわない。 例えば、 ェピポーラ拘束や基礎行列と呼ばれる、 カメラ間 の位置 （動き） と対応点との関係を示す評価値を用いてもよい。 （式 13) はそ の具体例である。
(式 13)
Figure imgf000030_0001
なお、 本実施形態では、 カメラ動き Mjについて、 n個の対応点に対する評価 値 E j iのうち小さい方から （nxR)番目の評価値を、 評価値 Ejとして選択 するものとしたが、 評価値 E jの特定方法はこれに限定されるものではない。 例 えば、 評価値 E j iの小さい方から （nxR)番目までの評価値の平均値を、 評 価値 E jとしてもよい。
また、 静止対応点率 Rの値に応じて、 評価値 E jの特定方法を変えてもよい。 例えば、 静止対応点率 Rが所定の閾値よりも大きい場合は、 評価値 E j iのうち の小さい方から （nxR)番目の評価値を Ejとし、 一方、 静止対応点率 Rが所 定の閾値よりも小さい場合は、 評価値 E j iのうちの小さい方から （nxR)番 目までの評価値の平均値を E jとするようにしてもよい。
この方法によると、 静止対応点率 Rが比較的小さいときは、 複数の評価値の平 均値をとることによって、 評価値に含まれる誤差の影響を抑制することができる 。 その一方で、 静止対応点率: Rが比較的高いときは、 1個の評価値を評価値 Ej として採用することによって、 （nxR)番目近辺の微小な差異が平均化されず に、 評価値 Ejに反映される、 という効果が得られる。
なお、 本実施形態において、 第 2の動き Mbの算出を、 第 1の動き Maを初期 値とした探索的手法によって、 行うものとしたが、 このような方法に限定される ものではない。 例えば、 第 1の動き算出部 102が、 第 1の動き Maの代わりに 、 第 1の動き Maの算出元となった部分対応点と平面式を、 第 2の動き算出部 1 03に出力するものとする。 そして第 2の動き算出部 103は、 この部分対応点 および平面式と、 対応点算出部 101から出力された n個の対応点とを用いて、 別の探索的手法により第 2の動き Mbを算出するとしてもよい。 別の探索的手法 として、 例えば、 平面式 S (a, b， c) を初期値として、 平面式の各パラメ夕 に対して所定の微小変化 （da， db， d c) をそれぞれ増減した、 o = 3一 3 個の平面式 Sk (a土 da， b土 db, c土 dc) (k= l, · · ·， o) を算 出する。 次に、 第 1の動き Maの算出元となった部分対応点と o個の平面式 Sk から、 （式 9) を用いてカメラ動き候補 Mbk (k二 1, · · · , o) を算出す る。 そして、 前述した手法によって、 n個の対応点に対する動き候補 Mbkの評 価値 Ebkを算出し、 評価値 Ebkが最小となる動き候補 Mbkを選択する。 さ らに、 選択された Mbkに対応する平面式 Skを初期値として、 上述した処理を 繰り返し実行し、 値が収束したとき、 または繰り返し回数が所定回数に達したと き、 処理を終了する。 そして、 そのとき選択されている動き候補 Mbkを第 2の 動き Mbとして出力する。 この方法は、 第 1の動き Maに含まれる誤差の主要因 が、 平面式 Sと実際の路面との差である場合に、 実際の路面に近い平面式 Sを元 にして動きを再計算することが期待できるため、 誤差の小さい動きを算出できる と考えられる。 また、 この方法を用いた場合、 探索空間を狭くできるため、 処理 が軽くなるというメリットが得られる。
また、 平面式 S (a, b， c) のパラメ夕のうち、 cはスケール項であること から、 cに対する探索は効果がない場合がある。 このような場合は、 ο = 3 Λ 3 個の平面式 Sk (a土 da, b士 db, c土 dc) (k= l, ■ · ■， o) 用い る代わりに、 0 = 3 ~ 2個の平面式3 （a土 da， b±db, c) (k= l, • · ■ , o) 用いてもよい。
さらに、 この方法を用いる場合、 視点座標系の平面式 S (a, b， c) を初期 値として、 一定の微小変化をそれぞれ増減した平面式 Skを用いて探索を行う場 合は、 図 12 (a) のように、 実際の路面の変動とは無関係な平面式を用いた探 索となる。 これに対して、 図 12 (b) のように、 例えば、 カメラ 120に近い 車軸中心 （図の場合は後輪軸中心） を基準に平面を傾けた場合の、 所定の微小変 化 （da, db, dc) を増減した平面式 S kを用いて探索を行う方が、 車両と 路面との位置変動を反映した探索であると言えることから、 より少ない繰り返し 回数での動き算出や、 より誤差の小さい動き算出が可能になる。 このような所定 の微小変化としては、 例えば、 図 1 2と類似の状態において、 さまざまな平面式 Sとその微小変ィ匕 （d a， d b , d c ) をあらかじめ計測しておき、 これを用い ればよい。
なお、 本実施形態では、 移動体動き算出装置 1 0 0から出力された第 2の動き M bを用いて、 図 7のように車両 1の移動軌跡を画像に重畳するものとしたが、 算出した移動体動きの利用形態は、 これに限られるものではない。
例えば、 画像合成装置 1 2 1は、 第 1の動き M aの評価値 E iが小さい方から ( n x R ) 個までの対応点について、 その画像座標と 3次元位置を入力する。 そ して、 （n x R) 個の対応点のうち、 その 3次元位置が移動体の進路上であり、 かつ、 所定の範囲内にあるものが存在するときは、 図 1 3のような注意メッセ一 ジを重畳合成するとともに、 該当する対応点の付近を枠で囲むなどの強調表示す るようにしてもよい。
また、 本実施形態では、 移動体動き算出装置 1 0 0の出力は、 画像合成装置 1 2 1とディスプレイ 1 2 2に供給されるものとしたが、 これは、 移動体動き算出 装置 1 0 0やその出力である移動体動きの用途を限定するものではなく、 他のど のような装置と組み合わせて用いてもかまわない。
なお、 本実施形態では、 カメラ 1 2 0は車両 1の後部に設置されており、 車両 後方を撮影するものとしたが、 カメラの位置や撮影範囲はこれに限定されるもの ではなく、 車両の周辺が撮影されるように設置されていれば、 カメラはどのよう な位置にあってもよい。
なお、 本実施形態では、 動き算出のために仮定した平面は路面であつたが、 仮 定する平面は路面に限定されるものではなく、 移動体との相対的な位置を予め与 えることができる平面であれば、 どのような平面であってもかまわない。
例えば、 図 1 4のような屋内においては、 天井面も移動体との相対的な位置を 予め与えることができるため、 路面の代わりに、 天井面を平面として仮定しても よい。 また、 移動体が壁面と所定の距離を保って移動するような場合には、 路面 の代わりに、 壁面を平面として仮定してもよい。 なお、 本実施形態では、 移動体の動きの表現や、 移動体の動きを表すための座 標系の取り方については、 特に制約はない。 例えば、 垂直軸が車両の非操舵輪の 車軸を含む直線と直交するように、 車両座標系を置き、 車両の路面上の動きを、 垂直軸周りの円弧運動として表してもよい。 図 15はいわゆるアツ力一マンモデ ルに基づく移動体の動きの表現を示す図である。 図 15では、 前輪が操舵輪、 後 輪が車体に車軸が固定された非操舵輪である車両について、 非操舵輪の車軸の延 長線上に、 水平面上での回転成分の中心がおかれている。
このとき、 図 15のように、 座標系の垂直軸 （Yc軸） を後輪の中心位置を通 るようにおくと、 車両の動きのうち （txc, t z c, w e) の間に拘束式が 成り立つ。 このような拘束条件をおくことによって、 未知パラメ夕の個数を減ら すことができ、 より安定に解を求めることができる。
(第 2の実施形態）
本発明の第 2の実施形態では、 第 1の実施形態と同様に構成された移動体動き 算出装置を、 ナビゲーシヨン装置と組み合わせて、 ナビゲーシヨンシステムを構 成する。 ここでのナビゲ一シヨン装置は、 人工衛星からの電波を用いて現在位置 を測定する機能と、 現在位置と地図情報を用いて現在位置の表示や目的地への誘 導表示を行う機能とを有するものとする。
図 16は本実施形態に係る移動体動き算出装置を含むナビゲ一シヨンシステム の構成図である。 図 16では、 図 2および図 3と共通の構成要素には同一の符号 を付しており、 ここではその詳細な説明を省略する。
図 16において、 ナビゲ一シヨン装置 130は、 位置情報取得部 131、 位置 情報算出部 132、 画像合成部 133および地図情報記憶部 134を備えている 。 位置情報取得部 131はいわゆる GPS機能を有しており、 複数の人工衛星か らの電波を受信して車両の位置に関する情報を取得する。 また、 位置情報取得部 131は、 衛星からの電波が受信可能であり、 GPS機能によって位置情報が取 得可能な場合 （GPS機能が有効な場合） は取得した位置情報を出力し、 衛星か らの電波が受信できないなどの理由により GPS機能によって位置倩報を取得で きない場合 （GPS機能が無効な場合） は、 位置情報が取得不可能であることを 示す情報を出力する。 位置情報算出部 1 3 2は位置情報取得部 1 3 1からの位置 情報と、 移動体動き算出装置 1 0 0から出力された移動体動き情報 （第 2の動き Mb ) とを入力して、 車両の現在位置を算出する。 地図情報記憶部 1 3 4は地図 座標に対応した道路や駐車場、 地図の情報を保持している。 画像合成部 1 3 3は 位置情報算出部 1 3 2から現在位置を入力して、 現在位置に対応する道路や地図 の情報を地図情報記憶部 1 3 4から読み出し、 画像に変換して出力する。
図 1 6のナビゲ一シヨンシステムの動作について、 説明する。 いま、 車両は屋 外にあり、 衛星からの電波が受信可能であり、 G P S機能が有効となっているも のとする。 このとき、 位置情報取得部 1 3 1は G P S機能によって得られた位置 情報を出力し、 位置情報算出部 1 3 2は位置情報取得部 1 3 1から受けた位置情 報を、 そのまま現在位置として出力するとともに、 一時的に記憶する。 画像合成 部 1 3 3は位置情報算出部 1 3 2から現在位置を受け、 地図情報記憶手段 1 3 4 に記憶された現在位置周辺の地図情報を読み出し、 地図画像に変換して出力する 。 この地図画像は、 ディスプレイ 1 2 2に表示される。
また、 位置情報取得部 1 3 1が G P S機能によって位置情報を取得可能である ときは、 位置情報算出部 1 3 2は、 移動体動き算出装置 1 0 2から入力された移 動体動きの情報を無視する。
次に、 車両が運転者の操作によって移動し、 屋内駐車場などのような、 衛星か らの電波が受信できず G P S機能によつて位置情報が取得できない場所に移動し たものとする。 このとき、 位置情報取得部 1 3 1は G P S機能が無効であること を表す情報を出力する。 位置情報算出部 1 3 2はこの情報を入力したとき、 内部 に記憶していた現在位置に、 移動体動き算出装置 1 0 0から出力された移動体動 きを積算し、 新たな現在位置として出力するとともに一時的に記憶する。 画像合 成部 1 3 3は G P S機能が有効な場合と同様に動作し、 位置情報算出部 1 3 2か ら現在位置を受け、 地図情報記憶手段 1 3 4に記憶された現在位置周辺の地図情 報を読み出し、 地図画像に変換して出力する。 この地図画像は、 ディスプレイ 1 2 2に表示される。
また、 位置情報算出部 1 3 2は、 現在位置を算出するとともに、 必要に応じて 地図情報記憶部 1 3 4に記憶された駐車場の情報を読み出し、 平面式算出部 1 1 4に送る。 平面式算出部 1 1 4は、 必要に応じて駐車場の情報に含まれる平面式 の情報を読み出して出力する。
さらに、 車両が運転者の操作によって移動し、 再び電波が受信可能となり、 G P S機能が有効になったとき、 位置情報算出部 1 3 2は、 位置情報取得部 1 3 1 で得られた位置情報を、 現在位置として出力する。
図 1 7は屋内駐車場において、 本発明に係る移動体動き算出方法を用いた実験 により得られた車両の移動軌跡を示すグラフである。 上述したように、 本発明に 係る移動体動き算出方法によると、 車両に設置したカメラ画像のみを用いて移動 体動きを算出することが可能であり、 しかも、 路面が傾いている場合であっても 誤差を小さく抑えることが可能であることから、 図 1 7に示すように、 屋内駐車 場においても誤差の小さい移動軌跡の算出が可能となる。
ところが、 図 8 ( b ) のグラフを用いてすでに説明したとおり、 移動体の移動 速度が速くなるほど、 対応点探索の誤差が大きくなる （対応点正解率が低下する ) 傾向があり、 対応点の誤差が大きくなりすぎた場合は、 移動体動きの誤差が大 きくなる。
このような問題に対して、 本実施形態のナビゲ一シヨンシステムでは、 移動体 動き算出装置 1 0 0の利用は衛星からの電波が受信できない屋内だけにとどめて 、 衛星からの電波が受信可能な屋外では、 G P S機能によって取得した位置情報 を現在位置として用いている。 これにより、 精度の高い現在位置の算出を可能に している。
すなわち、 本実施形態では、 G P S機能によって位置情報が取得できるか否か に応じて、 現在位置の算出方法を遷択的に切り替えることによって、 G P S機能 が有効となる屋外と、 電波が受信できない屋内とのいずれにおいても、 常に現在 位置の算出が可能となる。
なお、 本実施形態では、 現在位置の算出方法の切り換えを、 G P S機能によつ て現在位置が取得できるか否かに応じて行うものとしたが、 これに限られるもの ではなく、 例えば、 算出した現在位置が地図情報上で屋内駐車場の入り口近辺で あるか否かに応じて、 現在位置の算出方法を切り替えるようにしてもよい。 なお、 本実施形態においても、 第 1の実施形態と同様に、 動き算出のために仮 定した平面は、 路面に限定されるものではなく、 移動体との相対的な位置を予め 与えることができる平面であれば、 どのような平面であってもかまわない。 例えば、 屋内駐車場が図 1 4のような構造である場合に、 その天井を平面と仮 定した場合の平面式を、 地図情報とともに地図情報記憶部 1 3 4に記憶しておく 。 そして、 位置情報算出部 1 3 2は、 現在位置が図 1 4の屋内駐車場に該当する とき、 地図情報記億部 1 3 4から屋内駐車場の天井の平面式を読み出し、 第 1の 動き算出部 1 0 2の平面式算出部 1 1 4に出力する。 平面式算出部 1◦ 4は、 位 置情報算出部 1 3 2から平面式が出力されたときは、 その平面式を出力する。 こ れにより、 移動体動き算出装置 1 0 0は、 天井平面を基準とした移動体動きを算 出する。
本発明では、 カメラで撮影された画像を用いて移動体の動きを算出するため、 例えば、 路面が暗く、 天井が明るい屋内駐車場では、 路面よりも天井を平面とし て仮定した方が、 算出精度が向上することが期待される。 したがって、 位置情報 に対応した平面式を予め地図情報記憶部 1 3 4に記憶しておき、 移動体動き算出 装置 1 0 0が、 現在位置に応じて平面式を選択して用いることによって、 より精 度の高い移動体動きが算出可能となる。
なお、 上述の各実施形態では、 移動体として車両を例にとって説明を行ったが 、 移動体の種類は車両に限られるものではなく、 例えば、 ロボットであってもよ い。
なお、 上述の各実施形態において、 各処理手段および装置は、 ハードウヱァに よって実現されてもよいし、 ソフトウェアによって実現してもよい。 例えば、 図 1 8のような、 C P U 1 4 1、 R O M 1 4 2および R AM 1 4 3と、 画像の入出 力機能とを備えたコンピュータを用いて、 各処理手段をソフトウェアによって実 現してもよい。 産業上の利用可能性
本発明は、 カメラが設置された移動体の動きを、 カメラ画像を用いて精度良く 求めることができるので、 例えば、 車両ゃ自走ロボットなどに適用し、 その走行 制御や、 操作者に対する進路や位置の提示サービスなどに、 利用することが可能 である,

Claims

請求の範囲
1 . カメラが設置された移動体について、 前記カメラによって撮影された移動 体周囲の画像を用いて、 動きを求める方法であって、
撮影時刻が異なる 2枚の前記画像から、 複数の対応点を求める対応点算出ステ ップと、
前記画像内に所定の平面を仮定し、 前記複数の対応点を用いて、 前記移動体の 動きを示す第 1の動きを求める第 1の動き算出ステップと、
前記第 1の動きと、 前記複数の対応点とを用いて、 前記移動体の動きを示す第 2の動きを求める第 2の動き算出ステップとを備えた
ことを特徴とする移動体動き算出方法。
2 . 請求項 1において、
前記平面は、 路面、 天井面、 または壁面である
ことを特徴とする移動体動き算出方法。
3 . 請求項 1において、
前記第 1の動き算出ステップは、 前記対応点を 3個用いて、 前記第 1の動きを 求めるものである
ことを特徴とする移動体動き算出方法。
4 . 請求項 1において、
前記第 1の動き算出ステップは、
過去に求められた前記第 1の動きまたは前記第 2の動きを用いて、 前記平面を 規定する平面式を更新するステップを含む
ことを特徴とする移動体動き算出方法。
5 . 請求項 1において、 前記第 1の動き算出ステップは、
前記複数の対応点の中から、 m (mは 3以上の整数） 個の対応点で構成される 部分対応点を、 q ( qは 2以上の整数） 組選択する第 1のステップと、
前記第 1のステップにおいて選択した q組の部分対応点から、 それぞれ、 前記 平面を表す平面式を用いて、 前記移動体の動き候補を算出する第 2のステップと 前記第 2のステヅプにおいて算出した q個の動き候補を、 所定の評価手法によ つて評価し、 この評価結果に従って、 前記第 1の動きを特定する第 3のステップ とを含む
ことを特徴とする移動体動き算出方法。
6 . 請求項 5において、
前記第 1の動き算出ステップは、
前記複数の対応点と、 過去に求められた前記第 1の動きまたは前記第 2の動き とを用いて、 静止した対応点の比率である静止対応点率を算出するステヅプを含 み、
前記第 3のステップは、 算出された前記静止対応点率を用いて、 前記第 1の動 きを特定するための評価を行うものである
ことを特徴とする移動体動き算出方法。
7 . 請求項 5において、
前記第 1の動き算出ステップは、
過去に求められた前記第 1の動きまたは前記第 2の動きから、 前記移動体の動 きの予測値を求めるステツプを含み、
前記第 3のステップは、 前記予測値を加味して、 前記第 1の動きを特定するも のである
ことを特徴とする移動体動き算出方法。
8 . 請求項 1において 前記第 2の動き算出ステップは、
前記第 1の動きを初期値として、 探索手法によって、 前言 3複数の対応点に対す る評価を行いつつ、 前記第 2の動きを求めるものである
ことを特徴とする移動体動き算出方法。
9 . 請求項 8において、
前記第 2の動き算出ステップは、 静止した対応点の比率である静止対応点を用 いて、 前記評価を行うものである
ことを特徴とする移動体動き算出方法。
1 0 . 請求項 5において、
前記移動体は、 車両であり、
前記第 2の動き算出ステツプは、
前記第 1の動きの代わりに、 この第 1の動きに対応する部分対応点と、 前記平 面を表す平面式を用い、
前記平面式を初期値として、 探索手法によって、 前記カメラに近い車軸中心を 中心として平面を傾けながら、 前記複数の対応点に対する評価を行いつつ、 前記 第 2の動きを求めるものである
ことを特徴とする移動体動き算出方法。
1 1 . 請求項 1において、
前記移動体は、 車軸の方向が固定である非操舵輪を有し、
前記第 1および第 2の動き算出ステップにおいて、 前記移動体の動きを表すた めの座標系の垂直軸を、 前記非操舵輪の車軸を含む直線と直交するようにおき、 前記移動体の路面上の動きを、 前記垂直軸周りの円弧運動として、 求める ことを特徴とする移動体動き算出方法。
1 2 . 請求項 1 1において、 .
前記垂直軸は、 前記非操舵輪の中心位置を通るように、 おかれている ことを特徴とする移動体動き算出方法。
1 3 . カメラが設置された移動体について、 前記カメラによって撮影された移動 体周囲の画像を用いて、 動きを求める装置であって、
撮影時刻が異なる 2枚の前記画像から、 複数の対応点を求める対応点算出部と 前記画像内に所定の平面を仮定し、 前記対応点算出部によって求められた複数 の対応点を用いて、 前記移動体の動きを示す第 1の動きを求める第 1の動き算出 部と、
前記第 1の動き算出部によって求められた第 1の動きと、 前記複数の対応点と を用いて、 前記移動体の動きを示す第 2の動きを求める第 2の動き算出部とを備 えた
ことを特徴とする移動体動き算出装置。
1 4 . 移動体の位置情報を取得する位置情報取得部を有するナビゲーシヨン装 置と、
請求項 1 3の移動体動き算出装置とを備え、
前記位置情報取得部によって取得された位置情報と、 前記移動体動き算出装置 によって求められた前記移動体の動きとを基にして、 前記移動体の現在位置を求 める
ことを特徴とするナビゲーシヨンシステム。
PCT/JP2004/015677 2003-10-17 2004-10-15 移動体動き算出方法および装置、並びにナビゲーションシステム WO2005038710A1 (ja)

Priority Applications (3)

Application Number Priority Date Filing Date Title
EP04792819A EP1580687A4 (en) 2003-10-17 2004-10-15 METHOD FOR DETERMINING MOBILE UNIT MOTION, DEVICE AND NAVIGATION SYSTEM
JP2005514877A JP3843119B2 (ja) 2003-10-17 2004-10-15 移動体動き算出方法および装置、並びにナビゲーションシステム
US10/540,135 US7542834B2 (en) 2003-10-17 2004-10-15 Mobile unit motion calculating method, apparatus and navigation system

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2003357729 2003-10-17
JP2003-357729 2003-10-17

Publications (1)

Publication Number Publication Date
WO2005038710A1 true WO2005038710A1 (ja) 2005-04-28

Family

ID=34463254

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2004/015677 WO2005038710A1 (ja) 2003-10-17 2004-10-15 移動体動き算出方法および装置、並びにナビゲーションシステム

Country Status (4)

Country Link
US (1) US7542834B2 (ja)
EP (1) EP1580687A4 (ja)
JP (1) JP3843119B2 (ja)
WO (1) WO2005038710A1 (ja)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007256029A (ja) * 2006-03-23 2007-10-04 Denso It Laboratory Inc ステレオ画像処理装置
JP2007280387A (ja) * 2006-03-31 2007-10-25 Aisin Seiki Co Ltd 物体移動の検出方法及び検出装置
JP2008282386A (ja) * 2007-05-10 2008-11-20 Honda Motor Co Ltd 物体検出装置、物体検出方法及び物体検出プログラム
JP2009077022A (ja) * 2007-09-19 2009-04-09 Sanyo Electric Co Ltd 運転支援システム及び車両
JP2014519444A (ja) * 2011-06-15 2014-08-14 ローベルト ボツシユ ゲゼルシヤフト ミツト ベシユレンクテル ハフツング 駐車誘導用後付けセット
CN104678998A (zh) * 2013-11-29 2015-06-03 丰田自动车株式会社 自主移动体及其控制方法
JP2017042247A (ja) * 2015-08-25 2017-03-02 富士フイルム株式会社 基準点評価装置、方法およびプログラム、並びに位置合せ装置、方法およびプログラム

Families Citing this family (23)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6801244B2 (en) * 2000-02-29 2004-10-05 Kabushiki Kaisha Toshiba Obstacle detection apparatus and method
JP3843119B2 (ja) * 2003-10-17 2006-11-08 松下電器産業株式会社 移動体動き算出方法および装置、並びにナビゲーションシステム
US7379566B2 (en) * 2005-01-07 2008-05-27 Gesturetek, Inc. Optical flow based tilt sensor
US8666661B2 (en) * 2006-03-31 2014-03-04 The Boeing Company Video navigation
US20090089705A1 (en) * 2007-09-27 2009-04-02 Microsoft Corporation Virtual object navigation
DE102008045619A1 (de) 2008-09-03 2010-03-04 Daimler Ag Verfahren, Vorrichtung und System zur Ermittlung einer Geschwindigkeit eines Fahrzeugs
JP5419403B2 (ja) * 2008-09-04 2014-02-19 キヤノン株式会社 画像処理装置
DE102008061060A1 (de) 2008-12-08 2010-06-10 Daimler Ag Verfahren zur Ermittlung mindestens einer Rotationsachse eines Fahrzeugs
DE102009016562A1 (de) 2009-04-06 2009-11-19 Daimler Ag Verfahren und Vorrichtung zur Objekterkennung
JP5223811B2 (ja) * 2009-08-06 2013-06-26 株式会社日本自動車部品総合研究所 画像補正装置、画像補正方法、及びそれらに用いられる変換マップ作成方法
TWI407280B (zh) * 2009-08-20 2013-09-01 Nat Univ Tsing Hua 自動搜尋系統及方法
DE102010013093A1 (de) * 2010-03-29 2011-09-29 Volkswagen Ag Verfahren und System zur Erstellung eines Modells eines Umfelds eines Fahrzeugs
US11405841B2 (en) * 2012-07-20 2022-08-02 Qualcomm Incorporated Using UE environmental status information to improve mobility handling and offload decisions
CN103988241A (zh) 2012-11-13 2014-08-13 株式会社东芝 检测装置、方法以及程序
DE102012221572A1 (de) * 2012-11-26 2014-05-28 Robert Bosch Gmbh Autonomes Fortbewegungsgerät
JP5962689B2 (ja) * 2014-02-14 2016-08-03 トヨタ自動車株式会社 自律移動体、及びその故障判定方法
DE102014212819A1 (de) * 2014-07-02 2016-01-07 Zf Friedrichshafen Ag Ortspositionsabhängige Darstellung von Fahrzeugumfelddaten auf einer mobilen Einheit
KR102337209B1 (ko) * 2015-02-06 2021-12-09 삼성전자주식회사 주변 상황 정보를 통지하기 위한 방법, 전자 장치 및 저장 매체
IL238473A0 (en) * 2015-04-26 2015-11-30 Parkam Israel Ltd A method and system for discovering and mapping parking areas
CN109564434B (zh) * 2016-08-05 2023-07-25 深圳市大疆创新科技有限公司 用于定位可移动物体的系统和方法
WO2018049643A1 (en) * 2016-09-18 2018-03-22 SZ DJI Technology Co., Ltd. Method and system for operating a movable object to avoid obstacles
KR102313026B1 (ko) * 2017-04-11 2021-10-15 현대자동차주식회사 차량 및 차량 후진 시 충돌방지 보조 방법
JP6840024B2 (ja) * 2017-04-26 2021-03-10 株式会社クボタ オフロード車両及び地面管理システム

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2000275013A (ja) * 1999-03-24 2000-10-06 Mr System Kenkyusho:Kk 視点位置姿勢の決定方法、コンピュータ装置および記憶媒体
JP2001266160A (ja) * 2000-03-22 2001-09-28 Toyota Motor Corp 周辺認識方法および周辺認識装置
JP2003178309A (ja) * 2001-10-03 2003-06-27 Toyota Central Res & Dev Lab Inc 移動量推定装置

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5777690A (en) 1995-01-20 1998-07-07 Kabushiki Kaisha Toshiba Device and method for detection of moving obstacles
JP3463397B2 (ja) 1995-02-09 2003-11-05 日産自動車株式会社 走行路検出装置
US6192145B1 (en) 1996-02-12 2001-02-20 Sarnoff Corporation Method and apparatus for three-dimensional scene processing using parallax geometry of pairs of points
JP3729916B2 (ja) 1996-02-29 2005-12-21 富士通株式会社 ３次元情報復元装置
EP1257971A4 (en) 1999-11-26 2005-07-06 Mobileye Inc SYSTEM AND METHOD FOR ESTIMATING THE SELF-MOVING OF A VEHICLE IN MOTION USING SUCCESSIVE IMAGES RECORDED ALONG THE VEHICLE MOVEMENT TRACK
JP4425495B2 (ja) * 2001-06-08 2010-03-03 富士重工業株式会社 車外監視装置
JP3843119B2 (ja) * 2003-10-17 2006-11-08 松下電器産業株式会社 移動体動き算出方法および装置、並びにナビゲーションシステム
JP4107605B2 (ja) * 2005-02-01 2008-06-25 シャープ株式会社 移動体周辺監視装置、移動体周辺監視方法、制御プログラムおよび可読記録媒体

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2000275013A (ja) * 1999-03-24 2000-10-06 Mr System Kenkyusho:Kk 視点位置姿勢の決定方法、コンピュータ装置および記憶媒体
JP2001266160A (ja) * 2000-03-22 2001-09-28 Toyota Motor Corp 周辺認識方法および周辺認識装置
JP2003178309A (ja) * 2001-10-03 2003-06-27 Toyota Central Res & Dev Lab Inc 移動量推定装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
See also references of EP1580687A4 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007256029A (ja) * 2006-03-23 2007-10-04 Denso It Laboratory Inc ステレオ画像処理装置
JP2007280387A (ja) * 2006-03-31 2007-10-25 Aisin Seiki Co Ltd 物体移動の検出方法及び検出装置
JP2008282386A (ja) * 2007-05-10 2008-11-20 Honda Motor Co Ltd 物体検出装置、物体検出方法及び物体検出プログラム
US8300887B2 (en) 2007-05-10 2012-10-30 Honda Motor Co., Ltd. Object detection apparatus, object detection method and object detection program
JP2009077022A (ja) * 2007-09-19 2009-04-09 Sanyo Electric Co Ltd 運転支援システム及び車両
JP2014519444A (ja) * 2011-06-15 2014-08-14 ローベルト ボツシユ ゲゼルシヤフト ミツト ベシユレンクテル ハフツング 駐車誘導用後付けセット
CN104678998A (zh) * 2013-11-29 2015-06-03 丰田自动车株式会社 自主移动体及其控制方法
JP2017042247A (ja) * 2015-08-25 2017-03-02 富士フイルム株式会社 基準点評価装置、方法およびプログラム、並びに位置合せ装置、方法およびプログラム
US10242452B2 (en) 2015-08-25 2019-03-26 Fujifilm Corporation Method, apparatus, and recording medium for evaluating reference points, and method, apparatus, and recording medium for positional alignment

Also Published As

Publication number Publication date
JP3843119B2 (ja) 2006-11-08
JPWO2005038710A1 (ja) 2007-01-25
US7542834B2 (en) 2009-06-02
US20060055776A1 (en) 2006-03-16
EP1580687A1 (en) 2005-09-28
EP1580687A4 (en) 2006-01-11

Similar Documents

Publication Publication Date Title
WO2005038710A1 (ja) 移動体動き算出方法および装置、並びにナビゲーションシステム
US10762643B2 (en) Method for evaluating image data of a vehicle camera
Zhou et al. Ground-plane-based absolute scale estimation for monocular visual odometry
KR101725060B1 (ko) 그래디언트 기반 특징점을 이용한 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법
Badino A robust approach for ego-motion estimation using a mobile stereo platform
EP2948927B1 (en) A method of detecting structural parts of a scene
KR101776621B1 (ko) 에지 기반 재조정을 이용하여 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법
US8401783B2 (en) Method of building map of mobile platform in dynamic environment
US9014421B2 (en) Framework for reference-free drift-corrected planar tracking using Lucas-Kanade optical flow
KR101776620B1 (ko) 검색 기반 상관 매칭을 이용하여 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법
US20180308240A1 (en) Method for estimating the speed of movement of a camera
US11082633B2 (en) Method of estimating the speed of displacement of a camera
KR20150144728A (ko) 이동 로봇의 맵을 업데이트하기 위한 장치 및 그 방법
KR101544021B1 (ko) 3차원 지도 생성 장치 및 3차원 지도 생성 방법
CN109443348A (zh) 一种基于环视视觉和惯导融合的地下车库库位跟踪方法
US20130182896A1 (en) Gradient estimation apparatus, gradient estimation method, and gradient estimation program
KR102559203B1 (ko) 포즈 정보를 출력하는 방법 및 장치
CN113137968B (zh) 基于多传感器融合的重定位方法、重定位装置和电子设备
CN113345032B (zh) 一种基于广角相机大畸变图的初始化建图方法及系统
JP6826010B2 (ja) カメラ運動推定装置、カメラ運動推定方法及びプログラム
JP2007241326A (ja) 移動体動き算出装置
CN113744308A (zh) 位姿优化方法、装置、电子设备、介质及程序产品
Jurado et al. Inertial and imaging sensor fusion for image-aided navigation with affine distortion prediction
JP2022190173A (ja) 位置推定装置
Rabe Detection of moving objects by spatio-temporal motion analysis

Legal Events

Date Code Title Description
WWE Wipo information: entry into national phase

Ref document number: 2005514877

Country of ref document: JP

AK Designated states

Kind code of ref document: A1

Designated state(s): AE AG AL AM AT AU AZ BA BB BG BR BW BY BZ CA CH CN CO CR CU CZ DE DK DM DZ EC EE EG ES FI GB GD GE GH GM HR HU ID IL IN IS JP KE KG KP KR KZ LC LK LR LS LT LU LV MA MD MG MK MN MW MX MZ NA NI NO NZ OM PG PH PL PT RO RU SC SD SE SG SK SL SY TJ TM TN TR TT TZ UA UG US UZ VC VN YU ZA ZM ZW

AL Designated countries for regional patents

Kind code of ref document: A1

Designated state(s): GM KE LS MW MZ NA SD SL SZ TZ UG ZM ZW AM AZ BY KG KZ MD RU TJ TM AT BE BG CH CY CZ DE DK EE ES FI FR GB GR HU IE IT LU MC NL PL PT RO SE SI SK TR BF BJ CF CG CI CM GA GN GQ GW ML MR NE SN TD TG

WWE Wipo information: entry into national phase

Ref document number: 2004792819

Country of ref document: EP

ENP Entry into the national phase

Ref document number: 2006055776

Country of ref document: US

Kind code of ref document: A1

WWE Wipo information: entry into national phase

Ref document number: 10540135

Country of ref document: US

121 Ep: the epo has been informed by wipo that ep was designated in this application
WWP Wipo information: published in national office

Ref document number: 2004792819

Country of ref document: EP

WWW Wipo information: withdrawn in national office

Ref document number: 2004792819

Country of ref document: EP

WWP Wipo information: published in national office

Ref document number: 10540135

Country of ref document: US