JP2021099384A - Information processing apparatus, information processing method, and program - Google Patents

Abstract

To prevent sudden change in a position and a posture of an object that performs position estimation by using map information when the map information is updated.SOLUTION: An information processing apparatus has: first estimation means that estimates a first position of a movable body that is estimated from a feature of feature points registered in map information and a feature of the feature points acquired from sensor information measured while the movable body travels in an environment; second estimation means that estimates a second position of the movable body on the basis of information indicating the difference between positions of the feature points included in the map information and positions of the feature points acquired from the sensor information; control means that, when the difference between the first position and the second position is larger than a threshold, moves the movable body on the basis of a path that is determined on the basis of feature points newly acquired from the sensor information; and update means that updates the map information to a map created with the feature points newly acquired from the sensor information.SELECTED DRAWING: Figure 1

Description

The present invention relates to a technique for updating map information from an image.

Until now, when a moving body is allowed to travel independently, a map for calculating the position and orientation of the sensor is created using the sensor mounted on the moving body, and the position and orientation of the sensor are calculated by referring to the created map to move. There was a technique to control the body. At this time, the map is updated based on the sensor values at multiple times in order to improve the accuracy of the map, and the map is updated based on the time-series sensor values in order to calculate the position and orientation in the area where the map is not created. It was added (updated) at any time. Patent Document 1 discloses a method of automatically generating and updating a map while a moving body moves autonomously.

Patent No. 5444952

However, in the technique of Patent Document 1, for example, if the map is updated every time information about the position of a new feature point is input, the estimation result of the position and orientation of the sensor or the moving body changes depending on the updated map. In particular, when there is a large change between the original estimation result and the updated estimation result in the position / orientation estimation of the moving body, the moving speed and direction of the moving body may change rapidly accordingly.

The present invention has been made in view of the above problems, and suppresses a sudden change in the estimation result of the self-position posture of an object whose position is estimated using the map information when the map information is updated. With the goal.

The information processing device according to the present invention that achieves the above object is an information processing device that estimates the position of a moving body based on map information indicating the position of a feature point in an environment in which the moving body moves.
The first position of the moving body estimated from the features of the feature points registered in the map information and the features of the feature points acquired from the sensor information measured while the moving body is moving in the environment is estimated. First estimation means to do
A second estimation means for estimating the second position of the moving body based on the information indicating the difference between the position of the feature point included in the map information and the position of the feature point acquired from the sensor information.
When the difference between the first position and the second position is larger than the threshold value, the control means for moving the moving body based on the path determined based on the feature points newly acquired from the sensor information. ,
It is characterized by having an updating means for updating the map information to a map generated by newly acquired feature points from the sensor information.

According to the present invention, when the map information is updated, it is possible to suppress a sudden change in the estimation result of the self-position posture of the object whose position is estimated using the map information.

Block diagram showing a functional configuration example of an information processing device Flowchart showing the flow of processing executed by the information processing device The figure which shows the hardware configuration example of an information processing apparatus Diagram showing an example of GUI Flowchart showing processing details Flowchart showing processing details Block diagram showing a functional configuration example of an information processing device Block diagram showing a functional configuration example of an information processing device Flowchart showing the flow of processing executed by the information processing device Figure showing an example of the position estimation result of the moving body before and after updating the map A diagram showing an example of the timing of updating the map

Hereinafter, embodiments will be described with reference to the drawings. The configuration shown in the following embodiments is only an example, and the present invention is not limited to the illustrated configuration.

<Embodiment 1>
In the present embodiment, an example in which the information processing apparatus 100 of the present invention is applied to the information processing system 100 will be described. The information processing system is, for example, a system composed of a mobile body, a sensor (measuring device), and an information processing device. The position and orientation of the sensor are calculated based on the sensor information measured by the sensor mounted on the moving body, the control value for controlling the moving body to the destination is calculated based on the calculated position and orientation, and the control device 11 is based on the control value. Control the moving body to the destination. The configuration of the system is not limited to this, and may be, for example, a system composed of a server that generates a control value, an external sensor that measures a mobile body, and a mobile body that can communicate with the server.

In the present embodiment, the SLAM (Simultaneus Localization and Mapping) technology, which is a technology for calculating the position and orientation while creating and updating a map based on the sensor information, is used for measuring the position and orientation of the sensor. In SLAM technology, when a map is created / updated, the map elements used for position / orientation estimation change, so that the position / orientation estimation result of the moving body calculated before and after the map update changes. Due to this change, the speed and orientation of the moving body may change rapidly.

An example as described above will be described with reference to FIG. The first example is the case where the position of the feature point is updated due to the increase in the number of measurement viewpoints. For example, when a moving body (camera) moves, the same feature point is measured from a plurality of viewpoints. As a result, the depth accuracy of the motion stereo is improved, so that a more accurate three-dimensional position of the feature point can be calculated. When the map is rewritten with the calculated position of the feature point as the update value, the position of the feature point used for the position and orientation changes (changes from 1000 white points to 2000 black points), and the estimation result of the position and orientation of the camera is obtained. It changes from 1010 to 2010. The second example is the case where the position of the feature point is updated by the discovery of the loop close. Loop closing means, for example, that the first estimated start point and the estimated position of the start point when the moving object returns to the starting point after making one round in a traveling environment are matched with each other. It is a process to make it. To determine whether or not they are at the same point, the image captured at the start point is compared with the image captured when the image is returned to or near the same point as the start point, and the image is determined from a predetermined value. When the similarity of features becomes large, it is judged that they are the same point. At this time, even if the same point is determined, the feature used is used to recalculate the three-dimensional position of the map (feature point) so that the world coordinates of the feature points included in the images captured at the same point match. The three-dimensional position of the point will change, and the estimation result of the position and orientation of the camera will also change. In situations where loop closing processing is required, or in situations where the number of feature points used for position / orientation estimation increases or decreases or the position of the feature points changes, the position estimation error of the feature points is larger than a predetermined threshold value or errors accumulate. It can happen when the environment changes. The effect of the first error varies greatly depending on the measurement accuracy of the sensor and the performance of the device that performs the estimation process. In addition, changes in the environment include, for example, layout changes in the driving environment, changes due to the movement of a person or an object, and changes in lighting conditions. When such an environmental change occurs, the feature points on the map prepared in advance may increase or decrease, or the feature points measured at the beginning may move. Therefore, it is desirable to estimate the self-position and attitude while updating the map.

In the present embodiment, it is determined whether or not the moving body is located in a predetermined range from the region where the calculated position / orientation of the sensor 10 changes greatly with the map update, that is, the update region where the elements are rewritten in the map update. .. For example, a feature point whose coordinates are rewritten is extracted, and the convex hull area surrounded by the feature point is used as an update area. Then, when the moving body is located outside the predetermined range, the map is updated. This is because the map update does not affect the running of the moving object when the distance between the map update area and the position of the moving object is large.

As shown in FIG. 1, the information processing system 100 in the present embodiment includes a sensor 10, an information processing device 1, and a control device 11. Further, the information processing device 1 is composed of an update information input unit 110, a difference acquisition unit 120, and an update method determination unit 130. The information processing device 1 of the present embodiment further includes a sensor information input unit 12, a position / orientation estimation unit 13, a map holding unit 14, an update information generation unit 15, a map update unit 16, and a control value determination unit 17. The sensor information input unit 12, the position / orientation estimation unit 13, the map holding unit 14, the update information generation unit 15, the map update unit 16, and the control value determination unit 17 may be included in an external device of the information processing device.

The sensor 10 is a sensor (measuring device) that acquires information on the outside world as sensor information. In the present embodiment, the sensor 10 is a stereo camera and acquires a stereo image as sensor information. Since the sensor 10 in the present embodiment is a camera mounted on the moving body, it images the environment in which the moving body travels.

The control device 11 is a control device for controlling a moving body based on a control value calculated by a control value determining unit 17, that is, an actuator. For example, motors, power cylinders, and power converters for driving tires and steering. In the present embodiment, the control device 11 is a motor of an operating two-wheel type tire. It is a control value and specifically, the number of rotations of two tires on the left and right. By changing the control value, the moving body can move straight and turn.

The sensor information input unit 12 inputs the result of measuring the distance between the sensor (moving body) and the surrounding environment by the sensor 10 as sensor information. In the present embodiment, the image acquired by the sensor 10 is input as sensor information and output to the position / orientation estimation unit 13. Specifically, the sensor information is an image obtained by capturing the environment around the moving body with a camera mounted on the moving body. Further, it is assumed that the relative distance between the moving body (measuring device) and the surrounding object can be estimated from this sensor information.

The position / orientation estimation unit 13 calculates the position / orientation of the sensor 10 by using the sensor information input by the sensor information input unit 12 and the map information held by the map holding unit 14. The calculated position / orientation value is output to the control value determination unit 17. The map information in the present embodiment holds keyframe group information including one or more keyframe information indicating a specific object in the real space. Using the map information, the features of the feature points obtained from the sensor information can be compared with the features of the feature points measured in advance, and the position and orientation of the sensor in the map coordinate system can be measured. In the position / orientation estimation, as is generally used in SLAM technology, the sensor position / orientation is set so that the coordinates of the index extracted from the sensor information measured by the sensor and the coordinates of the index registered in the map information match. calculate.

The control value determination unit 17 inputs the position / attitude calculated by the position / attitude estimation unit 13 and calculates the control value for driving the control device 11. The control value is the rotation speed of each of the left and right tires of the operating two-wheeled tire. The control value determining unit 17 controls the control device 11 so that the position / posture of the moving body at the target point of the moving body input by the input means (not shown) and the position / posture calculated by the position / posture estimation unit 17 match. The control value for this is calculated and output to the control device 11.

The map holding unit 14 holds the map information used by the position / orientation estimation unit 13 to calculate the position / orientation of the sensor 10. The map holding unit 14 outputs the map information to the update information generation unit 15. It may be outside the information processing device 100. The map information referred to here is a three-dimensional map such as a three-dimensional model of the environment that expresses the structure and space of the environment. Specifically, it is a set of three-dimensional point cloud data (or a combination of three-dimensional point cloud data and color information) and keyframe data. Details of map information and keyframes will be described later.

The update information generation unit 15 generates update information for updating the map based on the positions of the feature points included in the map information held by the map holding means 14 and the positions of the feature points newly acquired from the sensor information. To do. Here, the update information is information indicating the difference between the position of the feature point in the map information and the position of the feature point acquired from the sensor information when the feature point included in the map information is measured by the sensor. To do. The updated information of the generated map is output to the update information input unit 110. The method of updating the map information will be described later. Specifically, the updated information includes the following information. First, the update information is modified so that different positions on the map coordinates are matched based on the positions of known feature points included in the map information and the positions of new feature points for the same predetermined point. Information. The same point is determined by whether or not the coordinates of the key frame B calculated by the two methods have the same coordinate value (a close value because there is a calculation error). For example, a predetermined keyframe viewed from another keyframe on the world coordinates is compared with a predetermined keyframe on the world coordinates, and when the difference is within a predetermined range, it is determined that the points are the same. Secondly, the updated information is information including new feature point coordinates obtained from the feature point positions on the image measured from a plurality of viewpoints. Thirdly, the update information is information including the new position of the feature point obtained from the position of the feature point on the image measured from a plurality of viewpoints and the new position of the measured camera (keyframe coordinates). .. In other words, the update information is information for updating the map information so that the position of the feature point when the feature point in the environment is measured by the sensor is associated with the position of the feature point in the map information and is consistent. is there.

The update information input unit 110 inputs the update information of the map generated by the update information generation unit 15. The input update information is output to the difference acquisition unit 120.

The difference acquisition unit 120 has a first position of the moving body estimated from the sensor information and the map information, and a second position of the moving body estimated based on the position of the feature point newly acquired from the sensor information. Get the difference between. The sensor information indicates the three-dimensional position of the feature point that measures the environment around the moving body. The map information includes information about the three-dimensional positions of the feature points that have already been measured. Based on the update information input by the update information input unit 110 and the map information held by the map holding unit 14, the difference from the position and orientation of the sensor 10 calculated by the position and orientation estimation unit 13 accompanying the update of the map is calculated. The difference is a value corresponding to the magnitude of the change in the position / posture of the moving body calculated by the position / posture estimation unit 13 before and after the map is updated. The difference acquisition means 120 outputs the calculated difference to the update method determination unit 130. Detailed processing will be described later.

The update method determination unit 130 determines the update method of the map information based on the difference calculated by the difference acquisition unit 120. The update method in the present embodiment is a value normalized to 0 to 1 used by the map update unit 16 to determine whether or not to execute the map update. The calculated update method is output to the map update unit 16. Here, it is determined that the larger the value (closer to 1), the more the update is performed, and the smaller the value (closer to 0), the more the update is suppressed.

The map updating unit 16 rewrites the map information held by the map holding unit 14 based on the updating method. Update the map information from the update information. Depending on the update method, it may be updated at once, or it may be updated in a plurality of times. If the acquired difference is small, the map update is suppressed at that stage.

The map information in the present embodiment holds keyframe group information including one or more keyframe information. Map information is generated by moving a moving body equipped with a sensor in the environment before moving the moving body. For example, a map is generated by synthesizing sensor information from a sensor mounted on a moving body while operating the moving body with a remote controller or a hand push. Alternatively, the moving body autonomously moves in the environment and is generated by using SLAM (Simultaneus Localization and Mapping) technology. SLAM is a technology that recognizes the surrounding environment with a sensor and at the same time estimates its own position and posture with high accuracy. When keyframe data is used as map information, first select the keyframe closest to the position and orientation estimated in the previous frame, and then use the position and orientation of the previous frame to use each pixel of the current frame based on the depth map. Is projected onto the keyframe. Next, the position and orientation are estimated so that the difference in brightness is minimized by matching the pixel value of the projected frame with the pixel value of the key frame. In addition, the position and orientation of the key frame are updated by the graph optimization method at a predetermined timing such as when the loop is closed.

The key frame is a map element of the smallest unit for the position / orientation estimation unit 13 to calculate the position / orientation of the sensor, and the key frame information described later obtained from the image information taken by the camera at a certain point is obtained at that time. Hold it together with the camera position and orientation. In the position / orientation estimation, the position / orientation of the camera with respect to the keyframe is calculated based on the image captured by the camera and the keyframe information, and the position / orientation of the sensor is calculated by multiplying the position / orientation of the keyframe.

Keyframe information is the data structure of keyframes. The keyframe information is a combination of color information acquired from an image by acquiring a depth map expressed by color shading by quantizing depth information viewed from a certain viewpoint in the environment at regular intervals. The key frame may be anything as long as it is for estimating the position and orientation, and may be a combination of depth information and image information (color information) at feature points in the image, for example. An ID for uniquely identifying a key frame, image information captured by the sensor 10, feature point information such as an angle detected from the image, and a matrix representing the position / orientation value of the key frame, that is, the position / orientation of the sensor at the time when the image was taken. To hold. The feature point information includes the two-dimensional coordinates u and v (hereinafter referred to as the two-dimensional position of the feature point or the position of the feature point) detected in the image, and the feature amount (a small image obtained by cutting out a small patch around the image). ), Holds three-dimensional coordinates X, Y, Z indicating a three-dimensional position in space. The position / orientation value is a matrix value in which 6 parameters, which are a combination of 3 parameters representing the position of the image pickup device in the world coordinates defined in the real space and 3 parameters representing the posture of the image pickup device, are expressed as a 4 × 4 matrix M. That is.

The map update information in the present embodiment is the update value (keyframe update information) of the keyframe information to be updated in the keyframe group information held by the map information. The keyframe update information is included in the ID for identifying each keyframe to be updated included in the keyframe group, the updated value of the position / orientation value of the keyframe to be updated, and the keyframe information to be updated. Contains the updated value of the 3D position of the feature point. In addition, a plurality of keyframe update information is collectively referred to as keyframe group update information. The keyframe group update information includes keyframe update information.

FIG. 3 is a diagram showing a hardware configuration of the information processing device 1. 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. The 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 device 1. The input unit H15 is a keyboard, mouse, button, and switch, and performs processing related to input of information and the like. The display unit H16 outputs the calculation result to the display device according to the instruction from 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 for performing information communication via a network, and the communication interface may be Ethernet (registered trademark), and any type such as USB, serial communication, and wireless communication may be used. In the present embodiment, the input of the target position of the moving body is performed via the communication interface H17. I / O. In the present embodiment, the update information input by the update information input unit 110 is input from the update information generation unit 15, and the update method calculated by the update method determination unit 130 is output to the map update unit 16 via the I / O. To do.

Next, the processing procedure in this embodiment will be described. FIG. 2 is a flowchart showing a processing procedure of an information processing system including the information processing device 1 in the present embodiment. The processing steps include initialization S11, sensor information input S12, position / orientation estimation S13, control S14, map update determination S15, update information generation S15, map update S16, end determination S17, update information input S110, difference acquisition S120, and update method. Decision S130 is performed.

In S11, the information processing device 1 initializes the system. That is, the program and map information are read from the external memory H14 into the RAM (H13). In addition, the position / orientation estimation unit 13 acquires the following values required for estimating the position. Internal parameters of sensor 11 (camera) (focal length fx (horizontal direction of image), fy (vertical direction of image), image center position cx (horizontal direction of image), cy (vertical direction of image), lens distortion parameter) To get. Further, various parameters such as control device parameters (moving body size, tire diameter, motor torque value, etc.) are read from the external memory H14 into the RAM (H13). Further, the sensor 10 and the control device 11 are activated so that the information processing system including the information processing device 1 can be operated and controlled. After the initialization is completed, the process proceeds to the sensor information input S12.

In S12, the sensor information input unit 12 inputs sensor information measured around the measuring device (moving body). Here, the image captured by the camera, which is the sensor 10, is input. The input image is converted into a corrected image in which the influence of lens distortion is removed by using internal parameters. The corrected image is output to the position / orientation estimation unit 13. Hereinafter, the corrected image is simply referred to as an image. Next, the position / orientation estimation S13 is started.

In S13, the position / orientation estimation unit 13 determines the position of the moving body and the position of the moving body based on the position of the feature point acquired from the sensor information measured around the measuring device and the map information including the position of the feature point existing in the environment. Estimate the posture (first position posture). That is, the position and orientation of the sensor 10 are calculated using the image input by the sensor information input unit 12 and the map information held by the map holding unit 14. The method of Raul et al. Is used for estimating the position and orientation of the sensor 10. (Raul Mur-Artal et. Al, ORB-SLAM: A Versaille and Accurate Monocular SLAM System. IEEE Transitions on Robotics). First, the feature points detected from the image, which is the sensor information, are matched with the feature points included in the keyframe information. Next, the position and orientation of the sensor are calculated by solving the PNP problem (Perspective-n-Pont Problem) from the correspondence between the three-dimensional coordinates of the matched feature points and the two-dimensional positions of the detected feature points on the image. The calculated position / orientation is output to the control unit 17 and moved to S14.

In S14, the control value determining unit 17 determines a control value so that the moving body approaches a preset target point based on the position and orientation of the moving body. That is, the moving body is moved based on the map information indicating the position and orientation of the target point (or route) of the moving body and the position and orientation of the sensor 10 estimated by the position and orientation estimation unit 13. In the calculation of the control value, all possible variations of the control value that reduce the Euclidean distance between the position and orientation of the moving body and the position and orientation of the target point in the map information are calculated as candidates for the control value. A Dynamic Window Approach (DWA) is used to select the optimum control value from the candidates. The calculated control value is output to the control device 11. After outputting the control value, the process proceeds to map update S15.

In S15, the update information generation unit 15 generates update information. For update information generation, loop detection and pose graph optimization described in Raul et al.'S method are used. (Raul Mur-Artal et. Al, ORB-SLAM: A Versaille and Accurate Monocular SLAM System. IEEE Transitions on Robotics.) First, from the keyframe group information included in the map information, from the keyframe group information included in the map information, physically However, loop detection is performed to select two keyframe information calculated as different positions and orientations. Next, by optimizing the pose graph, the update value of the position / orientation value of the surrounding keyframe information and the update value of the three-dimensional coordinates of the feature points are calculated so that the position / orientation held by the two keyframe information matches. To do. The procedure will be described in more detail. First, the feature points of the key frame A and the key frame B are matched. The 3D-2D position and orientation are calculated using the 3D coordinates of the feature points of the matched key frame A and the 2D coordinates of the feature points on the image of the key frame B. (This gives the coordinates of keyframe B as seen from keyframe A). Using the calculated position and orientation of A → B, the 3D coordinates of the feature point of the key frame A → the 2D projection process onto the key frame B is performed to check whether the projection is performed at a predetermined position. The overall optimization is performed so that the coordinates of the key frame B seen from the key frame A and the coordinates held by the key frame B match. The plurality of keyframe update information calculated in this way is input to the update information input unit 110 as keyframe group update information, that is, update information, and the process proceeds to the update information input S110.

In S110, the update information input unit 120 inputs the update information generated by the update information generation unit 15 into the difference acquisition unit 120, and moves to S120.

In S120, the difference acquisition unit 120 uses the change of the sensor position in the original map information and the sensor position in the update information and the update information to estimate the position / orientation map of the position / attitude estimation unit 13. Acquire the difference indicating the magnitude of the change before and after the update. First, the distance between the keyframe to be updated and the sensor is calculated. Specifically, based on the update value of the position of the key frame to be updated and the position of the sensor included in each key frame update information in the key frame group update information, the coordinates of each key frame to be updated and the sensor Get the three-dimensional Euclidean distance to the coordinates. Next, the minimum value of the Euclidean distance calculated for each keyframe to be updated is obtained. In this embodiment, the reciprocal of the minimum Euclidean distance is used as the difference value. That is, the larger the distance (there is no effect on the position / orientation measurement), the easier it is to update the map. The difference value calculated for the position / orientation measurement is output to the update method determination unit 130, and the process proceeds to S130.

In S130, the update method determination unit 130 determines the update method used by the update method determination unit 130 to determine whether or not the map update means 16 executes the map based on the difference calculated by the difference acquisition unit 120. As one of the update methods, the value indicating the ease of update is a real number normalized to 0 to 1, and the smaller the difference value, the closer to 0, and the larger the difference value, the closer to 1. In the present embodiment, an exponential function whose base is the value obtained by multiplying the difference value calculated by the difference acquisition unit 120 in S120 by -1 is used for normalization. Based on the value indicating the ease of updating determined in this way, if the value indicating the ease of updating is equal to or greater than a predetermined threshold value, the method of updating is determined. The update method at the time of updating is output to the map updating unit 16 and the process proceeds to S17. The update method may be set to be changed according to the value indicating the ease of updating or the update information. If the value indicating the ease of updating is less than the threshold value, the map is not updated, and information indicating that the map is not updated is input to the map updating unit.

In S16, the map updating unit 16 determines whether or not to update the map information held by the map holding unit 14 based on the updating method determined by the updating method determining unit 130 in S130, and maps. To update. In the present embodiment, the map information held by the map holding unit 14 is rewritten based on the updated information of the map, and the process proceeds to S17. If the value indicating the ease of updating is less than the threshold value, the map update is suppressed at that stage, and then the process proceeds to S17. In that case, for example, the map is updated after a predetermined time, or the map is updated stepwise in a plurality of times based on the update method. For example, when the map update area and the sensor are far from each other, the map is updated, and when the map update area and the sensor are close to each other, the map update is stopped.

In S17, it is determined whether or not to terminate the system. If a command to terminate the system is input from the user via the input unit H15 such as a mouse or keyboard, the system is terminated, otherwise the system returns to S12 and the position / orientation estimation and map update processing are continued.

<Effect>
In the first embodiment, the amount of change in the position measurement value in the update can be reduced by allowing the map update when the sensor, that is, the moving body is separated from the update area. As a result, sudden changes in speed and direction of the moving body are reduced.

<Modification example 1>
In the present embodiment, the difference calculated by the difference acquisition unit 120 is calculated based on the distance between each key frame to be updated and the sensor. However, the embodiment is not limited to the above. It suffices as long as it calculates the difference value of the sensor 10 calculated based on the key frame update information and calculated by the position / orientation calculation unit 13 so that the amount of change in the position / orientation due to the map update becomes small. .. For example, as the difference, the difference in posture between the individual keyframes to be updated and the sensor may be used. The posture difference is an angle difference between the posture update value of the key frame to be updated included in each key frame update information in the key frame group update information and the posture value of the sensor calculated by the position / posture estimation unit 13. .. Specifically, the map can be updated according to the direction of travel when the moving body runs backward on the route it was orbiting, or when it reciprocates and returns. Further, the convex hull space circumscribing the key frame group to be updated may be calculated and calculated as a difference based on the distance between the surface of the convex hull space and the sensor. Furthermore, the difference acquisition unit 120 may calculate a binary value of whether or not the sensor is located in the convex hull space. That is, if the sensor is located in the convex hull, the difference is 1 and if the sensor is not located in the convex hull, the difference is 0.

In this embodiment, the reciprocal of the distance between the sensor and the surrounding object is calculated as the difference. The setting method is not limited to the reciprocal of the distance, as long as the setting method decreases the difference value as the distance increases. That is, in order to calculate the difference from the distance between the sensor and the surrounding object, an inverse cotangent function may be used, an exponential function with the base as the negative value of the distance value may be used, or the distance value may be used as the sigmoid function. May be used by substituting, multiplying by -1, and adding 1.

Further, the difference may be calculated based on the distance between the three-dimensional coordinates of the feature points included in the individual keyframe update information in the keyframe group update information and the sensor 10. In such a configuration, the feature point information that minimizes the distance between the sensor 10 and the Euclidean distance is obtained from the feature point information included in the key frame group update information. At this time, the reciprocal of the distance value from the feature point that minimizes the distance is calculated as the difference value. It is not limited to the feature point that minimizes the distance, as long as it can measure the distance from the update range. That is, a predetermined number of feature points may be selected from those having a small distance to the sensor, and the average value or the median value of the distance between the feature points and the sensor may be used. Further, the number of feature points equal to or less than a predetermined distance from the sensor may be calculated as a difference value.

The difference can also be calculated based on the size of the range included in the update information, that is, the size of the area including the keyframe group. That is, the wider the update area, the larger the difference is calculated. Specifically, the volume of the convex hull of the keyframe group to be updated described above is calculated, and the reciprocal of the volume value is calculated as the difference value. Further, the difference value may be the reciprocal value of the surface area of the convex hull space. The reciprocal of the volume of the ellipse circumscribing or inscribed in the convex hull space and the length of the major axis may be used. The convex hull is calculated so as to include the key frame group, but the convex method including the feature points included in the update information may be used.

The update method is not limited to the method shown in the present embodiment as long as it can be seen that the timing of map update is changed. For example, the difference may be calculated for each keyframe to be updated, and the update method may be calculated for each keyframe to be updated. With such a configuration, the update is controlled for each key frame, the key frame with a small difference is updated quickly, and the key frame with a large difference is not updated. By doing so, it is possible to suppress sudden fluctuations in the calculated position and orientation while steadily updating the map.

The map update unit 16 can be updated so that the position and orientation of the keyframes and feature points gradually become the updated values over time as the difference between the estimated positions of the moving objects before and after the update is large. Specifically, the map uses the position / orientation of the keyframe to be updated included in the keyframe group update information and the composite position / orientation obtained by synthesizing the position / orientation of the keyframe held by the map holding unit 14 by a weighted sum. Update the keyframes held by the holding part. As shown in FIG. 11, when the map update method (here, the change in position before and after the feature point update) is large, the amount of map update is suppressed to a predetermined threshold value, and the feature is gradually divided into a plurality of times. By correcting the point position, it is possible to prevent the camera position from changing suddenly. In particular, the larger the map update method, the more gradual the camera position can be changed by increasing the number of corrections. The weight applied to the position / orientation of the keyframe including the keyframe group is increased each time the position / orientation is measured. At this time, the increase rate is set to a smaller value as the update method is larger. By doing so, when the influence on the position / orientation estimation is large, the map is updated slowly, so that the amount of change in the position measurement value in the update is reduced, and the sudden speed change and direction change of the moving body are reduced. ..

In the present embodiment, the map update information is the update of the values of the components of the map, that is, the update of the position / orientation of the key frame and the three-dimensional position of the feature point. The map updating method in the present embodiment can also be used when the map is switched, that is, when a part of the keyframe group included in the map information is replaced with a new keyframe group held by the holding means (not shown). In the case of such a configuration, the difference acquisition unit 120 calculates the difference using the new key frame group information as the key frame group update information. By doing so, the amount of change in the position measurement value calculated even when the map is switched is reduced, and the sudden speed change and direction change of the moving body are reduced.

Furthermore, it can be applied not only to switching the map but also when loading a global map of the holding means (not shown) as a partially local map to the holding means 14. In this way, for example, when a map created by another moving body is loaded and its own map is updated (added to the map held by the holding means 14), the speed and orientation of the moving body change abruptly. Less often. Similarly, for example, when loading a part of the map on the cloud server as a local map and estimating the position and orientation, the speed of the moving object when the map is loaded (added to the map held by the holding means 14). And the orientation is less likely to change suddenly.

In this embodiment, the map information is a group of keyframes. However, the map information is not limited to this, and any index used for calculating the position and orientation of the sensor may be used. For example, when the position is measured using an object as an index for position / orientation estimation, that is, the position / orientation is calculated based on the three-dimensional coordinates of the object photographed by the camera, the object and its three-dimensional coordinates become map information. .. In such a configuration, the difference can be calculated by the method of the present embodiment by treating the updated value of the position and orientation of the object as the key frame update information in the present embodiment.

Further, the map information may have a configuration in which keyframes are not retained, that is, a configuration in which only feature points are retained. Even in such a case, the difference in the present embodiment can be calculated by using the updated value of the three-dimensional position of the feature point as the key frame update information in the present embodiment.

When LiDAR is used as a sensor, a scan point cloud measured by LiDAR or an Occupancy Grid Map (a map having a data structure in which a passable area and a passable area, that is, an area such as a wall are plotted on a grid) are used. You can also do it. In the case of such a configuration using LiDAR, the present invention can be applied if each grid is regarded as a feature point described in the present embodiment. Specifically, the difference is calculated by the method used in this embodiment based on the distance value from each grid. Not limited to this method, if the grid to be updated is regarded as an update area, the distance of the sensor from the update area can be obtained. It is also possible to determine whether or not the sensor is located in the update area depending on whether or not the grid on which the sensor is located is the update target. The present invention can also be applied when LiDAR is used as such a configuration.

In the present embodiment, the update information is the keyframe update information included in the keyframe group update information, and is the value after the update of the keyframe to be updated or the value after the update of the feature point. The update information is not limited to this, and any information that can update the map may be used. That is, it may be a modified value (difference amount) of a map element held by the map holding means 110.

Further, in the present embodiment, the configuration in which the update information is held as the keyframe update information has been described, but the keyframe update information is provided as long as the configuration holds the update value for rewriting the map element held by the map holding means 110. It is not limited to the configuration to be held as. That is, it is also possible to have a configuration in which the ID for identifying the key frame and the value of only the three-dimensional position of the updated position / orientation and the feature point are held. Further, when the Occupancy Grid Map described in the modified example is used as the map information, the cell coordinates and the rewritten value may be retained as the update information.

In the present embodiment, in order to generate the update information, a loop is detected and the update value of the position and orientation of the key frame to be updated and the update value of the three-dimensional position of the feature point are obtained by optimizing the pose graph. On the other hand, the generation of update information is not limited to the above method. For example, instead of pose graph optimization, bundle adjustment described in Raul et al.'S method can also be used. (Raul Mur-Artal et. Al, ORB-SLAM: A Versaille and Accurate Monocular SLAM System. IEEE Transitions on Robotics.) IMU (Inertial Measurement Unit) The updated value of the position / orientation of the key frame may be obtained based on the position / orientation calculated by the Kalman filter for obtaining consistency with the measured value.

Furthermore, the present invention can also be applied when adding a map, that is, adding new keyframe information to the keyframe group information held by the map information. In such a case, the method of the present embodiment can be applied by using the new keyframe information to be added as the keyframe update information.

In this embodiment, the sensor 10 is a camera. The camera may be a monochrome camera, a color camera, a stereo camera, a depth camera, or any other camera that can acquire an image that can be used for position / orientation measurement. Depending on the camera, the sensor information may be any monochrome image, RGB image, depth map, or any other information that can estimate the position and orientation.

Further, the sensor 10 may be any sensor 10 as long as it can acquire sensor information that can be used for position measurement, such as LiDAR. For example, it may be 2D LiDAR or 3D LiDAR. Depending on the type of sensor, the sensor information may be a two-dimensional point cloud or a three-dimensional point cloud as long as the position and orientation can be estimated.

The sensor 10 is not limited to the above, and any sensor that can calculate the position and orientation based on the map information and the sensor value measured by the sensor 10 may be used.

The target control device 11 is not limited to tires, but can be anything that changes the speed, acceleration, angle, angular velocity, and angular acceleration of a moving body, such as steering and the amount of rotation of a propeller if the moving body is a drone. Good.

This information processing device can be applied to all configurations for calculating the position and orientation using the updated map. For example, the moving body may be an AGV / AMR, an automatic guided vehicle, an autonomous vehicle, a drone, or an autonomous mobile robot, and the movement control described in the present embodiment may be applied to them. In addition to the moving body that walks and runs on land, it may be applied to a moving body that flies in the air, moves on the water, and moves by diving in the water.

In the embodiment, the information processing device 1 has been described as a configuration mounted on the information processing system. However, the information processing device 1 may be arranged anywhere as long as the map update method can be calculated. For example, it may be placed on a cloud server separately from the information processing system, or it may be placed on another information processing system. The operation timing is also arbitrary. In the present embodiment, the processing step also describes the sequential processing, but as a specific processing, for example, S15 to S16 may be configured to operate in parallel with S12 to S14 as separate threads.

In the present embodiment, a configuration in which a map update determination is performed each time an image is input has been described. However, the configuration is not limited to the configuration in which the map update determination is performed each time an image is input, and the map update determination may be performed at a predetermined timing.

That is, the update information generation step S15 may be configured to execute step S110 and subsequent steps each time a key frame is added. Further, the steps S110 and subsequent steps may be executed only when the newly added keyframe and the keyframe held by the map holding unit 14 are matched and a loop is detected. In this way, step S110 and subsequent steps are executed only when the map can be updated, the processing load can be reduced, and the map update determination can be made.

<Modification 2>
The information processing device 1 may include a presentation unit (not shown). For example, if a red, yellow, and green three-color lamp is used for the presentation unit, it is presented so that it is green when the map is not updated and yellow when the map is being updated, and the map is being updated to the user. It can be visualized. The color of the display is arbitrary as long as the user can visualize that the map is being updated. An LED lamp or a liquid crystal display may be used as long as it can present information on the map update status to the user. The presenting device may be a speaker, and may be configured to play a specific alarm sound or a specific melody according to the map update status.

Differences, update methods, and update ranges may be presented to the presentation unit as information related to map update. FIG. 4 is an example of the GUI 100 in which a display is used as a presentation unit to present map information, differences, and an update range.

G110 is a presentation area that presents map information and differences. G111 is an index indicating the range for updating the map and the value of the difference. In this modification, the map update area is presented so that the larger the difference, the darker the color. G112 is a movement route of a moving body. G113 is the current position of the moving body.

The G120 is a GUI for the user to adjust the parameters for updating the map. G121 is a mouse cursor, and the threshold adjustment bar shown in G122 can be moved up and down to determine how much the difference is to allow map update. G123 is an index showing the magnitude of the difference.

In this way, it is possible to present the area where the map update is easily performed by the user and adjust the threshold value for allowing the map update. By doing so, the user can intuitively understand the difference due to the map update.

Although the configuration in which the display is used for the presentation unit has been described, the presentation device may be a speaker. It can also be configured to change the loudness, pitch, and melody type of the alarm sound according to the difference in the area through which the moving body passes.

<Embodiment 2>
In the first embodiment, the difference acquisition means 120 acquires the difference using the distance from the update area. In the second embodiment, the difference is calculated based on the amount of change of the update element. In the present embodiment, the amount of change is the magnitude of the change in position due to the update of the keyframe to be updated included in the keyframe group update information. The larger the change in the position of the key frame, the larger the amount of change in the position and orientation of the sensor to be calculated. Therefore, the larger the change in the position of the key frame, the larger the difference is calculated.

The configuration of the device in the second embodiment is different from FIG. 1 showing the configuration of the information processing device 1 described in the first embodiment, in that the difference acquisition unit 120 inputs the map information of the map holding unit 14. As the hardware configuration, the configuration as shown in FIG. 3 is used as in the first embodiment.

Since the processing procedure in the second embodiment is the same as that shown in FIG. 2 showing the processing procedure of the information processing apparatus 1 described in the first embodiment, the description thereof will be omitted. The details of the processing of the difference calculation S120 are different from those of the first embodiment.

The update method determination S130 calculates the amount of change in the position of the key frame to be updated included in the key frame update information included in the key frame group update information. The magnitude of the change in position in the present embodiment is the Euclidean distance between the coordinates of the keyframe before the update and the coordinates of the keyframe after the update. The maximum value of the distance value calculated for each update target key frame included in the key frame group update information is output as a difference to the update method determination unit 130, and the process proceeds to S17.

<Effect>
In the second embodiment, the map update method is calculated based on the amount of change in the update element. That is, if the change of the map update element is not more than a predetermined value, it is updated, and if the update method is more than a predetermined value, it is not updated. With such a configuration, the amount of change in the position measurement value during the update is prevented from exceeding a predetermined value. By doing so, the sudden speed change and direction change of the moving body are reduced.

<Modification example>
In the second embodiment, the difference is calculated based on the Euclidean distance of the coordinates of the keyframes before and after the map update. However, anything can be used as long as it is calculated based on the amount of change in the map update element. That is, it may be the amount of change in the posture of the key frame. Further, the amount of change in the three-dimensional position of the feature point may be used. Furthermore, the amount of change is not limited to the geometrical change in the position and orientation of the components of the map, and may be any amount of change in the map element used for position / orientation measurement. Specifically, when the feature point holds the feature amount such as SIFT or ORB having a histogram in the gradient direction in the local region of the smoothed image as the feature amount, the feature amount may be updated. Feature update is the averaging and representative value of feature points that are considered to be the same as the three-dimensional coordinates among the feature point information included in a plurality of keyframe information. It is a replacement process. The update amount of the feature amount is obtained by, for example, the norm and the cosine similarity value of the feature amount vector before and after the update of ORB and SIFT. Not limited to the Descriptor, when a patch in a small area of a feature point is smoothed and updated from a plurality of viewpoints, values such as the total, average, and maximum value of changes in brightness before and after the patch update may be used.

Further, the ratio of the changing index among the map indexes used by the position / orientation estimation device 16 for the position / orientation estimation of the sensor may be used as the amount of change. For example, among the feature points used for position / orientation estimation, the ratio of the feature points whose three-dimensional position moves by a predetermined distance or more before and after the map update is defined as the amount of change. Moreover, not only the change of the position but also the increase / decrease of the map element may be used as the amount of change. That is, the number of feature points used for position / orientation estimation may be increased or decreased. Furthermore, the number of increase / decrease of the referenced keyframe or the ratio thereof may be used.

When LiDAR is used for the sensor 10, it is possible to calculate to which grid a specific grid element of Occupancy Grid Map moves by updating the map, and use the Euclidean values of the two grids before and after the update as the difference. Furthermore, the amount of change in the value of each grid due to map update of Occupancy Grid Map may be used as the difference value. The value of each grid referred to here is a probability value of 0 to 1 indicating whether or not an object exists in the Occupancy Grid Map. That is, as the difference, the change value of the probability value of a specific grid before and after the map update may be used.

<Embodiment 3>
In the second embodiment, the difference is calculated based on the amount of change in the elements of the map due to the update. In the third embodiment, the difference is calculated based on the amount of change in the position / orientation of the sensor 10 calculated by the position / orientation estimation unit 13 before and after updating the map, instead of the elements of the map. That is, a method of calculating the difference so that the amount of change in the calculated position / posture increases with the map update will be described.

The configuration of the apparatus and the processing step in the third embodiment are the same as those in FIG. 1, which shows the configuration of the information processing apparatus 1 described in the first embodiment, and thus are omitted. Further, as the hardware configuration, the configuration as shown in FIG. 3 is used as in the first embodiment. What is different from the embodiment is a processing step in which the difference acquisition means 120 calculates the difference.

Since the procedure of the entire process in the third embodiment is the same as that shown in FIG. 2 showing the process procedure of the information processing apparatus 1 described in the first embodiment, the description thereof will be omitted. The difference from the first embodiment is that in the difference acquisition S120, the difference acquisition unit 120 estimates the position / orientation of the sensor 10 calculated by the position / orientation estimation unit 13 after updating the map.

FIG. 5 is a diagram showing details of the process S120 of the difference acquisition unit 120 in the third embodiment. In S1210, the difference acquisition unit 120 selects a map element around the sensor 10. In the present embodiment, a predetermined number of keyframe information within a predetermined distance from the sensor 10 is selected from the keyframes held by the map holding means 110 based on the position / orientation calculated by the position / orientation estimation unit 13. Move to S1320. The selected keyframe information is referred to as neighborhood keyframe information in the present embodiment.

In S1220, the difference acquisition unit 120 matches the ID included in each keyframe information of the neighborhood keyframe information from the keyframe update information of the keyframe group update information input by the update information input unit 110. Select. Next, move on to S1220. The selected keyframe update information is called neighborhood keyframe update information.

In S1230, the difference acquisition means 130 uses the neighborhood keyframe update information acquired in S1220 and the image input by the sensor 10 to position and orient the sensor 10, that is, the camera after updating the map (hereinafter referred to as the updated position and orientation). ) Is calculated. The update position / orientation is calculated by the method described in S13, except that the update information is used instead of the map information. After calculating the update position posture, the process proceeds to S1240.

In S1240, the difference acquisition means 130 calculates the distance between the updated position / orientation calculated in S1230 and the position / orientation of the sensor calculated by the position calculation unit 13. The position / posture distance referred to here is the Euclidean distance between the coordinates of the sensor 10 calculated in S1230 and the coordinates of the sensor 10 calculated by the position / posture estimation unit 13. The reciprocal of the Euclidean distance calculated in this way is used as the difference, and the process proceeds to S130.

<Effect>
In the third embodiment, the position and orientation of the sensor are calculated using the update element of the map, and the difference that increases as the amount of change in the position and orientation of the sensor before and after the update increases is calculated. By doing so, the sudden speed change and direction change of the moving body are reduced.

<Modification example>
In the present embodiment, the position and orientation of the sensor after updating the map are calculated using the updated information. The method is not limited to the above method as long as it can estimate the amount of change in the position and orientation of the sensor before and after updating the map. For example, instead of using the neighborhood keyframe information around the sensor, the map element used when the position / orientation estimation unit 13 calculates the position / orientation of the sensor 10 may be used. That is, the feature points calculated by the position / orientation estimation unit 13 are identified. The difference acquisition unit 120 may estimate the update position / orientation based on the update value of the feature point information that matches the identified feature point included in the update information.

In this embodiment, the position and orientation of the sensor are calculated using the updated information. It is also possible to estimate the amount of change in the position and orientation of the sensor from the amount of change in the surrounding map elements without estimating the position and orientation. Specifically, the rigid body transformation is obtained so that the distance between the coordinates before and after the update of the feature points is minimized, and this is regarded as the amount of change in the position and orientation of the sensor. Further, the rigid body transformation that minimizes the distance between the coordinates before and after the update of the keyframe may be obtained.

The reciprocal of the distance of the sensor coordinates calculated before and after the update was used to calculate the difference, but any setting method can be used as long as the difference value decreases as the distance increases. It may be an inverse conjunctive function, an exponential function with the base as the negative value of the distance value may be used, or a function of substituting the distance value into the sigmoid function, multiplying by -1, and adding 1 may be used. Good.

<Embodiment 4>
In the third embodiment, the difference is calculated based on the amount of change in the position and orientation of the sensor calculated before and after the map update. In the fourth embodiment, the difference is calculated based on the amount of change in the control value of the moving body calculated before and after the map update. Specifically, a method of calculating that there is a large effect when the orientation of a moving object changes significantly due to map update will be described.

The configuration of the apparatus and the diagram of the processing steps in the fourth embodiment are the same as those of FIG. 1 showing the configuration of the information processing apparatus 1 described in the first embodiment, and thus are omitted. The difference from the first embodiment is a processing step in which the difference acquisition unit 120 calculates the difference.

Since the procedure of the entire process in the fourth embodiment is the same as that shown in FIG. 2 showing the process procedure of the information processing apparatus 1 described in the first embodiment, the description thereof will be omitted. Unlike the first embodiment, in the difference acquisition S120, the difference acquisition means 120 not only estimates the position / orientation of the sensor calculated by the position / orientation estimation unit 13 after updating the map, but also the control value determination unit 17 after updating the map. Estimate the control value to be calculated. In addition, the amount of change in the control value before and after updating the map is calculated.

FIG. 6 is a diagram showing details of processing of the difference acquisition means 120 in the fourth embodiment. S1210 to S1230 are the same as the processing procedure described in the third embodiment. The difference from the third embodiment is that the control value after the map update and the change amount of the control value before and after the map update are calculated in S1310 based on the position and orientation of the sensor calculated after the update in S1230. In the present embodiment, the control value is the rotation speed of each of the left and right tires of the operating two-wheeled tire, and the change amount of the control value is the difference in the rotation speed of each tire before and after updating the map.

In S1310, the amount of change in the control value before and after the map update is calculated. First, the control value is calculated based on the position and orientation of the sensor after the map is updated, which is calculated by the difference acquisition means 130 in S1330. The control value is calculated by the DWA method that reduces the Euclidean distance between the coordinates of the sensor after updating the map and the target position of the moving object. In the present embodiment, the target position of the moving body is input to the difference acquisition means by an input means (not shown). Next, the difference between the calculated control value after updating the map and the control value calculated by the control value determination unit 17 is calculated. In the present embodiment, the control value is the number of revolutions of the operating two-wheeled tire. The average value of the difference in the number of revolutions before and after updating the map of the operating two-wheeled tire is calculated as the difference.

<Effect>
In the fourth embodiment, the difference is calculated as the amount of change in the control value of the moving body calculated before and after the map update increases. By doing so, the sudden speed change and direction change of the moving body are reduced.

<Modification example>
In the fourth embodiment, the difference is the average value of the rotation speeds of the operating two-wheeled tires before and after the map update, but any one that reduces the difference between the control values before and after the map update may be used. If an operating two-wheeled tire is used as in the present embodiment, the value on which the difference in the number of revolutions before and after updating the map is larger may be used as the difference. The smaller value may be used as the difference. The difference in the weighted rotation speed multiplied by a predetermined weight may be used as the difference. Further, in the present embodiment, the case where the control unit is an operating two-wheel tire has been described, but if it can be calculated as a difference that increases as the difference in control values due to map update increases, it depends on the mode of the control unit. Absent.

In the fourth embodiment, DWA is used for calculating the control value. Not limited to DWA, any method may be used as long as it can calculate a control value that can be compared before and after updating the map. Graph Search Approach is a method of calculating variations of control values at multiple times in the future, calculating position / orientation (predicted locus) at multiple times, and selecting control values that follow the predicted locus instead of one time. May be used. In addition, a certain area without obstacles is randomly sampled from the current position in the two-dimensional space. Any method can be used, such as Randomized Approach, which selects a predicted locus generated by repeatedly sampling the surrounding space at that point as a node.

Further, in the fourth embodiment, the control values are directly calculated and compared, but the amount of change of the element for calculating the control value may be used as the difference without calculating the control value. Specifically, it is the amount of change in the relative target position. The amount of change in the relative target position referred to here is the Euclidean distance between the coordinates of the target position with respect to the position and orientation of the sensor 10 before updating the map and the coordinates of the target position with respect to the position and orientation of the sensor 10 before updating the map. The calculated Euclidean distance is used as the difference value. Not limited to the Euclidean distance, the directional difference of the target position seen from the sensor 10 before and after the map update, the norm of the difference vector between the two target positions, and the like are arbitrary.

<Embodiment 5>
In the first to fourth embodiments, the difference is calculated based on the internal state of the information processing system, such as the change in the position and orientation of the sensor and the control value of the moving body due to the map update. In the fifth embodiment, the difference is calculated according to the situation around the moving body. In the present embodiment, the situation around the moving body is the arrangement of surrounding objects. A method of realizing a configuration in which the map is not updated when the moving body is close to the object and the map is updated when the moving body is away from the object will be described.

The configuration of the apparatus and the diagram of the processing steps in the fifth embodiment are the same as those of FIG. 2 showing the configuration of the information processing apparatus 1 described in the first embodiment, and thus are omitted. The difference from the first embodiment is the processing content of the processing step in which the difference acquisition means 130 calculates the difference. As the hardware configuration, the configuration as shown in FIG. 3 is used as in the first embodiment.

Since the procedure of the entire process in the fifth embodiment is the same as that shown in FIG. 4 showing the process procedure of the information processing apparatus 1 described in the first embodiment, the description thereof will be omitted. The difference from the first embodiment is that in the difference acquisition S130, the difference acquisition means 130 calculates the difference based on the distance value between the feature point and the sensor as the situation around the moving body.

In S130, the difference acquisition unit 120 Euclidean the three-dimensional position of each feature point included in all the keyframe information included in the map information held by the map holding unit 14 and the position calculated by the position / orientation estimation unit 130. Calculate the distance. Of the calculated individual Euclidean distances, the reciprocal of the minimum value is calculated as the difference.

<Effect>
In the fifth embodiment, the difference is calculated according to the surrounding situation, that is, the distance from the surrounding object. That is, the closer the distance to the surrounding object, the larger the difference value. By doing so, the closer the moving body is to the surrounding object, the less the sudden speed change and direction change.

<Modification example 1>
In the present embodiment, the surrounding state is the arrangement of surrounding objects, and the difference is calculated based on the distance between the sensor and the feature point. However, any method that can measure the distance between the sensor and surrounding objects will do.

For example, when the sensor 15 has a configuration such as a depth camera or a stereo camera capable of acquiring a depth value, the depth value that can be acquired from those cameras may be used as a distance to a surrounding object.

In addition, the depth value acquired using CNN (Convolutional Neural Network) learned to infer the depth map (data structure in which the depth value is stored in each pixel of the image) by inputting the image is the distance to the surrounding objects. May be used as.

When the sensor 15 is LiDAR, the distance to the surrounding measurement point may be used as the distance to the surrounding object.

If a moving body holds a distance sensor such as an infrared sensor or an ultrasonic sensor, those measured values may be used as the distance to a surrounding object.

Even if the moving body is not equipped with a distance sensor, the distance between the moving body and surrounding objects may be measured by using a sensor mounted on an object other than the moving body such as a surveillance camera or a proximity sensor. As long as the layout diagram around the moving body is held by the holding means (not shown), the distance to the surrounding object may be calculated from the position / orientation of the sensor calculated by the position / orientation estimation unit 13 and the layout diagram.

In this embodiment, the distance between the sensor and the surrounding object is used for difference acquisition. Since the moving body has a volume in a certain space, the shape model of the moving body may be held, and the shortest distance from the surface of the moving body to the object may be used as the distance value of the present embodiment. By doing so, the distance between the surrounding object and the moving body can be measured more accurately, and the difference can be calculated more accurately, so that the moving body can be operated more safely and stably. can do.

The reciprocal of the minimum distance between the sensor and each of the surrounding feature points was used to calculate the difference, but any setting method can be used as long as the difference value decreases as the distance increases. It may be an inverse conjunctive function, an exponential function with the base as the negative value of the distance value may be used, or a function of substituting the distance value into the sigmoid function, multiplying by -1, and adding 1 may be used. Good. Further, although the minimum distance between the sensor and each surrounding feature point is used for calculating the difference, the average value or the median value of the distance between the sensor and the feature points within a predetermined distance may be used.

<Modification 2>
In the present embodiment, the difference is calculated according to the distance to the object, but the attribute of the object may be used as the surrounding situation. When a camera is used as the sensor 15, an object is detected using a CNN learned in advance to determine the type of the object from the image of the camera, and the difference is calculated according to the type and the number of the objects. You can also. For example, the difference may be increased if a person or other moving object is nearby. By increasing the difference, the risk of the moving object suddenly changing its direction or speed as the map is updated is suppressed. The difference may be increased as the number of people and other moving objects increases. Furthermore, the difference may be increased near doors and stairs. You may recognize fragile objects and increase the difference. The difference may be increased as the weight of the surrounding object increases, or conversely, the difference may be calculated as the weight decreases depending on the application. Further, the difference may be increased as the moving speed of surrounding objects is increased. The measurement method is arbitrary as long as the moving speed of surrounding objects can be known. The change in the position of the surrounding moving object may be measured from the time-series image, or if the surrounding moving object is another moving object, the moving speed of the other moving object is received from the moving object management system (not shown). You may.

By doing so, when there are people or other moving objects, doors, stairs, expensive objects, etc. in the vicinity, or when the speed of moving objects of surrounding objects is high, the moving objects suddenly accompany the map update. Changes in speed and orientation are less likely to occur, and moving objects can be operated more safely and stably.

<Modification example 3>
As the surrounding situation, the difference may be calculated using the point information so that the difference increases at the point where the speed or direction of the moving body is not to be changed suddenly. Specifically, for example, the difference may be increased at a corner or an intersection. Further, if a specific object such as a door or an automatic machine is registered in the map or the operation route, the difference may be increased. Further, if the route is narrow, the difference may be increased. Furthermore, based on the operation route, the difference may be increased as long as the other moving body is traveling on a route that can operate in the vicinity.

Such point information may be detected in the area from the map or route information held by the map holding means, or may be input from a mobile object management system (not shown) via a communication I / F (H17). Good.

Further, if the sensor 10 is located in a specific setting area such as an intrusion detection area or a map update prohibited area, which is set in advance by a person by an input means (not shown), the difference may be increased.

<Embodiment 6>
In the fifth embodiment, the difference is calculated based on the object information around the moving body, that is, the distance to the object. In the sixth embodiment, the difference is calculated based on the situation of the moving body. The state of the moving body in the present embodiment is the mass of the moving body. As the mass of the moving body increases, the kinetic energy associated with the sudden speed and orientation change of the moving body increases. In such a case, a configuration for calculating a larger difference as the mass of the moving body is larger will be described.

The configuration of the apparatus and the diagram of the processing steps in the sixth embodiment are the same as those of FIG. 1 showing the configuration of the information processing apparatus 1 described in the first embodiment, and thus are omitted. What is different from the embodiment is a processing step in which the difference acquisition means 120 calculates the difference. As the hardware configuration, the configuration as shown in FIG. 3 is used as in the first embodiment.

Since the procedure of the entire process in the sixth embodiment is the same as that shown in FIG. 2 showing the process procedure of the information processing apparatus 1 described in the first embodiment, the description thereof will be omitted. In the first embodiment, in the difference acquisition S120, the difference acquisition means 120 calculates the difference based on the state of the moving body.

In S120, the difference acquisition means 120 acquires the mass information of the moving body held by the holding means (not shown). In this embodiment, the mass of the moving body is used as the difference value.

<Effect>
In the sixth embodiment, the difference is increased as the state of the moving body, that is, the mass of the moving body is increased. By doing so, the heavier the mass of the moving body, the less the sudden speed change and direction change of the moving body due to the map update.

<Modification example 1>
In the present embodiment, as the situation of the moving body, the difference is calculated based on the mass of the moving body. However, any method may be used to calculate the difference as the kinetic energy and potential energy of the moving body increase.

When the moving body carries an object, the mass of the moving body may be the mass including the transported object. Regardless of the method of inputting the mass of the transported object, it may be acquired from a mobile object management system (not shown) via a communication I / F (H17), or may be input by a user from a GUI (not shown). Alternatively, the measurement value of the weight measuring device (not shown) mounted on the moving body may be input from the I / O.

That is, the larger the moving speed and the angular velocity of the moving body, the larger the difference can be calculated. In addition, when a moving object floats in the air such as a drone, it is possible to calculate a larger difference as the altitude increases.

The situation of the moving body may be determined according to the priority and importance of the task of the moving body. For example, as the status of the moving body, the operation information of the moving body such as moving, stopping, carrying luggage, charging, and loading luggage may be used. The difference value based on the operation information is input by the user in advance using an input means (not shown). By doing this, for example, the map is not updated while the luggage is being transported, and the map is updated when the position measurement is not used, such as when the vehicle is stopped or charging. Will be able to update.

As the situation of the moving body, the difference may be calculated based on the attribute of the moving body. For example, the higher the rigidity of the moving body, the higher the difference may be calculated. Further, the moving body having a longer braking distance may be configured to calculate a higher difference. The smaller the detection distance setting parameter of the obstacle sensor, the higher the difference may be calculated. The price of the mobile itself may be used as an attribute of the mobile.

Further, as the situation of the moving body, the difference may be calculated based on the attributes of the loaded cargo. For example, the difference acquisition means 120 can acquire the price of the transported object from a mobile object management system (not shown), and the higher the price, the higher the difference can be calculated. Further, the higher the rigidity of the transported object and the heavier the weight, the higher the difference may be calculated. Further, a higher difference may be calculated as the number of transported items increases, or a higher difference may be calculated if the transported items are not stably stacked. Stable stacking means, for example, that the inclination of the luggage is small, and the luggage is photographed with a camera (not shown) and the inclination of the luggage is measured for determination. In this way, it is possible to reduce the sudden speed and orientation changes of the moving object due to the map update, depending on the attributes of the transported object.

<Modification 2>
Further, the difference acquisition means 12 may calculate the combined difference by combining the differences described in the first to sixth embodiments. The combination method is arbitrary as long as the composite difference is a method in which the composite difference increases as the change in the position / orientation of the sensor calculated before and after the map update and the control value of the moving body, that is, the moving body increases. Further, as described in the fifth and sixth embodiments, any method may be used as long as it can reduce the sudden speed and orientation change of the moving body due to the map update based on the state of the moving body and the attributes of surrounding objects. .. That is, it may be a weighted sum of various differences calculated by the methods described in the first to sixth embodiments, or it may be a weighted product. Furthermore, the synthetic difference may be calculated by combining only the methods of a specific embodiment. Further, the update method determining means 14 may determine the update method using the composite difference. By doing so, the map can be updated so as to satisfy the various conditions described in the first to sixth embodiments, and the moving body, that is, the moving body can be operated more safely and stably. ..

<Embodiment 7>
In the first to sixth embodiments, examples of applying the present invention to a mobile body have been described. The present invention is not limited to being mounted on a moving body, and can be applied to all configurations in which position and orientation are estimated based on sensors and map information and the map can be updated. In this embodiment, as one of its application examples, the processing apparatus of the present invention is mounted on a device that realizes MR (Mixed Reality), AR (Augmented Reality), and VR (Virtual Reality). The device is, for example, a device such as an HMD (Head Mounted Display), a smartphone, or a tablet. The configuration used for the information processing device mounted on these devices and used for calculating the presentation position of CG will be described.

FIG. 7 is a diagram showing an example of hardware configuration when the information processing device 1 in this embodiment is applied to an MR system. It is composed of a sensor 10, a display device 20, a sensor information input unit 12, a position / orientation estimation unit 13, a map holding unit 14, an update information generation unit 15, a map update unit 16, and a display information generation unit 21. Further, the information processing device 1 includes an update information input unit 110, a difference acquisition unit 120, and an update method determination unit 130.

The sensor 10, the sensor information input unit 12, the position / orientation estimation unit 13, the map holding unit 14, the update information generation unit 15, the map update unit 16, the update information input unit 110, the difference acquisition unit 120, and the update method determination unit 130 are implemented. Since it is the same as the configuration described in the first embodiment, the description thereof will be omitted. The difference from the first embodiment is that the display device 20 and the image component 21 are added.

The display information generation unit 21 renders a CG image of a virtual object using the position / orientation input by the position / orientation estimation unit 13 and the internal / external parameters of the camera held by the holding unit (not shown), and the sensor information input unit 12 renders the CG image. A composite image in which CG is superimposed on the input input image is generated. Further, the composite image is output to the display unit 13.

The display device 20 is a display of a mobile terminal, and displays a composite image generated by the display information generation unit 21.

Since the processing procedure in the seventh embodiment is the same as that shown in FIG. 2 showing the processing procedure of the information processing apparatus 1 described in the first embodiment, the description thereof will be omitted. In the first embodiment, the display information generation step is performed instead of the control value determination S14.

In the display information generation step, the display information generation unit 21 renders the CG image of the virtual object using the position / orientation of the sensor 10 derived by the position / orientation estimation unit 13 in S13, and superimposes and synthesizes the composite image on the input image. It is generated, input to the display device 20, and presented.

<Effect>
In the seventh embodiment, the map is updated so that the amount of change in the position and orientation calculated before and after the map is updated is small, so that the position and orientation by updating the map in the vicinity of the space where the user is using MR, AR, and VR. The sudden change of is small. As a result, CG deviation and CG skipping are small, and the user can present the presentation without being noticed, and the MR, AR, and VR experience of the user can be improved.

In the present embodiment, the right side of calculating the difference by the method described in the first embodiment has been described, but the difference in the present embodiment is the embodiment as long as the sudden change in the position and orientation due to the map update is small. It can be calculated by any method described in 1 to 6. The amount of change of the update element may be used as in the second embodiment. It may be calculated based on the amount of change in the position and orientation of the sensor calculated before and after updating the map as in the third embodiment. In the present embodiment, the control value as in the fourth embodiment is not calculated, but the target position can be read as the CG presentation position, and the difference can be calculated so that the CG position does not change. Further, as in the fifth embodiment, the difference may be calculated according to the distance to the surrounding object. As in the sixth embodiment, the difference may be calculated based on the state of the MR system, that is, the moving speed of the MR system, whether or not CG is being presented as the operating state, and the weight of the MR system.

Furthermore, the difference may be calculated based on attributes such as the type and size of the CG content. That is, the attribute that makes it easier for the user to notice the CG blur is calculated as the difference. For example, as the size of CG increases, a higher difference may be calculated. The higher the contrast, the higher the difference may be calculated. The higher the CG is in the center of the field of view, the higher the difference may be calculated. Furthermore, as a configuration in which an eye tracking device (not shown) is mounted, the value of the difference may be increased as the eye movement is smaller. With such a configuration, the CG blur perceived by the user due to the map update is reduced, and the user can experience MR, AR, and VR more comfortably.

<Embodiment 8>
In the first to seventh embodiments, the map update method is determined based on the calculated position / orientation of the sensor and the difference to the control value of the moving body due to the map update. In the seventh embodiment, a method of controlling the moving body in advance so that the speed and the direction of the moving body do not suddenly change with the map update will be described. As the hardware configuration, the configuration as shown in FIG. 3 is used as in the first embodiment. Here, the mobile body may be equipped with the hardware as shown in FIG. The moving body controls itself and updates the map using the result of measuring the surroundings with a sensor. Further, the hardware configuration example as shown in FIG. 3 may be implemented in a higher-level system that controls the entire factory or a middle-level system that controls the mobile body without mounting the hardware on the mobile body itself. In that case, the map information is updated while each moving body and the system exchange sensor information and map information by communication I / F.

FIG. 8 is a diagram showing a functional configuration example of the information processing device 20 according to the present embodiment. The information processing system in the present embodiment includes a sensor 10, a control device 11, a sensor information input unit 12, a position / orientation estimation unit 13, a map holding unit 14, an update information input means 15, and a map update unit 16. The information processing device 2 is composed of an update information input unit 110, an update position / orientation estimation unit 210, and a control value determination unit 220. Of these, the sensor 10, the control device 11, the sensor information input unit 12, the position / orientation estimation unit 13, the map holding unit 14, the update information input means 15, the map update unit 16, and the update information input unit 110 are the same as those in the first embodiment. Since they are the same, the description thereof will be omitted.

The difference from the first embodiment is that the difference acquisition means 12 is removed, and the update position / orientation estimation unit 210 and the control value determination unit 220 are newly added to the information processing device 2.

The update position / orientation estimation unit 210 predicts the position / orientation of the sensor after the map update calculated by the position / attitude estimation unit 13 based on the update information input by the update information input unit 110. That is, the updated position / posture estimation unit 210 estimates the predicted position / posture (second position / posture) of the moving body estimated based on the acquired new feature points. The calculated predicted position / orientation is output to the control value determination unit 220.

The control value determination unit 220 calculates the position / orientation calculated by the position / attitude estimation unit 13 based on the predicted position / attitude calculated by the update position / attitude estimation unit 210 so that the difference between the positions becomes small. The calculated control value is output to the control device 11.

FIG. 9 is a flowchart showing a processing procedure of the information processing apparatus 2 according to the present embodiment. The processing steps include initialization S11, sensor information input S12, position / orientation estimation S13, map update determination S15, update information generation S15, map update S16, end determination S17, update information input S110, update position / attitude estimation S210, and control value determination. It is composed of S220.

The initialization S11, the sensor information input S12, the position / orientation estimation S13, the map update determination S15, the update information generation S15, the map update S16, the end determination S17, and the update information input S110 are the same as those in the first embodiment, and thus the description thereof will be omitted. ..

In the update position / attitude estimation S210, the update position / attitude estimation unit 210 estimates the position / orientation calculated by the position / attitude estimation unit 13 after updating the map as the predicted position / attitude based on the update information input by the update information input unit 110. Specifically, from the feature point information held by each keyframe update information in the keyframe update information included in the update information, a feature point within a predetermined distance from the sensor is selected, and the relevant feature points before and after the map update are selected. The rigid transformation is obtained from the amount of movement of the feature points. Rigid transformation is a transformation matrix that uniquely transforms one three-dimensional coordinate into another three-dimensional coordinate, and is a 4-by-4 matrix in the present embodiment. The rigid transformation in such a configuration seeks to minimize the distance between the pre-update and post-update positions of the feature points. This is calculated as the amount of change in the position and orientation of the sensor, and the predicted position and orientation is obtained by multiplying this by the sensor position and orientation before updating the map.

In the control value determination S220, the control value for controlling the moving body is calculated so that the predicted position / posture estimated by the update position / posture estimation unit 210 is obtained. Specifically, a control value that reduces the Euclidean distance from the position / orientation of the sensor 10 calculated by the position / attitude estimation unit 13 is calculated with the predicted position / orientation as the target. In this embodiment, the DWA method is used to calculate the control value.

<Effect>
In the eighth embodiment, the position and orientation of the sensor calculated after updating the map is predicted in advance based on the change of the elements accompanying the update of the map, and the control value for moving the moving body in advance so as to be the predicted position and orientation is calculated. By doing so, the sudden speed change and direction change of the moving body are reduced.

<Modification example>
In the eighth embodiment, the position and orientation of the sensor after the map update is predicted based on the update amount of the map elements around the input position and orientation. As a method of calculating the position / orientation of the sensor after updating the map, the position / orientation may be calculated by using the updated value of the map element and the sensor information. Specifically, after the map is updated, the update position / orientation estimation unit 210 uses the keyframe group update information included in the image and the update information to calculate the position / orientation in the same manner as the position / attitude estimation unit 13. It is also possible to calculate the position and orientation of the sensor.

In the eighth embodiment, the control value is calculated so as to be the predicted position / orientation estimated by the update position / attitude estimation unit 210. The control value to be calculated is not limited to this method as long as it can reduce the sudden change in the speed and orientation of the moving body. For example, in order to prevent a sudden change in the moving direction of the moving body, only the posture may be calculated in advance so as to match the predicted posture. A control value may be calculated such that deceleration occurs when the position and orientation of the moving body are predicted in the traveling direction, and conversely, acceleration is performed when the position and orientation change in the opposite direction of travel is predicted.

In the eighth embodiment, the control value is calculated so as to be the predicted position / orientation of the sensor after the map is updated. On the other hand, instead of the predicted position of the sensor, the predicted value of the control value associated with the map update may be calculated, and the control value such that the difference between the control value before the map update and the predicted control value becomes small may be calculated. Good. Specifically, it is calculated from the control value calculated based on the predicted position / orientation of the sensor after updating the map and the weighted average of the current control values. By doing so, it is possible to reduce a sudden change in the control value.

In the eighth embodiment, the control value is calculated so as to be the position and posture after the map is updated. Further, the map update unit 16 may be configured to receive the progress of control from the moving body, and may wait for the execution of the map update until the position and orientation after the map update is reached. In this way, the moving body can be operated more safely and stably by updating the map after the control of the moving body is completed and the influence of the change in the position and orientation due to the updating of the map is reduced.

<Embodiment 9>
In the eighth embodiment, the control value is calculated so that the moving body becomes the position and posture after updating the map. Further, the control value can be calculated based on the difference described in the first to sixth embodiments. In the present embodiment, a configuration will be described in which the control value for calculating the control value such that the moving body is separated from the range in which the difference due to the map update is larger than the predetermined range is calculated by using the difference described in the first embodiment.

Since the configuration of the information processing apparatus in this embodiment is the same as that in the eighth embodiment, the description thereof will be omitted. As the hardware configuration, the configuration as shown in FIG. 3 is used as in the first embodiment. The difference from the eighth embodiment is that the update position / orientation estimation unit 210 shown in the eighth embodiment replaces the difference acquisition unit 120 described in the first embodiment, and the control value determination unit 220 inputs the difference calculated by the difference acquisition unit 120. To do. It is a point to calculate a control value that moves to a point where the difference becomes small. Further, the difference acquisition unit receives the movement route information held by the holding means (not shown). The movement route information is a list of coordinates to reach the destination.

Since the processing steps in the present embodiment are the same as those in FIG. 8 of the processing steps described in the eighth embodiment, the description thereof will be omitted. The difference from the eighth embodiment is that the update position / orientation estimation step 210 is replaced with the difference acquisition S120 described in the first embodiment, and the processing content of the control value determination S220.

In the control value determination S220 in the present embodiment, the control value determination unit 220 calculates the control value of the moving body with the outside of the map area where the difference calculated by the difference acquisition means 120 in the difference acquisition S120 is larger than the predetermined value as the target position. .. The target position outside the map area is defined as the point outside the map area for the first time by tracing the movement route information of the moving body in order from the current position and orientation of the sensor. The control value for moving to the target position is calculated by the DWA method.

<Effect>
In the ninth embodiment, the moving body is controlled to move out of the region where the change in the position / orientation of the sensor and the control value of the moving body due to the map update calculated based on the difference is large. By doing so, the sudden speed change and direction change of the moving body are reduced.

<Modification example>
As the difference acquisition S130 in the ninth embodiment, any method of the difference calculation method described in the first to sixth embodiments may be used.

The control value determining unit may use any method as long as it calculates a control value that reduces the difference. Specifically, for example, to a control value that moves away from the update range, a control value that moves to a point where the amount of change in the components of the map is small, or a point where the distance between the moving object and the surrounding object increases. It is a control value to move.

The method of searching for the target position at the point where the difference decreases is also arbitrary. The map area of the moving body may be divided into grids, the difference may be calculated for each grid, and the grid with the lowest difference may be calculated as the target position. By doing this, the map is updated without moving too far. Alternatively, the target point may be reselected by randomly selecting a moving route on the moving route of the moving body and selecting a point having a difference lower than a predetermined value. By doing so, the map is updated when the predetermined target point is reached without changing the route of the moving object.

In the ninth embodiment, the control value for moving the moving body to a point where the difference is smaller than a predetermined value is calculated. However, the method is not limited to the above method as long as it is a method of calculating a control value in which the speed and direction of the moving body do not change suddenly with the map update. For example, when the moving body is located at a point where the difference is larger than the predetermined value, the control value may be calculated so that the maximum value of the change amount of the control value of the moving body is equal to or less than the predetermined value. A control value may be calculated in advance so that the speed or angular velocity of the moving body is equal to or less than a predetermined value. A control value for pausing may be calculated. Further, the control value may be calculated so that the distance to the surrounding object increases.

In the ninth embodiment, the control value for moving the moving body to a point where the difference is smaller than a predetermined value is calculated. On the other hand, a point where the map element used for position / orientation estimation is smaller than a predetermined amount of change may be obtained from the difference, and the control value may be calculated so as to move to that point.

The method of calculating such a point is also arbitrary. For example, a range that can be measured by a sensor at an arbitrary point on the map may be calculated, and a point in which the movement amount associated with map update in that range is larger than a predetermined value and the ratio of feature points is less than a predetermined value may be calculated. A point where the proportion of keyframes in which the amount of movement due to map update is larger than the predetermined value is less than the predetermined value may be calculated.

<Summary of effects>
In the first embodiment, the amount of change in the position measurement value in the update can be reduced by allowing the map update when the sensor, that is, the moving body is separated from the update area. As a result, sudden changes in speed and direction of the moving body are reduced.

In the second embodiment, the map update method is calculated based on the amount of change in the update element. That is, if the change of the map update element is equal to or less than the predetermined value, the map is updated, and if the update method is greater than or equal to the predetermined value, the update is not performed. With such a configuration, the amount of change in the position measurement value during the update is prevented from exceeding a predetermined value. By doing so, the sudden speed change and direction change of the moving body are reduced.

In the third embodiment, the position and orientation of the sensor are calculated using the update element of the map, and the difference that increases as the amount of change in the position and orientation of the sensor before and after the update increases is calculated. By doing so, the sudden speed change and direction change of the moving body are reduced.

In the fourth embodiment, the difference is calculated as the amount of change in the control value of the moving body calculated before and after the map update increases. By doing so, the sudden speed change and direction change of the moving body are reduced.

In the fifth embodiment, the difference is calculated according to the surrounding situation, that is, the distance from the surrounding object. That is, the closer the distance to the surrounding object, the larger the difference value. By doing so, the closer the moving body is to the surrounding object, the less the sudden speed change and direction change.

In the sixth embodiment, the difference is increased as the state of the moving body, that is, the mass of the moving body is increased. By doing so, the heavier the mass of the moving body, the less the sudden speed change and direction change of the moving body due to the map update.

In the seventh embodiment, the map is updated so that the amount of change in the position and orientation calculated before and after the map is updated is small, so that the position and orientation by updating the map in the vicinity of the space where the user is using MR, AR, and VR. The sudden change of is small. As a result, CG deviation and CG skipping are small, and the user can present the presentation without being noticed, and the MR, AR, and VR experience of the user can be improved.

In the eighth embodiment, the position and orientation of the sensor calculated after updating the map is predicted in advance based on the change of the elements accompanying the update of the map, and the control value for moving the moving body in advance so as to be the predicted position and orientation is calculated. By doing so, the sudden speed change and direction change of the moving body are reduced.

In the ninth embodiment, the moving body is controlled to move out of the region where the change in the position / orientation of the sensor and the control value of the moving body due to the map update calculated based on the difference is large. By doing so, the sudden speed change and direction change of the moving body are reduced.

<Summary of definitions>
The update information input unit may be anything as long as it inputs the update information of the map element. For example, if the map element is a keyframe or a feature point, the position / orientation of the keyframe or the updated value of the three-dimensional coordinates of the feature point may be input. The key frame is a map element of the smallest unit for calculating the position and orientation of the sensor. The feature point is an index necessary for estimating the position and orientation. If the map element is an Occupancy Grid Map (a map that is a data structure in which a passable area and a passable area, that is, an area such as a wall are plotted on a grid), an updated value of a value held in each grid may be input. ..

The difference acquisition unit may be any one that calculates a value that increases as the amount of change in the position and orientation of the sensor increases with map update.

For example, it may be calculated based on the update area of the map. That is, the larger the update area of the map and the closer the distance between the update area and the sensor, the larger the value may be calculated. If the sensor is located in the update area, the difference value may be calculated to be larger than when the sensor is not located.

It may be calculated based on the amount of change in the map update element. That is, the ratio of the map elements in which the amount of movement of the map elements due to the map update and the amount of change in the map elements due to the map update among the map elements used for calculating the position and orientation of the sensor may be larger than a predetermined ratio.

The difference value is increased as the amount of change in the position and orientation of the sensor calculated before and after the map update increases.

The larger the amount of change in the control value for controlling the moving object due to the map update, the larger the difference may be calculated.

The difference may be calculated based on the surrounding conditions of the sensor. That is, the closer the distance to the surrounding object, the larger the difference may be calculated. As a specific surrounding situation, for example, it may be calculated that the difference increases as the type, size, number, moving speed, weight, and price of surrounding objects increase.

The difference may be calculated based on the situation of the moving body on which the sensor is mounted. That is, the heavier the weight of the moving body or the object on which it is mounted, the higher the difference may be calculated. The lower the stability of how the mounted objects are stacked, the higher the difference may be calculated.

The update method determination unit may be anything as long as it calculates a value that does not update the map as the difference increases. For example, the value of the update method may be a real number normalized to 0 to 1, and the update method may be configured so that the smaller the difference value is, the closer the update method is to 0, and the larger the difference value is, the closer the update method is. At this time, the normalization method of the difference value is arbitrary. That is, it may be an inverse cotangent function of the difference value, or an exponential function with the negative value of the difference value as the base may be used. A function obtained by substituting a difference value into a sigmoid function, multiplying it by -1, and adding 1 may be used.

The control value determining unit may be calculated by any method as long as the change in the position and orientation of the sensor due to the map update or the change in the control value due to the map update is reduced. For example, the control value for controlling the moving body may be calculated so as to be the position and orientation after updating the map, or the control value may be calculated so as to approach the control value calculated after updating the map. Further, the control value may be calculated so that the difference calculated by the difference acquisition unit is reduced.

Of the above-mentioned processing units, the position / orientation estimation unit 13, the update information generation unit 15, and the like may be processed by using a machine-learned trained model instead. In that case, for example, a plurality of combinations of input data and output data to the processing unit are prepared as training data, knowledge is acquired from them by machine learning, and output data for the input data is obtained based on the acquired knowledge. Generates a trained model that outputs as a result. The trained model can be constructed by, for example, a neural network model. Then, the trained model performs the processing of the processing unit by operating in collaboration with the CPU, GPU, or the like as a program for performing the same processing as the processing unit. The trained model may be updated after a certain process if necessary.

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.

1 Information processing device 10 Sensor 11 Control device 12 Sensor information input unit 13 Position / orientation estimation unit 14 Map holding unit 15 Update information generation unit 16 Map update unit 17 Control value determination unit 110 Update information input unit 120 Difference acquisition unit 130 Update method determination Department

Claims (15)

An information processing device that estimates the position of a moving object based on map information indicating the position of a feature point in the environment in which the moving object moves.
The first position of the moving body estimated from the features of the feature points registered in the map information and the features of the feature points acquired from the sensor information measured while the moving body is moving in the environment is estimated. First estimation means to do
A second estimation means for estimating the second position of the moving body based on the information indicating the difference between the position of the feature point included in the map information and the position of the feature point acquired from the sensor information.
When the difference between the first position and the second position is larger than the threshold value, the control means for moving the moving body based on the path determined based on the feature points newly acquired from the sensor information. ,
An information processing apparatus comprising: an update means for updating the map information to a map generated at feature points newly acquired from the sensor information. The control means is characterized in that when the magnitude of the change from the first and second positions is smaller than the threshold value, the moving body is moved based on the map information updated by the updating means. The information processing device according to claim 1. The information processing device according to claim 1, wherein the control means decelerates or stops the moving body when the magnitude of change from the first and second positions is larger than a threshold value. When the magnitude of the change from the first and second positions is larger than the threshold value, the control means moves the control means to a position where the magnitude of the change from the first and second positions is smaller than the threshold value. The information processing apparatus according to any one of claims 1 to 3. The control means is characterized in that when the magnitude of change from the first and second positions is larger than the threshold value, at least one of the speed, acceleration, angular velocity, and each acceleration of the moving body is reduced. The information processing apparatus according to any one of claims 1 to 4. The estimation means estimates the first position of the moving body by matching the position of the feature point included in the sensor information with the position of the feature point registered in the map information. The information processing apparatus according to claim 1. An information processing device that estimates the position of a moving object based on map information indicating the position of a feature point in the environment in which the moving object moves.
The first posture of the moving body estimated from the position of the feature points included in the map information including the feature points already measured in the environment in which the moving body travels, and the new feature points not included in the map information. An estimation means for estimating the second posture of the moving body estimated based on the position, and
An information processing device having a determination means for determining the control value such that the control value satisfies a predetermined threshold value based on the first and second postures. The information processing apparatus according to claim 7, wherein the determination means determines the control value so that the second posture approaches the first posture. The information processing according to claim 7, wherein the determination means determines the control value so that the control value based on the second posture approaches the control value based on the first posture. apparatus. The determination means determines the control value based on the second posture so that the amount of change in at least one of the position, posture, velocity, angular velocity, acceleration, and angular acceleration of the moving body is reduced. The information processing apparatus according to claim 7. The information processing according to any one of claims 1 to 10, wherein the first posture is a posture estimated based on a position posture measured by a sensor mounted on the moving body. apparatus. A map holding means for holding the map information and
A generation means for generating update information of the map information based on the positions of new feature points not included in the map information, and
The information processing apparatus according to any one of claims 1 to 11, further comprising an update means for updating a map based on the update information. The information processing apparatus according to any one of claims 1 to 12, further comprising a control means for moving the moving body to the target point based on the determined control value. A program for causing a computer to function as each means included in the information processing apparatus according to any one of claims 1 to 13. An information processing method that estimates the position of a moving object based on map information indicating the position of a feature point in the environment in which the moving object moves.
The first position of the moving body estimated from the features of the feature points registered in the map information and the features of the feature points acquired from the sensor information measured while the moving body is moving in the environment is estimated. The first estimation process to be performed and
A second estimation step of estimating the second position of the moving body based on the information indicating the difference between the position of the feature point included in the map information and the position of the feature point acquired from the sensor information.
When the difference between the first position and the second position is larger than the threshold value, the control step of moving the moving body based on the path determined based on the feature points newly acquired from the sensor information. ,
An information processing method comprising an update step of updating the map information to a map generated at feature points newly acquired from the sensor information.

Images