JP7175930B2 - Information processing device, information processing method, and program - Google Patents

Description

TECHNICAL FIELD Embodiments of the present invention relate to an information processing apparatus, an information processing method, and a program.

There is a technology for estimating the self-position of a moving body, etc., by using a change in the appearance of an object, etc., captured in a series of images acquired in time series, in which the moving body, etc. is equipped with an imaging means. .

In the related art, the following processing is performed. That is, the three-dimensional position of the object is determined by the principle of triangulation based on multiple images. Then, the position (first position) of the object on the image is obtained by projecting the obtained three-dimensional position into the image. Next, the position of the object on the image (second position) is determined by tracking the feature points in the time-series images. Finally, the three-dimensional position and the position and orientation of the moving object are adjusted so that the feature point error, which is the difference between the first position and the second position, becomes small. Through such processing, the mobile body estimates its own position and orientation.

Using the above-described conventional technology, a mobile object can estimate its own position and the like even in places where GPS (Global Positioning System) signals do not reach. In addition, the prior art described above has the advantage of not causing drift due to heat, which can occur when using an inertial measurement unit (IMU, Internal Measurement Unit).

On the other hand, the conventional technique described above has a problem that the estimation accuracy may decrease when the feature point cannot be stably tracked.

JP 2017-215193 A

The problem to be solved by the present invention is to provide an information processing device, an information processing method, and a program that can improve the position estimation accuracy when estimating the position of a mobile object based on an acquired image. be.

An information processing apparatus according to an embodiment has an estimation unit, an image acquisition unit, and a correction unit. The estimation unit estimates the position and orientation of the moving object. The image acquisition unit is provided on the moving body and acquires an image. a correcting unit for correcting the position and orientation of the moving body estimated by the estimating unit based on the acquired feature points of the object in the image and a motion model that is a constraint condition of the moving body; Calculate the amount of correction. Further, the estimating unit corrects the estimated position and orientation of the moving body based on the correction amount obtained by the correcting unit to obtain a newly estimated position.

1 is a block diagram showing a schematic functional configuration of an information processing apparatus according to a first embodiment; FIG. 4 is a flowchart showing the procedure of processing by the information processing apparatus according to the first embodiment; FIG. 4 is a schematic diagram showing an outline of a processing result of feature point matching (feature point tracking) between a plurality of images by a feature point tracking unit according to the first embodiment; Schematic diagram showing a calculation process for estimating the |F|-th position (initial value) of the moving object by the estimation unit of the first embodiment. 4A and 4B are schematic diagrams showing examples of feature point errors in an acquired image in the first embodiment; FIG. FIG. 2 is a schematic diagram showing an overview of the movement of a vehicle (moving body) that is constrained by a motion model in the first embodiment; FIG. 7 is a schematic diagram showing an outline of movement of a vehicle (moving body) that moves based on a motion model different from that of FIG. 6 in the first embodiment; 4A and 4B are schematic diagrams showing an example of motion of a moving body for explaining a motion model error in the first embodiment; FIG. FIG. 2 is a block diagram showing a schematic functional configuration of an information processing apparatus according to a second embodiment; FIG. 10 is a flow chart showing the procedure of processing by the information processing apparatus according to the second embodiment; FIG. 11 is a block diagram showing a schematic functional configuration of an information processing apparatus according to a third embodiment;

Hereinafter, an information processing apparatus, an information processing method, and a program according to embodiments will be described with reference to the drawings.

(First embodiment)
FIG. 1 is a block diagram showing a schematic functional configuration of an information processing apparatus 1 according to this embodiment. As illustrated, the information processing apparatus 1 includes an estimation unit 101, an image acquisition unit 102, a feature point tracking unit 103, a feature point error calculation unit 104, a motion model calculation unit 105, and a motion model error calculation unit 106. , and a correction unit 107 . Each of these functional units is implemented using electronic circuits, for example. In addition, each functional unit may be internally provided with storage means such as a semiconductor memory or a magnetic hard disk device, if necessary. Also, all or part of the functions of the information processing device 1 may be realized by a computer and software.

The information processing device 1 is provided, for example, in a mobile object. Here, the moving body is, for example, an automobile (including an electric vehicle), a motorcycle, a railroad vehicle, an aircraft, a drone, or the like. Note that, of the functions of the information processing apparatus 1, only the camera of the image acquisition unit 102 and other necessary functions may be provided in the moving body. In this case, the portion of the information processing device 1 that is attached to the moving object and the other portion communicate with each other as appropriate using a communication device or the like.

The estimation unit 101 estimates the position and orientation of the moving object based on the correction amount calculated by the correction unit 107 . That is, the estimating unit 101 receives feedback of the correction amount obtained by the correcting unit 107, corrects the estimated position and orientation of the moving body based on this correction amount, and obtains a new estimated position.

The position of the mobile object is the position of the mobile object in the three-dimensional space. The position of the moving body can be represented by three-dimensional coordinates, for example. However, if there are restrictions on the movement of the moving object, the position does not necessarily need to be represented by three-dimensional coordinates, and the information may be reduced as appropriate. For example, as a typical example, if the moving body is constrained to move only on a predetermined plane, the position of the moving body can be represented only by two-dimensional coordinates on the plane. However, the above constraint conditions are not limited to the constraint on the plane of the moving body. A constraint condition may be a condition relating to some constraint in a three-dimensional space. Further, when the moving body has a size, the position of a predetermined portion or a predetermined point included in the moving body may be regarded as the "position of the moving body". Also, the attitude of the mobile body is at least the orientation of the mobile body. When the moving object has a movable part (for example, the movable part is a joint part included in the crane of a mobile crane), the posture may include the state of the movable part. Here, the orientation of the moving body can be represented by, for example, a three-dimensional angle. However, if there are constraints on the orientation of the moving object, the information representing the orientation does not necessarily have independent three-dimensional coordinate values. As an example, if the moving object is a vehicle such as a four-wheeled vehicle, and the bottom surface of the tire of the vehicle is constrained so that it contacts a predetermined plane, or if the vehicle can be considered to have no moving parts. , the attitude of the vehicle can be represented by a one-dimensional numerical value of the rotation angle on the plane.

The image acquisition unit 102 acquires an image. The image acquisition unit 102 is configured to include, for example, a camera for capturing an optical image, a distance sensor for acquiring a distance image, and the like.

The image acquired by the image acquisition unit 102 is, for example, a color image, a grayscale image, a distance image, or the like. Color and grayscale images can be obtained from a camera (such as a video camera) connected to the computer. A range image can be obtained from a range sensor. Note that an image acquired online in real time by the image acquisition unit 102 may be processed. Further, after the image acquired by the image acquisition unit 102 is stored in a storage device or the like, the image may be processed offline.

Note that when the moving body is a rigid body, a camera or distance sensor attached directly to the moving body acquires an image based on the position and orientation of the moving body. The coordinates of the feature points included in this image are the basis for estimating the position and orientation in the information processing apparatus 1 . Here, the mounting positions of the camera and the distance sensor on the moving body, the orientation of the camera and the like when acquiring an image, the angle of view, and the like are fixed.

The feature point tracking unit 103 extracts predetermined feature points from the image acquired by the image acquisition unit 102 . A predetermined feature point is, for example, a feature point of an object in an image acquired by the image acquisition unit 102 . Further, the feature point tracking unit 103 tracks the same feature point in a plurality of time-series images. In other words, the feature point tracking unit 103 associates feature points extracted from a plurality of images captured at different times. That is, the feature point tracking unit 103 tracks the feature points among the plurality of images acquired at different times.

A feature point is generally a point having some characteristic on an image. By having such features, feature points can be more easily extracted. In the case of optical images, for example, intersections of a plurality of (for example two) segment elements are suitable as feature points. In the case of a distance image, for example, the maximum point or the minimum point of the distance, or the intersection of a plurality of line components can be used as feature points, as in the case of the optical image.

The feature point extraction and tracking process itself can be implemented using existing technology. Specifically, techniques such as SIFT (Scale-Invariant Feature Transform), ORB (Oriented FAST and Rotated BRIEF), and LIFT (Learned Invariant Feature Points) can be used. Note that SIFT is also described in reference 1 [D. G. Lowe. Distinctive image features from scale invariant keypoints. IJCV, 2004.]. ORB is also described in reference 2 [E. Rublee, V. Rabaud, K. Konolige, and G. Bradski. ORB: An efficient alternative to SIFT or SURF. In ICCV, 2011.]. LIFT is also described in Reference 3 [K. M. Yi, E. Trulls, V. Lepetit, and P. Fua. LIFT: Learned Invariant Feature Transform. In ECCV, 2016.].

Alternatively, each pixel included in an image may be regarded as a feature point, an image may be affine transformed, and the parameters of the affine transformation when the luminance difference when projected onto another image is minimized may be used as the tracking result. Alternatively, each pixel of an image may be tracked using optical flow or the like.

The feature point error calculation unit 104 calculates the feature point error from the tracking result of the feature points by the feature point tracking unit 103 and the position and orientation of the moving object estimated by the estimation unit 101 . In other words, the feature point error calculation unit 104 calculates the difference between the coordinates of the feature points obtained by tracking the feature points in the acquired image and the coordinates of the feature points obtained from the estimated position and orientation of the moving object. Ask for That is, the feature point error calculation unit 104 obtains the feature point error, which is the difference between the position of the moving object obtained based on the tracking result of the feature points and the position of the moving object estimated by the estimation unit 101 . That is, the feature point error calculation unit 104 calculates a feature point error, which is an error between the position of the feature point of the object in the acquired image and the three-dimensional position of the feature point of the object.

The motion model calculation unit 105 calculates the position and orientation of the mobile object based on the position and orientation of the mobile object estimated by the estimation unit 101 and the motion model, which is a constraint condition regarding the motion of the mobile object.

The motion model error calculation unit 106 obtains a motion model error, which is the difference between the position and orientation of the moving object calculated by the motion model calculation unit 105 and the position and orientation of the moving object estimated by the estimation unit 101 . Details of the method for calculating the motion model error will be described later.

The correction unit 107 calculates correction amounts for the position and orientation of the moving body from the feature point error calculated by the feature point error calculation unit 104 and the motion model error calculated by the motion model error calculation unit 106 . That is, the correction unit 107 obtains a correction amount for correcting the position of the moving object estimated by the estimation unit 101 based on the feature point error and the motion model error. In other words, the correcting unit 107 corrects the position and orientation of the moving object estimated by the estimating unit 101 based on the feature points of the object in the acquired image and the motion model that is the constraint condition of the moving object. Calculate the amount of correction for The correction unit 107 feeds back the calculated correction amount information to the estimation unit 101 . A method of obtaining the correction amount for reducing the error will be described later.

FIG. 2 is a flowchart showing the procedure of processing by the information processing apparatus 1. As shown in FIG. Here, the procedure of processing by the information processing apparatus will be described along this flowchart.

In step S101, the image acquisition unit 102 acquires an image.

In step S102, the feature point tracking unit 103 extracts feature points in the image based on the image acquired in step S101. Next, the feature point tracking unit 103 matches the extracted feature points with feature points extracted from other images acquired at different times. Thereby, the feature point tracking unit 103 associates the feature points with each other. That is, the feature point tracking unit 103 tracks feature points in time series.

In step S103, the estimation unit 101 estimates the position and orientation of the moving object. The position and orientation estimated in this step are the initial values of the position and orientation, respectively. How to estimate the initial values of the position and orientation will be described separately.

In step S104, the feature point error calculator 104 calculates feature point errors. This feature point error is calculated from the feature point tracking result obtained in step S102 and the position and orientation estimated by the estimation unit 101 in either step S103 or step S109 (most recently estimated position and orientation). be done.

In step S105, the exercise model calculator 105 calculates an exercise model. This motion model is calculated based on the position and orientation (most recently estimated position and orientation) estimated by the estimation unit 101 in either step S103 or step S109.

In step S106, the motion model error calculator 106 calculates a motion model error. This motion model error is obtained from the position and orientation (most recently estimated position and orientation) estimated by the estimation unit 101 in either step S103 or step S109 and the motion model calculated by the motion model calculation unit 105 in S105. , is calculated.

In step S107, the information processing apparatus 1 determines whether the feature point error calculated in step S104 and the motion model error calculated in step S106 are sufficiently small values. Specifically, the information processing apparatus 1 uses evaluation function values for comprehensively evaluating feature point errors and motion model errors. An example of this evaluation function is a function that simply adds the absolute value of the feature point error and the absolute value of the motion model error. Note that other evaluation functions may be used. The information processing apparatus 1 performs the determination in this step based on the predetermined threshold value of the evaluation function. As a result of the determination, if the error is sufficiently small (step S107: YES), the position and orientation are estimated within the desired error range, so the entire processing of this flowchart ends. If the error is not sufficiently small (step S107: NO), the process proceeds to step S108.

In step S108, the correction unit 107 calculates a correction amount so as to reduce the feature point error and the motion model error. The correction unit 107 passes the calculated correction amount to the estimation unit 101 .

In step S109, the estimation unit 101 estimates the corrected position and orientation of the moving object by applying the correction amount calculated by the correction unit 107 in step S108. Estimating section 101 uses the corrected position and orientation as a new estimation result.

Through the above series of processing procedures including repetition, it is possible to adjust the estimated position and orientation so as to reduce the error.

It should be noted that the processing order of each step shown in FIG. may be executed in parallel with each other. As an example, the motion model error may be calculated before finishing calculating the feature point error.

Further details of each process described with reference to FIG. 2 will be described below.

FIG. 3 is a schematic diagram showing an overview of the processing result of feature point matching (that is, tracking) between a plurality of images by the feature point tracking unit 103. As shown in FIG. The figure includes five images acquired by the image acquisition unit 102 . These five images correspond to discrete times T, T+1, T+2, T+3 and T+4, respectively. Each image is an image captured at each time using a camera provided in front of the traveling direction of the moving object. The black dots shown in each image are feature points extracted by the feature point tracking unit 103 . Also, a straight line connecting these black circle points indicates correspondence (symmetry) of feature points between images.

Next, a method for estimating the initial values of the position and orientation will be described. The method by which the estimating unit 101 estimates the initial position and orientation of the moving object is, for example, as follows. Specifically, the estimation unit 101 may estimate the current initial position and orientation from the position and orientation of the past frames and the velocity, assuming constant velocity motion.

In the i-th frame (i=1, 2, . . . , |F|, |F|-th frame is the latest frame), t _i is the position of the moving body, R _i is the attitude of the moving body, T i is the time, and T _i is the speed. Assuming that s _i , the initial position and orientation assuming constant velocity motion are as shown in equation (1). This formula (1) represents a calculation process for taking t _i−1 , t _i−2 , T _i , T _i−1 , T _i−2 , and s _i as inputs and outputting the position t _i . Equation (1) represents a calculation process for inputting R _i−1 , R _i−2 , T _i , T _i−1 , and T _i−2 and outputting attitude R _i .

In equation (1), r(R) represents the rotation angle of the rotation matrix R. Similarly, r ⁻¹ (θ) is the rotation matrix representation of the rotation angle θ.

Alternatively, the estimation unit 101 may calculate the initial position and orientation from the feature point tracking result using Nister's 5-point algorithm or an algorithm for solving the PnP (Perspective-n-Point) problem. Nister's five-point algorithm is described in reference 4 [Nister, D. An efficient solution to the five-point relative problem pose. IEEE Transactions on Pattern Analysis and Machine Intelligence, 26(6):756-770, 2004.] is also listed. For the algorithm to solve the PnP problem, refer to Reference 5 [V. Lepetit, F. Moreno-Noguer, and P. Fua, “EPnP: An accurate O(n) solution to the PnP problem,” International Journal of Computer Vision, vol. 81, no. 2, pp. 155-166, 2009.].

Nister's 5-point algorithm is based on the j-th feature point position x _ji (j=1 _{, 2 ,} . This is a method of calculating _i and the orientation R _i . The algorithm for solving the PnP problem is the position x _ji (j= 1, 2, . . . , M _i ) and the three-dimensional position X _j of x _ji to calculate the position t _i and the orientation R _i of the moving body.

However, Nister's 5-point algorithm cannot compute the scale of the position. Therefore, it is also necessary to determine the scale using the three-dimensional positions of the feature points separately determined using a range image or the like, or the speed of the moving object obtained from a speedometer or the like. As an example, we explain how to estimate the position and orientation using Nister's 5-point algorithm and the speed of a moving object obtained from a speedometer or the like. In the i-th frame (i=1, 2, . . . , |F|, |F|-th frame is the latest frame), t _i is the position of the moving body, R _i is the attitude of the moving body, T i is the time, and T _i is the speed. Let _si . Using the matching result (tracking result) between the feature points of the k-th frame and the feature points of the |F| Let _nk|F| be the relative position of the moving object in the |F|th frame. However, since the scale is unknown, the relative position is expressed by the following formula.

At this time, the |F|-th position t _{1 |F|} is represented by the following equation (2).

This equation (2) is expressed by the attitude R _k of the k-th frame, the relative position n _k|F| , and the positions t _k and t _|F|-1 of the k-th and (|F|-1)th frames. , |F|-th and (|F|-1)th frame times T _|F| and T _|F|-1 , and |F|-th frame speed s _| F| It shows the calculation process for outputting the position t _|F|-1 of the th frame. Note that the angle φ will be described later.

FIG. 4 is a schematic diagram showing a calculation process for the estimating unit 101 to estimate the |F|-th position (initial value) of the moving object by the above method. As shown in the figure, assuming constant velocity motion, the position of the moving object in the |F| can be estimated.

In FIG. 4, the position of the moving body in the _k -th frame is tk, and the posture is _Rk . The position of the moving object in the |F|-1th frame is t _|F|-1 . The position of the moving object in the |F|-th frame is t _|F| . The traveled distance calculated from the vehicle speed and time is |t _|F| −t _|F|−1 |, as expressed by the above equation (2). As shown in FIG. 4, the angle φ appearing in the equation (2) is defined by the position of the moving body in the |F|-1th frame and the position of the moving body in the |F| This is the angle formed with the position of the moving body.

Next, a method of calculating the feature point error by the feature point error calculator 104 will be described. The feature point error is defined as the position of the feature points in the three-dimensional space projected onto the frame image (two-dimensional coordinates on the image) and the position of the feature points extracted on the frame image (two-dimensional coordinates on the image). image) and the difference (distance, norm).

Note that the three-dimensional positions (positions in the three-dimensional space) of feature points can be obtained using existing techniques. For example, Nister's 5-point algorithm described above can be used to determine the three-dimensional positions of the feature points. Moreover, the three-dimensional position of the feature point can also be obtained using the result of distance image estimation or the like. If the image acquisition unit 102 or the like of the information processing apparatus 1 is equipped with a distance sensor, the three-dimensional position of the feature point can be obtained using the distance image.

The technique of estimating the distance image described above is described, for example, in Reference 6 [R. Mahjourian, M. Wicke, and A. Angelova. Unsupervised learning of depth and ego-motion from monocular video using 3d geometric constraints. IEEE Conference on Computer Vision and Pattern Recognition, 2018, pp. 5667-5675.] and Reference 7 [N. Mayer, E. Ilg, P. Hausser, P. Fischer, D. Cremers, A. Dosovitskiy, and T Brox. A large dataset to train convolutional networks for disparity, optical flow, and scene flow estimation. In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition, 2016, pp. 4040-4048.].

FIG. 5 is a schematic diagram illustrating an example of feature point errors in an acquired image. The figure includes images from the i-th frame to the N-th frame. Focusing on the k-th frame image (k=i, i+1, . . . , N), the following is true. The image of the k-th frame is an image obtained assuming the position and orientation of the moving object at the time of the frame. Here, the position x _jk (hat) corresponding to the j-th feature point on the k-th frame image is a position obtained by projecting the three-dimensional position of the j-th feature point onto the image. Note that the three-dimensional position of the j-th feature point is obtained by the principle of triangulation. On the other hand, the position x _jk on the k-th frame image is the position of the j-th feature point extracted on the image. The difference between these positions x _jk (hat) and position x _jk is the feature point error. In this figure, the _{arcuate lines} connecting the positions x _ji , x _ji+1 , . . . , the positions _{x jk} , .

A method of calculating the feature point error is as follows. In the _i -th frame ( _i =1, 2, . Let the position of the feature point be x _ji ( _j =1, 2, . That is, the feature point error) E is represented by the following equation (3). Note that the internal parameters of the camera are specifically a matrix composed of focal length and optical center (principal point) values.

This equation (3) is expressed by the position x _ji of the feature point on the image, the posture of the moving body R _i , the position t _i of the moving body, the three-dimensional position X _j of the feature point, and the camera internal parameter K is input and the feature point error E is output.

Here, the first frame (i=1) is not the first frame obtained after the camera is activated, but the most recently input frame among frames in which feature points can be associated with each other. point to the frame.

Next, the motion model will be explained. A motion model is, for example, a constraint condition when a moving object moves. For example, if the mobile object is a four-wheeled vehicle, the constraint conditions are as follows. That is, the four wheels are in contact with the ground and cannot be above or below the ground. In addition, the mobile body can basically only move in the direction of the front wheels. Also, the steering angle is limited (that is, there is a lower limit to the turning radius of the moving body). A moving body that is constrained by these constraint conditions cannot move horizontally or vertically (in a direction that lifts it off the ground).

A method for estimating the position and orientation of a moving object based on a motion model will be described. A case where the moving body is a vehicle will be described.

FIG. 6 is a schematic diagram showing an overview of movement of a vehicle (moving body) that is in motion constrained by a constraint condition. The vehicle position shown in the figure is calculated based on the dynamics of the vehicle. In the figure, the vehicle runs on the ground, which is the xy plane, in the xyz three-dimensional space. In other words, the bottom surfaces of the wheels of the vehicle are constrained on the xy plane. In addition, in the figure, the positive region side is viewed from the negative region side of the z-axis. As shown in the figure, as a motion model of the vehicle, a model of turning motion with reference to the center positions of the two rear wheels (referred to as the rear wheel center) may be defined. In this case, in the _i -th frame ( _{i =} 1, 2, . , the speed is s _i , and the turning radius (rotation radius) of the center of the rear wheel is ρ, the following equation (4) holds.

ρ shown in FIG. 6 is the radius of rotation of the turning motion about the rear wheels of the vehicle. θ is the angle at which the vehicle turns from the |F|-1th frame to the |F|th frame in the turning motion. Also, the moving distance of the vehicle from the |F|-1th frame to the |F|th frame calculated from the vehicle speed and the time is |t _|F| -t _|F|-1 |.

(|F|-1)th frame estimated position t _|F|-1 , (|F|-1)th frame orientation R _{1 |F|-1} , and |F|th frame Attitude R _|F| , l, (|F|-1)-th frame time T _|F|-1 , |F|-th time T _|F| , and |F|-th speed s _|F| is used as an input, and an estimated position t _{1 |F|} of the |F|-th frame is output.

In equation (4), l is the center position of the two front wheels (referred to as the front wheel center) when the origin is the center of the rear wheels. y(R) is the yaw angle calculated from the rotation matrix R; The rotation matrix R of the motion model may be obtained from the steering angle, estimated from the posture and speed of the past frame assuming constant velocity motion, or estimated from the initial position and posture of the moving object. may be obtained using the method described in the example.

For example, if the steering angle in the _i -th frame ( _i =1, 2, . , can also be expressed as in the following equation (5). This equation (5) represents a calculation process for outputting a rotation matrix R _i with input of the steering angle δ _i , the distance between the center of gravity of the vehicle body and the center of the front wheels, and the distance between the center of gravity of the vehicle body and the center of the rear wheels. .

However, y ⁻¹ (θ) is a rotation matrix calculated from the yaw angle θ. The distance between the center of gravity of the vehicle body and the center of the front and rear wheels is as follows.

FIG. 7 is a schematic diagram showing an overview of movement of a vehicle (moving body) that moves based on a definition of a motion model different from that of FIG. In the figure, the vehicle runs on the ground, which is the xy plane, in the xyz three-dimensional space. In other words, the bottom surfaces of the wheels of the vehicle are constrained on the xy plane. In addition, in the figure, the positive region side is viewed from the negative region side of the z-axis. In the motion model shown in FIG. 7, the rear wheels draw a straight trajectory even when the vehicle is rotating. θ is the angle formed by the vehicle attitude R 1 _| F|-1 in the |F|th frame and the vehicle attitude R _{1 |F|-1} in the |F|-1th frame. When using this motion model, the following equation (6) holds.

|t _|F| −t _|F|-1 | is the moving distance of the vehicle from the |F|-1th frame to the |F|th frame, which is calculated from the vehicle speed and the time.

(|F|-1)th frame estimated position t _|F|-1 , (|F|-1)th frame orientation R _{1 |F|-1} , and |F|th frame Attitude R _|F| , l, (|F|-1)-th frame time T _|F|-1 , |F|-th time T _|F| , and |F|-th speed s _|F| is used as an input, and the calculation process for outputting the estimated position t _{1 |F|} of the |F|-th frame is shown.

FIG. 8 is a schematic diagram showing an example of motion of a moving body for explaining motion model errors. The motion model error will be described with reference to this figure. The motion model error is the difference between the position and orientation of the moving object estimated by the estimation unit 101 and the position and orientation of the moving object obtained based on the motion model. For example, t _i is the position of the moving body obtained by estimation in the i-th frame (i=1, 2, . . . , |F|, |F| is the latest frame), Let t _i (hat) be the position of . Then, the motion model error e _ri is expressed by the following equation (7).

The above equation (7) is a calculation process for outputting the motion model error e _ri with the estimated moving object t _i and the position t _i (hat) of the moving object obtained by the motion model as inputs. represents

When the moving body is a vehicle, the method of calculating the motion model error based on the center of the rear wheels is as follows. That is, the motion model error e _ri is expressed by the following equation (8), where R _i is the posture of the moving body obtained by estimation, and R _i (hat) is the position of the moving body obtained by the motion model.

Equation (8) is an estimated position t _i of the i-th frame, a posture R _i of the i-th frame, a position t _i (hat) by the motion model of the i-th frame, and a posture R _i of the motion model of the i-th frame. (hat) and 1 are input, and the calculation process for outputting the motion model error e _ri is represented. However, l is the position of the center of the front wheel when the center of the rear wheel is taken as the origin.

Next, the correction amount obtained by the correction unit 107 will be described. A correction unit 107 calculates a correction amount based on the feature point error and the motion model error. An error E, which is a combination of the feature point error and the motion model error, is expressed by Equation (9) below. Note that the feature point error e _ji included in Equation (9) is as described in Equation (3). Also, the motion model error e _ri is as described in equation (7) or (8).

Equation (9) represents a calculation process for outputting total E with feature point errors e _ji and motion model errors e _ri as inputs. Note that λ in Equation (9) is a hyperparameter for adjusting the balance between the feature point error and the motion model error.

When the correction amount obtained by the correction unit 107 is the three-dimensional position of the feature point, the position of the moving body, and the attitude of the moving body, the correction amount is an amount for reducing the value of E described above. For example, the Gauss-Newton method, the Levenberg-Marquardt method, the conjugate gradient method, the genetic algorithm (GA), or the like is used by the correction unit 107 to obtain a correction amount value for reducing E. That is, by minimizing E, positions t _i and orientations R _i with small feature point errors and motion model errors can be obtained for all frames from the first frame to the |F|th frame. However, i=1, 2, . . . |F|, and the |F|th frame is the latest frame.

When using Nister's 5-point algorithm, etc., the position and orientation are estimated using the matching result (tracking result) between the k-th frame feature point and the |F|-th frame feature point, but depending on the value of k, Error E changes. The correction unit 107 can obtain a correction amount that reduces the feature point error and the motion model error by exhaustively searching for the correction amount. Then, it is possible to correct the position and posture of the moving body by using this correction amount.

As described above, according to the present embodiment, the motion model calculator 105 obtains the position of the moving body based on the motion conditions. Also, the motion model error calculation unit 106 calculates a motion model error based on the position of the mobile object estimated by the estimation unit 101 and the position of the mobile object obtained by the motion model calculation unit 105 . Therefore, the correction unit 107 can obtain the correction amount based not only on the feature point error, but also on the motion model error. This improves the accuracy of estimation of the position of the mobile object.

(Second embodiment)
Next, a second embodiment will be described. In addition, description may be abbreviate|omitted below about the matter already demonstrated in the previous embodiment. Here, the description will focus on matters specific to this embodiment.

FIG. 9 is a block diagram showing a schematic functional configuration of the information processing device 2 according to this embodiment. As illustrated, the information processing apparatus 2 includes an estimation unit 201, an image acquisition unit 202, a feature point tracking unit 203, a feature point error calculation unit 204, a motion model calculation unit 205, and a motion model error calculation unit 206. , a correction unit 207 , a sensor information acquisition unit 211 , a sensor model calculation unit 212 , and a sensor model error calculation unit 213 .

A feature of the present embodiment is that the information processing device 2 has a sensor information acquisition unit 211 and calculates a correction amount based on the acquired sensor information as well.

The estimation unit 201, the image acquisition unit 202, the feature point tracking unit 203, the feature point error calculation unit 204, the motion model calculation unit 205, the motion model error calculation unit 206, and the correction unit 207 are respectively The estimation unit 101, the image acquisition unit 102, the feature point tracking unit 103, the feature point error calculation unit 104, the motion model calculation unit 105, the motion model error calculation unit 106, and the correction unit 107 in Embodiment 1 , are similar or have similar functions. A configuration unique to this embodiment is that the information processing device 2 includes a sensor information acquisition unit 211 , a sensor model calculation unit 212 , and a sensor model error calculation unit 213 . These sensor information acquisition unit 211, sensor model calculation unit 212, and sensor model error calculation unit 213 can also be realized as dedicated functions using electronic circuits, or can be realized using general-purpose computers and programs. can also be implemented. The function of each part constituting the information processing device 2 is as follows.

The estimation unit 201 estimates the position and orientation of the mobile object based on the correction amount calculated by the correction unit 207 . That is, the estimating unit 201 has the same function as the estimating unit 101 of the first embodiment.

The image acquisition unit 202 acquires an image. That is, the image acquisition unit 202 has the same functions as the image acquisition unit 102 of the first embodiment.

The feature point tracking unit 203 extracts predetermined feature points from the image acquired by the image acquisition unit 202 . Also, the feature point tracking unit 203 tracks the same feature point in a plurality of time-series images. That is, the feature point tracking unit 203 has the same function as the feature point tracking unit 103 of the first embodiment.

The feature point error calculation unit 204 calculates the feature point error from the tracking result of the feature points by the feature point tracking unit 203 and the position and orientation of the moving object estimated by the estimation unit 201 . That is, the feature point error calculator 204 has the same function as the feature point error calculator 104 of the first embodiment.

The motion model calculation unit 205 calculates a motion model based on the position and orientation of the moving object estimated by the estimation unit 201 . In other words, the exercise model calculator 205 has the same function as the exercise model calculator 105 of the first embodiment.

The motion model error calculation unit 206 calculates a motion model error from the motion model calculated by the motion model calculation unit 205 and the position and orientation of the moving object obtained by the estimation unit 201 . In other words, the motion model error calculator 206 has the same function as the motion model error calculator 106 of the first embodiment.

Based on the feature point error calculated by the feature point error calculation unit 204, the motion model error calculated by the motion model error calculation unit 206, and the sensor model error calculated by the sensor model error calculation unit 213, the correction unit 207 A correction amount for the position and orientation of the moving body is calculated. A difference from the correcting unit 107 in the first embodiment is that the correcting unit 207 calculates the correction amount also based on the sensor model error. The correction unit 207 feeds back the calculated correction amount information to the estimation unit 201 . A method of obtaining the correction amount for reducing the error will be described later.

The sensor information acquisition unit 211 acquires sensor information from a sensor provided on a moving body or the like.

Here, the sensor specifically means a speedometer, an acceleration sensor, a gyro sensor, a steering angle sensor, a GPS terminal device, or the like. A speedometer acquires speed information or velocity information of a moving body. If it is an acceleration sensor, it acquires the acceleration information of the moving object. A gyro sensor acquires the orientation or posture of a moving body and their time differential values (angle information and angular velocity information). A GPS terminal device acquires location information of a mobile object. Also, a video camera, a network camera, a distance sensor, or the like may be used as the sensor. Even if the same devices as the video camera, network camera, distance sensor, etc. used in the image acquisition unit 202 are used, the image acquisition unit 202 can be controlled by attaching these devices to positions different from the sensor of the image acquisition unit 202. different information can be obtained. Also, by changing the frame rate (the number of frames per unit time) or the angle of view of the camera or distance sensor, the sensor information acquisition unit 211 can acquire information that cannot be obtained by the image acquisition unit 202. . Furthermore, as the sensor, a sensor combining a plurality of sensors such as an inertial measurement unit (IMU) or a yaw rate G sensor may be used.

The sensor model calculator 212 calculates a sensor model from the sensor information acquired by the sensor information acquirer 211 . In other words, the sensor model calculation unit 212 calculates the position and orientation of the moving object based on the acquired sensor information, using a calculation procedure specific to the sensor type.

The sensor model error calculation unit 213 calculates a sensor model error from the sensor model calculated by the sensor model calculation unit 212 and the position and orientation of the mobile object estimated by the estimation unit 201 . In other words, the sensor model error calculation unit 213 calculates the sensor model error, which is the difference between the position of the mobile object estimated by the estimation unit 201 and the position of the mobile object obtained by the sensor model calculation unit 212 . Details of how to calculate the sensor model error will be described later.

FIG. 10 is a flowchart showing the procedure of processing by the information processing device 2. As shown in FIG. Here, the procedure of processing by the information processing device 2 will be described along this flowchart.

In step S201, the image acquisition unit 202 acquires an image.

In step S202, the feature point tracking unit 203 extracts feature points in the image based on the image acquired in step S201. Then, the feature point tracking unit 203 performs matching between the extracted feature points and feature points extracted from other images acquired at different times. That is, the feature point tracking unit 203 tracks feature points in time series.

In step S203, the sensor information acquisition unit 211 acquires sensor information.

In step S204, the sensor model calculation unit 212 calculates a sensor model based on the sensor information acquired by the sensor information acquisition unit 211 in step S203.

In step S205, the estimation unit 201 estimates the position and orientation of the moving object based on the positions of the feature points in the image. The position and orientation estimated in this step are the initial values of the position and orientation, respectively.

In step S206, the feature point error calculator 204 calculates feature point errors. This feature point error is calculated from the feature point tracking result obtained in step S202 and the position and orientation estimated by the estimation unit 201 in either step S205 or step S212 (most recently estimated position and orientation). be done.

In step S207, the sensor model error calculator 213 calculates a sensor model error. This sensor model error is the sensor model calculated by the sensor model calculation unit 212 in step S204, and the position and orientation estimated by the estimation unit 201 in either step S205 or step S212 (most recently estimated position and orientation). calculated from

In step S208, the exercise model calculator 205 calculates an exercise model. This motion model is calculated based on the position and orientation (most recently estimated position and orientation) estimated by the estimation unit 201 in either step S205 or step S212.

In step S209, the motion model error calculator 206 calculates a motion model error. This motion model error is obtained from the position and orientation (most recently estimated position and orientation) estimated by the estimation unit 201 in either step S205 or step S212 and the motion model calculated by the motion model calculation unit 205 in S208. , is calculated.

In step S210, the information processing apparatus 2 determines that the feature point error calculated in step S206, the sensor model error calculated in step S207, and the motion model error calculated in step S209 are sufficiently small values. determine whether Specifically, the information processing device 2 uses evaluation function values for comprehensively evaluating feature point errors, sensor model errors, and motion model errors. An example of this evaluation function is a function that simply adds the absolute value of the feature point error, the absolute value of the sensor model error, and the absolute value of the motion model error. Note that other evaluation functions may be used. The information processing device 2 makes the determination in this step based on the predetermined threshold value of the evaluation function. As a result of the determination, if the error is sufficiently small (step S210: YES), it is considered that the position and orientation are estimated within the desired error range, so the entire processing of this flowchart ends. If the error is not sufficiently small (step S210: NO), the process proceeds to step S211.

In step S211, the correction unit 207 calculates correction amounts so as to reduce the feature point error, the sensor model error, and the motion model error. The correction unit 207 passes the calculated correction amount to the estimation unit 201 .

In step S212, the estimation unit 201 estimates the corrected position and orientation of the moving object by applying the correction amount calculated by the correction unit 207 in step S211. Estimating section 201 uses the corrected position and orientation as a new estimation result.

Through the above series of processing procedures including repetition, it is possible to adjust the estimated position and orientation so as to reduce the error.

It should be noted that the processing order of each step shown in FIG. may be executed in parallel with each other. As an example, the motion model error may be calculated before finishing calculating the feature point error and the sensor model error.

Next, the sensor model and sensor model error will be described. A sensor model is a value that can be directly or indirectly compared with the estimation result of the position/orientation of a moving object. The sensor model error is the difference between the value of the sensor model and the value obtained using the estimation results of the position and orientation of the moving object.

For example, when a speedometer is used as a sensor, the sensor model calculation unit 212 can calculate the moving distance of the moving body by integrating the speed acquired by the speedometer over time. The sensor model error calculation unit 213 can calculate the sensor model error as the difference between the movement distance obtained by this sensor model and the movement distance calculated from the position and orientation estimated by the estimation unit 201 . In the i-th frame (i=1, 2, . . . , |F|, |F|-th is the latest frame), T _i is the time, s _i is the speed obtained from the speedometer, and t is the estimated position of the moving object. _{Let i} . Then, the sensor model and the sensor model error are calculated by the following equation (10).

Equation (10) is a sensor model with input of _i -th frame time T _i , (i−1)th frame time T _i−1 , and i-th frame speed si,

represents the computational process that outputs
Further, Equation (10) is the estimated position t i of the i-th frame, the estimated position t _{i -1} of the (i-1)th frame, and the sensor model,

is the input and the calculation process outputs the sensor model error e _si .

For example, if an IMU is used as a sensor, the position and orientation of the moving object can be calculated from the acceleration and angular velocity obtained from the IMU. The sensor model error can be calculated by comparing this position and orientation with the estimation result of the position and orientation estimation unit 201 of the moving body. Let T _i be the time in the _ith frame ( _i =1, 2, . Then, the position t _i (hat) and orientation R _i (hat) of the sensor model and the sensor model error e _si are obtained by the following equation (11).

This equation (11) is expressed by (i-1)th estimated position t _i-1 , (i-1)th frame velocity v _i-1 , (i-1)th frame posture R _{i- 1} , the acceleration a _i of the i-th frame, the time T _i of the i-th frame, and the time T _i-1 of the (i−1)th frame, the position t _i ( Hat) is the output. Further, this equation (11) is expressed by the (i-1)-th frame attitude R _i-1 , the i-th frame angular velocity ω _i , the i-th frame time T _i , and the (i-1)-th frame It represents a calculation process in which the time T _i−1 is input and the orientation R _i (hat) of the i-th frame sensor model is output. Also, this equation (11) represents a calculation process in which the position t _i (hat) and the orientation R _i (hat) of the sensor model are input and the sensor model error e _si is output.

However, in Equation (11), Ω _i represents ω _i with a rotation matrix.

Next, the correction amount when the sensor model is introduced will be described. The correction unit 207 adds the sensor model error _esi to the error obtained in the first embodiment when calculating the correction amount in the present embodiment. In this embodiment, the total error considering the sensor model error _esi is calculated by the following equation (12).

Equation (12) represents a calculation process for outputting the total error E with the feature point error e _ji , the motion model error e _ri , and the sensor model error e _si as inputs. In Equation (12), λ _r and λ _s are hyperparameters for adjusting the balance among feature point error, motion model error, and sensor model error.

As described above, according to the present embodiment, the sensor information acquisition unit 211 acquires sensor information. The sensor model calculation unit 212 calculates the position and orientation of the moving body by performing calculation using a predetermined model based on the above sensor information. The sensor model error calculation unit 213 calculates a sensor model error as a difference between the position and orientation of the mobile object estimated by the estimation unit 201 and the position and orientation of the mobile object calculated by the sensor model calculation unit 212 . Accordingly, the correction unit 207 can obtain the correction amount based not only on the feature point error and the motion model error, but also on the sensor model error. This further improves the accuracy of estimating the position and the like estimated by the estimating unit 201 .
In addition, it does not have the feature point error calculation unit 104 or the motion model error calculation unit 206, that is, does not calculate the feature point error or the soil movement model error, and based only on the sensor model error calculated by the sensor model error calculation unit 213 , the correction unit 207 may obtain the correction amount.

(Third embodiment)
Next, a third embodiment will be described. In addition, description may be abbreviate|omitted below about the matter already demonstrated by the previous embodiment. Here, the description will focus on matters specific to this embodiment.

FIG. 11 is a block diagram showing a schematic functional configuration of the information processing device 3 according to this embodiment. As illustrated, the information processing device 3 includes an estimation unit 301, an image acquisition unit 302, a feature point tracking unit 303, a feature point error calculation unit 304, a motion model calculation unit 305, and a motion model error calculation unit 306. , a correction unit 307 , and a speed information acquisition unit 321 .

A feature of this embodiment is that the information processing device 3 has a speed information acquisition unit 321, and the motion model calculation unit 305 calculates a motion model based on the acquired speed information as well.

The estimation unit 301, the image acquisition unit 302, the feature point tracking unit 303, the feature point error calculation unit 304, the motion model calculation unit 305, the motion model error calculation unit 306, and the correction unit 307 each The estimation unit 101, the image acquisition unit 102, the feature point tracking unit 103, the feature point error calculation unit 104, the motion model calculation unit 105, the motion model error calculation unit 106, and the correction unit 107 in Embodiment 1 , are similar or have similar functions. The feature of this embodiment resides in the speed information acquisition unit 321 and the motion model calculation unit 305 . The function of each part of the information processing apparatus 3 can be realized as a dedicated function using an electronic circuit, or can be realized using a general-purpose computer and program.

The speed information acquisition unit 321 acquires speed information of the moving object from a so-called speedometer. The velocity information may be a vector or a scalar (absolute value of velocity represented by a vector). If the moving object is an automobile or the like, the speed information can also be obtained from the speedometer originally provided in the automobile. The speed information acquisition unit 321 passes the acquired speed information to the exercise model calculation unit 305 .

The motion model calculation unit 305 receives speed information from the speed information acquisition unit 321 . The motion model calculation unit 305 performs calculation based on the speed information received from the speed information acquisition unit 321 when calculating the position and orientation of the moving object. For example, the motion model calculation unit 305 can calculate the amount of movement of the moving body at that time based on the speed and the elapsed time. In this case, the motion model calculation unit 305 can obtain the position of the moving object and the like so as to match the calculated movement amount. In other words, the motion model calculation unit 305 calculates the position of the mobile object based on the acquired velocity as well. In other words, the motion model error calculation unit 106 acquires the position and orientation of the mobile body according to the motion model, which is also based on the speed acquired by the speed information acquisition unit 321, and calculates the error of the motion model.

In addition, you may implement combining 2nd Embodiment and 3rd Embodiment. In that case, the information processing device acquires sensor information (information acquired by a general sensor) through the sensor information acquisition unit. Then, the sensor model calculation unit obtains the position and orientation based on the sensor information. In the information processing device, the motion model calculation unit calculates the position and orientation of the moving object based on the speed information acquired by the speed information acquisition unit.

(First modification)
Next, a modified example will be described. The first modification is applicable to each of the above embodiments. In this modified example, the information processing device (1, 2, 3) may interrupt part of the process in the middle of the process when the motion model error is greater than a predetermined value. In other words, execution of some processes may be skipped. As a result of calculating the motion model error by the motion model error calculation unit (106, 206, 306), the motion model error is larger than other possible errors (feature point error or sensor model error depending on the embodiment). If extremely large, other errors are likely to be small enough to be ignored. In such a case, the correction unit (107, 207, 307) may calculate the correction amount based only on the motion model error, assuming that the other errors are negligible. Specifically, in this modification, when the value (absolute value) of the motion model error is equal to or greater than a predetermined threshold value, the feature point error and the sensor model error are not calculated, or the feature point error and the sensor model error are calculated. The process is interrupted on the way and the process proceeds to the process of obtaining the correction amount. At this time, the correction unit (107, 207, 307) obtains the correction amount by setting these errors to 0 in order to consider the errors that have not been calculated to be negligible.

In this modification, for example, the motion model error calculator (106, 206, 306) compares the calculated absolute value of the motion model error with the above threshold. When the absolute value of the motion model error is equal to or greater than the threshold, the motion model error calculation unit (106, 206, 306) sends a notification to the other units to notify the calculation of the feature point error and the sensor model error. Suppress the processing for Also, in that case, the motion model error calculation unit (106, 206, 306) sends a notification to the correction unit (107, 207, 307), and the correction unit (107, 207, 307) detects the motion model error Control is performed so that errors other than 0 are treated as 0.

When implementing this modified example, it is possible to reduce the amount of calculation for calculating feature point errors and sensor model errors. As a result, it is possible to effectively utilize computational resources and shorten the processing time of the entire information processing apparatus (1, 2, 3).

In other words, the motion model error calculation unit (106, 206, 306) in this modified example, when the absolute value of the obtained motion model error is equal to or greater than a predetermined threshold value, the feature point error calculation unit (104, 204, 304) may be controlled to suppress the process of obtaining the feature point error, and the correction unit (107, 207, 307) may be controlled to obtain the correction amount assuming the feature point error to be zero.

Further, in this modified example, the motion model error calculation unit (206) of the second embodiment is characterized by the feature point error calculation unit (204) when the absolute value of the obtained motion model error is equal to or greater than a predetermined threshold value. The sensor model error calculation unit (213) is controlled to suppress the process of obtaining the point error, and the correction unit (207) controls the feature point error and the sensor model error to suppress the process of obtaining the sensor model error. may be regarded as zero and the correction amount may be obtained.

(Second modification)
The second modification is applicable to each of the above embodiments. Moreover, you may implement combining a 2nd modification with a 1st modification. In this modification, the information processing devices (1, 2, 3) dynamically change the hyperparameters according to the situation. Here, the hyperparameters to be changed are λ in equation (7) of the first embodiment and λ _r and λ _s in equation (10) of the second embodiment. That is, in the present modification, the information processing device (1, 2, 3) calculates the feature point error, the motion model error, and the sensor model (in the case of the second embodiment) when calculating the overall error. The balance with the error can be changed dynamically.

For example, if the moving object is a vehicle, the motion of the vehicle is constrained by the constraint conditions defined by the motion model. However, in reality, a situation may arise in which the tires of the vehicle are more likely to slip on the road surface depending on the speed, weather, etc., or the air resistance to which the vehicle body receives changes due to strong winds. If the motion model does not assume these disturbance factors, the position and orientation calculated from the motion model may contain large errors depending on the degree of disturbance.

Further, for example, even when performing calculations based on the sensor model in the second embodiment, the position and orientation calculated by the sensor model may include large errors due to the influence of disturbances. For example, when an IMU is used as a sensor, errors in acquired sensor information may increase due to the effects of heat. Therefore, the position and orientation calculated based on the IMU may contain large errors.

In this modification, the information processing device (1, 2, 3) changes the weight of the motion model error and the sensor model error according to the degree of influence of the disturbance in consideration of the above circumstances. Specifically, the value of λ in equation (7) and the values of λ _r and λ _s in equation (10) are dynamically changed according to the degree of influence of the disturbance. As a result, the influence of the disturbance on the calculation of the total correction amount E can be reduced.

As an example, when the information processing device (1, 2, 3) is equipped with a speedometer, let λ=λ ₀ ·exp(−b·s _i ) in equation (7). where _si is the speed of the moving object obtained from the speedometer. Also, b is a predetermined positive constant. Also, λ ₀ is the value of the hyperparameter when the velocity is zero. By making λ variable in this way, the weight of the motion model error becomes relatively large when the speed of the moving object is slow, and the weight of the feature point error becomes relatively large when the speed of the moving object is fast. growing. As a result, it is possible to reduce the influence of tire slip, which is relatively likely to occur when the moving body moves at high speed, on the calculation of the total correction amount.

As another example, when the information processing apparatus 2 of the second embodiment uses an IMU as a sensor, the following can be done. That is, in equation (10), for example, λ _r =λ _r0 ·exp(-b ₁ ·s _i ) and λ _s =λ _s0 ·exp(-b ₂ ·c _i ) may be set. Here, s _i is the velocity of the moving object as described above. Also, _ci is the temperature (absolute temperature or Celsius temperature) obtained from a temperature sensor or the like. Also, b ₁ and b ₂ are predetermined positive constants. Also, λ _r0 is the reference value of the hyperparameter λ _r when the velocity is zero. Also, λ _s0 is the reference value of the hyperparameter λ _s when the temperature is the reference value. In this way, by dynamically varying λ _r and λ _s , errors in the motion model due to slippage during high-speed movement of the moving body, errors in the sensor model due to high temperature, etc., are reduced to total errors. It is possible to appropriately adjust the influence on E.

That is, in this modification, the correction unit (107, 207, 307) weights the feature point error and the motion model error when obtaining the correction amount based on the feature point error and the motion model error. The correction amount may be obtained while being dynamically variable.

In addition, in this modified example, the correction unit (107, 207, 307) is based on the acquired absolute value of the speed of the moving body so that the greater the absolute value of the speed, the smaller the weight of the motion model error. Alternatively, the correction amount may be obtained while dynamically varying the weighting between the feature point error and the motion model error.

Further, in this modified example, the correction unit (207) determines the amount of correction based on the feature point error, the motion model error, and the sensor model error. The correction amount may be obtained while dynamically varying the weighting between the users.

In addition, in the present modification, the correction unit (207) is based on the acquired absolute value of the velocity of the moving object, and the weight of the motion model error is relatively decreased as the absolute value of the velocity increases. Based on the obtained temperature information, the correction amount may be obtained while dynamically varying the weighting among the three so that the weighting of the sensor model error is relatively variable according to the temperature.

(Third modification)
A third modification is applicable to each of the above embodiments. Also, the third modification may be combined with the first modification and the second modification. In this modification, the information processing devices (1, 2, 3) may be distributed and implemented in a plurality of different locations. At this time, a camera, a distance sensor, and the like, which are means for acquiring an image, are fixed to the moving body. Further, when a sensor is used, among various sensors, if it is necessary to operate the sensor while being fixed to the moving body, the sensor is fixed to the moving body. Other functions possessed by the information processing devices (1, 2, 3) may be provided at a location separate from the mobile object. For example, arithmetic functions for calculating various positions and calculating errors may be provided in a remote external device (eg, computer server, etc.). This remote external device may also be implemented as a so-called cloud server. At this time, data can be exchanged appropriately by communication via a communication line between the function fixedly provided on the mobile body and the function provided on the remote external device.

According to at least one embodiment described above, the motion model calculator calculates the position of the moving body. Therefore, it is possible to obtain the error between the position estimated by the estimation unit and the position calculated by the motion model calculation unit. This error is called a motion model error. Therefore, the correction unit 107 can obtain the correction amount based not only on the feature point error, but also on the motion model error. This improves the accuracy of estimation of the position of the mobile object.

At least part of the functions of the information processing apparatus in the above-described embodiments (including modifications) may be realized by a computer. In that case, a program for realizing this function may be recorded in a computer-readable recording medium, and the program recorded in this recording medium may be read into a computer system and executed. It should be noted that the "computer system" referred to here includes hardware such as an OS and peripheral devices. In addition, “computer-readable recording media” refers to portable media such as flexible discs, magneto-optical discs, ROMs, CD-ROMs, DVD-ROMs, USB memories, and storage devices such as hard disks built into computer systems. Say things. Furthermore, "computer-readable recording medium" refers to a program that dynamically retains programs for a short period of time, like a communication line when transmitting a program via a network such as the Internet or a communication line such as a telephone line. It may also include things that hold programs for a certain period of time, such as volatile memories inside computer systems that are servers and clients in that case. Further, the program may be for realizing part of the functions described above, or may be a program capable of realizing the functions described above in combination with a program already recorded in the computer system.

While several embodiments of the invention have been described, these embodiments have been presented by way of example and are not intended to limit the scope of the invention. These embodiments can be implemented in various other forms, and various omissions, replacements, and modifications can be made without departing from the scope of the invention. These embodiments and their modifications are included in the scope and spirit of the invention, as well as the scope of the invention described in the claims and equivalents thereof.

DESCRIPTION OF SYMBOLS 1, 2... Information processing apparatus 101... Estimation part 102... Image acquisition part 103... Feature point tracking part 104... Feature point error calculation part 105... Motion model calculation part 106... Motion model error calculation part 107 Correction unit 201 Estimation unit 202 Image acquisition unit 203 Feature point tracking unit 204 Feature point error calculation unit 205 Motion model calculation unit 206 Motion model error calculation unit 207 Correction unit 211... Sensor information acquisition unit 212... Sensor model calculation unit 213... Sensor model error calculation unit 301... Estimation unit 302... Image acquisition unit 303... Feature point tracking unit 304... Feature point error calculation unit 305... Motion model calculation unit 306 Motion model error calculation unit 307 Correction unit 321 Speed information acquisition unit

Claims (12)

an estimating unit that estimates the position and orientation of a moving object;
an image acquisition unit provided in the moving body for acquiring an image;
A correction amount for correcting the position and orientation of the moving object estimated by the estimating unit is obtained based on the acquired feature points of the object in the image and a motion model that is a constraint condition of the moving object. a correction unit;
with
the moving body is a vehicle,
The constraint condition of the moving object is to constrain the motion of the vehicle by the condition that the bottom surface of the wheel of the vehicle is in contact with a predetermined surface, and the vehicle constrained by the constraint condition moves laterally or upwards. it is impossible to
The estimating unit corrects the estimated position and orientation of the moving body based on the correction amount obtained by the correcting unit to obtain a newly estimated position.
Information processing equipment. a feature point error calculation unit that calculates a feature point error, which is an error between the position of the feature point of the object in the acquired image and the three-dimensional position of the feature point of the object;
Movement for obtaining a motion model error that is a difference between the position and orientation of the moving body calculated based on a motion model that is a constraint condition of the moving body and the position and orientation of the moving body that is estimated by the estimating unit. a model error calculator;
further comprising
the correction unit obtains the correction amount based on the feature point error and the motion model error;
The information processing device according to claim 1 . a sensor information acquisition unit that acquires sensor information from the sensor;
a sensor model calculation unit that calculates the position of the moving object based on the sensor information;
a sensor model error calculation unit that calculates a sensor model error that is a difference between the position of the mobile object estimated by the estimation unit and the position of the mobile object obtained by the sensor model calculation unit;
further comprising
The correction unit obtains the correction amount further based on the sensor model error,
The information processing apparatus according to claim 2. a speed information acquisition unit that acquires the speed of the moving object;
further comprising
The motion model error calculation unit calculates the position and orientation of the moving body calculated based on the motion model, also based on the acquired velocity.
The information processing apparatus according to claim 2 or 3. When the absolute value of the calculated motion model error is equal to or greater than a predetermined threshold, the motion model error calculation unit controls the feature point error calculation unit to suppress the process of obtaining the feature point error, and controlling the correction unit to determine the correction amount by regarding the feature point error as zero;
The information processing apparatus according to any one of claims 2 to 4. The motion model error calculation unit controls the feature point error calculation unit to suppress the process of obtaining the feature point error when the absolute value of the calculated motion model error is equal to or greater than a predetermined threshold; The sensor model error calculation unit controls to suppress processing for obtaining the sensor model error, and the correction unit determines the correction amount by regarding both the feature point error and the sensor model error as zero. do,
The information processing apparatus according to claim 3. When the correction amount is obtained based on the feature point error and the motion model error, the correction unit dynamically changes the weighting between the feature point error and the motion model error to perform the correction. ask for quantity
The information processing apparatus according to any one of claims 2 to 6. When the correction amount is calculated based on the feature point error and the motion model error, the correction unit determines, based on the acquired absolute value of the speed of the moving object, that the larger the absolute value of the speed, the greater the motion. obtaining the correction amount while dynamically varying the weighting between the feature point error and the motion model error so that the weight of the model error is relatively small;
The information processing apparatus according to claim 4. The correction unit, when obtaining the correction amount based on the feature point error, the motion model error, and the sensor model error, calculates the error between the feature point error, the motion model error and the sensor model error. Obtaining the correction amount while dynamically varying the weighting of
The information processing apparatus according to claim 3. an estimating unit that estimates the position and orientation of a moving object;
an image acquisition unit provided in the moving body for acquiring an image;
A correction amount for correcting the position and orientation of the moving object estimated by the estimating unit is obtained based on the acquired feature points of the object in the image and a motion model that is a constraint condition of the moving object. a correction unit;
a feature point error calculation unit that calculates a feature point error, which is an error between the position of the feature point of the object in the acquired image and the three-dimensional position of the feature point of the object;
Movement for obtaining a motion model error that is a difference between the position and orientation of the moving body calculated based on a motion model that is a constraint condition of the moving body and the position and orientation of the moving body that the estimation unit estimates. a model error calculator;
a sensor information acquisition unit that acquires sensor information from the sensor;
a sensor model calculation unit that calculates the position of the moving object based on the sensor information;
a sensor model error calculation unit that calculates a sensor model error that is a difference between the position of the mobile object estimated by the estimation unit and the position of the mobile object obtained by the sensor model calculation unit;
a speed information acquisition unit that acquires the speed of the moving object;
with
the moving body is a vehicle,
the constraint condition of the moving object is to constrain the movement of the vehicle by a condition that the bottom surface of the wheel of the vehicle contacts a predetermined surface;
The estimating unit corrects the estimated position and orientation of the moving object based on the correction amount obtained by the correcting unit to obtain a newly estimated position;
the correction unit obtains the correction amount based on the feature point error and the motion model error;
The correction unit obtains the correction amount further based on the sensor model error,
The motion model error calculation unit calculates the position and orientation of the moving object calculated based on the motion model, also based on the acquired velocity,
When the correction amount is obtained based on the feature point error, the motion model error, and the sensor model error, the correction unit determines that the absolute value of the speed is based on the obtained absolute value of the speed of the moving object. The feature points are such that the weight of the motion model error becomes relatively smaller as the feature point increases, and the weight of the sensor model error becomes relatively variable according to the temperature based on the acquired temperature information. determining the correction amount while dynamically varying the weighting among the error, the motion model error, and the sensor model error;
Information processing equipment. an estimation process for estimating the position and orientation of a moving object;
an image acquisition process provided in the moving body for acquiring an image;
A correction amount for correcting the position and orientation of the moving object estimated in the estimation process is obtained based on the acquired feature points of the object in the image and the motion model that is the constraint condition of the moving object. a correction process;
including
the moving body is a vehicle,
The constraint condition of the moving object is to constrain the motion of the vehicle by the condition that the bottom surface of the wheel of the vehicle is in contact with a predetermined surface, and the vehicle constrained by the constraint condition moves laterally or upwards. it is impossible to
Based on the correction amount obtained in the correction process, the position and orientation of the moving body estimated in the estimation process are corrected to obtain a newly estimated position.
Information processing methods. an estimation process for estimating the position and orientation of a moving object;
an image acquisition process provided in the moving body for acquiring an image;
A correction amount for correcting the position and orientation of the moving object estimated in the estimation process is obtained based on the acquired feature points of the object in the image and the motion model that is the constraint condition of the moving object. a correction process;
including
the moving body is a vehicle,
The constraint condition of the moving object is to constrain the motion of the vehicle by the condition that the bottom surface of the wheel of the vehicle is in contact with a predetermined surface, and the vehicle constrained by the constraint condition moves laterally or upwards. it is impossible to
Based on the correction amount obtained in the correction process, the position and orientation of the moving body estimated in the estimation process are corrected to obtain a newly estimated position.
A program that causes a computer to execute an information processing method.

Images