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

Abstract

To provide an information processing device, an information processing method and a program capable of improving position estimation accuracy in estimating the position of a movable body on the basis of an acquired image.SOLUTION: According to an embodiment, an information processing device includes an estimation unit, an image acquisition unit, and a correction unit. The estimation unit estimates the position and the posture of a movable body. The image acquisition unit is provided in the movable body to acquire an image. The correction unit acquires a correction amount for correcting the position and the posture of the movable body estimated by the estimation unit on the basis of a feature point of an object in the acquired image and a motion model being a constraint of the movable body. Further, the estimation unit corrects the estimated position and posture of the movable body to be a newly estimated position on the basis of the correction amount acquired by the correction unit.SELECTED DRAWING: Figure 1

Description

Embodiments of the present invention relate to information processing devices, information processing methods, and programs.

There is a technique for estimating the self-position of a moving object or the like by using an apparent change of an object or the like that is provided with an imaging means and is photographed in a sequence of images acquired in time series. ..

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

Using the above-mentioned prior art, the moving body can estimate its own position and the like even in a place where the GPS (Global Positioning System) signal does not reach. Further, the above-mentioned conventional technique has an advantage that drift due to heat, which may occur when an inertial measurement unit (IMU, Internal Measurement Unit) is used, does not occur.

On the other hand, the above-mentioned conventional technique has a problem that the estimation accuracy may be lowered when the feature points cannot be tracked stably.

Japanese Unexamined Patent Publication No. 2017-215193

The problem to be solved by the present invention is to provide an information processing device, an information processing method, and a program capable of improving the position estimation accuracy when estimating the position of a moving body based on the acquired image. be.

The information processing device of the embodiment includes an estimation unit, an image acquisition unit, and a correction unit. The estimation unit estimates the position and posture of the moving body. The image acquisition unit is provided on the moving body to acquire an image. The correction unit is for correcting the position and posture of the moving body estimated by the estimation unit based on the acquired feature points of the object in the image and the motion model which is the restraint condition of the moving body. Find the amount of correction. Further, the estimation unit is a newly estimated position by correcting the estimated position and posture of the moving body based on the correction amount obtained by the correction unit.

The block diagram which shows the schematic functional structure of the information processing apparatus of 1st Embodiment. The flowchart which shows the procedure of the processing by the information processing apparatus of 1st Embodiment. The schematic diagram which shows the outline of the result of the process of associating feature points (tracking of feature points) between a plurality of images by the feature point tracking unit of 1st Embodiment. The schematic diagram which shows the calculation process for the estimation part of 1st Embodiment for estimating | F | frame position (initial value) of a moving body. The schematic diagram which shows the example of the feature point error in the acquired image in 1st Embodiment. In the first embodiment, the schematic diagram which shows the outline of the movement of the vehicle (moving body) which makes the movement restrained by the movement model. In the first embodiment, the schematic diagram which shows the outline of the movement of the vehicle (moving body) which moves based on the movement model different from FIG. The schematic diagram which shows the example of the motion of the moving body for demonstrating the motion model error in 1st Embodiment. The block diagram which shows the schematic functional structure of the information processing apparatus of 2nd Embodiment. The flowchart which shows the procedure of the process by the information processing apparatus of 2nd Embodiment. The block diagram which shows the schematic functional structure of the information processing apparatus of 3rd Embodiment.

Hereinafter, the information processing apparatus, the information processing method, and the program of the embodiment will be described with reference to the drawings.

(First Embodiment)
FIG. 1 is a block diagram showing a schematic functional configuration of the information processing apparatus 1 according to the present embodiment. As shown in the figure, the information processing device 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 the correction unit 107. Each of these functional parts is realized by using, for example, an electronic circuit. Further, each functional unit may be provided with a storage means such as a semiconductor memory or a magnetic hard disk device, if necessary. Further, all or a part of the functions of the information processing apparatus 1 may be realized by a computer and software.

The information processing device 1 is provided on a mobile body, for example. 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. Of the functions of the information processing device 1, only the camera of the image acquisition unit 102 and other necessary functions may be provided on the moving body. In this case, the part of the information processing device 1 that is attached to the mobile body and the other parts 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 body based on the correction amount calculated by the correction unit 107. That is, the estimation unit 101 receives feedback of the correction amount obtained by the correction unit 107, corrects the estimated position and posture of the moving body based on the correction amount, and sets the estimated position as a new estimated position.

The position of the moving body is the position of the moving body in the three-dimensional space. The position of the moving body can be represented by, for example, three-dimensional coordinates. However, when there is a constraint condition regarding the movement of the moving body, the position does not necessarily have to be represented by the three-dimensional coordinates, and the information may be reduced as appropriate. For example, as a typical example, when the moving body is bound by the condition that it moves only on a predetermined plane, the position of the moving body can be represented only by the two-dimensional coordinates of the plane. However, the above-mentioned constraint conditions are not limited to the constraint on the plane of the moving body. The constraint condition may be a condition related to some constraint in the three-dimensional space. Further, when the moving body has a size, the position of a predetermined place or a predetermined point included in the moving body may be regarded as the above-mentioned "position of the moving body". The posture of the moving body is at least the orientation of the moving body. When the moving body has a movable part (for example, the joint part included in the crane of the crane vehicle is the movable part), 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, when there is a constraint condition regarding the orientation of the moving body, the information representing the posture does not necessarily have to have independent three-dimensional coordinate values. As an example, when the moving body is a vehicle such as a four-wheeled vehicle and the bottom surface of the tire of the vehicle is restrained so as to be in contact with a predetermined plane, or when the vehicle can be regarded as having no moving part. Can be represented by a one-dimensional numerical value of the angle of rotation 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 images and grayscale images can be acquired from a camera (video camera, etc.) connected to a computer. The distance image can be obtained from the distance sensor. The image acquired online by the image acquisition unit 102 in real time 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.

When the moving body is a rigid body, a camera or a distance sensor directly attached 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 device 1. Here, the mounting position of the camera or the distance sensor in the moving body, the orientation of the camera or the like when acquiring an image, the angle of view, etc. are fixed.

The feature point tracking unit 103 extracts a predetermined feature point from the image acquired by the image acquisition unit 102. The predetermined feature point is, for example, a feature point of an object in the image acquired by the image acquisition unit 102. In addition, the feature point tracking unit 103 tracks the same feature points in a plurality of time-series images. In other words, the feature point tracking unit 103 associates the extracted feature points with each other among a plurality of images taken 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 feature on an image. By having such features, feature points can be more easily extracted. In the case of an optical image, for example, the intersection of a plurality of (eg, two) line segment elements is suitable as a feature point. In the case of a distance image, for example, a maximum or minimum point of a distance or an intersection of a plurality of line components can be used as a feature point as in the case of an optical image.

The feature point extraction and tracking process itself can be performed using existing technology. Specifically, technologies such as SIFT (Scale-Invariant Feature Transform), ORB (Oriented FAST and Rotated BRIEF), and LIFT (Learned Invariant Feature Points) can be used. SIFT is also described in Reference 1 [D.G. Lowe. Distinctive image features from scale invariant keypoints. IJCV, 2004.]. The 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 the image may be regarded as a feature point, and the parameters of the affine transformation when the brightness difference when the certain image is affine-transformed and projected onto another image may be minimized may be used as the tracking result. In addition, each pixel of the image may be tracked by 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 point by the feature point tracking unit 103 and the position and posture of the moving body estimated by the estimation unit 101. In other words, the feature point error calculation unit 104 is 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 body. 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 body obtained based on the tracking result of the feature points and the position of the moving body estimated by the estimation unit 101. That is, the feature point error calculation unit 104 calculates the feature point error, which is the error between the position of the feature point of the object in the acquired image and the position on the three dimensions of the feature point of the object.

The motion model calculation unit 105 calculates the position and posture of the moving body based on the position and posture of the moving body estimated by the estimation unit 101 and the motion model which is a restraint condition regarding the movement of the moving body.

The motion model error calculation unit 106 obtains the motion model error, which is the difference between the position and posture of the moving body calculated by the motion model calculation unit 105 and the position and posture of the moving body estimated by the estimation unit 101. The details of the method of calculating the motion model error will be described later.

The correction unit 107 calculates the correction amount of the position and posture 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 body estimated by the estimation unit 101 based on the feature point error and the motion model error. In other words, the correction unit 107 corrects the position and orientation of the moving body estimated by the estimation unit 101 based on the feature points of the object in the acquired image and the motion model which is the restraint condition of the moving body. Find the correction amount of. The correction unit 107 feeds back the calculated correction amount information to the estimation unit 101. The method of obtaining the correction amount for reducing the error will be described later.

FIG. 2 is a flowchart showing a processing procedure by the information processing apparatus 1. Here, the procedure of processing by the information processing apparatus will be described with reference to this flowchart.

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

In step S102, the feature point tracking unit 103 extracts the 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 the feature points extracted from other images acquired at different times. As a result, the feature point tracking unit 103 associates the feature points with each other. That is, the feature point tracking unit 103 tracks the feature points in the time series.

In step S103, the estimation unit 101 estimates the position and posture of the moving body. The positions and postures estimated in this step are the initial values of the positions and postures, respectively. The method of estimating the initial values of the position and the posture will be described separately.

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

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

In step S106, the motion model error calculation unit 106 calculates the motion model error. This motion model error is derived from the position and posture (most recently estimated position and posture) 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. , Calculated.

In step S107, the information processing apparatus 1 determines whether or not 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 an evaluation function value for comprehensively evaluating the feature point error and the motion model error. 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. In addition, other evaluation functions may be used. The information processing device 1 makes a determination in this step based on a predetermined threshold value of the evaluation function. If the error is sufficiently small as a result of the determination (step S107: YES), the position and orientation within the desired error range are estimated, and the entire process of this flowchart is terminated. If the error is not sufficiently small (step S107: NO), the process proceeds to step S108.

In step S108, the correction unit 107 calculates the correction amount so that the feature point error and the motion model error are smaller. 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 posture of the moving body by applying the correction amount calculated by the correction unit 107 in step S108. The estimation unit 101 uses the corrected position and posture as a new estimation result.

By the above series of processing procedures including repetition, the estimated position and orientation can be adjusted so that the error becomes small.

In addition, the order of processing of each step shown in FIG. 2 may be changed or some steps may be performed as long as no logical contradiction (for example, contradiction of value dependency) occurs in the processing for calculating various values. May be executed in parallel with each other. As an example, the motion model error may be calculated before the feature point error is calculated.

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

FIG. 3 is a schematic diagram showing an outline of the result of processing of associating (that is, tracking) feature points between a plurality of images by the feature point tracking unit 103. 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 taken at each time using a camera provided in front of the moving body in the traveling direction. The black circle points shown in each image are the feature points extracted by the feature point tracking unit 103. In addition, the straight line connecting these black circle points indicates the correspondence (pairing) of the feature points between the images.

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

i-th frame (i = 1,2, ..., | F |, | F | th frame latest frame) in the position of the moving body _{t i,} the attitude of the mobile _{R i,} the time the _{T i,} the speed When s _i, the initial position and orientation assumed a uniform motion will as shown in equation (1). The equation (1) _{is, t i-1, t i} -2, T i, the _{T i-1, T i-} 2, s i as an input, representing a calculation process for outputting the position _{t i.} Further, the equation (1) represents a calculation process for outputting the posture R _{i by inputting R i-1} , R _i-2 , T _i , T _{i -1} , and T _i-2.

In the equation (1), r (R) is a rotation angle representation of the rotation matrix R. Similarly, r ^-1 (θ) is a 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 by using Nister's 5-point algorithm or an algorithm for solving the PnP (Perspective-n-Point) problem. For more information on Nister's five-point algorithm, see Reference 4 [Nister, D. An efficient solution to the five-point relative pose problem. IEEE Transactions on Pattern Analysis and Machine Intelligence, 26 (6): 756-770, 2004.] Is also described. For the algorithm for solving the PnP problem, see 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 uses the position of the jth feature point extracted from the image of the i-frame (i = 1, 2, ..., | F |, | F | frame is the latest frame) x _ji (j = 1). , 2, ..., _{M i)} and, extracted from the image of the k th frame (k ≠ _i), using the positional relationship between the position _{x jk} of a feature point is a tracking result of _{x ji,} the position of the moving body t _This is a method for calculating i and posture R _{i. The algorithm for solving the PnP problem is the position x ji} (j =) of the jth feature point extracted from the image in the i-frame (i = 1, 2, ..., | F |, | F | frame is the latest frame). 1,2, ..., and _{M i),} and a three-dimensional position _{X j} of _{x ji,} a method for calculating the position _{t i} and orientation _{R i} of the moving body.

However, Nister's 5-point algorithm cannot calculate the scale of the position. Therefore, it is also necessary to obtain the scale by using the three-dimensional position of the feature point separately obtained by using a distance image or the like, the speed of a moving body acquired from a speedometer (speedometer) or the like. As an example, a method of estimating the position and posture using Nister's 5-point algorithm and the speed of a moving object obtained from a speedometer or the like will be described. i-th frame (i = 1,2, ..., | F |, | F | th frame latest frame) in the position of the moving body _{t i,} the attitude of the mobile _{R i,} the time the _{T i,} the speed Let _si . Using the association result (tracking result) between the feature point of the k-th frame and the feature point of the | F | frame, the position / orientation of the moving body in the k-frame estimated by Nister's 5-point algorithm is used as the origin. _{Let n k | F |} be the relative position of the moving body at the | F | frame. However, since the scale is unknown, the relative position is as shown in the following equation.

At this time, the position t _{| F |} of the | F | frame eye is expressed by the following equation (2).

In this equation (2), the posture 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 frame and the (| F | -1) frame are expressed. , | a _{-1, |} | F | th frame and (| F | -1) th frame time _{T | F |} and _{T | F} as inputs and, | _| F | speed th frame _{s | F} F | It represents the calculation process for outputting _{the position t | F | -1} of the frame eye. The angle φ will be described later.

FIG. 4 is a schematic view showing a calculation process for the estimation unit 101 to estimate the position (initial value) of the | F | frame of the moving body by the above method. As shown in the figure, assuming constant velocity motion, the position of the moving body in the | F | frame is determined from the state of the moving body in the kth frame and the state of the moving body in the (| F | -1) frame. Can be estimated.

4, the position of the moving body in the k-th frame is t _k, the posture is R _k. The position of the moving body in the | F | -1st frame is t _{| F | -1} . The position of the moving body at the | F | frame is t _{| F |} . The moving distance calculated from the vehicle speed and the time is | t _{| F |} −t _{| F | -1} |, as expressed by the above equation (2). As shown in FIG. 4, the angle φ also appearing in the equation (2) is centered on the position of the moving body in the kth frame, the position of the moving body in the | F | -1st frame, and the | F | frame. It is the angle formed by the position of the moving body.

Next, a method of calculating the feature point error by the feature point error calculation unit 104 will be described. The feature point error is the position when the feature point in the three-dimensional space is projected on the frame image (two-dimensional coordinates on the image) and the position of the feature point extracted on the frame image (two-dimensional on the image). It is the difference (distance, norm) from the image).

The three-dimensional position (position in the three-dimensional space) of the feature point can be obtained by using the existing technology. For example, the three-dimensional position of the feature point can be obtained by using the above-mentioned Nister 5-point algorithm. It is also possible to obtain the three-dimensional position of the feature point by using the result of distance image estimation or the like. When the image acquisition unit 102 or the like of the information processing device 1 is provided with a distance sensor, the three-dimensional position of the feature point can be obtained using the distance image.

The above-mentioned distance image estimation technique is described in, for example, Reference 6 [R. Mahjourian, M. Wicke, and A. Angelova. Unsupervised learning of depth and ego-motion from monocular video using 3d geometric constraints. In Proceedings of the 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 view showing an example of feature point error in the acquired image. The figure includes images from the i-th frame to the N-th frame. Here, focusing on the image of the kth frame (k = i, i + 1, ..., N), it is as follows. The image of the k-th frame is an image acquired on the premise of the position and orientation of the moving body at the time of the frame. _{Here, the position x jk} (hat) corresponding to the j-th feature point on the image of the k-th frame is the position where the three-dimensional position of the j-th feature point is projected onto the image. 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 image of the k-th frame is the position of the j-th feature point extracted on the image. The difference between these positions x _jk (hat) and the position x _jk is the feature point error. In this figure, the _{arcuate line connecting the position x ji} , the position x _{ji + 1} , _... , the position x _jk , _... , And the position x _jN sequentially indicates the tracking of the feature points between the images.

The method of calculating the feature point error is as follows. i-th frame (i = 1,2, ..., | F |, | F | th frame latest frame) at the position of the moving body _{t i,} the posture of the moving body _{R i,} j-th extracted from the image Assuming that the position of the feature point is x _ji (j = 1, 2, ..., M), the three-dimensional position of the _jth feature point is X j, and the internal parameter of the camera is K, the projection error of the feature point ( That is, the feature point error) E is expressed by the following equation (3). The internal parameters of the camera are, specifically, a matrix composed of the values of the focal length and the optical center (principal point).

The equation (3) is set to the position x _ji of feature points on the image, the posture of the moving body and R _i, and position t _i of the moving body, a three-dimensional position X _j of the feature points, the internal parameters of the camera K Represents a calculation process in which and is used as an input and the feature point error E is output.

Here, the first frame (i = 1) is not the frame that was first acquired by starting the camera, but was input in the past among the frames that could associate the feature points with each other. Refers to the frame.

Next, the motion model will be described. The motion model is, for example, a constraint condition when a moving body moves. For example, when the moving body is a four-wheeled vehicle, the constraint conditions are as follows. That is, the four wheels are in contact with the ground and cannot exist either above the ground or below the ground. Also, the moving body can basically travel only in the direction of the front wheels. Also, the steering angle is limited (ie, the radius of gyration of the moving body has a lower limit). A moving body constrained by these constraint conditions cannot move to the side or directly above (in the direction of floating from the ground).

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

FIG. 6 is a schematic view showing an outline of the movement of a vehicle (moving body) that moves while being restrained by a constraint condition. The vehicle position shown in the figure is calculated based on the kinematics of the vehicle. In the figure, in the three-dimensional space of xyz, the vehicle travels on the ground which is the xy plane. In other words, the bottom surface of the wheels of the vehicle is constrained on the xy plane. Further, 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 that makes a turning motion based on the center positions of the two rear wheels (referred to as the center of the rear wheels) may be defined. In this case, i-th frame (i = 1,2, ..., | F |, | F | th frame latest frame) in the position of the moving body _{t i,} the attitude of the mobile _{R i,} the time the _{T i} , the speed s _i, the rear wheel center of the turning radius (turning radius) and [rho, the following equation (4) holds.

Note that ρ shown in FIG. 6 is the turning radius of the turning motion of the center of the rear wheel of the vehicle. Further, θ is an angle at which the vehicle turns between the | F | -1st frame and the | F | frame in the turning motion. Further, the moving distance of the vehicle between the | F | -1st frame and the | F | frame, which is calculated from the vehicle speed and the time, is | t _{| F |} −t _{| F | -1} |.

In the formula (4), the estimated position t _{| F | -1} of the (| F | -1) frame, the posture R _{| F | -1} of the (| F | -1) frame, and | F | Attitudes R _{| F |} and l, (| F | -1) frame eye time T _{| F | -1} , | F | frame eye time T _{| F |} , | F | frame eye speed It represents a calculation process in which _{s | F |} is input and the estimated position t _{| F | of the | F | frame is output.}

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

For example, if the steering angle in the i-frame (i = 1, 2, ..., | F |, | F | frame is the latest frame) is δ _{i , the rotation matrix R i} of the motion model calculated from the steering angle is , Can also be expressed as the following equation (5). This equation (5) represents a calculation process for outputting the _{rotation matrix R i by inputting 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 ^-1 (θ) is a rotation matrix calculated from the yaw angle θ. The distance between the center of gravity of the vehicle body and the centers of the front and rear wheels is as follows.

FIG. 7 is a schematic diagram showing an outline of the movement of a vehicle (moving body) that moves based on the definition of a movement model different from that of FIG. In the figure, in the three-dimensional space of xyz, the vehicle travels on the ground which is the xy plane. In other words, the bottom surface of the wheels of the vehicle is constrained on the xy plane. Further, 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. Incidentally, theta is, | F | a _-1, | _{| F |} orientation _R of the vehicle in the th frame F | -1 posture of the vehicle in the th frame _{R | F |} is the _{angle -1} and forms. When this motion model is used, the following equation (6) holds.

| T _{| F |} −t _{| F | -1} | is the moving distance of the vehicle between the | F | -1st frame and the | F | frame, which is calculated from the vehicle speed and the time.

In the formula (6), the estimated position t _{| F | -1} of the (| F | -1) frame, the posture R _{| F | -1} of the (| F | -1) frame, and | F | Attitudes R _{| F |} and l, (| F | -1) frame eye time T _{| F | -1} , | F | frame eye time T _{| F |} , | F | frame eye speed It represents a calculation process for outputting _{the estimated position t | F |} of the | F | frame with _{s | F | as an input.}

FIG. 8 is a schematic view showing an example of the motion of a moving body for explaining the motion model error. The motion model error will be described with reference to this figure. The error of the motion model is the difference between the position and posture of the moving body estimated by the estimation unit 101 and the position and posture of the moving body obtained based on the motion model. For example, i-th frame (i = 1,2, ..., | F |, | F | th frame latest frame) at the position of t _i of the moving body obtained by _estimation, the moving body obtained by the motion model the position and _{t i} (hat). Then, the motion model error _eri is expressed by the following equation (7).

The above equation (7), and t _i of the estimated mobile as input the position t _i of the moving body obtained by the motion model _(hat), the calculation process for outputting a motion model error e _ri 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 attitude to R _i of the moving body obtained by estimating _the position of the obtained moving object by the motion model as R _{i (hat),} motion model error e _ri is expressed by the following equation (8).

Equation (8), the estimated position _{t i} of the i th frame, and the orientation _{R i} of i-th frame, and the position _{t i} by i-th frame of the motion model (hat), posture by i-th frame of the motion model _{R i} (Hat) and l are input to represent the calculation process for outputting the _{motion model error eri.} However, l is the position of the center of the front wheels when the center of the rear wheels is the origin.

Next, the correction amount required by the correction unit 107 will be described. The correction unit 107 calculates the correction amount based on the feature point error and the motion model error. The error E, which is a combination of the feature point error and the motion model error, is expressed by the following equation (9). _{The feature point error e ji} included in the equation (9) is as described in the equation (3). The motion model error _eri is as described in the equation (7) or the equation (8).

Equation (9) represents a calculation process for outputting a comprehensive E with the feature point error e _ji and the motion model error _{eri as inputs.} In addition, λ in the 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 posture of the moving body, the correction amount is an amount for reducing the value of E described above. For example, using the Gauss-Newton method, the Levenberg-Marquardt method, the conjugate gradient method, the genetic algorithm (GA), or the like, the correction unit 107 obtains the value of the correction amount for reducing E. That is, by minimizing E, from the first frame | can be obtained for all the frames up to frame, and the feature point error smaller position t _i and orientation R _i of motion model errors | F. However, i = 1, 2, ..., | F |, and the | F | frame is the latest frame.

When using Nister's 5-point algorithm, the position and orientation are estimated using the association result (tracking result) between the feature point of the k-th frame and the feature point of the | F | frame, but depending on the value of k, The error E changes. The correction unit 107 can obtain a correction amount that reduces the feature point error and the motion model error by fully searching the correction amount. Then, the position and posture of the moving body can be corrected by this correction amount.

As described above, according to the present embodiment, the motion model calculation unit 105 obtains the position of the moving body based on the motion conditions. Further, the motion model error calculation unit 106 calculates the motion model error based on the position of the moving body estimated by the estimation unit 101 and the position of the moving body obtained by the motion model calculation unit 105. Therefore, the correction unit 107 can obtain the correction amount not only based on the feature point error but also based on the motion model error. As a result, the estimation accuracy regarding the position of the moving body and the like is improved.

(Second Embodiment)
Next, the second embodiment will be described. The matters already described in the previous embodiment may be omitted below. Here, the matters peculiar to the present embodiment will be mainly described.

FIG. 9 is a block diagram showing a schematic functional configuration of the information processing apparatus 2 according to the present embodiment. As shown in the figure, the information processing device 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. The correction unit 207, the sensor information acquisition unit 211, the sensor model calculation unit 212, and the sensor model error calculation unit 213 are included.

A feature of this 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.

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 the first. 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 according to the first embodiment. , Have similar or similar functions. The configuration peculiar to the present 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 by using an electronic circuit, or by using a general-purpose computer and a program. Can also be realized. The functions of each part constituting the information processing device 2 are as follows.

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

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

The feature point tracking unit 203 extracts a predetermined feature point from the image acquired by the image acquisition unit 202. In addition, 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 point by the feature point tracking unit 203 and the position and orientation of the moving body estimated by the estimation unit 201. That is, the feature point error calculation unit 204 has the same function as the feature point error calculation unit 104 of the first embodiment.

The motion model calculation unit 205 calculates the motion model based on the position and posture of the moving body estimated by the estimation unit 201. That is, the motion model calculation unit 205 has the same function as the motion model calculation unit 105 of the first embodiment.

The motion model error calculation unit 206 calculates the motion model error from the motion model calculated by the motion model calculation unit 205 and the position and posture of the moving body obtained by the estimation unit 201. That is, the motion model error calculation unit 206 has the same function as the motion model error calculation unit 106 of the first embodiment.

The correction unit 207 is 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. Calculate the correction amount of the position and posture of the moving body. The difference from the correction unit 107 in the first embodiment is that the correction unit 207 calculates the correction amount based on the sensor model error. The correction unit 207 feeds back the calculated correction amount information to the estimation unit 201. The 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 is specifically a speedometer, an acceleration sensor, a gyro sensor, a steering angle sensor, a GPS terminal device, or the like. The speedometer acquires speed information or speed information of a moving body. If it is an acceleration sensor, it acquires acceleration information of a moving body. If it is a gyro sensor, it acquires the orientation or posture of the moving body and their time derivative values (angle information and angular velocity information). The GPS terminal device acquires the position information of the moving body. Further, as the sensor, a video camera, a network camera, a distance sensor, or the like may be used. Even when the same equipment as the video camera, network camera, distance sensor, etc. used in the image acquisition unit 202 is used, the image acquisition unit 202 can be installed at a position different from the sensor of the image acquisition unit 202. It is possible to obtain information different from that of. Further, by changing the frame rate (the number of frames per unit time) or the angle of view of the camera or the distance sensor, the sensor information acquisition unit 211 can acquire information that cannot be obtained by the image acquisition unit 202. .. Further, as the sensor, a sensor in which a plurality of sensors are combined, such as an inertial measurement unit (IMU) or a yaw rate G sensor, may be used.

The sensor model calculation unit 212 calculates the sensor model from the sensor information acquired by the sensor information acquisition unit 211. In other words, the sensor model calculation unit 212 calculates the position and posture of the moving body based on the acquired sensor information by a calculation procedure peculiar to the sensor type.

The sensor model error calculation unit 213 calculates the sensor model error from the sensor model calculated by the sensor model calculation unit 212 and the position and orientation of the moving body 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 moving body estimated by the estimation unit 201 and the position of the moving body obtained by the sensor model calculation unit 212. Details of the method for calculating the sensor model error will be described later.

FIG. 10 is a flowchart showing a processing procedure by the information processing apparatus 2. Here, the procedure of processing by the information processing apparatus 2 will be described with reference to this flowchart.

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

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

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

In step S204, the sensor model calculation unit 212 calculates the 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 body based on the positions of the feature points in the image. The positions and postures estimated in this step are the initial values of the positions and postures, respectively.

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

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

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

In step S209, the motion model error calculation unit 206 calculates the motion model error. This motion model error is derived from the position and posture (most recently estimated position and posture) 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. , Calculated.

In step S210, the information processing apparatus 2 has sufficiently small values of 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. Determine if. Specifically, the information processing device 2 uses an evaluation function value for comprehensively evaluating the feature point error, the sensor model error, and the motion model error. 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. In addition, other evaluation functions may be used. The information processing device 2 makes a determination in this step based on a predetermined threshold value of the evaluation function. As a result of the determination, when the error is sufficiently small (step S210: YES), it is considered that the position and the posture within the desired error range are estimated, so that the processing of the entire flowchart is terminated. If the error is not sufficiently small (step S210: NO), the process proceeds to step S211.

In step S211 the correction unit 207 calculates the correction amount so that the feature point error, the sensor model error, and the motion model error become smaller. 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 posture of the moving body by applying the correction amount calculated by the correction unit 207 in step S211. The estimation unit 201 uses the corrected position and posture as a new estimation result.

By the above series of processing procedures including repetition, the estimated position and orientation can be adjusted so that the error becomes small.

In addition, the order of processing of each step shown in FIG. 10 may be changed or some steps may be performed as long as no logical contradiction (for example, contradiction of value dependency) occurs in the processing for calculating various values. May be executed in parallel with each other. As an example, the motion model error may be calculated before the feature point error and the sensor model error are calculated.

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

For example, when a speedometer is used as the sensor, the sensor model calculation unit 212 can calculate the moving distance of the moving body by integrating the speed acquired by the speedometer with 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 posture estimated by the estimation unit 201. i-th frame (i = 1,2, ..., | F |, | F | th frame latest frame) at the time the _{T i,} speed _{s i} obtained from the speedometer, the position of a moving body estimated t _{Let i} be. Then, the sensor model and the sensor model error are calculated by the following equation (10).

Equation (10), and the time _{T i} of the i-th frame, as input (i-1) at time _{T i-1} th frame, a rate _{s i} of the i th frame, a sensor model,

Represents the calculation process that outputs.
Further, equation (10), the estimated position _{t i} of the i th frame, the estimated position _{t i-1} of the (i-1) th frame, which is the sensor model,

Is used as an input to represent the calculation process for outputting the _{sensor model error e si.}

For example, when the IMU is used as the sensor, the position and orientation of the moving body can be calculated from the acceleration and the angular velocity acquired from the IMU. The sensor model error can be calculated by comparing this position and posture with the estimation result by the estimation unit 201 of the position and posture of the moving body. i-th frame (i = 1,2, ..., | F |, | F | th frame latest frame) time the _{T i} in, the acceleration obtained from the IMU and _{a i,} omega the angular velocity ω _i. Then, the position of the sensor model _{t i} (hat) and postures _{R i} (hat), the sensor model error _{e si} is calculated by the following equation (11).

In this equation (11), the estimated position ti _-1 of the (i-1) th frame, the speed vi _-1 of the (i-1) th frame, and the posture R _{i- of the (i-1) th frame are shown. 1,} and the acceleration _{a i} of i-th frame, and the time _{T i} of the i-th frame, (i-1) as inputs and time _{T i-1} th frame, the position of the sensor model i th frame _t i ( Represents the calculation process with hat) as the output. In addition, the equation (11), (i-1) and the attitude _{R i-1} th frame, and the i-th frame of the angular velocity ω _i, and time _{T i} of the i-th frame, (i-1) th frame of It represents a calculation process in which the _{time Ti-1 is input and the attitude Ri} (hat) of the sensor model in the i-frame is output. Further, the equation (11) as input the position of the sensor model _{t i} (hat) and postures _{R i} (hat), representing a calculation process of an output of the sensor model error _{e si.}

However, in the equation (11), Ω _i represents ω _i by a rotation matrix.

Next, the correction amount when the sensor model is introduced will be described. When the correction amount in the present embodiment is obtained, the correction unit 207 further adds the _{sensor model error e si to the error obtained in the first embodiment.} In the present embodiment, _{the total error considering the sensor model error e si} is calculated by the following equation (12).

Equation (12) represents a calculation process for outputting the total error E by inputting the feature point error e _ji , the motion model error _eri, and the sensor model error e _si. In equation (12), λ _r and λ _s are hyperparameters for adjusting the balance between the feature point error, the motion model error, and the sensor model error.

As described above, according to the present embodiment, the sensor information acquisition unit 211 acquires the sensor information. Based on the above sensor information, the sensor model calculation unit 212 performs calculations using a predetermined model to calculate the position and posture of the moving body. The sensor model error calculation unit 213 calculates the sensor model error as the difference between the position and posture of the moving body estimated by the estimation unit 201 and the position and posture of the moving body calculated by the sensor model calculation unit 212. As a result, 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. As a result, the estimation accuracy of the position and the like estimated by the estimation unit 201 is further improved.
Further, it does not have the feature point error calculation unit 104 or the motion model error calculation unit 206, that is, it does not calculate the feature point error or the soil transportation model error, and is 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, the third embodiment will be described. The matters already explained up to the previous embodiment may be omitted below. Here, the matters peculiar to the present embodiment will be mainly described.

FIG. 11 is a block diagram showing a schematic functional configuration of the information processing apparatus 3 according to the present embodiment. As shown in the figure, 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. The correction unit 307 and the speed information acquisition unit 321 are included.

The feature of the present embodiment is that the information processing device 3 has the speed information acquisition unit 321 and the motion model calculation unit 305 calculates the motion model based on the acquired velocity information.

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 are the first, 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 according to the first embodiment. , Have similar or similar functions. The features of this embodiment are the speed information acquisition unit 321 and the motion model calculation unit 305. The functions of each part of the information processing apparatus 3 can be realized as a dedicated function by using an electronic circuit, or can be realized by using a general-purpose computer and a program.

The speed information acquisition unit 321 acquires information on the speed of the moving body from a so-called speedometer. The velocity information may be a vector or a scalar (absolute value of velocity represented by a vector). When the moving body is a car or the like, speed information can be obtained from the speedometer originally provided in the car. The speed information acquisition unit 321 passes the acquired speed information to the motion 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 a calculation based on the speed information received from the speed information acquisition unit 321 when calculating the position and posture of the moving body. 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 body and the like so as to match the calculated movement amount. That is, the motion model calculation unit 305 calculates the position of the moving body based on the acquired velocity. That is, the motion model error calculation unit 106 acquires the position and posture of the moving body according to the motion model based on the speed acquired by the velocity information acquisition unit 321 and calculates the error of the motion model.

The second embodiment and the third embodiment may be combined and implemented. In that case, in the information processing device, the sensor information acquisition unit acquires sensor information (information acquired by a general sensor). Then, the sensor model calculation unit obtains the position and posture based on the sensor information. Further, in the information processing device, the motion model calculation unit calculates the position and posture of the moving body 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 modification, the information processing apparatus (1, 2, 3) may interrupt some processing in the middle of the processing when the motion model error is larger than a predetermined value. In other words, the execution of some processing may be skipped. As a result of the motion model error calculation unit (106, 206, 306) calculating the motion model error, the motion model error is larger than other assumed errors (feature point error and sensor model error depending on the embodiment). If it is extremely large, other errors are likely to be negligible. 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 error can be ignored. Specifically, in this modification, when the value (absolute value) of the motion model error is equal to or more than a predetermined threshold value, the feature point error or the sensor model error is not calculated, or the feature point error or the sensor model error is calculated. It interrupts in the middle and moves to the process of obtaining the correction amount. At this time, in order to consider that the uncalculated errors can be ignored, the correction unit (107, 207, 307) obtains the correction amount with those errors as 0.

In this modification, for example, the motion model error calculation unit (106, 206, 306) compares the calculated absolute value of the motion model error with the above threshold value. When the absolute value of the motion model error is equal to or greater than the threshold value, the motion model error calculation unit (106, 206, 306) calculates the feature point error and the sensor model error by sending a notification to each other unit. Suppress the processing for. 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) sends a notification to the motion model error. Control is performed so that the processing is executed with an error other than 0 as 0.

When this modification is implemented, the amount of calculation for calculating the feature point error and the sensor model error can be reduced. This makes it possible to make effective use of computational resources and shorten the overall processing time of the information processing apparatus (1, 2, 3).

That is, the motion model error calculation unit (106, 206, 306) in this modification is the feature point error calculation unit (104, 204, 304) when the absolute value of the obtained motion model error is equal to or greater than a predetermined threshold value. May be controlled so as 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 by regarding the feature point error as zero.

Further, in the present modification, 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 more than a predetermined threshold value. The sensor model error calculation unit (213) controls to suppress the process of obtaining the point error, and the correction unit (207) controls the feature point error and the sensor model error. May be controlled so that the correction amount is obtained by regarding both of them as zero.

(Second modification)
The second modification is applicable to each of the above embodiments. Further, the first modification may be combined with the second modification. In this modification, the information processing device (1, 2, 3) dynamically changes the hyperparameters according to the situation. Here, the hyperparameters to be changed are λ in the formula (7) of the first embodiment and λ _{r and λ s} in the formula (10) of the second embodiment. That is, in this modification, the information processing apparatus (1, 2, 3) has a feature point error, a motion model error, and a sensor model (in the case of the second embodiment) when calculating the total error. The balance with the error can be changed dynamically.

For example, when the moving body is a vehicle, the motion of the vehicle is constrained by the constraint conditions defined by the motion model. However, in reality, there may be a situation in which the tires of the vehicle are likely to slip with the road surface depending on the speed, the weather, etc., or the air resistance received by the vehicle body changes due to the influence of strong wind. If the motion model does not assume these disturbance factors, the position and posture calculated from the motion model may include a large error depending on the degree of disturbance.

Further, for example, even when the calculation based on the sensor model is performed in the second embodiment, the position and the posture calculated by the sensor model may include a large error due to the influence of the disturbance. For example, when the IMU is used as a sensor, the error of the acquired sensor information may become large due to the influence of heat. Therefore, the position and attitude calculated based on the IMU may include a large error.

In this modification, the information processing apparatus (1, 2, 3) changes the weights 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 the equation (7) and the values of λ _r and λ _s in the 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, the information processing apparatus (1, 2, 3) in the case of providing the speedometer, in equation (7), and _{λ = λ 0 · exp (-b} · s i). Here, s _i is the velocity of the moving body obtained from the speedometer. Further, b is a predetermined positive constant. Further, λ ₀ is a hyperparameter value when the velocity is 0. By making λ variable in this way, the weight of the motion model error becomes relatively large when the speed of the moving body is slow, and the weight of the feature point error becomes relatively large when the speed of the moving body is high. 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.

Further, as another example, when the information processing device 2 of the second embodiment uses the IMU as a sensor, the following can be performed. That is, in equation (10), for _{example, λ r = λ r0 · exp} (-b 1 · s i), may be a _{λ s = λ s0 · exp (} -b 2 · c i). Here, s _i is the speed of the likewise mobile. Further, c _i is the obtained from the temperature sensor such as temperature (absolute temperature or Celsius temperature). Further, b ₁ and b ₂ are predetermined positive constants. Further, λ _r0 is a reference value of the _{hyperparameter λ r} when the velocity is 0. Further, λ _s0 is a reference value of the hyperparameter λ _s when the temperature is a reference value. By making λ _r and λ _s dynamically variable in this way, the error of the motion model due to slip during high-speed movement of the moving body, the error of the sensor model due to high temperature, etc. become the total error. The effect on E can be adjusted appropriately.

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 making it dynamically variable.

Further, in this modification, the correction unit (107, 207, 307) is based on the acquired absolute value of the velocity of the moving body, so that the larger the absolute value of the velocity, the smaller the weight of the motion model error becomes. In addition, the correction amount may be obtained while dynamically changing the weighting between the feature point error and the motion model error.

Further, in this modification, when the correction unit (207) obtains the correction amount based on the feature point error, the motion model error, and the sensor model error, the correction unit (207) has three of the feature point error, the motion model error, and the sensor model error. The correction amount may be obtained while dynamically changing the weighting between the persons.

Further, in this modification, the correction unit (207) is acquired so that the weight of the motion model error becomes relatively smaller as the absolute value of the velocity increases based on the acquired absolute value of the velocity of the moving body. The correction amount may be obtained while dynamically changing the weighting among the three parties so that the weight of the sensor model error is relatively variable according to the temperature based on the temperature information.

(Third variant)
The third modification is applicable to each of the above embodiments. Further, the first modification and the second modification may be combined with the third 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, or the like, which is a means for acquiring an image, is fixedly provided to the moving body. Further, when a sensor is used, if it is necessary to operate the sensor in a state of being fixed to the moving body among various sensors, the sensor is provided fixed to the moving body. Other functions of the information processing apparatus (1, 2, 3) may be provided in a place different from the moving body. For example, a calculation function for calculating various positions and calculating errors may be provided in a remote external device (for example, a computer server or the like). Moreover, the external device in this remote place may be implemented as a so-called cloud server. At this time, data can be exchanged between the function fixed to the mobile body and the function provided in the external device at a remote location by communication via a communication line as appropriate.

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

In addition, at least a part of the functions of the information processing apparatus in the above-described embodiment (including the modified example) may be realized by a computer. In that case, the program for realizing this function may be recorded on a computer-readable recording medium, and the program recorded on the recording medium may be read by the computer system and executed. The term "computer system" as used herein includes hardware such as an OS and peripheral devices. The "computer-readable recording medium" is a portable medium such as a flexible disk, a magneto-optical disk, a ROM, a CD-ROM, a DVD-ROM, or a USB memory, or a storage device such as a hard disk built in a computer system. Say that. Further, a "computer-readable recording medium" is a communication line for transmitting a program via a network such as the Internet or a communication line such as a telephone line, and dynamically holds the program for a short period of time. It may also include a program that holds a program for a certain period of time, such as a volatile memory inside a computer system that serves as a server or a client in that case. Further, the above-mentioned program may be a program for realizing a part of the above-mentioned functions, and may be a program for realizing the above-mentioned functions in combination with a program already recorded in the computer system.

Although some embodiments of the present invention have been described, these embodiments are presented as examples 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 changes can be made without departing from the gist of the invention. These embodiments and modifications thereof are included in the scope and gist of the invention, as well as in the scope of the invention described in the claims and the equivalent scope thereof.

1, 2, ... Information processing device, 101 ... Estimating unit, 102 ... Image acquisition unit, 103 ... Feature point tracking unit, 104 ... Feature point error calculation unit, 105 ... Motion model calculation unit, 106 ... Motion model error calculation unit, 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 estimation unit that estimates the position and orientation of the moving body,
An image acquisition unit provided on the moving body and acquiring an image,
A correction amount for correcting the position and posture of the moving body estimated by the estimation unit based on the acquired feature points of the object in the image and the motion model which is a restraining condition of the moving body is obtained. Correction part and
With
The estimation unit is a newly estimated position by correcting the estimated position and posture of the moving body based on the correction amount obtained by the correction unit.
Information processing device. A feature point error calculation unit that calculates a feature point error that is an error between the acquired position of the feature point of the object in the image and the three-dimensional position of the feature point of the object.
Motion to obtain the motion model error, which is the difference between the position and posture of the moving body calculated based on the motion model which is the restraint condition of the moving body and the position and posture of the moving body estimated by the estimation unit. Model error calculation unit and
With more
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. The sensor information acquisition unit that acquires sensor information from the sensor, and
A sensor model calculation unit that obtains the position of the moving body based on the sensor information,
A sensor model error calculation unit that calculates a sensor model error that is the difference between the position of the moving body estimated by the estimation unit and the position of the moving body obtained by the sensor model calculation unit.
With more
The correction unit further obtains the correction amount based on the sensor model error.
The information processing device according to claim 1. Speed information acquisition unit that acquires the speed of the moving body,
With more
The motion model error calculation unit calculates the position and posture of the moving body calculated based on the motion model based on the acquired velocity.
The information processing device according to claim 2 or 3. When the absolute value of the obtained motion model error is equal to or greater than a predetermined threshold value, the motion model error calculation unit controls the feature point error calculation unit to suppress the process of obtaining the feature point error, and at the same time, the motion model error calculation unit suppresses the process of obtaining the feature point error. The correction unit controls to obtain the correction amount by regarding the feature point error as zero.
The information processing device 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 obtained absolute value of the motion model error is equal to or greater than a predetermined threshold value. The sensor model error calculation unit controls to suppress the process of obtaining the sensor model error, and the correction unit controls to obtain the correction amount by regarding both the feature point error and the sensor model error as zero. do,
The information processing device according to claim 3. When the correction amount is obtained based on the feature point error and the motion model error, the correction unit makes the correction dynamically variable between the feature point error and the motion model error. Ask for quantity,
The information processing device according to any one of claims 2 to 6. Based on the acquired absolute value of the velocity of the moving body, the correction unit performs the feature point error and the motion model so that the larger the absolute value of the velocity, the smaller the weight of the motion model error becomes. The correction amount is obtained while dynamically changing the weighting with the error.
The information processing device according to claim 7. When the correction amount is obtained based on the feature point error, the motion model error, and the sensor model error, the correction unit is between the feature point error, the motion model error, and the sensor model error. The correction amount is obtained while dynamically changing the weighting of.
The information processing device according to claim 3. The correction unit is based on the acquired absolute value of the velocity of the moving body so that the larger the absolute value of the velocity is, the smaller the weight of the motion model error is, and based on the acquired temperature information. The correction amount is obtained while dynamically changing the weighting among the three parties so that the weight of the sensor model error is relatively variable according to the temperature.
The information processing device according to claim 9. The estimation process for estimating the position and orientation of the moving object, and
An image acquisition process provided on the moving body to acquire an image, and
A correction amount for correcting the position and posture of the moving body estimated in the estimation process is obtained based on the acquired feature points of the object in the image and the motion model which is a restraining condition of the moving body. The correction process and
Including
Based on the correction amount obtained in the correction process, the position and posture of the moving body estimated in the estimation process are corrected to obtain a newly estimated position.
Information processing method. The estimation process for estimating the position and orientation of the moving object, and
An image acquisition process provided on the moving body to acquire an image, and
A correction amount for correcting the position and posture of the moving body estimated in the estimation process is obtained based on the acquired feature points of the object in the image and the motion model which is a restraining condition of the moving body. The correction process and
Including
Based on the correction amount obtained in the correction process, the position and posture of the moving body estimated in the estimation process are corrected to obtain a newly estimated position.
A program that allows a computer to execute an information processing method.

Images