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

Description

The present invention relates to a technology for acquiring the position and orientation of a moving object.

Autonomous vehicles such as automated guided vehicles (AGVs) are used in factories and logistics warehouses. A known method for estimating the position and orientation of such autonomous vehicles is to use a camera or LiDAR (Laser Imaging Detection and Ranging) as a sensor to obtain the time-based position and orientation difference from the results of measuring the environment around the vehicle. Another known method is to calculate the position and orientation value by comparing it with environmental map data.

When a moving body creates environmental information indicating the shape and position of objects in the environment in which the moving body travels, based on information obtained from a sensor, the accuracy of obtaining the relative position between the moving body and surrounding objects can decrease due to vibrations of the moving body. Patent Document 1 discloses a technology that determines the reliability of sensor measurement values based on the difference between the measured value of acceleration applied to the moving body and the value assumed for the acceleration, and removes sensor information from times with low reliability.

JP 2009-174898 A

However, in the method described in Patent Document 1, when the corresponding sensor information is used for position and orientation measurement, there is a problem that the accuracy of the position and orientation measurement decreases or the measurement is interrupted at times when the information is unreliable.

The present invention has been made in consideration of the above problems, and aims to provide an information processing device that can stably obtain the position and orientation of a moving object. It also aims to provide a method and program for doing so.

The information processing device according to the present invention has the following configuration: an input information acquiring means for acquiring input information from a sensor mounted on a moving body, a vibration information acquiring means for acquiring vibration information including the magnitude and direction of vibration received by the moving body, and a position and orientation acquiring means for acquiring the position and orientation of the moving body based on corrected input information obtained by correcting the input information acquired by the input information acquiring means using the vibration information when the sensor is subjected to vibration of a magnitude exceeding a threshold value.

The present invention makes it possible to stably obtain the position and orientation of a moving object.

FIG. 1 is a diagram showing a configuration of a mobile system according to a first embodiment; FIG. 1 is a block diagram showing a functional configuration of an information processing apparatus according to a first embodiment; FIG. 1 is a flowchart showing the flow of information processing according to a first embodiment. FIG. 1 is a flowchart showing details of the flow of impact vibration information acquisition processing according to the first embodiment. FIG. 1 is a schematic diagram showing a result of feature point tracking in the first embodiment; FIG. 1 is a flowchart showing details of the flow of position and orientation calculation processing according to the first embodiment. FIG. 1 is a diagram showing a configuration of a mobile system according to a second embodiment; FIG. 11 is a flowchart showing the flow of information processing according to the second embodiment.

The following describes in detail the embodiments of the present invention with reference to the drawings. Note that the following embodiments do not limit the scope of the present invention, and not all of the combinations of features described in the following embodiments are necessarily essential to the present invention.

[First embodiment]
DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS A moving body system, a position and orientation measurement system, an information processing device, an information processing method, and a computer program according to a first embodiment of the present invention will be described in detail below with reference to the drawings.

In the first embodiment, an example will be described in which a mobile object equipped with a camera (imaging sensor) as a sensor moves within an environment, and the position and orientation of the mobile object are estimated. Here, the environment refers to the area in which the sensor moves and the three-dimensional space around it, and the position and orientation are values of six degrees of freedom that combine three-dimensional position information and orientation information of three degrees of freedom. In the following explanation, the vertical direction of the environment is the z-axis, and the mobile object basically travels on an xy plane.

In this embodiment, the problem is that camera images are blurred by shocks or vibrations applied to a moving object, resulting in a decrease in the accuracy of measuring the position and orientation by tracking feature points in the camera image. The information processing device performs image correction according to the magnitude and direction of the shock or vibration, thereby achieving stable position and orientation measurement.

(Configurations of the moving body system, position and orientation measurement system, and information processing device)
1 is a diagram showing an example of the configuration of a mobile body system 101 according to this embodiment. The mobile body system 101 is composed of an information processing device 103, a position and orientation measurement system 102 including a camera 104, a communication unit 105, a control device 106, and a wheel unit 107.

The information processing device 103 estimates the position and orientation of the moving object based on the input information input from the camera 104. In addition, when the moving object moves autonomously, the information processing device 103 issues a movement instruction to the control device 106. The detailed configuration of the information processing device 103 will be described later.

The camera 104 is fixed on the mobile system 101 and is capable of continuously acquiring grayscale luminance images. For example, it is a stereo camera. For ease of explanation, it is assumed that the internal parameters of the camera, such as the focal length and angle of view, are known, and that the image is output without any image distortion or with distortion corrected. The camera 104 also captures images at a frame rate of, for example, 30 times per second.

The communication unit 105 receives instructions from the user regarding the movement and rotation of the mobile system 601, and the start and end of the position and orientation measurement process performed by the information processing device 103.

The control device 106 drives the wheel unit 107 based on instructions from the information processing device 103 and the communication unit 105 to move and turn the mobile system 101.

The wheel section 107 is made up of multiple tires, some or all of which are connected to a power source.

(Configuration of information processing device)
The information processing apparatus 103 has the functions of a general embedded PC device, and is composed of a CPU 111, a ROM 112, a RAM 113, a storage unit 114 such as an HDD or SSD, a general-purpose I/F 115 such as a USB, and a system bus 116.

The CPU 111 uses the RAM 113 as a work memory to execute an operating system (OS) and various computer programs stored in the ROM 112, the storage unit 114, etc., and controls each unit via the system bus 116. For example, the programs executed by the CPU 111 include programs for executing the processes described below.

In addition, a sensor for measuring position and orientation (camera 104 in this embodiment), a communication unit 105, and a control device 106 are connected to the information processing device 103 via a general-purpose I/F 115.

(Functional configuration of information processing device)
The functional configuration of the information processing device according to this embodiment will be described below. The processing of each unit described below is implemented as software by loading a computer program from the ROM 112 or the like onto the RAM 113 and then executing the program by the CPU 111. A part or all of the functional configuration may be configured as hardware such as an ASIC or an FPGA.

Figure 2 is a block diagram showing the functional configuration of the information processing device 103.

The sensor input information acquisition unit 201 acquires sensor information from a sensor for measuring position and orientation (the camera 104 in this embodiment) via the general-purpose I/F 115. In this embodiment, it acquires image information.

The shock and vibration information acquisition unit 202 acquires information on the shock and vibration applied to the mobile system 101 based on sensor information acquired through the general-purpose I/F 115.

The position and orientation acquisition unit 203 calculates the position and orientation of the mobile system 101 at the time the sensor information is acquired, based on the sensor information acquired by the sensor input information acquisition unit 201 and the shock and vibration information acquired by the shock and vibration information acquisition unit 202. The calculated position and orientation information is output to the control device 106 and used for automatic driving control of the mobile system 101. Alternatively, the position and orientation information can be output to a display device (not shown) and used to display the current position of the mobile system on a UI. It is also possible to transmit the sensor information acquired by the sensor input information acquisition unit 201 to an external device via the communication unit 105, and have the position and orientation calculated by the external device be acquired by the position and orientation acquisition unit 205 via the communication unit 105.

(Position and orientation measurement processing)
Next, a position and orientation measurement process according to an information processing method using the information processing device of this embodiment will be described with reference to Fig. 3 to Fig. 6. The process shown in Fig. 3 and the flowcharts thereafter is realized by the CPU 111 reading out a program stored in the ROM 112 or the storage unit 114, calculating or processing information, and controlling each piece of hardware. Note that some or all of the steps shown in each flowchart may be realized by hardware such as an ASIC or FPGA.

Figure 3 is a flowchart showing the flow of position and orientation measurement processing in this embodiment.

In step S301, the information processing device 103 is initialized. The information processing device sets the initial position and orientation of the mobile system 101 with the current position as the origin in the environment and the current orientation as a specified direction (for example, the positive Y-axis direction). In addition, the sensor input information acquisition unit 201 acquires initial sensor information from each sensor (the camera 104 in this embodiment) and stores it in the work memory as "sensor information from the previous time" acquired when step S302 is executed for the first time.

The subsequent processing from step S302 to step S304 is repeated until an instruction to end position and orientation measurement is given by the user or the like. In the following explanation, one series of processing from step S302 to step S304 will be explained as processing at a certain time t. Here, t is an integer value, and increases by 1 each time a series of processing is performed. In addition, the following explanation assumes a situation in which the mobile body system 101 was traveling on a flat surface until time t = 1, and at time t = 2, an impact causes the attitude of each sensor to swing significantly upward. Specific examples of impact situations include a situation in which the mobile body runs over a step on a sidewalk, a situation in which the tires run over an obstacle such as a stone or branch, or a situation in which the mobile body is subjected to intermittent vibrations from cobblestones, etc.

In step S302, the sensor input information acquisition unit 201 acquires the latest sensor information from each sensor. It also acquires the sensor information from the previous time stored in the work memory.

In step S303, the shock and vibration information acquisition unit 202 acquires information about the shock and vibration applied to the mobile system 101. Details of the processing in step S303 will be described later.

In step S304, the position and orientation acquisition unit 203 calculates the position and orientation of the mobile system 101. Details of the processing in step S304 will be described later.

Step S305 is a step where it is determined whether or not to end the position and orientation measurement process. If the position and orientation measurement is to be ended due to a user instruction or the like, the process ends. If not, the process returns to step S302 and the next processing is performed.

(Details of shock vibration information acquisition process)
Next, the shock and vibration information acquisition process shown in S303 will be described in detail. In this embodiment, the magnitude and direction of the shock or vibration received by the mobile system 101 are analyzed based on image data acquired by the camera 104. Fig. 4 is a flowchart showing the flow of the shock and vibration information acquisition process in this embodiment.

In step S401, the impact vibration information acquisition unit 202 acquires image data acquired by the camera 104 from the latest and previous sensor information acquired in step S302.

In step S402, the impact vibration information acquisition unit 202 detects and tracks feature points between the two images acquired in step S401, and acquires the motion vectors of the feature points that were successfully tracked.

Figure 5 (a) shows the results of feature point tracking between an image captured at time t=1 and the image captured just before that (t=0). The penalty points in the figure indicate feature points that were successfully tracked, and the arrows accompanying the penalty points indicate the amount of movement (motion vector) of the feature point from the previous time.

Figure 5(b) illustrates the results of tracking feature points between the image captured at the next time t=2 and the image captured at the previous time (t=1). At time t=2, the image is blurred vertically as a result of an impact being applied to the camera 104 when the image was taken. Furthermore, as a result of the blurring of the image, some of the feature points that were successfully tracked in Figure 5(a) have failed to be tracked.

Figure 5 (c) shows the results of tracking feature points between the image taken at time t=2 and the image taken at time (t=1) after image blurring has been corrected.

In step S403, the impact vibration information acquisition unit 202 compares the group of motion vectors acquired in step S402 with the group of motion vectors calculated at the previous time, and analyzes the magnitude and direction of the impact applied to the mobile system 101. Specifically, it calculates the difference vector of the motion vectors of the feature points that were successfully tracked at both times, and acquires the average vector as the impact vector (magnitude and direction of the impact) at that time.

(Details of Position and Orientation Calculation Processing)
Next, the position and orientation calculation process shown in S304 will be described in detail. In this embodiment, the position and orientation of the mobile body system 101 is calculated based on image data acquired by the camera 104. The camera 104 is fixed on the mobile body system 101, and the relative position and orientation thereof are known and stored in the RAM 113, and the position and orientation values of the camera 104 and the mobile body system 101 are mutually convertible. For ease of explanation, in the following explanation, it is assumed that the position and orientation values of the camera 104 and the mobile body system 101 are the same.

Figure 6 is a flowchart showing the flow of the position and orientation calculation process in this embodiment.

Step S601 is a branch to determine whether image blur correction is required for position and orientation calculation. Here, if the magnitude of the impact vector acquired in step S403 is equal to or greater than a preset threshold, it is determined that image correction is required, and processing proceeds to step S602. The threshold for the magnitude of the impact vector is set so that it is determined that image correction is not required when the moving object is going up a slope or turning a curve. If image correction is not required, processing proceeds to S604. The method of determination here is not limited to the magnitude of the impact vector, and it is also possible to determine that image correction is required, for example, if the number or percentage of successfully tracked feature points is equal to or less than a threshold.

In step S602, the shock vibration information acquisition unit 202 performs image blur recovery processing on the latest image based on the shock vector acquired in step S403. Specifically, the acquired shock vector is first decomposed into vertical and horizontal components of the image, and the magnitude of the components in each direction is calculated. Next, the image is converted into spatial frequency components using a known two-dimensional Fourier transform. After that, for each spatial frequency, a recovery coefficient is obtained based on a two-dimensional lookup table prepared in advance, which takes the vertical frequency and the magnitude of the vertical component of the shock vector as input, and these are integrated. This recovers the frequency components reduced by the blur caused by the vertical vibration. The frequency components are similarly recovered in the horizontal direction. Finally, a two-dimensional inverse Fourier transform is performed on the spatial frequency component image with the frequency components restored, to obtain a luminance image with the frequency components restored.

In step S603, the position and orientation acquisition unit 203 uses the image corrected in step S602 to again detect and track feature points between the two images.

In step S604, the position and orientation acquisition unit 203 updates the current position and orientation of the mobile system 101. Specifically, if image correction has been performed, the position and orientation acquisition unit 203 calculates the position and orientation difference from the previous time of the mobile system 101 in step S603, or if not, based on the feature point detection result in step S402. Specifically, the position and orientation difference is calculated using a known bundle adjustment process. This is a process of assuming three-dimensional coordinates in space for at least eight or more feature points that have been successfully tracked, and optimizing the relative position and orientation between images and the three-dimensional coordinates of the feature points so that the difference (reprojection error) between the reprojection results on both images and the detected positions of the feature points on the images is reduced. In this case, if the three-dimensional coordinates of the feature points can be estimated from stereo distance measurement using a stereo camera or past feature point tracking results, they can be used as initial values or fixed values of the three-dimensional coordinates.

In step S605, the position and orientation acquisition unit 203 adds the position and orientation difference calculated in step S604 to the position and orientation at the previous time to calculate the position and orientation value at the latest time.

The above process allows position and orientation measurement to continue stably even if the camera used for position and orientation measurement is subjected to an impact.

Second Embodiment
In the first embodiment, an example was described in which a camera is used alone as a sensor for measuring position and orientation, and position and orientation measurement processing is performed according to impact information acquired from a camera image. In the present embodiment, an example will be described in which position and orientation measurement processing is performed by combining a plurality of sensors.

In this embodiment, position and orientation measurement is performed using a camera, an IMU, and tire drive amount as sensors for position and orientation measurement. These multiple measurement methods have different resistance to vibration and impact depending on their principles.

For example, the accuracy of a capacitance-type acceleration sensor used in some types of IMU decreases when it is subjected to stronger acceleration or continuous vibration than expected. The method of calculating the amount of movement by integrating the amount of tire drive assumes movement on a flat surface, so the accuracy decreases due to unevenness of the ground, and it becomes difficult to calculate the correct amount of movement, especially when the tire floats and spins due to a vertical impact such as a step. It is also difficult to calculate the correct amount of movement when the tire slips due to a strong lateral impact, but in this case, the relationship between the strength of the impact and the degree of decrease in the accuracy of the estimation of the amount of movement differs between the vertical and horizontal directions. Although the camera generates image blur as described in the first embodiment, it does not have a mechanical structure that is directly affected by impacts and vibrations, so it can be said to be a sensor that is relatively resistant to vibration and impacts.

In this embodiment, the contribution of sensors that are estimated to have high position and orientation measurement accuracy depending on the occurrence of vibration or impact is set relatively high, and the results of position and orientation measurement by multiple sensors are combined according to their contribution, thereby achieving highly accurate position and orientation measurement.

(Configuration of moving body system and position and orientation measurement system)
7 is a diagram showing an example of the configuration of a mobile body system 701 according to this embodiment. A position and orientation measurement system 702 according to this embodiment is composed of an information processing device 103, a camera 104, and an IMU 704. The hardware configuration of the information processing device 103 and the camera 104 are similar to those in the first embodiment, and therefore description thereof will be omitted. In this embodiment, the camera 104, the control device 106, and the IMU 704 function as sensors for position and orientation measurement.

The IMU 704 is an inertial sensor fixed on the mobile system 701. Like the camera 104, the IMU 704 is connected to the information processing device 103 via the general-purpose I/F 115. The IMU 704 acquires three-dimensional acceleration and angular velocity applied to the mobile system 701. However, the IMU 704 has the characteristic that the accuracy of acquiring acceleration and angular velocity decreases when a strong acceleration above a threshold is applied.

For ease of explanation, it is assumed that the IMU 704 is attached to the center of the mobile system 701, and in the following explanation, it is assumed that the position, orientation, acceleration, and angular velocity of the mobile system 701 are the same as those of the IMU 704.

In addition, both the camera 104 and the IMU 704 are fixed on the mobile system 701, and their relative positions and orientations are known and stored in the RAM 113. Therefore, the position and orientation information of the camera 104 and the position and orientation information of the IMU 704 (i.e., the position and orientation information of the mobile system 701) can be converted into each other. For ease of explanation, in the following explanation, the position and orientation information measured using the camera 104 is assumed to indicate values that have been converted into the position and orientation information of the IMU 704.

The functional configuration of the information processing device 103 according to this embodiment is similar to that of the first embodiment shown in FIG. 2, and therefore a detailed description thereof will be omitted. In addition, the basic flow of the position and orientation measurement process and the details of the shock and vibration information acquisition process are similar to those of the first embodiment shown in FIG. 3 and FIG. 4, and therefore a detailed description thereof will be omitted.

(Details of Position and Orientation Calculation Processing)
The position and orientation calculation process in step S304 in this embodiment is described in detail below. In this embodiment, the position and orientation of the mobile body system 701 are calculated based on data acquired from the camera 104, the control device 106, and the IMU 704.

Figure 8 is a flowchart showing the flow of the position and orientation calculation process in this embodiment. Furthermore, the processes from step S801 to step S803 are executed synchronously in parallel for each sensor.

In step S801, the position and orientation acquisition unit 203 calculates a position and orientation difference value from the previous time based on the camera 104, as in the first embodiment. This process is similar to the processes in steps S601 to S604 described in the first embodiment, so a detailed description will be omitted.

In step S802, the position and orientation acquisition unit 203 integrates the acceleration and angular velocity values obtained from when the camera 104 acquires the previous image to when it acquires the latest image, and calculates a position and orientation difference value based on the IMU.

In step S803, the position and orientation acquisition unit 203 integrates the amount of tire movement obtained by the camera 104 from when it acquires the previous image to when it acquires the latest image, and calculates a position and orientation difference value based on the amount of tire movement. This position and orientation difference value is calculated on the assumption that the mobile system 101 is traveling on a flat surface. Therefore, this position and orientation difference value is composed of a movement vector on an xy plane that corresponds to a flat surface such as a floor, and an amount of rotation about the z axis perpendicular to the plane.

In step S804, the position and orientation acquisition unit 203 determines the contribution of each sensor to be used when synthesizing the position and orientation difference values calculated based on each sensor. The contribution is calculated based on the impact vector calculated by the impact vibration information acquisition unit 203. Specifically, the acquired impact vector is decomposed into a vertical component and a horizontal component of the image, and the contribution value is determined based on the magnitude of each directional component and the calculation method set in advance for each sensor.

The contribution of each sensor is calculated using a predetermined function with the magnitude of the impact or vibration on each axis as an argument. In this embodiment, the camera 104 is considered to be more resistant to vibration and impact than other sensors, so it is used as the reference sensor for position and orientation measurement, and the contribution of the camera 104 is fixed at 1.

The accuracy of IMU704 decreases when an acceleration of a certain strength or greater is applied, regardless of the direction of the vibration or impact obtained from the image of camera104. Therefore, the contribution of IMU704 refers only to the magnitude of the impact vector, and uses a function such that the contribution is 1 when the magnitude of the impact vector is below threshold α1, 0 when it is above threshold α2, and decreases linearly between thresholds α1 and α2. The thresholds α1 and α2 are values defined according to the vibration resistance performance of the IMU used.

The position and orientation difference measurement based on the tire drive amount calculates the contribution rate for each of the vertical and horizontal components of the impact vector and adds them up. For the vertical direction, accuracy decreases due to unevenness of the ground, and accuracy decreases significantly especially when the impact causes the tires to spin. For this reason, a function is used in which the contribution rate is 1 when the vertical component of the impact vector is 0, and 0 when it is equal to or greater than threshold value β1, and the contribution rate decreases linearly between 0 and threshold value β. Threshold value β is a value defined as the magnitude of the vertical component of the impact vector when an impact equivalent to gravitational acceleration 1G is applied to the mobile system 101 in the vertical direction.

In the lateral direction, impacts that do not cause the tire to slip are thought to have no significant adverse effect on position and orientation measurement based on the drive amount, but if slippage occurs, accurate calculation of the position and orientation difference becomes impossible. Therefore, if the lateral component of the impact vector is less than the threshold γ, the contribution is set to 1, and if it is equal to or greater than the threshold γ, it is set to 0. The threshold γ is a value defined based on the assumed coefficient of friction between the tire and the ground.

In step S805, the position and orientation acquisition unit 203 calculates a position and orientation difference value of the mobile body system 701 based on the position and orientation difference values based on each sensor calculated in steps S801 to S803 and the contribution of each sensor calculated in step S804. Basically, the position and orientation difference calculated by each sensor is separated into position differences in the x-, y-, and z-axes and rotation amounts about the x-, y-, and z-axes, and a weighted average value is calculated for each, with the contribution of each sensor as a weighting factor. Here, since the position and orientation difference value based on the driving amount of the tires does not have information on the position difference in the z-axis direction and the rotation amounts about the x-, y-axes, the weights for these values are treated as 0.

In step S806, the position and orientation acquisition unit 203 adds the position and orientation difference calculated in step S805 to the position and orientation at the previous time, calculates the position and orientation value at the latest time, and determines the position and orientation.

By performing the above processing, even when performing position and orientation measurement processing by combining multiple sensors, it is possible to suppress deterioration of accuracy due to shocks and vibrations and perform stable position and orientation measurement.

[Modification]
In the first embodiment, a method of adding a position and orientation difference calculated from the feature point tracking results of a plurality of images to an initial position and orientation value is described as an example of a position and orientation measurement method using a camera, but the position and orientation measurement method using a camera is not limited to this. For example, a known position and orientation measurement method such as SLAM (Simultaneous Localization and Mapping) or SVO (Semi-Direct Monocular Visual Odometry) may be used.

A specific example of using SLAM will be described. The mobile system holds, as map information of the environment, 3D point cloud information indicating the shape features of objects in the environment in which the mobile body carrying the camera travels. The 3D point cloud information is the 3D coordinates in world coordinates of image feature points. Map information of the environment is generated in advance based on an image capturing the shapes of objects existing in the environment and is stored in a storage device. Based on the input image and map information, the position and orientation acquisition unit 203 minimizes the difference (reprojection error) between the positions of feature points (edge features indicating shape) on the input image and the positions when the feature points on the map are projected onto the input image using the position and orientation of the camera when the input image was captured. This estimates the position and orientation of the camera.

The scope of this invention also includes environmental map creation systems that use these techniques to create an environmental map of the surroundings of a moving object at the same time as measuring its position and orientation.

In the second embodiment, a method was described in which position and orientation measurements were performed based on the respective sensors of the camera, IMU, and tire drive amount, and the results were synthesized based on vibration and impact information, but the sensors that can be used are not limited to these. For example, LiDAR (Laser Imaging Detection and Ranging) may be added to the sensor or some may be replaced. In general, mirror rotation type LiDAR has a mechanical rotation mechanism, so its accuracy is easily reduced by impact, and it is better to set a lower contribution level than the camera when vibration or impact occurs. On the other hand, MEMS (Micro Electro Mechanical Systems) type LiDAR does not have a mechanical mechanism. Since image blurring like a camera is small, a higher contribution level than the camera is set for strong impacts.

Alternatively, position information obtained by GNSS (Global Navigation Satellite System) may be used, or position and orientation values calculated from the results of observing the mobile system with a sensor such as a camera installed outside the mobile body may be used. For example, a method is conceivable in which video from a surveillance camera capturing the mobile system while it is moving is processed on a server machine, and the position and orientation information of the mobile system recognized in the video is calculated and transmitted to the mobile system via a communication unit.

In the second embodiment, the contribution of each sensor was calculated based on a predetermined formula. However, the method of calculating the contribution is not limited to this, and any method may be used as long as it sets a relatively high contribution to a sensor with high position and orientation measurement accuracy estimated from the acquired vibration and impact information. For example, a two-dimensional lookup table may be used in which the magnitudes of the vertical and horizontal components of the impact vector are input. Alternatively, the contribution may be determined based not only on the impact vector acquired at the relevant time, but also on the history of impact vectors at each recent time.

Alternatively, the degree of contribution may be determined by taking into account information other than vibration and impact information. For example, the degree of contribution may be set lower by considering that the accuracy of tire drive amount and position and orientation measurement based on GNSS is relatively lower than other sensors even in ideal conditions. Alternatively, in a vehicle with front and rear wheels, a certain amount of tire slippage occurs during steering operation, so that the degree of contribution of position and orientation measurement based on tire drive amount may be reduced while control is being performed to turn the moving body.

In the first and second embodiments, the results of tracking feature points in a camera image are used as a means for acquiring shock vibration, but the means for acquiring shock vibration that can be used in the present invention are not limited to this. Also, in the first and second embodiments, in addition to the magnitude of the shock or vibration, the direction is acquired in two axes, but acquiring the direction of the shock or vibration is not essential, and the format in which it is acquired is also not limited to this. For example, acceleration values may be acquired in three axes from the IMU. Alternatively, only the magnitude of the shock or vibration may be acquired.

In addition, shock and vibration information may be obtained using information other than that of a sensor on the mobile system. For example, video of an external device such as a security camera capturing a moving mobile system from a position where the mobile system can be observed may be processed on a server machine, and the degree of shaking of the mobile system recognized in the video may be analyzed and transmitted to the mobile system via a communication unit. The degree of vibration may be obtained by tracking feature points obtained by analyzing the video. The method of tracking the feature points may be the method described in step S402. Alternatively, the magnitude of vibration or impact predicted in accordance with position and orientation information may be obtained from electronic map data stored on an information processing device. Specifically, position information of steps and rough roads recorded on a map is obtained, and position and orientation measurement is performed assuming that a predetermined magnitude of shock or vibration will occur at that location.

When performing blur correction processing on an image as in the first embodiment, if three-axis shock or vibration information is acquired, the three-axis information can be projected onto the image plane and converted into two-axis shock or vibration information, allowing blur correction processing to be performed in the same manner as in the first embodiment. Alternatively, if only information on the magnitude of the shock or vibration is acquired, blur correction processing is performed without distinguishing between directions on the image.

When the contribution of each sensor is set according to vibration or impact information as in the second embodiment, the contribution may be calculated using a function or lookup table according to the number of axes of the acquired vibration or impact information. Alternatively, the contribution may be calculated based on vibration or impact information converted into a smaller number of axes.

In the first and second embodiments, a mobile body system having multiple tires as the wheel portion has been described as an example, but the wheel portion of the mobile body system is not limited to this. For example, the mobile body system may be a flying object such as a multicopter, or a walking mobile body with multiple legs. It may also be a manned mobile body that a user can board and directly operate. It may also be a mobile body equipped with the function of autonomously traveling on a pre-set route.

The present invention can also be realized by supplying a program that realizes one or more of the functions of the above-mentioned embodiments to a system or device via a network or storage medium, and having one or more processors in the computer of the system or device read and execute the program. It can also be realized by a circuit (e.g., an ASIC) that realizes one or more functions.

REFERENCE SIGNS LIST 101 Mobile system 102 Position and orientation measurement system 103 Information processing device 104 Camera 105 Communication unit 106 Control device 107 Wheel unit 201 Sensor input information acquisition unit 202 Impact vibration information acquisition unit 203 Position and orientation acquisition unit

Claims (10)

An input information acquisition means for acquiring input information from a sensor mounted on the moving object;
a vibration information acquiring means for acquiring vibration information including a magnitude and a direction of vibration received by the sensor due to vibration of the moving body;
and a position and orientation acquisition means for acquiring a position and orientation of the moving body based on corrected input information obtained by correcting the input information acquired by the input information acquisition means using the vibration information when the sensor is subjected to vibration exceeding a threshold value . An input information acquisition means for acquiring input information from a sensor mounted on the moving object;
a vibration information acquiring means for acquiring vibration information including a magnitude and a direction of vibration received by the sensor due to vibration of the moving body;
a position and orientation acquisition means for acquiring a position and orientation of the moving object based on input information corrected by using the vibration information,
The information processing device, characterized in that the vibration information acquisition means acquires the vibration information by tracking feature points obtained from image information of the moving object captured by an external device capable of observing the moving object. An input information acquisition means for acquiring input information from a sensor mounted on the moving object;
a vibration information acquiring means for acquiring vibration information including a magnitude and a direction of vibration received by the sensor due to vibration of the moving body;
a position and orientation acquisition means for acquiring a position and orientation of the moving object based on input information corrected by using the vibration information,
the input information acquisition means acquires image information from an image sensor mounted on the moving object;
the vibration information acquiring means acquires the vibration information by tracking feature points obtained from the image information. The information processing device according to claim 3, characterized in that, when the proportion of the trackable feature points is equal to or less than a threshold value, the position and orientation acquisition means acquires the position and orientation of the moving body based on corrected input information obtained by correcting the input information using the vibration information. The mobile device further includes a means for acquiring map information of an environment in which the mobile object moves,
5. The information processing apparatus according to claim 1 , wherein the vibration information acquisition means acquires the vibration information based on environmental information included in the map information. An input information acquisition means for acquiring input information from each of a plurality of sensors mounted on the moving object;
a vibration information acquiring means for acquiring vibration information including a magnitude and a direction of the vibration received by the moving body;
and a position and orientation determining means for determining a position and orientation of the moving body by weighting the position and orientation of the moving body acquired using each of the input information based on the vibration information. 2 . The information processing apparatus according to claim 1 , wherein the input information acquisition means acquires image information from an image sensor mounted on the moving body, and the position and orientation acquisition means performs blur correction processing on the image information as a correction for the input information . An input information acquisition step of acquiring input information from a sensor mounted on the moving object;
a vibration information acquiring step of acquiring vibration information including a magnitude and a direction of vibration received by the sensor due to vibration of the moving body;
and a position and orientation acquisition step of acquiring a position and orientation of the moving body based on corrected input information obtained by correcting the input information acquired in the input information acquisition step using the vibration information when the sensor is subjected to vibration exceeding a threshold value . An input information acquisition step of acquiring input information from each of a plurality of sensors mounted on the moving object;
a vibration information acquiring step of acquiring vibration information including a magnitude and a direction of vibration received by the sensor due to vibration of the moving body;
and a position and orientation determination step of determining a position and orientation of the moving body by weighting the position and orientation of the moving body obtained using each of the input information based on the vibration information. A program that causes a computer to function as each of the means of an information processing device according to any one of claims 1 to 7.

Images