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

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

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

In a moving body equipped with a sensor that measures the distance between an object existing in the environment and the sensor, there is a technique for estimating the position of the moving body based on the measurement information obtained from the sensor. By mounting a plurality of sensors on the moving body and switching the position of the moving body estimated by each sensor according to the situation, the position of the moving body can be estimated with higher accuracy. At this time, since the position estimated by each sensor is the position estimated independently for each sensor, it is necessary to make the positions consistent among the sensors, that is, to unify the coordinate system in order to use it for the movement control of the moving body. There is. In Non-Patent Document 1, the positional relationship between the camera and the laser sensor is measured in advance so that the position and orientation measured by the camera and the laser can be handled in the same coordinate system.

W. Dong, et al. , "A Novel Method for the External Calibration of a 2D Laser Rangefinder and a Camera," ICRA'17, 2017.

However, even if the arrangement relationship of the sensors is calibrated by the method of Non-Patent Document 1, a difference in position and orientation occurs due to a measurement error of each sensor. The movement of a moving object may become unstable in an attempt to eliminate a difference that does not actually exist. The present invention has been made in view of the above problems, and an object of the present invention is to stably move a moving body when the position and orientation estimated from the measured values of two or more sensors are switched and used. And.

The information processing apparatus according to the present invention has the following configurations. That is, the first position information acquisition means for acquiring the first position information of the moving body estimated based on the input information from the first sensor fixed to the moving body, and the moving body fixed to the moving body. The second position information acquisition means for acquiring the second position information of the moving body estimated based on the input information from the second sensor, the first position information in the same coordinate system, and the second position information. Of the position information of the above, the selection means for selecting the position information for controlling the movement of the moving body and the same of the first position information and the second position information synchronized with the first position information. When the difference acquisition means for acquiring the difference information in the coordinate system and the position information selected by the selection means are switched, the position information selected by the selection means is corrected by using the difference information acquired by the difference acquisition means. Correction means.

According to the present invention, the moving body can be stably moved when the position / posture estimated based on the measured values from two or more sensors is switched and used.

Diagram showing a system configuration example of an information processing system Block diagram showing a functional configuration example of an information processing device The figure which shows an example of the hardware composition of an information processing apparatus. Flow chart explaining the processing executed by the information processing device Flowchart explaining the process executed by the difference acquisition unit Flow chart explaining the processing executed by the information processing device Flow chart explaining the processing executed by the information processing device

<Embodiment 1>
In this embodiment, an information processing system including a moving body in which an image sensor and a distance sensor are mounted on the moving body will be described. Here, it is assumed that the moving body is specifically an automated guided vehicle (Automated Guided Vehicle or Self Driving Vehicle). This system estimates the position and orientation of the moving body in two systems based on the input information from the image sensor and the distance sensor. It is not always possible to estimate the position and orientation of two systems, and if the estimation of one position and orientation fails, the position and orientation are switched to the other and used for movement control of the moving body. If the position / orientation estimation result of one system is input, the movement control of the moving body is possible. In the present embodiment, the position / posture estimated from the image sensor is mainly used for the movement control of the moving body, and the input information from the distance sensor is used when the position / posture cannot be estimated from the input information from the image sensor. Use the position and orientation estimated by. Of course, conversely, the position and orientation estimated by the distance sensor can be prioritized. It is decided in advance which sensor gives priority to the position / orientation estimated by the sensor, depending on the reliability of the position / orientation estimated by the sensor. In this embodiment, priority is given to the position and orientation estimated by the image sensor. If the sensor that acquires the position / orientation estimation result is switched from the image sensor to the distance sensor while the moving object is moving, the acquired position / attitude may be displaced. The moving object may cause unexpected behavior such as a sudden change of direction in an attempt to return to the position and posture before switching. Therefore, in the present embodiment, a method of acquiring the difference in position / posture between the two sensors and correcting the position / posture at the time of switching based on the difference at the time of switching will be described. The position and orientation that should have been estimated by the image sensor before switching is obtained by using the position and orientation estimated by the distance sensor after switching and the difference. The arrangement relationship between the image sensor and the distance sensor is obtained in advance, and based on the arrangement relationship, the position and orientation of the two systems are converted into the position and orientation on the same coordinate system in order to be used for the movement control of the moving body. Suppose there is. It is not essential to convert both of the coordinate systems of the two systems of position and orientation, and it is sufficient that each of them can be acquired as position information in the same coordinate system.

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

FIG. 1 shows a system configuration diagram of a moving body according to the present embodiment. The moving body 10 in the present embodiment is composed of an image sensor 12, a distance sensor 13, an information processing device 11, and a control device 14 for controlling the movement of the moving body as sensors for estimating the position and posture. The image sensor 12 and the distance sensor 13 are fixedly attached to a moving body.

A specific example of the position and orientation obtained from the input information from the image sensor 12 and the distance sensor 13 in the present embodiment will be described. 6 parameters including 3 parameters (X, Y, Z) representing the position of the sensor in any world coordinate system defined in the real space and 3 parameters (Roll, Pitch, Yaw) representing the posture of the sensor. Is.

FIG. 2 is a diagram showing a configuration example of a mobile system including the information processing device 11 in the present embodiment.

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. Further, the first position information acquisition unit 111 is connected to the first estimation unit 121, and the first estimation unit 121 is connected to the image sensor 12. The second position information acquisition unit 112 is connected to the second estimation unit 131, and the second estimation unit 131 is connected to the distance sensor 13. Further, the correction unit 116 is connected to the control device 14.

The first estimation unit 121 estimates the position information based on the input information obtained from the image sensor 12, and outputs the estimation result as the 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 on the feature points in the image and estimates the three-dimensional position information of the feature points. Hereinafter, the input information from the image sensor 12 before performing the stereoscopic measurement will be described as measurement information or measurement value for convenience.

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

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

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

The selection unit 113 selects one of the position information input by the first position information acquisition unit 111 and the second position information acquisition unit 112, and outputs the selection result and the selected position information to the correction unit 116.

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

The difference holding unit 115 holds 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 held by the difference holding unit 115. The corrected position information is output to the control device 14.

The moving body 10 can be equipped with the information processing device 11, or the information processing device 11 on the cloud can control the moving body 10.

FIG. 3 is a diagram showing a hardware configuration of the information processing apparatus 11. The H11 is a CPU and controls various devices connected to the system bus H21. The H12 is a ROM and stores a BIOS program and a boot program. H13 is a RAM and is used as a main storage device of the H11 which is a CPU. H14 is an external memory and stores a program processed by the information processing apparatus 11. The input unit H15 is a keyboard, a mouse, and a robot controller, and performs processing related to input of information and the like. The display unit H16 outputs the calculation result of the information processing device 11 to the display device according to the instruction from the H11. The display device may be of any type, such as a liquid crystal display device, a projector, or an LED indicator. H17 is a communication interface, which performs information communication via a network, and the communication interface may be Ethernet, and any type such as USB, serial communication, and wireless communication may be used. H17 is an I / O, and image data is input from the image sensor H18 and distance data is input from the distance sensor H19. The image sensor H18 and the distance sensor H19 are the image sensor 12 and the distance sensor 13 described above. H20 is the above-mentioned control device 14.

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. Further, these means may be realized by cooperation between the CPU and the control circuit that controls the operation of the image processing device. Further, the CPU does not have to be a single CPU, and may be a plurality of CPUs. In this case, a plurality of CPUs can be distributed to execute the process. Further, the plurality of CPUs may be arranged in a single computer, or may be arranged in a plurality of physically different computers. The means realized by the CPU executing the program may be realized by a dedicated circuit.

FIG. 4 is a flowchart showing a processing procedure in the present embodiment. When the start of traveling of the AGV is instructed, the processing of this flowchart is started. Hereinafter, it is assumed that the flowchart is realized by the CPU executing the control program.

In step S101, the system is initialized. That is, the program is read from the external memory H14 and the information processing apparatus 11 is put into an operable state. Further, the parameters of each device connected to the information processing apparatus 11 are written in the RAM H13. In addition, each control device of the moving body is activated so that it can be operated and controlled.

In step S102, the first position information acquisition unit 111 acquires the position / posture estimated by the first estimation unit 121 based on the input information acquired from the image sensor 12. In the present embodiment, as a position / orientation estimation method using an image sensor, SLAM (Simultaneus Localization and Mapping) technology, Angel et al.'S method (J. Angel, T. Schps, and D. Cremers. LSD-SLAM: Larg- Small Direction Monocular SLAM. In European Conference on Computer Vision (ECCV), 2014) is used. This method simultaneously performs self-position estimation and cartography in a wide area environment such as outdoors. Here, when this technique is used, the estimation of the position and orientation may fail depending on the measured value of the sensor. Specifically, the estimation of the position and orientation fails when the feature points (edge intersections, corners, etc.) in the image required for estimating the position and orientation are not sufficiently included in the image obtained from the image sensor. do. For example, when the image is a black image acquired when the lens portion of the image sensor is blocked, or an image in which a pure white wall surface without a pattern is shown on the whole, the estimation of the position and orientation fails. When the estimation of the position / posture fails, the value of the position / posture cannot be obtained. If the position / orientation estimation fails, the first position information acquisition unit 111 acquires the position / attitude at the time before the failure, and outputs all the position / attitudes after the 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 position / orientation for a period retroactively from the time when the first position information acquisition unit 111 fails to estimate the position / attitude. The difference acquisition unit 114 acquires the difference, and the correction unit 116 narrows down to the output of the position / posture for the period required for correcting the position information, so that the exchange of information between the functional configurations can be reduced.

As described above, the timing of outputting the position / posture to the selection unit 113 and the difference acquisition unit 114 may be the timing after the first position information acquisition unit 111 fails to estimate the position / posture. Further, after the system is initialized, the position and orientation can be continuously output to the selection unit 113 and the difference acquisition unit 114.

The position information is array data having a pair of position / posture 6 parameters and time data as one element.

In step S103, the second position information acquisition unit 112 acquires the position / posture estimated by the second estimation unit 131 based on the measurement information acquired from the distance sensor 13. SLAM technology (Ji Zhang, and Sanjiv Singh, "LOAM: Lidar Odomery and Mapping in Real-time", In Robotics: Sensors) using a 3D laser scanner as a method for estimating the position and orientation using a distance sensor. , 2014). If the second position information acquisition unit 112 also fails to estimate the position / orientation, the position / attitude at the time before the failure is acquired, and all the positions / attitudes after system initialization are output to the selection unit 113 and the difference acquisition unit 114. Further, it is also possible to output to the selection unit 113 and the difference acquisition unit 114 the position / orientation for a period retroactive by a predetermined time from the time when the second position information acquisition unit 112 fails to estimate the position / attitude.

As described above, the timing of outputting the position / posture to the selection unit 113 and the difference acquisition unit 114 may be the timing after the second position information acquisition unit 112 fails to estimate the position / posture. Further, after the system is initialized, the position and orientation can be continuously output to the selection unit 113 and the difference acquisition unit 114.

In this embodiment, the image sensor 12 and the distance sensor 13 operate in synchronization with the time, and the position and orientation are estimated by inputting the measured values at the same time.

In step S104, the selection unit 113 selects one of the position information input by the first position information acquisition unit 111 and the second position information acquisition unit 112. In the present embodiment, basically, the position information input by the first position information acquisition unit 111 is preferentially selected. When the position information of the first position information acquisition unit 111 does not include the position / orientation at the current time, that is, when the estimation of the position / orientation using the measured value of the image sensor 12 fails, the second position information acquisition unit Select the position information entered by 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 from the two position information input from the first position information acquisition unit 111 and the second position information acquisition unit 112, and outputs the difference as the difference information to the difference holding unit 115. Step S105 does not depend on the processing of step S104, and can start execution if the processing of steps S102 and S103 is completed. The difference information is the position / orientation 6 parameter. The detailed difference calculation method will be described later. The two position information is the position information after conversion to 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 held by the difference holding unit 115. The detailed correction method will be described later. By outputting the corrected position information to the control device 14, the automatic guided vehicle is moved and controlled.

In step S107, it is determined whether or not to terminate the system (stop the moving body). Specifically, it ends when the end instruction from the user is received by the input unit H15. If not, the process returns to step S102 and the process is continued.

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

In step S201, a combination of positions and postures to be input for obtaining a 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 the present embodiment, one set is selected at the latest time and in which the positions and orientations of both the first position information acquisition unit 111 and the second position information acquisition unit 112 exist.

In step S202, the relative position / posture is obtained from the two position / postures selected in step S201. Specifically, the position / orientation 6 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 rotation components (Roll, Pitch, Yaw). Obtain the relative position and relative posture from each component. The relative position is obtained by subtracting the translational component of the position / posture acquired by the second position information acquisition unit 112 from the translational component of the position / posture acquired by the first position information acquisition unit 111 for each parameter. The relative posture is obtained by converting the rotation component into a 3 × 3 rotation matrix. _Let R _V be the rotation matrix converted from the position and orientation acquired by the first position information acquisition unit ₁₁₁ , and _RL be the rotation matrix converted from the position and orientation acquired by the second position information acquisition unit 112. The transformation matrix M is obtained. This transformation matrix M represents a relative posture. The obtained relative position and relative posture 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, there is no difference in the position and orientation of the moving body.

In the difference acquisition processing procedure described above, when the position / orientation estimation based on the measured value of the image sensor 12 fails, that is, the selection unit 113 switches the selection from the first position information acquisition unit 111 to the second position information acquisition unit 112. It is a processing procedure to be executed at the time. On the other hand, when the position / orientation estimation using the image sensor 12 is successful again, that is, when the selection unit 113 switches the selection from the second position information acquisition unit 112 to the first position information acquisition unit 111, the correction is made. Since there is no need for, execute the procedure to reset the difference information to the initial value. The initial value is zero for all components of the relative position (X, Y, Z), and the rotation matrix is the unit matrix for the relative posture.

Next, the processing procedure of the correction method in step S106 will be described.

The position and orientation input by the selection unit 113 to the correction unit 116 are corrected by adding the relative position and the relative posture held by the difference holding unit 115. Specifically, the correction is also divided into a translation component and a rotation component in the same manner as when the difference is acquired, and the calculation is performed for each component. The translation component is obtained by adding the relative position included in the difference information to the translation component of the position and orientation input by the selection unit 113 for each parameter. As the rotation component, the rotation component of the position and orientation input by the selection unit 113 is converted into a 3 × 3 rotation matrix _RS , and the matrix product _MRS with the relative posture included in the difference information is obtained. The position / orientation corrected as described above is output to the control device 14 as position information.

In a system that uses the position and orientation estimated from the two sensor values for the movement control of the moving body, the position and orientation to be used is estimated from the position and orientation estimated by the input information from the image sensor, and the position estimated by the input information from the distance sensor. The case of switching to the posture was explained. The difference in 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 that should have been estimated by the image sensor before switching is obtained by using the position and orientation estimated by the distance sensor after switching and the difference. As a result, even if the position and posture estimated in each system shift before and after switching, it is possible to prevent the moving body from becoming unstable. Since the position / orientation of the moving body corrected in S106 approaches the position / orientation estimated by the sensor before switching (the difference becomes small), it becomes easy to maintain continuity. The information processing device 11 of the present embodiment is not limited to the AGV, and can also control the flight of an unmanned aerial vehicle such as a drone equipped with an image sensor and a distance sensor.

Furthermore, the position / orientation estimation based on the failed image sensor (first sensor) will succeed again, and the position / attitude can be corrected even when switching to the position / attitude estimation based on the measured value of the image sensor. can. Determine the period from the position and orientation estimation based on the measured values of the distance sensor to the position and orientation estimation based on the measured values of the image sensor, and use the weighted average to gradually switch within that period. Describes how to integrate postures.

In addition to the functional configuration of the information processing apparatus 11 described with reference to FIG. 2, a period determination unit, a weight determination unit, and an integration unit are further provided.

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

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

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

FIG. 6 is a diagram showing a flowchart of the processing procedure in the present embodiment. The steps that are the same as those in FIG. 4 will be omitted, and different processing procedures will be described.

In step S301, if the selection result stored in the 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, when the sensor that is the input of the position / orientation estimation is switched from the distance sensor 13 to the image sensor 12, the process proceeds to S302. If not, the process is skipped up to S303.

In step S302, the period until the period determination unit completes the switching from the position / orientation estimation based on the measured value of the distance sensor (second sensor) to the position / attitude estimation based on the measured value of the image sensor (first sensor). To decide. In the present embodiment, it is assumed that this period is 10 iterations (1 iteration is one position / posture output per iteration).

In step S303, the weight determination unit determines the weights for each of the position / posture estimated from the measured value of the image sensor 12 and the position / posture estimated from the measured value of 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 ₀ at the start of the period between 10 _iterations , and it is desired to gradually increase toward the end of the period. Then, _WC = (current number of iterations) / 10. On the other hand, if the weight of the position / orientation of the distance sensor is WL (0 ≦ _WL ≦ 1), it is necessary to match the weight of the position / orientation of the image sensor, so _{WL =} 1- _WC . The weight determination unit stores the current number of iterations in H13, which is a RAM, and adds 1 for each execution of step S303 to gradually increase the weight of the image sensor with respect to the distance sensor. For example, at the start of the period, the weight of the position / posture estimated from the measured value of the distance sensor 13 is 1, and at the end of the period, the weight of the position / posture estimated from the measured value of the image sensor is 1.

In step S304, the integration unit uses a weighted average of the position / posture estimated from the measured value of the image sensor 12 and the position / posture estimated from the measured value of the distance sensor 13 based on the weight input by the weight determination unit. Integrate. Specifically, the position and orientation after integration are obtained by calculating the weighted average for each of the six position and orientation parameters.

In this way, after switching the sensor used for estimation once, when returning to the original sensor again, the period until the switching is completed is determined, and the position is positioned using the weighted average so that the switching can be performed gradually within that period. Integrate posture. As a result, it is possible to prevent the movement of the moving body from becoming unstable due to the occurrence of displacement of the position and posture at the time of switching.

<Modification 1-1>
In the first embodiment, the first position information acquisition unit 111 has acquired 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 has acquired the position and orientation estimated by the SLAM technique using the distance sensor 13. However, the sensor to be combined with the SLAM technology is not limited to these, and may be any sensor. The position and orientation may be estimated by combining SLAM technology with an active distance sensor such as Time of Light or pattern light projection.

Further, the estimation method is not limited to the SLAM technique, and the technical idea of the present embodiment can be applied as long as the position and orientation can be estimated. For example, a method based on radio field intensity measurement in wireless communication such as Wifi may be used. A method using satellite positioning such as GPS, a magnetic tracking sensor, or a method using a time difference in sensing with a large number of optical sensors may be used. Further, a method of model fitting based on CAD data may be used by using an image acquired by a camera installed at 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 the position / posture 6 parameter. However, the present invention is not limited to this, and the technical idea of the present embodiment can be applied as long as the information indicating the position and posture is included. For example, a position 2 parameter (positions X and Y on a plane horizontal to the floor in real space) and a posture 1 parameter (rotation direction on a plane horizontal to the floor in real space) indicating a two-dimensional position and orientation. ) May be a total of three parameters. Further, the posture parameter may not be included and only the position parameter may be used. Further, a method of estimating the position / posture based on the acceleration / angular velocity acquired by the IMU (Inertial Measurement Unit) or a method of estimating the position / posture by odometry based on the rotation angle of the tire may be used.

<Modification 1-3>
In the first embodiment, the difference acquisition unit 114 has selected one set as the combination of positions and postures used for the difference acquisition. However, the present invention is not limited to this, and a plurality of combinations may be selected as input for difference acquisition. As a method of calculating the difference when a combination of a plurality of positions and postures is input, a method of obtaining a relative positional relationship is used. As a difference calculation method, for example, there are a method of minimizing the variation in the relative value posture between the two systems of position and posture by using the least squares method, and a method of obtaining the average value for each position and posture parameter.

Further, in the first embodiment, the image sensor 12 and the distance sensor 13 operate in synchronization with the time, and the position and orientation are estimated by inputting the measured values at the same time, but the time is not limited to this. The position and orientation may be estimated at each timing without synchronization. In such a case, the following modification is used as a method of selecting a combination of positions and postures to be calculated by the difference acquisition unit 114. For example, the position / posture estimated from the measured value of the distance sensor 13 which is the closest time to the time of the position / posture estimated from the measured value of the image sensor 12 is selected. Further, there is a case where the position / posture is not estimated from the measured value of the distance sensor 13 at a close time to the position / posture to be the difference acquisition target estimated from the measured value of the image sensor 12. At that time, the position / orientation at a desired time may be obtained by interpolation or prediction based on the position / orientation at another time of the distance sensor 13.

Further, in the first embodiment, the difference acquisition unit 114 executes the difference calculation only when the first position information acquisition unit 111 is switched to the second position information acquisition unit 112, but the difference calculation is not limited to this, and other than that. The difference calculation may be executed at the timing of. For example, after switching to the second position information acquisition unit 112, it may move a long distance until the position / orientation estimation using the image sensor 12 succeeds again. In that 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 be larger or smaller than the difference amount calculated at the time of switching. Therefore, the difference calculation may be re-executed to update the difference information. Further, the difference information may be updated at a fixed distance interval or a fixed time interval, and the parameter of the interval may be input and determined by the user via the input unit H15.

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

<Modification example 1-5>
In the first embodiment, the moving body system 10 uses the position and orientation for controlling the movement of the moving body. However, the present invention is not limited to this, and may be used for purposes other than the movement control of the moving body. For example, it may be applied to technologies such as MR (Mixed Reality), VR (Virtual Reality), and AR (Augmented Reality) to be used for estimating the viewpoint position and posture of a user.

<Modification example 1-6>
The information processing apparatus 11 may include a presentation unit (not shown). For example, if a three-color lamp of red, yellow, and green is used for the presentation unit, green is lit when the selection unit 113 selects the first position information acquisition unit 111. When the second position information acquisition unit 112 is selected, the yellow light is turned on. When switching from the first position information acquisition unit 111 to the second position information acquisition unit 112, the red light is turned on. The selection result can be visualized to the user. An LED lamp or a liquid crystal display may be used as long as it can present information regarding the selection result of the position and orientation to the user. The presenting device may be a speaker, or may be configured such that a specific alarm sound or a specific melody is played according to the selection result of the position / posture. Further, the information to be presented may be information other than the selection result of the position / orientation, and may be, for example, the difference information held by the difference holding unit 115.

<Modification example 1-7>
In the above-described embodiment, when the estimation of the position / orientation with respect to the measured value of the image sensor fails and the sensor used for the estimation is switched to the distance sensor, the difference between the positions / attitudes based on the two sensors is obtained, and the difference is used. It was corrected. Further, when the sensor used for estimation is returned to the image sensor (first sensor) again, the two types of position / orientation are integrated to suppress the deviation of the position / orientation at the time of switching. However, the present invention can be applied even when the position and orientation estimation is successful with both of the two types of sensors. Therefore, a method of automatically switching the input sensor according to the priority associated with the position / orientation in the situation where both of the two sensors have succeeded in estimating the position / attitude will be described.

Only the processing procedure different from the processing described with reference to FIG. 4 will be described, and the description of the other procedures will be omitted assuming that the processing is the same as that of FIG.

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. The reliability is an index showing the accuracy of the estimation result of the position and posture. The reliability is calculated so that the larger the number of feature points used for estimation of the position and orientation, the higher the reliability. The acquired reliability is output to the selection unit 113 as a priority.

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

In a situation where both of the two types of sensors succeed in estimating the position / orientation, the position information acquired by the first position information acquisition unit 111 and the second position information acquisition unit 112 acquire the information according to the priority associated with the position / attitude. Automatically switches between location information. At the time of switching, the difference between the two types of position / posture is calculated, and the position / posture after switching is corrected based on the difference. As a result, it is possible to suppress the movement of the moving body from becoming unstable due to the occurrence of the displacement of the position / posture at the time of switching while selecting the position / posture with higher reliability.

<Modification example 1-8>
In the modified example 1-8, the priority is the reliability indicating the accuracy of the estimation result of the position and orientation. However, the present invention is not limited to this, and can be applied as long as the priority is given to the location information. For example, the number of feature points detected when estimating the position / orientation from the input of the image sensor may be used as the priority. Further, the priority may be determined by a process other than the position / orientation estimation. For example, if the brightness in the image is uniform, it can be estimated that the estimation of the position and orientation may fail, so the priority is set low. Alternatively, when the average value of the luminance in the image is extremely low (or high), the priority for the image sensor is set low, while the priority for the distance sensor is set high. Furthermore, if the distances of all the point clouds obtained by the distance sensor are greater than or equal to the threshold value, the position and orientation estimation accuracy may be lowered, and the priority is set low. The priority may be determined directly from the measured value of the sensor. Further, the priority may be determined by using a value other than the measured value of the sensor. For example, when the map information of a part of the route on which the moving object is moved is held and the map information is used as the input data for estimating the position and orientation, whether or not the map information for the current position of the moving object exists. May be used to determine the priority. If there is a map for the image sensor, the priority of the position / orientation estimated by the image sensor is determined high, and if there is a map for the distance sensor, the priority of the position / orientation estimated by the distance sensor is set. Decide high.

<Embodiment 2>
In the first embodiment, while the position / orientation estimation fails with one sensor, when switching to the other sensor, the difference between the position / attitude based on the two sensors is obtained, and the difference is used for correction. .. The second embodiment further describes a method of assisting the position / orientation estimation by passing the position / attitude based on the other sensor to the sensor that has failed to estimate the position / attitude.

The functional configuration in the present embodiment is different from FIG. 2, which explains the functional configuration of the information processing apparatus 11 described in the first embodiment, in that the first estimation unit 121 and the second estimation unit 131 are connected to each other.

FIG. 7 is a diagram showing a flowchart of the processing procedure in the present embodiment. The steps that are the same as those in FIG. 4, which explains the processing procedure of the information processing apparatus 11 described in the first embodiment, will be omitted, and the processing procedure different from that of the first embodiment will be described.

In step S401, the first estimation unit 121 estimates the position / orientation based on the measured value of the image sensor 12, and outputs the position / orientation to the first position information acquisition unit 111 and the second estimation unit 131. At the time of estimation, the position / posture is estimated with the previous position / posture estimation result as the initial value. If the first estimation unit 121 fails to estimate the immediately preceding position / posture, the position / orientation is estimated with the position / orientation input by the second estimation unit 131 as the initial value.

In step S402, the second estimation unit 131 estimates the position / posture based on the measured value of the distance sensor 13, and outputs the position / orientation to the second position information acquisition unit 112 and the first estimation unit 121. Similar to the first estimation unit, the position / orientation is estimated with the previous position / orientation estimation result as the initial value at the time of estimation. If the second estimation unit 131 fails to estimate the immediately preceding position / posture, the position / posture is estimated with the position / posture input by the first estimation unit 121 as the initial value.

In the second embodiment, in the system in which the position / posture estimated from the two types of sensor values is used for the movement control of the moving body, the estimation of the position / posture based on the image sensor value fails, and the position / posture to be used is estimated by the image sensor. The position and attitude are switched to the position and attitude estimated by the distance sensor. When switching, the difference between the position and orientation based on the two types of sensor values is acquired, and the position and orientation based on the second sensor value is corrected based on the difference. As a result, it is possible to prevent the movement of the moving body from becoming unstable due to the occurrence of displacement of the position and posture after switching. Further, when the estimation of the position / orientation based on the image sensor (first sensor) value fails, the position / orientation based on the distance sensor (second sensor) value is input as an estimation parameter, and the image sensor (first sensor) is used. Sensor) Assists in position and orientation estimation based on values. The position and orientation can be obtained by more efficiently utilizing the position information based on the two types of sensor values.

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

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

In the functional configuration of the present embodiment, the distance sensor 13 is replaced with the IMU, and the IMU is the first estimation unit 121 and the second estimation unit with respect to FIG. 2 for explaining the functional configuration of the information processing device 11 described in the first embodiment. The difference is that it is connected to 131.

Since the flowchart of the processing procedure in the present embodiment is the same as FIG. 4 showing the flowchart of the first embodiment, it is omitted. Here, the description of the steps that are the same as those of the first embodiment will be omitted, and the processing procedure different from that of the first embodiment will be described.

In step S102, the first position information acquisition unit 111 acquires the position / posture estimated by the first estimation unit 121 based on the input information acquired from the image sensor 12 and the IMU. In the present embodiment, as a position / orientation estimation method using an image sensor and an IMU, ORB-SLAM3 (Carlos Campos et.al, ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial SLAM3) . Use the method of Cornell University). In this method, matching is performed with an image feature detected from image data taken by a camera from a plurality of image features saved as a map for position / orientation measurement. Image feature The position / orientation that minimizes the total sum (residual) of the difference between the position / orientation calculated by combining the position and the inertial data is measured. In the present embodiment, the position / posture measurement map is created by the first estimation unit 121 before the moving body 10 starts autonomous traveling, and is held by the first estimation unit 121. Here, even when this technique is used, the estimation of the position / posture may fail depending on the measured value of the sensor, as in the SLAM technique described in the first embodiment, but the first position information acquisition unit 111 has the position / posture. If the estimation fails, the position / orientation at the time before the failure is acquired, and all the positions / attitudes after system initialization are output to the selection unit 113 and the difference acquisition unit 114.

In step S103, the second position information acquisition unit 112 acquires the position / posture estimated by the second estimation unit 131 based on the input information acquired from the IMU. In the position / orientation estimation using the IMU in the present embodiment, the position immediately after the IMU is started is set as the origin of the IMU coordinate system, and the acceleration and the angular velocity are integrated twice in a unit time, and the displacement amount is calculated to calculate the position. Use the method of estimating the posture.

According to the method of the present embodiment, even when the position / orientation of one system is estimated based on the output information of a plurality of sensors in order to improve the stability of the position / attitude measurement, the deviation of the position / attitude after switching is performed. It is possible to prevent the movement of the moving body from becoming unstable due to the occurrence. The position and orientation that should have been estimated by the image sensor before switching and the IMU are obtained by using the position and orientation estimated by the IMU after switching and the difference. Since the position / orientation of the moving body corrected in S106 approaches the position / orientation estimated by the image sensor and the IMU before switching, it becomes easy to maintain continuity.

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

For example, the position / orientation may be estimated by combining the SLAM technique using an image sensor and the odometry that estimates the position / orientation based on the rotation angle of the tire or the steering. Further, the position and orientation may be estimated by combining the SLAM technique using the image sensor and the IMU and the odometry. Further, the position and orientation may be estimated by combining SLAM technique using an image sensor and satellite positioning information by GPS.

On the other hand, the second position information acquisition unit 112 has acquired 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 may be used as long as it is a sensor that outputs input information capable of estimating the position and orientation. For example, image sensors, control information of moving objects, sensor values such as GPS may be used as input information for position / orientation estimation.

Further, the first position information acquisition unit and the second position information acquisition unit mentioned above may be combined in any combination. For example, the image sensor and GPS may be used in the first position information acquisition unit, and the image sensor and IMU may be used in the second position information acquisition unit.

The present invention supplies a program that realizes one or more functions of the above-described embodiment to a system or device via a network or storage medium, and one or more processors in the computer of the system or device reads and executes the program. It can also be realized by the processing to be performed. It can also be realized by a circuit (for example, ASIC) that realizes one or more functions. It can also be realized by a system composed of a plurality of devices (for example, a host computer, an interface device, an image pickup device, a web application, etc.).

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

Claims (14)

It is an information processing device
A first position information acquisition means for acquiring the first position information of the moving body estimated based on the input information from the first sensor fixed to the moving body, and
A second position information acquisition means for acquiring the second position information of the moving body estimated based on the input information from the second sensor fixed to the moving body, and
A selection means for selecting the position information that controls the movement of the moving body from the first position information and the second position information in the same coordinate system.
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.
When the position information selected by the selection means is switched, the difference information acquired by the difference acquisition means is used to correct the position information selected by the selection means so as to approach the position information selected before the switching. Correction means to be done,
An information processing device characterized by having. The difference acquisition means selects a combination to be the difference acquisition target from the first position information and the second position information before the position information selected by the selection means is switched, and uses the selected position information combination. The information processing apparatus according to claim 1, wherein a difference is acquired based on the above. The selection means preferentially selects the first position information, and when the position information cannot be acquired by the first position information acquisition means, the position acquired by the second position information acquisition means. The information processing apparatus according to claim 1 or 2, wherein information is selected. When switching the selection from the second position information to the first position information by the selection means,
A period determining means for determining a period from the second position information to switching to the first position information, and a period determining means.
At the beginning of the period determined by the period determining means, the second position information is prioritized, and at the end of the period, the first position information is prioritized, so that the first position information and the second position information are prioritized. A weight determination means that determines the weight for position information,
An integrated means that integrates the position information by a weighted average of the first position information and the second position information corrected by the correction means by using the weight determined by the weight determination means.
The information processing apparatus according to claim 3, further comprising. Claim 1 or 2 characterized in that the selection means selects either the first position information or the second position information based on the priority for each of the first position information and the second position information. The information processing device described in. The information processing apparatus according to any one of claims 1 to 5, wherein the selection means outputs information indicating a selection result by the selection means to a display device. The information processing apparatus according to any one of claims 1 to 6, wherein the first sensor is a sensor that inputs image information as the input information. The second sensor according to any one of claims 1 to 7, wherein the second sensor is a distance sensor that measures the distance between the second sensor and an object existing in the environment as the input information. Information processing device. The difference acquisition means acquires the difference information at predetermined intervals.
The information according to any one of claims 1 to 8, wherein the correction means corrects the position information selected by the selection means by using the latest difference information acquired by the difference acquisition means. Processing equipment. It is an information processing device
A first position information acquisition means for acquiring the first position information of the moving body estimated based on the input information from the first sensor and the second sensor fixed to the moving body, and
A second position information acquisition means for acquiring the second position information of the moving body estimated based on the input information from the second sensor, and
A selection means for selecting the position information that controls the movement of the moving body from the first position information and the second position information in the same coordinate system.
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.
When the position information selected by the selection means is switched, the difference information acquired by the difference acquisition means is used to correct the position information selected by the selection means so as to approach the position information selected before the switching. Correction means to be done,
An information processing device characterized by having. It is an information processing method
The first position information acquisition step of acquiring the first position information of the moving body estimated based on the input information from the first sensor fixed to the moving body, and
A second position information acquisition step of acquiring the second position information of the moving body estimated based on the input information from the second sensor fixed to the moving body, and
A selection step of selecting the position information for controlling the movement of the moving body from the first position information and the second position information in the same coordinate system.
A difference acquisition step of acquiring a difference in the same coordinate system between the first position information and the second position information synchronized with the first position information.
When the position information selected in the selection process is switched, the difference information acquired in the difference acquisition process is used to correct the position information selected in the selection process so as to approach the position information selected before the switching. Correction process and
An information processing method characterized by having. It is an information processing method
The first position information acquisition step of acquiring the first position information of the moving body estimated based on the input information from the first sensor and the second sensor fixed to the moving body, and
The second position information acquisition step of acquiring the second position information of the moving body estimated based on the input information from the second sensor, and the process of acquiring the second position information.
A selection step of selecting the position information for controlling the movement of the moving body from the first position information and the second position information in the same coordinate system.
A difference acquisition 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.
When the position information selected in the selection process is switched, the difference information acquired in the difference acquisition process is used to correct the position information selected in the selection process so as to approach the position information selected before the switching. Correction process and
An information processing device characterized by having. It ’s a moving body,
A first position information acquisition means for acquiring the first position information of the moving body estimated based on the input information from the first sensor fixed to the moving body, and
A second position information acquisition means for acquiring the second position information of the moving body estimated based on the input information from the second sensor fixed to the moving body, and
A selection means for selecting the position information that controls the movement of the moving body from the first position information and the second position information in the same coordinate system.
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.
When the position information selected by the selection means is switched, the difference information acquired by the difference acquisition means is used to correct the position information selected by the selection means so as to approach the position information selected before the switching. A moving body whose movement is controlled by an information processing device having a correction means. A program for making a computer function as each means of the information processing apparatus according to any one of claims 1 to 10.

Images