Information processing device, information processing method, and program

Abstract

To suppress unstable movement of a moving body, resulting from an occurrence of a position information deviation, when a piece of position information used to control a movement of the moving body is switched among pieces of position information obtained from two systems.SOLUTION: Difference information in the same coordinate system as that of second position information obtained in synchronization with first position information of a moving body, estimated based on measurement information by a first sensor, is obtained; position information used for controlling a movement of the moving body is selected from the first position information and the second position information; and when the position information used for controlling the movement of the moving body is switched, the selected information is corrected using the difference information, so as to be close to the position information selected before the switching thereof.SELECTED DRAWING: Figure 4

Description

[Technical Field]

[0001] The present invention relates to a technique for estimating the position of a moving object from measurement information. [Background technology]

[0002] There is a technology for estimating the position of a mobile object equipped with a sensor that measures the distance between the sensor and an object 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 higher 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 unify the coordinate systems, in order to use them for controlling 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 positions and orientations measured by the camera and laser to be handled in the same coordinate system. [Prior art documents] [Non-patent literature]

[0003] [Non-Patent Document 1] W. Dong, et al., “A Novel Method for the Extrinsic Calibration of a 2D Laser Rangefinder and a Camera,” ICRA'17, 2017. Summary of the Invention [Problem to be solved by the invention]

[0004] However, even if the sensor arrangement relationship is calibrated using the method of Non-Patent Document 1, differences in position and orientation occur due to measurement errors of each sensor. Attempting to eliminate differences that do not actually exist can result in unstable movement of the moving object. The present invention has been made in view of the above-mentioned problems, and aims to enable stable movement of the moving object when switching between positions and orientations estimated from measurement values ​​of two or more sensors. [Means for solving the problem]

[0005] The information processing device according to the present invention comprises 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, and a means for acquiring a first position information of the moving body estimated based on input information from a second sensor fixed to the moving body, and a means for acquiring a second ... When the first position information is available, the first position information is selected as the position information for controlling the movement of the moving body, and when the first position information is unavailable, the second position information is selected as the position information for controlling the movement of the moving body. a selection means for selecting a position of the first position information and a position of the second position information synchronized with the first position information; a 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; From the first location information to the second location information If switched, When the first location information selected before the switching is selected, the first location information is selected again. The difference information acquired by the difference acquisition means In addition , the above Second Location information update do update means. [Effects of the Invention]

[0006] According to the present invention, it is possible to stably move a moving object when switching between positions and orientations estimated based on measurement values ​​from two or more sensors. [Brief explanation of the drawings]

[0007] [Figure 1] A diagram showing an example of the system configuration of an information processing system. [Figure 2] A block diagram showing an example of the functional configuration of an information processing device. [Figure 3]FIG. 1 is a diagram illustrating an example of a hardware configuration of an information processing device. [Figure 4] 10 is a flowchart illustrating a process executed by an information processing device. [Figure 5] 10 is a flowchart illustrating a process executed by a difference acquisition unit. [Figure 6] 10 is a flowchart illustrating a process executed by an information processing device. [Figure 7] 10 is a flowchart illustrating a process executed by an information processing device. DETAILED DESCRIPTION OF THE INVENTION

[0008] <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 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. It is not always possible to estimate the position and orientation using both methods. If one method fails to estimate the position and orientation, the other method is used to control the movement of the mobile object. The movement of the mobile object can be controlled by inputting the position and orientation estimation results from one method. In this embodiment, the position and orientation estimated using the image sensor is primarily used as a priority for the movement control of the mobile object. When the position and orientation cannot be estimated using the input information from the image sensor, the position and orientation estimated using the input information from the range sensor is used. Conversely, the position and orientation estimated using the range sensor can also be prioritized. The priority of the position and orientation estimated using 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 using the image sensor is prioritized. When the sensor acquiring the position and orientation estimation results is switched from an image sensor to a range sensor while a moving object is moving, a discrepancy may occur in the acquired position and orientation. This may result in 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 in 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 and 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 calculated in advance, and that the positions and orientations of the two systems are converted into 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 the coordinate systems of both the positions and orientations of the two systems; it is sufficient that each system is acquired as position information in the same coordinate system.

[0009] Hereinafter, preferred embodiments of the present invention will be described with reference to the accompanying drawings.

[0010] 1 shows a system configuration diagram of a moving body in this embodiment. A moving 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 moving body. The image sensor 12 and the distance sensor 13 are fixedly attached to the moving body.

[0011] A specific example of the position and orientation obtained from input information from the image sensor 12 and distance sensor 13 in this embodiment will be described below. 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.

[0012] FIG. 2 is a diagram showing an example of the configuration of a mobile system including an information processing device 11 according to this embodiment.

[0013] The information processing device 11 is composed of a first position information acquisition unit 111, a second position information acquisition unit 112, a selection unit 113, a difference acquisition unit 114, a difference holding unit 115, and a correction unit 116. The first position information acquisition unit 111 is connected to a first estimation unit 121, which is connected to the image sensor 12. The second position 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.

[0014] 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 the stereo measurement is performed will be described as measurement information or measurement values.

[0015] 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, the distance sensor 13 is realized by a LiDAR or a ToF sensor. Hereinafter, the input information from the distance sensor 13 will be described as measurement information or measurement value for convenience.

[0016] 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 .

[0017] 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 .

[0018] The selection unit 113 selects one of the pieces of location information input by the first location information acquisition unit 111 and the second location information acquisition unit 112, and outputs the selection result and the selected location information to the correction unit .

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

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

[0021] The correction unit 116 corrects the position information input by the selection unit 113 based on the difference information held in the difference holding unit 115. The corrected position information is output to the control device 14.

[0022] The mobile object 10 may be equipped with the information processing device 11, or the information processing device 11 on the cloud may control the mobile object 10.

[0023] FIG. 3 illustrates 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 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, USB, serial communication, wireless communication, or any other type. H17 is an I / O that receives image data from an image sensor H18 and distance data from a distance sensor H19. The image sensor H18 and distance sensor H19 refer to the image sensor 12 and distance sensor 13 described above. H20 is the control device 14 mentioned above.

[0024] 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 may function as these means. These means may also be realized by 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 may be used. In this case, the multiple CPUs can execute processing in a distributed manner. The multiple CPUs may be located in a single computer, or in multiple physically separate computers. The means realized by the CPU executing a program may also be realized by a dedicated circuit.

[0025] 4 is a flowchart showing the processing procedure in this embodiment. The processing of this flowchart begins when an instruction to start traveling of the AGV is given. Hereinafter, the flowchart is assumed to be realized by the CPU executing a control program.

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

[0027] 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. In this embodiment, the SLAM (Simultaneous Localization and Mapping) technology, a method by Engel et al. (J. Engel, T. Schps, and D. Cremers. LSD-SLAM: Large-Scale Direct Monocular SLAM. In European Conference on Computer Vision (ECCV), 2014) is used as a position and orientation estimation method using an image sensor. 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 may fail when feature points (such as edge intersections and corners) in the image required for position and orientation estimation are not sufficiently included in the image acquired from the image sensor. For example, position and orientation estimation may fail when an all-black image acquired when the lens of the image sensor is blocked or when an image showing a plain white wall is captured in its entirety. If position and orientation estimation fails, the 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 at the time before the failure, and outputs all positions and orientations after system initialization to the selection unit 113 and the difference acquisition unit 114. Note that it is also possible to output positions and orientations for a predetermined period going back from the time when the first position information acquisition unit 111 failed to estimate the position and orientation to the selection unit 113 and the difference acquisition unit 114. The difference acquisition unit 114 acquires the difference, and the correction unit 116 limits the output of positions and orientations to the period required to correct the position information, thereby reducing the exchange of information between functional components.

[0028] As described above, the timing for outputting the position and orientation to the selection unit 113 and the difference acquisition unit 114 can be the timing after the first position information acquisition unit 111 fails to estimate the position and orientation. Also, the position and orientation can be continuously output to the selection unit 113 and the difference acquisition unit 114 after the system is initialized.

[0029] The position information is an array of data with each element consisting of a pair of six position and orientation parameters and time data.

[0030] 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. As a position and orientation estimation method using a distance sensor, 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) is used. 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 the difference acquisition unit 114. It is also possible to output to the selection unit 113 and the difference acquisition unit 114 the positions and orientations for a predetermined period going back from the time when the second position information acquisition unit 112 failed to estimate the position and orientation.

[0031] As described above, the timing for outputting the position and orientation to the selection unit 113 and the difference acquisition unit 114 can be the timing after the second position information acquisition unit 112 fails to estimate the position and orientation. Also, the position and orientation can be continuously output to the selection unit 113 and the difference acquisition unit 114 after the system is initialized.

[0032] In this embodiment, the image sensor 12 and the distance sensor 13 operate in time synchronization, and estimate the position and orientation using measurement values ​​taken at the same time as input.

[0033] 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 unit 113 basically gives priority to the position information input by the first position information acquisition unit 111. If the position information of the first position information acquisition unit 111 does not include the position and orientation at the current time, that is, if estimation of the position and orientation 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.

[0034] 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 been completed. 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.

[0035] 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, which controls the movement of the automatic guided vehicle.

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

[0037] FIG. 5 is a flowchart showing the processing procedure of the difference acquisition method in step S105.

[0038] 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, one combination is selected that contains the position and orientation at the latest time and that includes both the first position information acquisition unit 111 and the second position information acquisition unit 112.

[0039] In step S202, a relative position and orientation is calculated from the two position and orientation 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 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 for each parameter. The relative orientation is calculated by converting the rotational components into a 3 × 3 rotation matrix. The rotation matrix converted from the position and orientation acquired by the first position information acquisition unit 111 is called R V , the rotation matrix converted from the position and orientation acquired by the second position information acquisition unit 112 is R L Then, R V =MR L This transformation matrix M represents the relative orientation. The calculated relative position and relative orientation are output to the difference holding unit 115 as difference information. 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.

[0040] The above-described difference acquisition process is a process that 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 process is executed to reset the difference information to initial values. 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.

[0041] Next, the procedure of the correction method in step S106 will be described.

[0042] The position and orientation that the selection unit 113 inputs 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 difference acquisition, and each component is calculated. The translational components are calculated by adding the relative position included in the difference information for each parameter to the translational components of the position and orientation that the selection unit 113 inputs. The rotational components are calculated by adding the rotational components of the position and orientation that the selection unit 113 inputs to the correction unit 116 using a 3×3 rotation matrix R S and the matrix product MR S The position and orientation corrected as described above are output to the control device 14 as position information.

[0043] In this system, a position and orientation estimated from two types of sensor values ​​is used to control the movement of a moving object. The system switches the position and orientation from one estimated based on input information from an image sensor to one estimated based on 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 acquired, and the position and orientation after switching is corrected based on the difference. The position and orientation estimated by the image sensor before switching is calculated using the difference between the position and orientation estimated by the distance sensor after switching. 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. The information processing device 11 of this embodiment can control not only AGVs but also unmanned aerial vehicles such as drones equipped with an image sensor and a distance sensor.

[0044] Furthermore, if the 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 measurement values ​​of the image sensor.We will describe a method for determining the period until the switch from position and orientation estimation based on the measurement values ​​of the range sensor to position and orientation estimation based on the measurement values ​​of the image sensor is completed, and integrating the position and orientation using a weighted average so that the switch can be made gradually within that period.

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

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

[0047] The weight determination unit determines a weight using the period information input by the period determination unit, and outputs the weight to the integration unit.

[0048] The integrating unit integrates the two pieces of position information acquired by the first position information acquiring unit 111 and the correcting unit 116 based on the weights input by the weight determining unit.

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

[0050] 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. That is, the process proceeds to S302 when the sensor used as the input for position and orientation estimation is switched from the distance sensor 13 to the image sensor 12. Otherwise, the process skips to S303.

[0051] In step S302, the period determination unit determines the period until switching from position and orientation estimation based on the measurement values ​​of the range 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).

[0052] In step S303, the weight determination unit determines a weight for each of the position and orientation estimated from the measurement values ​​of the image sensor 12 and the distance 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 during 10 iterations and gradually increased toward the end of the period. Therefore, the weight for the position and orientation of the image sensor is set to W C (0≦W C ≦1), then W C = (current iteration number) / 10. On the other hand, the weight of the position and orientation of the range sensor is W L (0≦W L ≦1), it is necessary to match the weight of the position and orientation of the image sensor, so W L =1-W CThe weight determination unit stores the current iteration number in H13, which is 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, at the start of the period, the weight of the position and orientation estimated from the measurement values ​​of the range sensor 13 is 1, and at the end of the period, the weight of the position and orientation estimated from the measurement values ​​of the image sensor becomes 1.

[0053] 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.

[0054] In this way, after switching the sensor used for estimation once, when switching back to the original sensor, a period until the switching is completed 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 deviations in the position and orientation when switching.

[0055] <Variation 1-1> In the first embodiment, the first position information acquisition unit 111 acquires the position and orientation estimated by the SLAM technique 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 technique using the distance sensor 13. However, the sensors combined with the SLAM technique are not limited to these, and any sensor may be used. The position and orientation may be estimated by combining the SLAM technique with an active distance sensor such as time of flight or pattern light projection.

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

[0057] <Variation 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 limiting, 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 orientation may be a position parameter only, 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.

[0058] <Variation 1-3> In the first embodiment, the difference acquisition unit 114 selects one combination of position and orientation to use 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 the average value for each position and orientation parameter.

[0059] In the first embodiment, the image sensor 12 and the distance sensor 13 operate in time synchronization, and estimate the position and orientation using input measurement values ​​at the same time. However, this is not limiting, and the position and orientation may be estimated independently. In such a case, the following modified example is used as a method for selecting a combination of positions and orientations for which the difference acquisition unit 114 calculates the difference. 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 estimated from the measurement values ​​of the distance sensor 13 at a time close to the position and orientation for which the difference is to be acquired from the measurement values ​​of the image sensor 12 has not been estimated. In such cases, the position and orientation at a desired time may be obtained by interpolation or prediction based on the position and orientation of the distance sensor 13 at another time.

[0060] Furthermore, in the first embodiment, 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 successful again. 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 a user input via the input unit H15.

[0061] <Variation 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 an automatically driven vehicle, an autonomous mobile robot, or an unmanned aerial vehicle.

[0062] <Variation 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 system may be used for purposes other than controlling the movement of the mobile body. For example, the system 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.

[0063] <Variation 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 turned on when the selection unit 113 selects the first position information acquisition unit 111. The yellow light is turned on when the second position information acquisition unit 112 is selected. The red light is turned on 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, and 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.

[0064] <Variation 1-7> In the above-described embodiment, when position and orientation estimation for the measurement values ​​of the image sensor fails and the sensor used for estimation is switched to the 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 types of position and orientation are integrated to suppress position and orientation deviation at the time of switching. However, the present invention can also be applied when position and orientation estimation is successful using both types of sensors. Therefore, a method for automatically switching the input sensor according to the priority associated with the position and orientation when both sensors are successful in position and orientation estimation will be described.

[0065] Only the processing steps that differ from the processing described using FIG. 4 will be described, and the other steps will be omitted as they are the same processing as in FIG.

[0066] 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 the reliability is an index indicating the accuracy of the position and orientation estimation result. The reliability is calculated so that it becomes higher as the number of feature points used for estimating the position and orientation increases. The acquired reliability is output to the selection unit 113 as a priority.

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

[0068] In a situation where both types of sensors are successful in estimating the position and orientation, 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 are automatically switched according to 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 makes it possible to select a position and orientation with a higher degree of reliability, while suppressing instability in the movement of the moving object due to deviations in the position and orientation at the time of switching.

[0069] <Variation 1-8> In Modification Examples 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 the position and orientation estimation. For example, if the brightness in the 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 equal to or greater than 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 for the moving object's current position exists. If a map for an image sensor exists, the position and orientation estimated by the image sensor is given a high priority, and if a map for a range sensor exists, the position and orientation estimated by the range sensor is given a high priority.

[0070] <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 will be described in which the position and orientation estimation is further supported by transferring the position and orientation based on the other sensor to the sensor that has failed to estimate the position and orientation.

[0071] The functional configuration of this embodiment differs from that of the information processing device 11 of the first embodiment shown in FIG. 2 in that a first estimating unit 121 and a second estimating unit 131 are connected.

[0072] 7 is a flowchart showing the processing procedure in this embodiment. Explanation of steps that are the same as those in FIG. 4, which explains the processing procedure of the information processing device 11 described in the first embodiment, will be omitted, and processing procedures that differ from the first embodiment will be explained.

[0073] 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 the result to the first position information acquisition unit 111 and the second estimation unit 131. Note that during estimation, the position and orientation are estimated using the previous position and orientation estimation result as an initial value. If the first estimation unit 121 has 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 an initial value.

[0074] 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 the result 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 an initial value. If the second estimation unit 131 failed to estimate the immediately preceding position and orientation, the first estimation unit 121 estimates the position and orientation using the input position and orientation as an initial value.

[0075] In the second embodiment, in a system in which a position and orientation estimated from two types of sensor values ​​is used to control the movement of a moving object, if estimation of the position and orientation based on the 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, a 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 the difference. This makes it possible to prevent the movement of the moving object from becoming unstable due to a deviation in the position and orientation after switching. Furthermore, when estimation of the position and orientation 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 the position and orientation estimation based on the image sensor (first sensor) values. The position and orientation can be obtained by more efficiently utilizing position information based on the two types of sensor values.

[0076] <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.

[0077] In this embodiment, a system is assumed in which a position and orientation estimated from output information of 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 of an image sensor and an 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.

[0078] The functional configuration in this embodiment differs from that shown in Figure 2, which illustrates the functional configuration 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 estimation unit 121 and the second estimation unit 131.

[0079] The flowchart of the processing procedure in this embodiment is the same as that of embodiment 1 shown in Fig. 4, and therefore will not be described here. Here, explanations of steps that are the same as those in embodiment 1 will be omitted, and processing procedures that differ from embodiment 1 will be described.

[0080] 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 the IMU. In this embodiment, 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) is used as the position and orientation estimation method using the image sensor and the IMU. This method matches image features detected from image data captured by a camera with a plurality of image features stored as a position and orientation measurement map. The position and orientation are measured so that the sum of differences (residuals) between 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 object 10 starts autonomous traveling and is stored by the first estimation unit 121. Here, even when this technology is used, as with the SLAM technology described in the first embodiment, estimation of the position and orientation may fail depending on the measurement values ​​of the sensor. However, if estimation of the position and orientation 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 the difference acquisition unit 114.

[0081] 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, the position and orientation estimation using the IMU uses a method in which the position immediately after the IMU is started 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.

[0082] According to the method of this embodiment, 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, it is possible to prevent the movement of the moving object from becoming unstable due to the occurrence of a position and orientation deviation after switching. The position and orientation that would have been estimated by the image sensor and IMU before switching is obtained using the difference between the position and orientation estimated by the IMU after switching. The position and orientation of the moving object corrected in S106 approaches the position and orientation estimated by the image sensor and IMU before switching, making it easier to maintain continuity.

[0083] <Variation 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.

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

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

[0086] Furthermore, the first location information acquisition unit and the 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 a GPS, and the second location information acquisition unit may use an image sensor and an IMU.

[0087] The present invention can also be realized by supplying a program that realizes one or more functions of the above-described embodiments to a system or device via a network or a 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., 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.). [Explanation of symbols]

[0088] 10 Information Processing Systems 11 Information processing equipment 12 Image Sensor 13 Distance Sensor 14 Control device

Claims

1. 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 update means for updating the second location information by adding difference information acquired by the difference acquisition means at the time of switching when the location information selected by the selection 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:

2. The information processing device according to claim 1, characterized in that the difference acquisition means selects a combination of the first location information and the second location information before the location information selected by the selection means is switched, from which a difference is to be acquired, and acquires a difference based on the selected combination of location information.

3. 3. The information processing apparatus according to claim 1, wherein the selection means outputs information indicating a result of the selection by the selection means to a display device.

4. The first sensor is a sensor that inputs image information as the input information.

4. The information processing device according to claim 1, wherein:

5. The second sensor is a distance sensor that measures the distance between the second sensor and an object present in the environment as the input information.

5. The information processing device according to claim 1, wherein:

6. 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 update means for updating the second location information by adding difference information acquired by the difference acquisition means at the time of switching when the location information selected by the selection 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:

7. 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:

8. 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:

9. 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.

10. A program for causing a computer to function as each of the means of the information processing apparatus according to any one of claims 1 to 6.

Images