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

Description

The present invention relates to technology for estimating the position of a moving object from measurement information.

There is technology for estimating the position of a mobile object equipped with a sensor that measures the distance between the sensor and objects in the environment, based on the measurement information obtained from the sensor. By equipping a mobile object with multiple sensors and switching between the positions of the mobile object estimated by each sensor depending on the situation, the position of the mobile object can be estimated with greater accuracy. Since the positions estimated by each sensor are estimated independently, it is necessary to ensure consistency between the positions of the sensors, i.e., to use them to control the movement of the mobile object. In Non-Patent Document 1, the relative positions of the camera and laser sensor are measured in advance, allowing the position and orientation measured by the camera and laser to be handled in the same coordinate system.

W. Dong, et al. , “A Novel Method for the Extrinsic Calibration of a 2D Laser Rangefinder and a Camera,” ICRA’17, 2017.

However, even if the sensor placement relationship is calibrated using the method in Non-Patent Document 1, differences in position and orientation will occur due to measurement errors from each sensor. Attempting to eliminate differences that do not actually exist can result in unstable movement of the moving object. The present invention was made in light of the above issues, and aims to enable stable movement of a moving object when switching between position and orientation estimated from measurement values from two or more sensors.

An information processing device according to the present invention has the following configuration: first position information acquisition means for acquiring first position information of a moving body estimated based on input information from a first sensor fixed to the moving body, second position information acquisition means for acquiring second position information of the moving body estimated based on input information from a second sensor fixed to the moving body, selection means for selecting, from the first position information and the second position information in the same coordinate system, the first position information as position information for controlling movement of the moving body when the first position information is acquireable, and selecting the second position information as position information for controlling movement of the moving body when the first position information is not acquireable , difference acquisition means for acquiring difference information in the same coordinate system between the first position information and the second position information synchronized with the first position information, and update means for, when the position information selected by the selection means is switched from the first position information to the second position information , updating the second position information by adding the difference information acquired by the difference acquisition means at the time of switching until the first position information selected before the switching is reselected .

According to the present invention, it is possible to stably move a moving object when switching between position and orientation estimates based on measurement values from two or more sensors.

A diagram showing an example of the system configuration of an information processing system. A block diagram showing an example of the functional configuration of an information processing device. FIG. 1 is a diagram illustrating an example of a hardware configuration of an information processing device. 10 is a flowchart illustrating a process executed by an information processing device. 10 is a flowchart illustrating a process executed by a difference acquisition unit. 10 is a flowchart illustrating a process executed by an information processing device. 10 is a flowchart illustrating a process executed by an information processing device.

<Embodiment 1>
In this embodiment, an information processing system including a mobile object equipped with an image sensor and a range sensor will be described. Specifically, the mobile object is assumed to be an automated guided vehicle (or a self-driving vehicle). This system estimates the position and orientation of the mobile object using two methods, based on input information from an image sensor and a range sensor. Estimating the two methods is not always possible. If one method fails to estimate the position and orientation, the other method is used to control the movement of the mobile object. Inputting the results of one method of position and orientation estimation enables the movement of the mobile object. In this embodiment, the position and orientation estimated from the image sensor is primarily used as a priority for controlling the movement of the mobile object. When the position and orientation cannot be estimated based on the input information from the image sensor, the position and orientation estimated based on the input information from the range sensor is used. Conversely, the position and orientation estimated by the range sensor can also be prioritized. The priority of the position and orientation estimated by the sensor is determined in advance based on the reliability of the position and orientation estimated by the sensor. In this embodiment, the position and orientation estimated by the image sensor is prioritized. Switching the sensor acquiring the position and orientation estimation results from an image sensor to a range sensor while a moving object is moving can cause a discrepancy in the acquired position and orientation. This can lead to unexpected behavior, such as a sudden change in direction as the moving object attempts to return to the position and orientation before the switch. Therefore, this embodiment describes a method for acquiring the difference between the position and orientation between two sensors and correcting the position and orientation at the time of switching based on the difference at the time of switching. The position and orientation estimated by the image sensor before the switch is calculated using the difference between the position and orientation estimated by the range sensor after the switch. It is assumed that the positional relationship between the image sensor and the range sensor is determined in advance, and that the two positions and orientations are converted to positions and orientations in the same coordinate system based on this positional relationship for use in controlling the movement of the moving object. It is not necessary to convert both of the coordinate systems of the two positions and orientations; it is sufficient that each is acquired as position information in the same coordinate system.

A preferred embodiment of the present invention will now be described with reference to the accompanying drawings.

Figure 1 shows a system configuration diagram of a mobile body in this embodiment. The mobile body 10 in this embodiment is composed of an image sensor 12 and a distance sensor 13 as sensors for estimating the position and orientation, an information processing device 11, and a control device 14 that controls the movement of the mobile body. The image sensor 12 and distance sensor 13 are fixedly attached to the mobile body.

Here is a specific example of the position and orientation obtained from the input information from the image sensor 12 and distance sensor 13 in this embodiment. These are six parameters: three parameters (X, Y, Z) that represent the position of the sensor in an arbitrary world coordinate system defined in real space, and three parameters (Roll, Pitch, Yaw) that represent the orientation of the sensor.

Figure 2 is a diagram showing an example configuration of a mobile system equipped with an information processing device 11 in this embodiment.

The information processing device 11 is composed of a first location information acquisition unit 111, a second location information acquisition unit 112, a selection unit 113, a difference acquisition unit 114, a difference storage unit 115, and a correction unit 116. The first location information acquisition unit 111 is connected to a first estimation unit 121, which is connected to the image sensor 12. The second location information acquisition unit 112 is connected to a second estimation unit 131, which is connected to the distance sensor 13. The correction unit 116 is connected to the control device 14.

The first estimation unit 121 estimates position information based on input information obtained from the image sensor 12 and outputs the estimation result as position information to the first position information acquisition unit 111. The image sensor 12 is, for example, a stereo camera, and acquires image information as input information. The first estimation unit 121 performs stereo measurement of feature points in the image and estimates three-dimensional position information of the feature points. Hereinafter, for convenience, the input information from the image sensor 12 before stereo measurement is performed will be described as measurement information or measurement values.

The second estimation unit 131 estimates position information based on input information obtained from the distance sensor 13 and outputs the estimation result as position information to the second position information acquisition unit 112. The distance sensor 13 measures the distance between the sensor and an object present in the environment, and obtains the distance information as input information. For example, this is realized by a LiDAR or ToF sensor. Hereinafter, for convenience, the input information from the distance sensor 13 will be described as measurement information or measurement value.

The first location information acquisition unit 111 acquires the location information input by the first estimation unit 121 and outputs the location information to the selection unit 113 and the difference acquisition unit 114.

The second location information acquisition unit 112 acquires the location information input by the second estimation unit 131 and outputs the location information to the selection unit 113 and the difference acquisition unit 114.

The selection unit 113 selects either the location information input by the first location information acquisition unit 111 or the second location information acquisition unit 112, and outputs the selection result and the selected location information to the correction unit 116.

The difference acquisition unit 114 acquires the difference between the two pieces of location information from the location information input by the first location information acquisition unit 111 and the second location information acquisition unit 112, and outputs this as difference information to the difference holding unit 115. The location information input by the first location information acquisition unit 111 and the second location information acquisition unit 112 is converted into location information in the same coordinate system.

The difference storage unit 115 stores the difference information input by the difference acquisition unit 114.

The correction unit 116 corrects the position information input by the selection unit 113 based on the difference information stored in the difference storage unit 115. The corrected position information is output to the control device 14.

In addition, the mobile object 10 can be equipped with an information processing device 11, or an information processing device 11 on the cloud can control the mobile object 10.

Figure 3 shows the hardware configuration of the information processing device 11. H11 is a CPU that controls various devices connected to the system bus H21. H12 is a ROM that stores BIOS programs and boot programs. H13 is a RAM that serves as the main storage device for the CPU H11. H14 is an external memory that stores programs processed by the information processing device 11. The input unit H15 includes a keyboard, mouse, or robot controller, and performs processing related to the input of information, etc. The display unit H16 outputs the results of calculations performed by the information processing device 11 to a display device in accordance with instructions from H11. The display device may be of any type, such as an LCD display, projector, or LED indicator. H17 is a communication interface that communicates information over a network. The communication interface may be Ethernet, or may be USB, serial communication, wireless communication, or any other type. H17 is an I/O that receives image data from the image sensor H18 and distance data from the distance sensor H19. Note that image sensor H18 and distance sensor H19 refer to the image sensor 12 and distance sensor 13 mentioned above. H20 refers to the control device 14 mentioned above.

The CPU can function as various means by executing a program. A control circuit such as an ASIC that operates in cooperation with the CPU can also function as these means. These means can also be realized through cooperation between the CPU and a control circuit that controls the operation of the image processing device. The CPU does not need to be a single CPU; multiple CPUs can also be used. In this case, the multiple CPUs can execute processing in a distributed manner. The multiple CPUs can be located in a single computer, or in multiple physically separate computers. The means realized by the CPU executing a program can also be realized by dedicated circuits.

Figure 4 is a flowchart showing the processing procedure in this embodiment. The processing of this flowchart begins when an instruction to start driving the AGV is given. Hereinafter, the flowchart will be realized by the CPU executing a control program.

In step S101, the system is initialized. That is, a program is read from external memory H14, and the information processing device 11 is put into an operable state. Furthermore, parameters for each device connected to the information processing device 11 are written to RAM H13. Furthermore, each control device of the mobile object is started up, and put into an operable and controllable state.

In step S102, the first position information acquisition unit 111 acquires the position and orientation estimated by the first estimation unit 121 based on the input information acquired from the image sensor 12. In this embodiment, the position and orientation estimation method using an image sensor is SLAM (Simultaneous Localization and Mapping) technology, the Engel et al. method (J. Engel, T. Schps, and D. Cremers. LSD-SLAM: Large-Scale Direct Monocular SLAM. In European Conference on Computer Vision (ECCV), 2014). This method simultaneously performs self-localization and map creation in a wide-area environment such as outdoors. However, when using this technology, position and orientation estimation may fail depending on the sensor measurement values. Specifically, position and orientation estimation fails when image feature points (such as edge intersections and corners) required for position and orientation estimation are not sufficiently included in the image acquired from the image sensor. For example, position and orientation estimation fails when an all-black image acquired when the lens of the image sensor is blocked or an image showing a plain white wall is captured in its entirety. When position and orientation estimation fails, position and orientation values cannot be obtained. If position and orientation estimation fails, the first position information acquisition unit 111 acquires the position and orientation from the time before the failure and outputs all positions and orientations after system initialization to the selection unit 113 and difference acquisition unit 114. Note that the first position information acquisition unit 111 can also output positions and orientations for a predetermined period going back from the time when position and orientation estimation failed to the selection unit 113 and difference acquisition unit 114. The difference acquisition unit 114 acquires the difference, and the correction unit 116 narrows down the output to the position and orientation for the period required to correct the position information, thereby reducing the exchange of information between functional components.

As described above, the timing for outputting the position and orientation to the selection unit 113 and difference acquisition unit 114 can be the timing after the first position information acquisition unit 111 fails to estimate the position and orientation. Furthermore, the position and orientation can be continuously output to the selection unit 113 and difference acquisition unit 114 after system initialization.

Note that position information is array data, with each element consisting of a pair of six position and orientation parameters and time data.

In step S103, the second position information acquisition unit 112 acquires the position and orientation estimated by the second estimation unit 131 based on the measurement information acquired from the distance sensor 13. The position and orientation estimation method using a distance sensor uses SLAM technology using a 3D laser scanner (Ji Zhang and Sanjiv Singh, "LOAM: Lidar Odometry and Mapping in Real-time", In Robotics: Science and Systems (RSS), 2014). If the second position information acquisition unit 112 also fails to estimate the position and orientation, it acquires the position and orientation at the time before the failure and outputs all positions and orientations after system initialization to the selection unit 113 and difference acquisition unit 114. Additionally, the position and orientation for a predetermined period going back from the time when the second position information acquisition unit 112 failed to estimate the position and orientation can also be output to the selection unit 113 and the difference acquisition unit 114.

As described above, the timing for outputting the position and orientation to the selection unit 113 and difference acquisition unit 114 can be the timing after the second position information acquisition unit 112 fails to estimate the position and orientation. Furthermore, the position and orientation can be continuously output to the selection unit 113 and difference acquisition unit 114 after system initialization.

In this embodiment, the image sensor 12 and distance sensor 13 operate in time synchronization, and the position and orientation are estimated using measurement values taken at the same time as input.

In step S104, the selection unit 113 selects either the position information input by the first position information acquisition unit 111 or the second position information acquisition unit 112. In this embodiment, the selection basically gives priority to the position information input by the first position information acquisition unit 111. If the position information from the first position information acquisition unit 111 does not include the position and orientation at the current time, i.e., if position and orientation estimation using the measurement values of the image sensor 12 has failed, the selection unit 113 selects the position information input by the second position information acquisition unit 112. The selection result and the selected position information are output to the difference acquisition unit 114 and the correction unit 116.

In step S105, the difference acquisition unit 114 calculates the difference between the two pieces of position information input from the first position information acquisition unit 111 and the second position information acquisition unit 112, and outputs the difference information to the difference storage unit 115. Step S105 does not depend on the processing of step S104, and can start execution once the processing of steps S102 and S103 has finished. The difference information is six position and orientation parameters. A detailed method for calculating the difference will be described later. The two pieces of position information are position information after being converted into the same coordinate system.

In step S106, the correction unit 116 corrects the position information input by the selection unit 113 based on the difference information stored in the difference storage unit 115. The detailed correction method will be described later. The corrected position information is output to the control device 14, thereby controlling the movement of the automated guided vehicle.

In step S107, it is determined whether or not to terminate the system (stop the moving object). Specifically, the system terminates when an instruction to terminate is received from the user via input unit H15. If not, the system returns to step S102 and continues processing.

Figure 5 is a flowchart showing the processing steps for the difference acquisition method in step S105.

In step S201, a combination of position and orientation to be input for calculating the difference is selected from the position information input from the first position information acquisition unit 111 and the second position information acquisition unit 112. In this embodiment, a combination of position and orientation at the latest time and including both the first position information acquisition unit 111 and the second position information acquisition unit 112 is selected.

In step S202, a relative position and orientation is calculated from the two position and orientations selected in step S201. Specifically, the six position and orientation parameters acquired from the first position information acquisition unit 111 and the second position information acquisition unit 112 are divided into translational components (X, Y, Z) and rotational components (Roll, Pitch, Yaw), and the relative position and relative orientation are calculated from each component. The relative position is calculated by subtracting, for each parameter, the translational components of the position and orientation acquired by the second position information acquisition unit 112 from the translational components of the position and orientation acquired by the first position information acquisition unit 111. The relative orientation is calculated by converting the rotational components into a 3 × 3 rotation matrix. If the rotation matrix converted from the position and orientation acquired by the first position information acquisition unit 111 is R _V and the rotation matrix converted from the position and orientation acquired by the second position information acquisition unit 112 is R _L , a transformation matrix M is calculated such that R _V = M R _L. This transformation matrix M represents the relative orientation. The calculated relative position and relative orientation are output as difference information to the difference holding unit 115. The difference held in the difference holding unit 115 is, for example, a difference due to a measurement error between the image sensor 12 and the distance sensor 13. In the real world, when the selection unit 113 switches the selection from the first position information acquisition unit 111 to the second position information acquisition unit 112, no difference occurs in the position and orientation of the moving object.

The difference acquisition process procedure described above is executed when position and orientation estimation based on the measurement values of the image sensor 12 fails, that is, when the selection unit 113 switches selection from the first position information acquisition unit 111 to the second position information acquisition unit 112. On the other hand, when position and orientation estimation using the image sensor 12 is successful again, that is, when the selection unit 113 switches selection from the second position information acquisition unit 112 to the first position information acquisition unit 111, correction is no longer necessary, so a procedure is executed to reset the difference information to its initial value. The initial values are such that all components of the relative position (X, Y, Z) are zero, and the rotation matrix of the relative orientation is a unit matrix.

Next, we will explain the processing procedure for the correction method in step S106.

The position and orientation input by the selection unit 113 to the correction unit 116 are corrected by adding the relative position and relative orientation held by the difference holding unit 115. Specifically, the correction is also divided into translational components and rotational components, as in the case of obtaining the difference, and each component is calculated separately. The translational components are calculated by adding the relative position included in the difference information to the translational components of the position and orientation input by the selection unit 113 for each parameter. The rotational components are calculated by converting the rotational components of the position and orientation input by the selection unit 113 into a 3 × 3 rotation matrix R _S , and calculating the matrix product _MRS with the relative orientation included in the difference information. The position and orientation corrected as described above is output to the control device 14 as position information.

In a system that uses position and orientation estimated from two types of sensor values to control the movement of a moving object, the system switches from the position and orientation estimated using input information from an image sensor to the position and orientation estimated using input information from a distance sensor. The difference between the position and orientation estimated by the image sensor (first sensor) and the distance sensor (second sensor) is obtained, and the position and orientation after switching is corrected based on that difference. The position and orientation that would have been estimated by the image sensor before switching is calculated using the position and orientation estimated by the distance sensor after switching and the difference. This prevents the moving object from becoming unstable even if a discrepancy occurs between the position and orientation estimated by each system before and after switching. The position and orientation of the moving object corrected in S106 approaches the position and orientation estimated by the sensor before switching (the difference becomes smaller), making it easier to maintain continuity. Note that the information processing device 11 of this embodiment can control not only AGVs, but also unmanned aerial vehicles such as drones equipped with image sensors and distance sensors.

Furthermore, if position and orientation estimation based on the image sensor (first sensor) that had previously failed becomes successful again, the position and orientation can be corrected when switching to position and orientation estimation based on the image sensor measurement values. We describe a method for determining the period until the switch from position and orientation estimation based on the range sensor measurement values to position and orientation estimation based on the image sensor measurement values is completed, and integrating the position and orientation using a weighted average so that the switch can be made gradually within that period.

In addition to the functional configuration of the information processing device 11 described in Figure 2, it further includes a period determination unit, a weight determination unit, and an integration unit.

The period determination unit determines the period until switching from the second location information acquisition unit 112 to the first location information acquisition unit 111 is completed, and outputs this as period information to the weight determination unit.

The weight determination unit determines weights using the period information input by the period determination unit and outputs them to the integration unit.

The integration unit integrates the two pieces of location information acquired by the first location information acquisition unit 111 and the correction unit 116 based on the weights input by the weight determination unit.

Figure 6 is a flowchart showing the processing procedure in this embodiment. Explanations of steps that are the same as those in Figure 4 will be omitted, and only different processing procedures will be explained.

In step S301, if the selection result stored in RAM H13 is the second position information acquisition unit 112 and the selection result input by the selection unit 113 is the first position information acquisition unit 111, the process proceeds to S302. In other words, if the sensor used as input for position and orientation estimation is to be switched from the distance sensor 13 to the image sensor 12, the process proceeds to S302. If not, the process skips to S303.

In step S302, the period determination unit determines the period until the switch from position and orientation estimation based on the measurement values of the distance sensor (second sensor) to position and orientation estimation based on the measurement values of the image sensor (first sensor) is completed. In this embodiment, this period is set to 10 iterations (one iteration is one position and orientation output).

In step S303, the weight determination unit determines weights for the position and orientation estimated from the measurement values of the image sensor 12 and the position and orientation estimated from the measurement values of the range sensor 13, based on the period information input by the period determination unit. The weight for the position and orientation of the image sensor is set to 0 at the start of the period over 10 iterations and is intended to gradually increase toward the end of the period. Therefore, if the weight for the position and orientation of the image sensor is W _C (0≦W _C ≦1), W _C = (current number of iterations)/10. On the other hand, if the weight for the position and orientation of the range sensor is W _L (0≦W _L ≦1), W _L = 1 − W _C because it is necessary to maintain consistency with the weight for the position and orientation of the image sensor. The weight determination unit saves the current number of iterations in H13, which is a RAM, and gradually increases the weight of the image sensor relative to the range sensor by adding 1 each time step S303 is executed. For example, the weight of the position and orientation estimated from the measurement values of the distance sensor 13 is 1 at the start of the period, and the weight of the position and orientation estimated from the measurement values of the image sensor is 1 at the end of the period.

In step S304, the integration unit integrates the position and orientation estimated from the measurement values of the image sensor 12 and the position and orientation estimated from the measurement values of the distance sensor 13 using a weighted average based on the weights input by the weight determination unit. Specifically, the integrated position and orientation is determined by calculating a weighted average for each of the six position and orientation parameters.

In this way, after switching the sensor used for estimation once, when switching back to the original sensor, a period until the switching is complete is determined, and the position and orientation are integrated using a weighted average so that the switching can be performed gradually within that period. This makes it possible to prevent the movement of the moving object from becoming unstable due to position and orientation deviations occurring when switching.

<Modification 1-1>
In the first embodiment, the first position information acquisition unit 111 acquires the position and orientation estimated by the SLAM technology using the image sensor 12. On the other hand, the second position information acquisition unit 112 acquires the position and orientation estimated by the SLAM technology using the distance sensor 13. However, the sensors combined with the SLAM technology are not limited to these, and any sensor may be used. The position and orientation may be estimated by combining the SLAM technology with an active distance sensor such as time of flight or pattern light projection.

The estimation method is not limited to SLAM technology; the technical concept of this embodiment can be applied to any method that can estimate position and orientation. For example, it may be a method based on measuring radio wave strength in wireless communication such as Wi-Fi. It may also be a method using satellite positioning such as GPS, a magnetic tracking sensor, or a method using the time difference between detections by multiple optical sensors. It may also be a method of model fitting based on CAD data using images acquired by a camera installed in a specific location, such as a surveillance camera.

<Modification 1-2>
In the first embodiment, the position information acquired by the first position information acquisition unit 111 and the second position information acquisition unit 112 includes six position and orientation parameters. However, this is not limited to this, and the technical concept of this embodiment can be applied as long as information representing the position and orientation is included. For example, the position and orientation may be a total of three parameters indicating two-dimensional position and orientation: two position parameters (position X, Y on a plane horizontal to the floor in real space) and one orientation parameter (rotation direction on a plane horizontal to the floor in real space). Alternatively, the position and orientation may be only position parameters without including the orientation parameter. Furthermore, the position and orientation may be estimated based on acceleration and angular velocity acquired by an IMU (Inertial Measurement Unit), or by odometry based on the rotation angle of the tires.

<Modification 1-3>
In the first embodiment, the difference acquisition unit 114 selects one combination of position and orientation to be used for difference acquisition. However, this is not limiting, and multiple combinations may be selected as input for difference acquisition. When multiple position and orientation combinations are input, a method for calculating the difference is used, which can determine the relative positional relationship. Examples of difference calculation methods include a method that uses the least squares method to minimize the variation in relative value orientations between two systems of position and orientation, and a method that calculates an average value for each position and orientation parameter.

In addition, in the first embodiment, the image sensor 12 and the distance sensor 13 operate in time synchronization, and the position and orientation are estimated using measurement values at the same time as input. However, this is not limited to this, and the position and orientation may be estimated at each timing without time synchronization. In such cases, the following modified method is used to select the combination of position and orientation to be used for difference calculation in the difference acquisition unit 114. For example, the position and orientation estimated from the measurement values of the distance sensor 13 at the time closest to the time of the position and orientation estimated from the measurement values of the image sensor 12 is selected. Furthermore, there may be cases where the position and orientation has not been estimated from the measurement values of the distance sensor 13 at a time close to the position and orientation to be used for difference acquisition estimated from the measurement values of the image sensor 12. In such cases, the position and orientation at the desired time may be obtained by interpolation or prediction based on the position and orientation of the distance sensor 13 at another time.

Furthermore, in embodiment 1, the difference acquisition unit 114 performs difference calculation only when switching from the first position information acquisition unit 111 to the second position information acquisition unit 112. However, this is not limited to this, and difference calculation may be performed at other times. For example, after switching to the second position information acquisition unit 112, a long distance may be traveled until position and orientation estimation using the image sensor 12 is again successful. In this case, the amount of deviation between the position and orientation acquired by the first position information acquisition unit 111 and the second position information acquisition unit 112 may become larger or smaller than the amount of difference calculated at the time of switching. Therefore, difference calculation may be performed again to update the difference information. Furthermore, the difference information may be updated at regular distance intervals or regular time intervals, and the interval parameter may be determined by input by the user via the input unit H15.

<Modification 1-4>
In the first embodiment, the mobile body system 10 is not limited to an automated guided vehicle (mobile body). For example, the mobile body system 10 may be a self-driving car, an autonomous mobile robot, or an unmanned aerial vehicle.

<Modification 1-5>
In the first embodiment, the mobile body system 10 uses the position and orientation to control the movement of the mobile body. However, this is not limiting and the position and orientation may be used for purposes other than controlling the movement of the mobile body. For example, the position and orientation may be applied to technologies such as MR (Mixed Reality), VR (Virtual Reality), and AR (Argumented Reality) to estimate the user's viewpoint position and orientation.

<Modification 1-6>
The information processing device 11 may be configured to include a presentation unit (not shown). For example, if the presentation unit uses three color lights of red, yellow, and green, the green light is lit when the selection unit 113 selects the first position information acquisition unit 111. The yellow light is lit when the selection unit 113 selects the second position information acquisition unit 112. The red light is lit when switching from the first position information acquisition unit 111 to the second position information acquisition unit 112. The selection result can be visualized to the user. Any device can be used, such as an LED lamp or a liquid crystal display, as long as it can present information about the position and orientation selection result to the user. The presentation device may be a speaker, or may be configured to play a specific alarm sound or a specific melody depending on the position and orientation selection result. Furthermore, the presented information may be information other than the position and orientation selection result, such as difference information stored in the difference storage unit 115.

<Modification 1-7>
In the above-described embodiment, when position and orientation estimation based on the measurement values of an image sensor fails and the sensor used for estimation is switched to a range sensor, the difference between the position and orientation based on the two sensors is calculated and correction is performed using this difference. Furthermore, when the sensor used for estimation is switched back to the image sensor (first sensor), the two positions and orientations are integrated to suppress position and orientation deviation during switching. However, the present invention can also be applied when position and orientation estimation is successful using both of the two sensors. Therefore, a method for automatically switching the input sensor according to the priority associated with the position and orientation when both of the two sensors are successful in position and orientation estimation will be described.

Only the processing steps that differ from those described using Figure 4 will be explained; other steps are the same as those described using Figure 4 and will not be explained again.

In step S102, the first position information acquisition unit 111 and the second position information acquisition unit 112 acquire the reliability of the position information in addition to the position information. Note that reliability is an index that indicates the accuracy of the position and orientation estimation result. The reliability is calculated so that it becomes higher the more feature points are used to estimate the position and orientation. The acquired reliability is output to the selection unit 113 as a priority.

In step S103, location information is acquired from the first location information acquisition unit 111 and the second location information acquisition unit 112, and the location information with the higher priority is selected.

In a situation where both types of sensors are successful in estimating the position and orientation, the system automatically switches between the position information acquired by the first position information acquisition unit 111 and the position information acquired by the second position information acquisition unit 112, depending on the priority associated with the position and orientation. When switching, the difference between the two types of position and orientation is calculated, and the position and orientation after switching is corrected based on that difference. This allows the system to select a position and orientation with a higher degree of reliability, while preventing the movement of the moving object from becoming unstable due to position and orientation deviations occurring during switching.

<Modification 1-8>
In Modification 1-8, the priority refers to the reliability representing the accuracy of the position and orientation estimation result. However, this is not limited to this, and any priority for position information can be applied. For example, the number of feature points detected when estimating the position and orientation from the input of an image sensor may be used as the priority. Furthermore, the priority may be determined based on processing other than position and orientation estimation. For example, if the brightness in an image is uniform, it can be inferred that the position and orientation estimation may fail, and therefore the priority may be set low. Alternatively, if the average brightness in the image is extremely low (or high), the priority for the image sensor may be set low, while the priority for the range sensor may be set high. Furthermore, if the distances of all point clouds obtained by the range sensor are greater than or equal to a threshold, it can be inferred that the position and orientation estimation accuracy may be low, and therefore the priority may be set low. The priority may be determined directly from the sensor measurement values. Furthermore, the priority may be determined using a value other than the sensor measurement values. For example, when map information for part of the route a moving object travels is stored and the map information is used as input data for estimating the position and orientation, the priority may be determined based on whether or not map information exists for the moving object's current location. If a map for an image sensor exists, the priority of the position and orientation estimated by the image sensor is determined to be high, and if a map for a range sensor exists, the priority of the position and orientation estimated by the range sensor is determined to be high.

<Embodiment 2>
In the first embodiment, when one sensor fails to estimate the position and orientation, the difference between the positions and orientations based on the two sensors is calculated and correction is performed using the difference when switching to the other sensor. In the second embodiment, a method for assisting position and orientation estimation by transferring the position and orientation based on the other sensor to the sensor that has failed to estimate the position and orientation will be described.

The functional configuration in this embodiment differs from that of the information processing device 11 described in embodiment 1 in that the first estimation unit 121 and the second estimation unit 131 are connected.

Figure 7 is a flowchart showing the processing procedure in this embodiment. Explanations of steps that are the same as those in Figure 4, which explains the processing procedure of the information processing device 11 described in embodiment 1, will be omitted, and processing procedures that differ from embodiment 1 will be explained.

In step S401, the first estimation unit 121 estimates the position and orientation based on the measurement values of the image sensor 12 and outputs them to the first position information acquisition unit 111 and the second estimation unit 131. Note that when estimating, the position and orientation are estimated using the previous position and orientation estimation result as the initial value. If the first estimation unit 121 failed to estimate the immediately preceding position and orientation, the second estimation unit 131 estimates the position and orientation using the input position and orientation as the initial value.

In step S402, the second estimation unit 131 estimates the position and orientation based on the measurement values of the distance sensor 13 and outputs them to the second position information acquisition unit 112 and the first estimation unit 121. As with the first estimation unit, the position and orientation are estimated using the previous position and orientation estimation result as the initial value. If the second estimation unit 131 failed to estimate the immediately preceding position and orientation, the position and orientation are estimated using the position and orientation input by the first estimation unit 121 as the initial value.

In embodiment 2, in a system in which position and orientation estimated from two types of sensor values are used to control the movement of a moving object, if position and orientation estimation based on image sensor values fails, the position and orientation to be used is switched from that estimated by the image sensor to that estimated by the range sensor. When switching, the difference between the position and orientation based on the two types of sensor values is obtained, and the position and orientation based on the second sensor values is corrected based on this difference. This makes it possible to prevent the movement of the moving object from becoming unstable due to a position and orientation deviation after switching. Furthermore, when position and orientation estimation based on the image sensor (first sensor) values fails, the position and orientation based on the range sensor (second sensor) values is input as an estimation parameter to assist position and orientation estimation based on the image sensor (first sensor) values. The position and orientation can be determined by more efficiently utilizing position information based on the two types of sensor values.

<Embodiment 3>
In the first and second embodiments, a correction method was described for switching sensors used for position and orientation estimation in a case where two systems of position and orientation are estimated based on input information from two sensors. On the other hand, in this embodiment, a case where one system of position and orientation is estimated based on output information from multiple sensors in order to improve the stability of position and orientation measurement will be described. Here, the multiple sensors are an image sensor and an IMU.

In this embodiment, a system is assumed in which position and orientation estimated from output information from an image sensor and an IMU is used to control the movement of a moving object. When position and orientation estimation based on output information from the image sensor and IMU fails, the position and orientation to be used is switched from the position and orientation estimated by the image sensor and IMU to the position and orientation estimated by the IMU alone. When switching, the difference between the two positions and orientations is obtained, and the position and orientation based on the second sensor value is corrected based on this difference. The two positions and orientations are one position and orientation estimated by the image sensor and IMU, and the other position and orientation estimated by the IMU.

The functional configuration of this embodiment differs from that of the information processing device 11 described in embodiment 1 in that the distance sensor 13 is replaced with an IMU, and the IMU is connected to the first estimator 121 and the second estimator 131.

The flowchart of the processing procedure in this embodiment is the same as that in embodiment 1 shown in Figure 4, and therefore will not be repeated here. Here, we will omit explanations of steps that are the same as in embodiment 1, and will instead explain processing procedures that differ from embodiment 1.

In step S102, the first position information acquisition unit 111 acquires the position and orientation estimated by the first estimation unit 121 based on input information acquired from the image sensor 12 and IMU. In this embodiment, the position and orientation estimation method using the image sensor and IMU is the ORB-SLAM3 method (Carlos Campos et al., ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM. Cornell University). This method matches image features detected from image data captured by a camera with multiple image features stored as a map for position and orientation measurement. The position and orientation are measured so that the sum of the differences (residual) between the positions and orientations calculated by combining the image feature positions and the inertial data is minimized. Note that in this embodiment, the position and orientation measurement map is created by the first estimation unit 121 before the mobile body 10 starts autonomous traveling and is stored by the first estimation unit 121. Even when this technology is used, as with the SLAM technology described in embodiment 1, position and orientation estimation may fail depending on the sensor measurement values. However, if position and orientation estimation fails, the first position information acquisition unit 111 acquires the position and orientation at the time before the failure, and outputs all positions and orientations after system initialization to the selection unit 113 and difference acquisition unit 114.

In step S103, the second position information acquisition unit 112 acquires the position and orientation estimated by the second estimation unit 131 based on the input information acquired from the IMU. In this embodiment, position and orientation estimation using the IMU uses a method in which the position immediately after IMU startup is set as the origin of the IMU coordinate system, acceleration and angular velocity are integrated twice per unit time, and the amount of displacement is calculated to estimate the position and orientation.

The method of this embodiment can prevent the movement of a moving object from becoming unstable due to deviations in the position and orientation after switching, even when the position and orientation of one system is estimated based on output information from multiple sensors in order to improve the stability of position and orientation measurement. The position and orientation that would have been estimated using the image sensor and IMU before switching is calculated using the difference between this and the position and orientation estimated using the IMU after switching. The position and orientation of the moving object corrected in S106 approaches the position and orientation estimated using the image sensor and IMU before switching, making it easier to maintain continuity.

<Modification 3-1>
In the third embodiment, the first position information acquisition unit 111 acquires the position and orientation estimated by the SLAM technique using the image sensor 12 and the IMU. However, the combination of sensors used to acquire the position information is not limited to this, and any combination of sensors may be used as long as it is possible to estimate the position and orientation.

For example, position and orientation may be estimated by combining SLAM technology using an image sensor with odometry, which estimates position and orientation based on the rotational angles of the tires and steering wheel. Position and orientation may also be estimated by combining SLAM technology using an image sensor and an IMU with odometry. Furthermore, position and orientation may also be estimated by combining SLAM technology using an image sensor with satellite positioning information from GPS.

On the other hand, the second position information acquisition unit 112 acquires an estimated position and orientation based on input information from the IMU. The sensor used in the second position information acquisition unit is not limited to an IMU, and any sensor that outputs input information that allows position and orientation to be estimated may be used. For example, sensor values from an image sensor, control information for a moving object, GPS, etc. may be used as input information for position and orientation estimation.

Furthermore, the first location information acquisition unit and second location information acquisition unit described above may be combined in any combination. For example, the first location information acquisition unit may use an image sensor and GPS, and the second location information acquisition unit may use an image sensor and IMU.

The present invention can also be realized by supplying a program that realizes one or more of the functions of the above-described embodiments to a system or device via a network or storage medium, and having one or more processors in the computer of that 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. It can also be realized by a system consisting of multiple devices (e.g., a host computer, an interface device, an imaging device, a web application, etc.).

10 Information processing system 11 Information processing device 12 Image sensor 13 Distance sensor 14 Control device

Claims (10)

An information processing device,
a first position information acquiring means for acquiring first position information of the moving body estimated based on input information from a first sensor fixed to the moving body;
a second position information acquiring means for acquiring second position information of the moving object estimated based on input information from a second sensor fixed to the moving object;
a selection means for selecting, from the first position information and the second position information in the same coordinate system, the first position information as the position information for controlling the movement of the moving object when the first position information is obtainable, and selecting, from the second position information and the first position information in the same coordinate system, the second position information as the position information for controlling the movement of the moving object when the first position information is not obtainable;
a difference acquiring means for acquiring difference information in the same coordinate system between the first position information and the second position information synchronized with the first position information;
an updating means for updating the second location information by adding difference information acquired by the difference acquiring means at the time of switching when the location information selected by the selecting means is switched from the first location information to the second location information until the first location information selected before the switching is reselected;
An information processing device comprising: The information processing device described in claim 1, characterized in that the difference acquisition means selects a combination of the first location information and the second location information from which the difference is to be acquired, from the first location information and the second location information before the location information selected by the selection means is switched, and acquires the difference based on the selected combination of location information. The information processing device according to claim 1 or 2, characterized in that the selection means outputs information indicating the selection result by the selection means to a display device. The information processing apparatus according to claim 1 , wherein the first sensor is a sensor that inputs image information as the input information. The information processing apparatus according to claim 1 , wherein the second sensor is a distance sensor that measures a distance between the second sensor and an object present in the environment as the input information. An information processing device,
a first position information acquiring means for acquiring first position information of the moving object estimated based on input information from a first sensor and a second sensor fixed to the moving object;
a second position information acquiring means for acquiring second position information of the moving object estimated based on input information from the second sensor;
a selection means for selecting, from the first position information and the second position information in the same coordinate system, the first position information as the position information for controlling the movement of the moving object when the first position information is obtainable, and selecting, from the second position information and the first position information in the same coordinate system, the second position information as the position information for controlling the movement of the moving object when the first position information is not obtainable;
a difference acquiring means for acquiring difference information in the same coordinate system between the first position information and the second position information synchronized with the first position information;
an updating means for updating the second location information by adding difference information acquired by the difference acquiring means at the time of switching when the location information selected by the selecting means is switched from the first location information to the second location information until the first location information selected before the switching is reselected;
An information processing device comprising: An information processing method, comprising:
a first position information acquisition step of acquiring first position information of the moving object estimated based on input information from a first sensor fixed to the moving object;
a second position information acquisition step of acquiring second position information of the moving object estimated based on input information from a second sensor fixed to the moving object;
a selection step of selecting, from the first position information and the second position information in the same coordinate system, the first position information as the position information for controlling the movement of the moving body when the first position information is obtainable, and selecting, from the second position information and the first position information in the same coordinate system, the second position information as the position information for controlling the movement of the moving body when the first position information is not obtainable;
a difference acquiring step of acquiring a difference between the first position information and the second position information synchronized with the first position information in the same coordinate system;
an updating step of, when the location information selected in the selecting step is switched from the first location information to the second location information, updating the second location information by adding the difference information acquired in the difference acquiring step at the time of switching until the first location information selected before the switching is reselected;
An information processing method comprising: An information processing method, comprising:
a first position information acquisition step of acquiring first position information of the moving object estimated based on input information from a first sensor and a second sensor fixed to the moving object;
a second position information acquisition step of acquiring second position information of the moving object estimated based on input information from the second sensor;
a selection step of selecting, from the first position information and the second position information in the same coordinate system, the first position information as the position information for controlling the movement of the moving body when the first position information is obtainable, and selecting, from the second position information and the first position information in the same coordinate system, the second position information as the position information for controlling the movement of the moving body when the first position information is not obtainable;
a difference acquiring step of acquiring difference information in the same coordinate system between the first position information and the second position information synchronized with the first position information;
an updating step of, when the location information selected in the selecting step is switched from the first location information to the second location information, updating the second location information by adding the difference information acquired in the difference acquiring step at the time of switching until the first location information selected before the switching is reselected;
An information processing method comprising: A mobile object,
a first position information acquiring means for acquiring first position information of the moving body estimated based on input information from a first sensor fixed to the moving body;
a second position information acquiring means for acquiring second position information of the moving object estimated based on input information from a second sensor fixed to the moving object;
a selection means for selecting, from the first position information and the second position information in the same coordinate system, the first position information as the position information for controlling the movement of the moving object when the first position information is obtainable, and selecting, from the second position information and the first position information in the same coordinate system, the second position information as the position information for controlling the movement of the moving object when the first position information is not obtainable;
a difference acquiring means for acquiring difference information in the same coordinate system between the first position information and the second position information synchronized with the first position information;
and an update means for, when the location information selected by the selection means is switched from the first location information to the second location information, updating the second location information by adding difference information acquired by the difference acquisition means at the time of the switching until the first location information selected before the switching is reselected. A program for causing a computer to function as each of the means of the information processing device described in any one of claims 1 to 6.