Position Estimation Apparatus, Position Estimation Method and Program Recording Medium

• US 20070265741A1
• Filed: 05/08/2007
• Published: 11/15/2007
• Est. Priority Date: 05/09/2006
• Status: Active Grant

• Current Assignee: Sony Corporation (Sony Group Corp.)
• Original Assignee: Sony Corporation (Sony Group Corp.)
• Inventors: Yoshigahara, Takayuki, Fukuchi, Masaki, OI, Kenichiro, Gutmann, Steffen, Fujita, Masahiro
• Granted Patent: US 7,873,447 B2
• Time in Patent Office: Days
• US Class Current: 701/23
• CPC Class Codes: G01C 21/12 executed aboard the object being navigated; Dead reckoning
• CPC Class Codes: G01C 22/02 by conversion into electric waveforms and subsequent integration, e.g. using tachometer generator G01C22/002, G01C22/004, G01C22/006 take precedence

Abstract

The present invention provides a position estimation apparatus capable of determining whether an estimated value deviates from an actual value and trying a process to correct the estimated value if the estimated value is determined to be incorrect. The position estimation apparatus employs a position prediction unit configured to produce a prediction of the position of a mobile object having an odometry sensor mounted thereon, an environment observation unit configured to keep track of each feature point in the environment of the mobile object, a prediction-error check unit configured to determine whether the position prediction produced by the position prediction unit is correct or incorrect, a position-prediction correction unit configured to correct a wrong position prediction and a position/posture updating unit configured to update the position and/or posture of the mobile object on the basis of a correct position prediction.

Claims

1. A position estimation apparatus for a mobile object, said position estimation apparatus comprising:

a position prediction unit configured to drive a position prediction filter to carry out a filtering process on a sensor value generated by an odometry sensor mounted on said mobile object as a sensor value for the present frame and a result of estimating the position of said mobile object for a previous frame in order to produce a position prediction as a prediction of said position of said mobile object for the present frame;

an environment observation unit configured to measure the environment of said mobile object by making use of an environment observation sensor other than said odometry sensor used by said position prediction unit and observing positions of feature points in said environment by keeping track of said feature points from frame to frame;

a prediction-error check unit configured to produce a result of determination as to whether a position prediction produced by said posit-on prediction unit as a prediction of the position of said mobile object is correct or incorrect on the basis of a result generated by said environment observation unit as a result of an operation to keep track of said feature points in said environment;

a position prediction correction unit configured to correct a position prediction produced by said position prediction unit as a prediction of the position of said mobile object if said prediction-error check unit produces a determination result indicating that said position prediction is incorrect; and

a position/posture updating unit configured to update the position and/or posture of said mobile object if said prediction-error check unit produces a determination result indicating that a position prediction produced by said position prediction unit as a prediction of the position of said mobile object is correct.

2. The position estimation apparatus according to claim 1 wherein said environment observation unit finds a search range by making use of a position and/or a posture, which are predicted by said position prediction unit, and keeps track of said feature points.

3. The posit on estimation apparatus according to claim 2 wherein said environment observation unit:

estimates positions of said feature points from positions known in advance as positions of said feature points in a three-dimensional space and from data estimated by a Kalman filter as the positional data of said mobile object;

deforms a template seen from the position of said mobile object as a template of the vicinity of each of said feature points in accordance with affinity constraints from luminance image data in the vicinity of each of said feature points and from posture data of a luminance image in said vicinity of each of said feature points; and

keeps track of said feature points by adoption of a template matching technique on the basis of said search range and said templates.

4. The position estimation apparatus according to claim 1 wherein said prediction-error check unit:

counts the number of feature points each successfully passing an operation carried out by said environment observation unit to keep track of said feature points and the number of feature points failing in the same operation; and

produces a determination result indicating that a position prediction produced by said position prediction unit as a prediction of the position of said mobile object is correct if said operation to keep track of feature points has been carried out by said environment observation unit at a success rate higher than a certain probability;

or otherwise,

produces a determination result indicating that said position prediction produced by said position prediction unit as a prediction of the position of said mobile object is incorrect.

5. The position estimation apparatus according to claim 1 wherein said position/posture updating unit makes said position prediction unit use an estimation result of a position, which has been updated by said position/posture updating unit as the position of said mobile object, in an operation to produce a position prediction for the next frame as an estimation result of the position of said mobile object for the previous frame.

6. The position estimation apparatus according to claim 2 wherein said position prediction correction unit changes an existence probability distribution model showing a distribution of probabilities of existence of said mobile object in order to increase the value of the standard deviation of said existence probability distribution model and hence increase the width of said search range used by said environment observation unit if said prediction-error check unit produces a determination result indicating that a position prediction produced by said position prediction unit as a prediction of the position of said mobile object is incorrect.

7. A position estimation method for a mobile object, said position estimation method comprising the steps of:

driving a position prediction filter to carry out a filtering process on a sensor value generated by an odometry sensor mounted on said mobile object as a sensor value for the present frame and a result of estimating the position of said mobile object for a previous frame in order to produce a position prediction as a prediction of the position of said mobile object for the present frame;

measuring the environment of said mobile object by making use of an environment observation sensor other than said odometry sensor used in said position prediction process and observing positions of feature points in said environment by keeping track of said feature points from frame to frame;

producing a result of determination as to whether a position prediction produced in said position prediction process as a prediction of the position of said mobile object is correct or incorrect on the basis of a result generated in said environment observation process as a result of an operation to keep track of said feature points in said environment;

correcting a position prediction produced in said position prediction process as a prediction of the position of said mobile object if said prediction-error check process produces a determination result indicating that said position prediction is incorrect; and

updating the position and/or posture of said mobile object if said prediction-error check process produces a determination result indicating that a position prediction produced in said position prediction process as a prediction of the position of said mobile object is correct.

8. A recording medium configured to record a position estimation program to be executed by a position estimation apparatus configured to predict the position of a mobile object wherein said position estimation program comprises:

a position prediction process of driving a position prediction filter to carry cut a filtering process on a sensor value generated by an odometry sensor mounted on said mobile object as a sensor value for the present frame and a result of estimating the position of said mobile object for a previous frame in order to produce a position prediction as a prediction of the position of said mobile object for the present frame;

an environment observation process of measuring the environment of said mobile object by making use of an environment observation sensor other than said odometry sensor used in said position prediction process and observing positions of feature points in said environment by keeping track of said feature points from frame to frame;

a prediction-error check process of producing a result of determination as to whether a position prediction produced in said position prediction process as a prediction of the position of said mobile object is correct or incorrect on the basis of a result generated in said environment observation process as a result of an operation to keep track of said feature points in said environment;

a position prediction correction process of correcting a position prediction produced in said position prediction process as a prediction of the position of said mobile object if said prediction-error check process produces a determination result indicating that said position prediction is incorrect; and

a position/posture updating process of updating the position and/or posture of said mobile object if said prediction-error check process produces a determination result indicating that a position prediction produced in said position prediction process as a prediction of the position of said mobile object is correct.