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

Abstract

To provide the present invention that realizes stable acquisition of a position and a posture according to the inclination of a movable body.SOLUTION: An information processing apparatus according to the present invention solving the above-mentioned problem is an information processing apparatus that prospects the position and posture of a movable body having an imaging apparatus mounted thereon, and has: six degree-of-freedom prospect means that prospects first information indicating the position and posture of the imaging apparatus in terms of the six degree-of-freedom; and determination means that, when the inclination the movable body relatively to a surface to be a reference is larger than a predetermined value, determines the position and posture of the movable body in terms of a three degree-of-freedom based on the first information, and when the inclination of the movable body relatively to the surface to be the reference is smaller than the predetermined value, determines the position and posture of the movable body in terms of the three degree-of-freedom based on second information indicating the position and posture in terms of the three degree-of-freedom of the movable body.SELECTED DRAWING: Figure 1

Description

The present invention relates to self-positioning and posture estimation of a moving body.

Wheel robots such as automatic guided vehicles (automated guided vehicles) used in factories and distribution warehouses generally travel on a flat surface such as a floor or a road. A 2D laser scanner (2D LiDAR (Light Detection and Ranking)) may be used to measure the position and orientation required to control such moving objects and wheel robots (collectively referred to as moving objects). There are many. The 2D LiDAR is installed on the moving body so as to be parallel to the floor surface during traveling, and the position (2 degrees of freedom) and the posture (1 degree of freedom) on the plane (floor surface) on which the moving body travels, a total of 3 degrees of freedom. Position and posture are measured. Further, by setting the target position of the moving body in the map information of three degrees of freedom, the moving body can be moved toward the target position.

Japanese Unexamined Patent Publication No. 2015-141580

In Patent Document 1, the position of a moving body is estimated using the measurement result of a distance sensor that measures the distance between the moving body on a predetermined surface and an object existing in the environment. Furthermore, if there is a moving object in the area where the landmarks installed in the environment can be photographed, the landmarks are recognized from the image captured by the stereo camera and the position of the moving object is estimated. However, in Patent Document 1, when the moving body is tilted on a slope or the like, the tilt of the moving body is not taken into consideration in the position estimation by the distance sensor, so that the position and posture of the moving body may be estimated incorrectly. The present invention has been made in view of the above problems, and an object of the present invention is to stably obtain the position and posture of a moving body.

The information processing device according to the present invention that solves the above problems is an information processing device that estimates the position and posture of a moving body equipped with an image pickup device, and is the first that indicates the position and posture of the image pickup device with six degrees of freedom. When the inclination formed by the 6-DOF estimation means for estimating information and the moving body and the reference surface is larger than a predetermined value, the position and posture of the moving body with 3 degrees of freedom based on the first information. If the inclination formed by the moving body and the reference surface is smaller than a predetermined value, the moving body has three freedoms based on the second information indicating the position and posture of the moving body with three degrees of freedom. It is characterized by having a determination means for determining the position and posture of the degree.

According to the present invention, the position and posture of the moving body can be stably acquired.

Conceptual diagram explaining the inclination of a moving body The figure explaining the state of the movement control of a moving body The figure which shows the hardware configuration example of an information processing apparatus 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 explaining the processing executed by the information processing device Flowchart explaining the processing executed by the information processing device Block diagram showing a functional configuration example of an information processing device Flowchart explaining the processing executed by the information processing device Block diagram showing a functional configuration example of an information processing device Flowchart explaining the processing executed by the information processing device

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, a case where the present invention is applied to the movement control of a moving body will be described. A general moving object such as a robot that transports luggage in a factory or a warehouse automatically operates while estimating its own position and orientation using an image sensor or a distance sensor. A general moving object is supposed to run on a horizontal surface, but it is a horizontal plane that is set in advance by map information etc. depending on the slopes and steps existing in the driving environment and the luggage loaded on itself. The moving body itself may tilt. In such a case, the result of self-position / orientation estimation estimated from the measured value of the two-dimensional distance sensor that scans the plane may be estimated to be different from the absolute position in the map information.

In the present embodiment, the moving body is assumed to travel on a plane (floor surface) on which a slope partially exists, and is estimated based on image information taken by an image sensor (camera) mounted on the moving body. The mileage of the moving body is calculated based on the position and orientation of 6 degrees of freedom. FIG. 1 is a diagram simulating a situation in which the moving body 1 travels on a surface S2 that is tilted by an angle Θ with respect to a reference surface S1. At this time, it is assumed that the correct mileage in which the moving body has traveled from the point P1 to the specific time t is D1 connecting the points P1 and P2. The distance D2 connecting the point P1 and the point P2'on the horizontal plane is a mileage calculated based on the position / orientation measurement result (by a distance sensor that scans two dimensions) with three degrees of freedom. In estimating the position and posture with three degrees of freedom, D1 and D2 do not match because the position and posture are estimated on the assumption that the moving body 1 travels on the reference surface S1. If D1 and D2 do not match, for example, when managing the timing of battery charging, maintenance, and parts replacement of the AGV based on the mileage, maintenance cannot be performed at the optimum timing.

In the present embodiment, when the moving body travels on a slope so that the difference between D1 and D2 does not become large, the measurement results of the positions (X, Y, Z) and the postures (Roll, Pitch, Yaw) with 6 degrees of freedom. The position and orientation of the moving body with three degrees of freedom required for the movement control of the moving body are estimated by converting. As a result, the position and posture of the moving body can be acquired.

(Explanation of Configuration) FIG. 2 is a diagram illustrating a state of movement control of a moving body in the present embodiment. In the present embodiment, the movement control of the AGV (Automated Guided Vehicle), which is responsible for the physical distribution in the factory or the warehouse, will be described as a specific example. Basically, the moving body may be a movable robot that can autonomously move its own position by giving a certain instruction without human operation.

The information processing device 11 mounted on the moving body 1 estimates the position and posture of the moving body based on an image obtained by capturing the surroundings of the moving body with a stereo camera, and controls the movement of the moving body based on the estimation result. .. The information processing device 11 is connected to an image sensor 12, a distance sensor 13, a contact sensor 14, an actuator 15, an odometer 17, and a storage unit 18. Further, the mobile body 1 performs wireless communication with the host management system 16.

FIG. 3 is a diagram showing a hardware configuration example of the information processing device. The central processing unit (CPU) 101 uses the RAM 103 as a work memory to read and execute the OS and other programs stored in the ROM 102 and the storage device 104, and controls each configuration connected to the system bus 109 to control various configurations. Performs processing operations and logical judgments. The process executed by the CPU 101 includes the information processing of the embodiment. The storage device 104 is a hard disk drive, an external storage device, or the like, and stores programs and various data related to information processing of the embodiment. The input unit 105 is an image pickup device such as a camera, an input device such as a button for inputting a user instruction, a keyboard, and a touch panel. The storage device 104 is connected to the system bus 109 via an interface such as SATA, and the input unit 105 is connected to the system bus 109 via a serial bus such as USB, but the details thereof will be omitted. The communication I / F 106 communicates with an external device by wireless communication. The display unit 107 is a display. The sensor 108 is an image sensor or a distance sensor.

The image sensor 12 is specifically a camera. The image sensor 12 is an image pickup device that images the surroundings of a moving body. The environment around the moving body is imaged by the image sensor, and the captured measurement result is output to the image information input unit 111 as image information. It is assumed that the image sensor in the present embodiment is a stereo camera composed of two monochrome cameras having an angle of view in the horizontal and vertical directions. The stereo camera is attached to the moving body so that the optical axis faces slightly upward from the horizontal plane. It is assumed that the internal parameters (focal length, image center, distortion parameter) of the two cameras constituting the stereo camera and the external parameters (relative position / orientation) between the cameras are pre-calibrated by a known method. In the present embodiment, a three-dimensional coordinate system in which the optical center of the camera is the origin, the optical axis direction is the Z axis, the horizontal direction of the image is the X axis, and the vertical direction of the image is the Y axis is defined as the camera coordinate system. It is assumed that the camera coordinate system of the stereo camera is the camera coordinate system of the left camera (or the right camera). It is assumed that the mounting position of the image sensor 12 on the moving body is known by the design value. A distance sensor capable of measuring a distance with 6 degrees of freedom may be used instead of the image sensor. Specifically, 3D-LiDAR or the like may be used.

The distance sensor 13 is a device that measures the distance information of the scene from the sensor. The distance sensor in this embodiment is 2D LiDAR. 2D LiDAR is a distance sensor that acquires distance information by scanning a laser beam on a plane. The distance sensor 13 is attached to the moving body so that the plane on which the laser beam is scanned is a substantially horizontal plane. It is assumed that the mounting position of the distance sensor 13 on the moving body is known by the design value. The measured distance information is output to the distance information input unit 118 of the information processing device 11. Instead of the distance sensor, a marker indicating a two-dimensional absolute position may be recognized from the image captured by the image sensor.

The contact sensor 14 is a safety sensor. When the contact sensor detects the contact between the moving body and another object, the moving body outputs information to the control unit 115 so as to make an emergency stop.

The actuator 15 controls the motor based on the instruction from the information processing device 11 and the output of the obstacle sensor and the safety sensor to move or stop the moving body.

The upper management system 16 is a system that manages the operation of a plurality of mobile objects. The host management system 16 transmits the target position / orientation to the information processing device 11 of each mobile body. In the present embodiment, the target position / posture of the moving body is a position / posture with three degrees of freedom on a plane. Communication between the host management system 16 and the information processing device 11 is performed by wireless communication such as Wifi, BlueTouch, or 5G.

The odometer 17 measures the mileage from, for example, the amount of rotation of the wheels of the moving body.

The storage unit 18 stores map information about the environment in which the moving body travels. In addition, it holds information necessary for the mileage of the moving body and movement control. As shown in FIG. 4, the storage unit 18 may be in a form other than that mounted on the moving body, or may be inside the upper management system 16.

(Details of Information Processing Device 11)
FIG. 4 shows a diagram showing a functional configuration example of the information processing device according to the present embodiment. The information processing device 11 is composed of an image information input unit 111, a 6-DOF estimation unit 112, a conversion unit 113, a control unit 115, a map information acquisition unit 116, and a plane information acquisition unit 117. The information processing device 11 is further composed of a distance information input unit 118, a degree of freedom estimation unit 119, a determination unit 120, a second map information acquisition unit 121, and a measurement unit 122. Further, the moving body 1 includes a measuring unit 122, a control unit 115, a distance information input unit 118, a three-degree-of-freedom estimation unit 119, a determination unit 120, and a second map information acquisition unit 121. The information processing device 11 is, for example, a PC, an embedded system, or a PLC (Programmable Logical Controller).

The image information input unit 111 inputs image information from the image sensor 12 and sends the input image information to the 6-DOF estimation unit 112. Since the image sensor 12 in this embodiment is a stereo camera, the image information is a stereo image (two images taken by the left and right cameras). The image sensor 12 captures image information of a factory or a distribution warehouse in which a moving body travels. Therefore, the image information in the present embodiment refers to a stereo image obtained by capturing the traveling environment of the moving body by an imaging device mounted on the moving body.

The 6-DOF estimation unit 112 collates the image information (stereo image) input from the image information input unit 111 with the map information of the scene, so that the position and orientation (position 3 degrees of freedom) of the camera in the three-dimensional space can be adjusted. Degree, posture 3 degrees of freedom) is estimated. The position / orientation with six degrees of freedom is a position / orientation (first information) based on a three-dimensional coordinate system (hereinafter, world coordinate system) defined in the map information indicating the environment in which the moving body travels. In this embodiment, the map information of the scene is generated and the position / orientation estimation of the camera with 6 degrees of freedom is performed by the method of Mur-Artal et al. (R. Mur-Artal et al., "ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras, 2017.). The method of Mur-Artal et al. This is a method to be used. As the map information of the scene, the information of the three-dimensional point group in the scene in which the moving object travels, such as a factory or a distribution warehouse in which the moving object travels, is held. Specifically, the three-dimensional point group in the scene is held. The information is the three-dimensional coordinates of the feature points in the world coordinate system. In the present embodiment, the map of the scene is previously obtained by the method of Mur-Artal et al., Based on the image taken while the moving body is running. Information is generated. It is assumed that the map information of the scene is stored in the storage unit 18. The method of Mur-Artal et al. Uses the position of the feature point on the image and the estimated position / orientation to map. The position and orientation of the camera are estimated by minimizing the difference in position (reprojection error) when the above feature points are projected onto the image. The 6-degree-of-freedom estimation unit is a moving object imaged by the image pickup device. An image of the surroundings may be input, and the position and orientation of the moving body with three degrees of freedom may be estimated based on the trained model that outputs the position and orientation of the moving body with three degrees of freedom.

The conversion unit 113 converts the position / orientation of the camera with 6 degrees of freedom estimated by the 6-DOF estimation unit 112 into the position / orientation with 3 degrees of freedom so that the moving body can be controlled. That is, the conversion unit 113 indicates the position and orientation of three degrees of freedom by using the first information which is the position and orientation based on the three-dimensional coordinate system (hereinafter, the world coordinate system) defined in the map information. 3 Output information. The inclination of the moving body may be determined, for example, by the value (Z) in the height direction of the estimation result of the position and the posture with six degrees of freedom. For example, when Z = 0, it is considered that the moving body is traveling on a reference surface, and the moving body is controlled by using the measurement result of the distance sensor measuring with three degrees of freedom. On the other hand, when Z> 0 (or Z <0), it is considered that the slope is climbing (or descending), so the result of converting the position and posture of 6 degrees of freedom into 3 degrees of freedom is used. Try to control. As described above, the position / orientation with three degrees of freedom is the position / orientation on the plane on which the moving body travels. Of the three degrees of freedom, two degrees of freedom represent the two-dimensional position on the plane, and the remaining one degree of freedom represents the posture (orientation on the plane) around the normal of the plane. The method of estimating the plane on which the moving body travels will be described later. It is assumed that the information on the plane on which the moving body travels is stored in the plane information acquisition unit 117.

The control unit 115 controls the moving body based on the position and posture of the moving body determined by the determination unit 120. The moving body is controlled based on the second information or the third information indicating the position and posture of the three degrees of freedom converted by the conversion unit 113. Specifically, the control unit 115 calculates the route based on the difference between the preset target position / orientation (3 degrees of freedom) of the moving body and the position / orientation of the 3 degrees of freedom output by the conversion unit 112. An instruction such as a rotation speed of the motor is sent to the actuator 15 so as to follow the set path. When the position / orientation of the moving body substantially matches the target position / posture, the moving body is stopped and waits for the next target position / posture to be sent from the upper management system 16. The route calculation may use an algorithm such as an A * algorithm or an RRT (Rapidly exploring random stream), or a machine learning method such as Q-learning or DQN.

The map information acquisition unit 116 acquires map information of three degrees of freedom in the environment in which the moving body travels from the storage unit 18. In addition, the target position and posture of the three degrees of freedom of the moving body set in advance are acquired.

The plane information acquisition unit 117 acquires data for plane estimation by running the moving body on a plane that serves as the same reference as during operation. The data for plane estimation is position / orientation data at a plurality of positions (N points) when the moving body travels. The position-posture data is the 6-DOF position-posture of the image sensor 112 estimated by the 6-DOF position-posture estimation. Even when traveling on an inclined surface inclined by Θ from the reference surface, the position and orientation of 6 degrees of freedom are estimated based on the image captured by the image pickup device so that the position and orientation of 3 degrees of freedom can be correctly estimated.

The measuring unit 122 measures the mileage of the moving body based on the amount of rotation of the wheels of the moving body measured by the odometer 17. Based on the measurement result of the measuring unit 122, the position and orientation of the moving body are determined in the determining unit 120.

The distance information input unit 118 inputs the distance information obtained by measuring the distance between the moving body and the object around it from the distance sensor 13, and sends the input distance information to the three-degree-of-freedom estimation unit 219. As described above, the distance sensor in this embodiment is 2D LiDAR. The 2D LiDAR is attached to the moving body so that the plane on which the laser beam is scanned is a substantially horizontal plane. As a result, the distance sensor 13 acquires the distance information in the horizontal plane at the height to which the distance sensor 13 is attached.

The three-degree-of-freedom estimation unit 119 attaches the moving body to the moving body based on the two-dimensional map information indicating the environment in which the moving body travels and the distance information obtained by measuring the distance between the object and the moving body in the environment in which the moving body travels. Estimate the second information indicating the position and orientation of the mounted distance sensor with three degrees of freedom. That is, the position and orientation of the distance sensor 13 are estimated by collating the distance information input from the distance information input unit 118 with the map information of the scene. In the present embodiment, the three-degree-of-freedom estimation unit 119 estimates the position and orientation (position two-degree-of-freedom, posture one-degree-of-freedom) of the distance sensor 13 on the plane. It is assumed that the two-dimensional map information necessary for the movement control of the moving body is generated in advance and acquired in advance by the second map information acquisition unit 121. The position / orientation with three degrees of freedom is a position / orientation based on the plane (two-dimensional) world coordinate system (hereinafter, two-dimensional world coordinate system) defined in the above-mentioned map information. Map information based on distance information and estimation of position and orientation are performed by the method of Grisette et al. Using a particle filter. (G. Grisette, et al., "Improved Technologies for Grid Mapping with Lao-Blackwellized Particle Filters," 2007.). The position / orientation of the distance sensor is converted into the position / orientation of the moving object in the three-degree-of-freedom estimation unit or the determination unit based on the known mounting position of the distance sensor with respect to the moving object. Estimating the position and orientation with three degrees of freedom has the advantage that the processing load is smaller than the estimation of position and orientation with six degrees of freedom because the process of converting from six degrees of freedom to three degrees of freedom is not required. Therefore, when the moving body travels on a flat surface, the moving body can be controlled with as little power as possible by performing a position and orientation with three degrees of freedom. Although the method of estimating the position and posture from the distance information is described here, any information may be used as long as the position and posture of three degrees of freedom can be estimated. For example, a method may be used in which a marker indicating two-dimensional coordinates on a map is installed in the environment and the position of the moving body is specified by recognizing the marker as an image.

The determination unit 120 determines the position and orientation of the moving body by selectively using at least one of the second information and the third information according to the inclination of the moving body. That is, when the inclination formed by the moving body and the reference surface is larger than a predetermined value, the third information based on the first information is determined as the position and posture of the moving body having three degrees of freedom. When the inclination formed by the moving body and the reference surface is smaller than a predetermined value, that is, when traveling on a substantially horizontal surface, the determination unit 120 determines the position and posture of the moving body with three degrees of freedom based on the second information. To determine. When the moving body is tilted, for example, when traveling on a slope, the displacement in the direction of gravity cannot be measured from the measurement result of the distance of three degrees of freedom. On the other hand, the mileage including the displacement in the gravity direction can be estimated by using the first information that can measure the position and attitude with 6 degrees of freedom. By properly using the measurement results according to the inclination of the moving body in the direction of gravity in this way, it is possible to stably obtain the position / orientation estimation results with three degrees of freedom. If the inclination formed by the moving body and the reference surface is smaller than a predetermined value, the position / orientation may be determined based on the first information in addition to the second information. In that case, for example, the average value or the median value is determined as the position and orientation of the moving body from the results of the first information and the second information.

FIG. 6 shows a flowchart of a processing procedure when the present invention is applied to the movement control of a moving body. In the following description, the notation of the process (step) is omitted by adding S at the beginning of each process (step). However, the information processing device 11 does not necessarily have to perform all the steps described in this flowchart.

In S1010, the plane information acquisition unit 117 estimates the plane on which the moving body travels as a preliminary preparation. By using the estimated plane, it is possible to convert the 6-DOF position / posture estimated by the 6-DOF position estimation unit 112 into a 3-DOF position / posture on the plane. The plane on which the moving body travels is estimated by fitting the plane to the position estimated when the moving body is run on the plane. The details of the method of estimating the plane on which the moving body travels will be described later.

In S1020, the map information acquisition unit 116 acquires the target position / posture of the moving body from the upper management system 16. In the present embodiment, the target position / posture of the moving body is a position / posture with three degrees of freedom on a plane.

In S1030, the 6-DOF estimation unit 112 estimates the first information indicating the position and orientation of the 6-DOF of the moving body based on the image captured by the imaging device at least around the moving body. Specifically, the 6-DOF estimation unit 112 estimates the position and orientation of the camera based on the image information captured by the stereo camera. When estimating the position and orientation, the map information acquired in advance by the map information acquisition unit 116 is used. The method of Mur-Artal et al. described above is used to estimate the position and orientation of the camera with 6 degrees of freedom. Since the map has already been generated, the map information is not updated and only the position and orientation of the camera are estimated from the image information. It should be noted that the position and orientation may be performed while updating the map information using SLAM technology.

In S1035, the three-degree-of-freedom estimation unit 119 provides second information indicating the position and orientation of the three-degree-of-freedom of the moving body based on the distance information obtained by measuring the distance between the object and the moving body in the environment in which the moving body travels. presume. That is, the position and orientation of the AGV with three degrees of freedom are estimated. The 3 degree of freedom estimation unit 119 estimates the position and orientation of the distance sensor 13 by collating the distance information input from the distance information input unit 117 with the map information of the scene stored in the second map information acquisition unit 121. .. Let the estimated position and orientation of the distance sensor be (xL, yL, θL).

In S1040, the determination unit 120 determines whether or not the inclination formed by the moving body and the reference surface is larger than a predetermined value based on the first information. This inclination is determined by, for example, the magnitude of the position of the moving body in the Z direction. Other values may be used as a reference. When the inclination formed by the moving body and the reference surface is equal to or less than a predetermined threshold, the process proceeds to S1045, and the position and orientation of the camera with 6 degrees of freedom estimated by the 6-DOF estimation unit 112 are moved based on the inclination. The position / orientation with 6 degrees of freedom is converted into the position / orientation with 3 degrees of freedom so that the body can be controlled. When the determination unit 120 determines that the inclination formed by the moving body and the reference surface is equal to or greater than a predetermined threshold value, the process proceeds to the position / posture S1050 with three degrees of freedom. The conversion unit 113 determines the position and orientation of the moving body with six degrees of freedom based on the first information which is the position and orientation estimation result of the image pickup apparatus.

In S1050, the determination unit 120 determines the position and posture of the moving body by selectively using at least one of the second information and the third information according to the inclination of the moving body. From the position / orientation estimated in S1030 and the position / orientation estimated in S1035, the position / orientation to be finally output to the control unit 15 is determined. Alternatively, the conversion unit 130 converts the position / orientation with 6 degrees of freedom estimated in S1030 based on the estimation result of the plane parameter on which the moving body travels estimated in S1010, so that the moving body has 3 degrees of freedom. Position Position Determine the posture. Further, the position and orientation of the camera are converted into the position and orientation of the moving body based on the mounting position of the image sensor 12 on the moving body. Finally, the position and orientation of the three degrees of freedom to be output to the control unit 115 are determined.

In S1060, the control unit 115 compares the position / posture of the moving body acquired in S1050 with the target position / posture of the moving body acquired in S1020, and determines whether the moving body has reached the target position / posture. In the present embodiment, threshold values (for example, 1 m) are set for each of the difference between the current position and the target position and the difference between the current posture and the target posture, and both the difference in position and the difference in posture are below the corresponding threshold values. If so, it is determined that the target position has been reached. In the present embodiment, threshold values are set for each of the difference between the current position and the target position and the difference between the current posture and the target posture, and when both the difference in position and the difference in posture are below the corresponding threshold values, the target is set. It is determined that the position has been reached. When the target position is reached, the process returns to S1020 and a new target position / attitude is received from the upper management system.

In S1070, the control unit 115 determines the position and orientation of the moving body with three degrees of freedom determined by the determining unit 120, and the front moving body based on the map information representing the environment in which the moving body travels with three degrees of freedom. To the target position indicated in the map information. That is, the route calculation is performed based on the difference between the position / posture estimation result of the moving body having three degrees of freedom determined in S1050 and the target position / posture of the moving body transmitted from the upper management system 16. The actuator 15 of the moving body is controlled so as to follow the calculated path.

In S1080, the control unit 115 determines the end of the movement control. When a termination command is sent from the higher management system 16, the movement control is terminated. Otherwise, it returns to S1030 and continues the movement control.

FIG. 7 is a flowchart showing a processing procedure of the method of estimating the plane on which the moving body travels in S1010. In S1100, the plane information acquisition unit 117 acquires data for plane estimation by running the moving body on the same plane as during operation. The data for plane estimation is position / orientation data at a plurality of positions (N points) when the moving body travels. The position-posture data is the 6-DOF position-posture of the image sensor 112 estimated by the 6-DOF position-posture estimation. Here, the 6-DOF position / orientation of the camera in the i-th (1 ≦ i ≦ N) position / orientation data is expressed as (Xvi, Yvi, Zvi, Wxvi, Wyvi, Wzvi). Xvi, Yvi, and Zvi represent the three-dimensional position of the camera in the three-dimensional space, and Wxvi, Wyvi, and Wzvi represent the three-dimensional posture. In this embodiment, a posture expression is used in which the norm of the three-dimensional vector (Wxvi, Wyvi, Wzvi) is the rotation angle and the direction is the rotation axis. Further, the 3 × 3 rotation matrix of the posture represented by Wxvi, Wyvi, and Wzvi is represented by Rvi, and the three-dimensional vector (Xvi, Yvi, Zvi) t is represented by tvi. When acquiring the plane estimation data, the moving body is moved so that the locus of the moving body draws a closed curve, for example, instead of a linear locus so that the plane parameters can be estimated from the moving locus of the moving body. It should be noted that the deviation from the plane due to the partial slope, the vibration of the moving body, the unevenness of the floor surface, etc. can be absorbed by the plane fitting.

In S1110, plane fitting is performed on N position data (Xvi, Yvi, Zvi) t having three degrees of freedom. The parameters (normal vector nv = (nvx, nvy, nvz) t, passing position cv = (cvx, cvy, cvz) t) of the plane on which the moving body traveled when acquiring the plane estimation data are estimated. It is assumed that the normal vector nv is a unit vector. The passing position cv is obtained by calculating the center of gravity of (Xvi, Yvi, Zvi) t. The normal vector nv is obtained by eigenvalue-decomposing a covariance matrix of N three-dimensional vectors (Xvi-cvx, Yvi-cvy, Zvi-cvz) t and obtaining it as an eigenvector corresponding to the minimum eigenvalue. The estimated plane parameters are stored in the plane information acquisition unit 117.

Next, the processing procedure of the method of converting the position / posture of 6 degrees of freedom into the position / posture of 3 degrees of freedom in S1045 will be described.

The 6-DOF position / orientation (Xv, Yv, Zv, Wxv, Wyv, Wzv) is projected on the plane estimated in S1000 and converted into the 3-DOF position / orientation (xv, yv, θv) t (Note that). Rv is a 3 × 3 rotation matrix with postures represented by Wxv, Wyv, and Wzv). Specifically, a new three-dimensional coordinate system (hereinafter referred to as a plane coordinate system) with the plane passing position cv as the origin and the plane normal vector nv as the Z axis is defined, and six degrees of freedom in the three-dimensional world coordinate system. Convert the position / orientation to the plane coordinate system. From the definition, in the three-dimensional world coordinate system, the origin (0,0,0) t of the plane coordinate system is cvt, and the Z axis (0,0,1) t is nv. From this, the three-dimensional coordinate Xp (= (Xp, Yp, Zp) in the plane coordinate system is converted into the three-dimensional coordinate Xv (= (Xv, Yv, Zv) in the three-dimensional world coordinate system as shown in the following equation. ..

Xv = RpvXp + cv
Here, Rpv is a 3 × 3 rotation matrix that transforms the Z-axis (0,0,1) t into a plane normal vector nv. Rpv is calculated with the rotation axis as the outer product of (0,0,1) t and nv, and the rotation angle as the inverse cosine (arccos) of the inner product of (0,0,1) t and nv. Therefore, the three-dimensional coordinate Xv in the three-dimensional world coordinate system is converted into the three-dimensional coordinate Xp in the plane coordinate system as shown in the following equation.

Xp = Rpv-1 (Xv-cv)
Calculate Xp when tv is input as Xv. Of the positions and orientations with three degrees of freedom, xv and yv correspond to the Xp component and the Yp component of the three-dimensional coordinates Xp. θv is the direction of the vector that projects the optical axis of the image sensor onto a plane. In the present embodiment, the optical axis of the camera on the left side of the stereo camera is the optical axis of the image sensor. Assuming that the three-dimensional vector representing the optical axis of the image sensor in the camera coordinate system defined by the image sensor is vc, the optical axis vo of the image sensor in the plane coordinate system is expressed by the following equation.

vo = Rpv-1Rvvc
As described above, in the present embodiment, in order to attach the stereo camera to the automatic guided vehicle so that the optical axis of the stereo camera faces slightly upward from the horizontal plane, the optical axis vo is projected onto the XY plane of the plane coordinate system to calculate θv. The optical axis vo'projected on the XY plane of the plane coordinate system is calculated by the following equation.

vo'= vo- (vo · (0,0,1) t) vo
In the present embodiment, the angle of vo'with respect to the X axis of the plane coordinate system is θv.

The configuration of the information processing device 11 is not limited to the example described in FIG. For example, a functional configuration example of the information processing device as shown in FIG. 5 may be used. The information processing device 11 includes an image information input unit 111, a 6-DOF estimation unit 112, a conversion unit 113, a map information acquisition unit 116, a plane information acquisition unit 117, a measurement unit 122, and a determination unit 120. The moving body 1 has a distance information input unit 118, a degree of freedom estimation unit 119, a control unit 115, and a second map information acquisition unit 221.

In S1040, the determination unit 120 determines whether or not the moving body is tilted, but it is not always necessary to determine. For example, when traveling on an uneven road where the inclination of the moving body changes frequently, if the estimation is changed from 6 degrees of freedom to 3 degrees of freedom each time it becomes horizontal, the position and orientation estimation of the moving body becomes unstable. There is. When a tilt larger than a predetermined value is detected in the moving body so that the estimation method is not changed frequently, the determination unit 120 determines the predetermined time (for example, 1 minute) even if the tilt of the moving body is eliminated. ) Elapses, the position and orientation are determined based on the measurement results of 6 degrees of freedom. That is, when the inclination of the moving body becomes smaller than the predetermined value from the state where the inclination of the moving body is larger than the predetermined value, the determination unit 120 has three degrees of freedom after a certain time from the time when the inclination of the moving body becomes smaller than the predetermined value. The position and orientation of the moving body are determined based on the position and orientation measurement results. Alternatively, when passing through a small step, the inclination of the moving body is eliminated in an instant. In such a case, since an error does not occur so much even if the position / orientation measurement is not performed with 6 degrees of freedom, the position / orientation estimation with 3 degrees of freedom is continued without switching to only the position / orientation estimation with 6 degrees of freedom. That is, when the tilt of the moving body is held larger than the predetermined value for a time longer than the predetermined time, the determination unit 120 is based on the position / posture measurement result of 6 degrees of freedom, and the position / posture of the moving body has 3 degrees of freedom. To determine. In this way, the position and orientation of the moving body can be determined more stably by changing the processing of the determination unit 120 according to the time when the method of estimating the position and orientation is switched or the time when the inclination of the moving body is held.

As described above, in the present embodiment, when the inclination of the moving body is larger than a predetermined value, the movement is performed using the information obtained by converting the position / posture of 6 degrees of freedom estimated from the image information into the position / posture of 3 degrees of freedom. Determine the position and posture of the body. As a result, the position and posture can be stably determined even if there is a slope on the floor on which the moving body travels.

<Embodiment 2>
In the above-described embodiment, when the inclination of the moving body is larger than a predetermined value, the position / posture of the moving body is determined by using the information obtained by converting the position / posture of 6 degrees of freedom estimated from the image information into the position / posture of 3 degrees of freedom. Made it possible to decide. In the second embodiment, it is assumed that the moving body travels on a substantially flat surface (reference surface), and the inclination of the moving body that should not originally tilt during traveling is detected. Tilt of the moving body occurs, for example, when the luggage on the moving body is not loaded on the loading platform in a well-balanced manner. In this case, since the load may collapse, it is necessary to notify the user of a warning when the inclination is detected.

FIG. 8 shows a block diagram of a functional configuration example of the information processing device 21 according to the present embodiment. The information processing device 21 mounted on the moving body 2 includes an image sensor 12, a distance sensor 13, a contact sensor 14, an actuator 15, an odometer 17, and a storage unit 18 as in the information processing device 11, and is further connected to a gravity sensor 29. Has been done. The gravity sensor 29 detects the inclination of the moving body in the direction of gravity and measures the posture with three degrees of freedom.

The information processing device 21 has a tilt detection unit 214 and an output unit 221 in addition to the configuration of the first embodiment. Similar to the information processing device 11, the information processing device 21 is, for example, a PC, an embedded system, or a PLC (Programmable Logical Controller).

The tilt detection unit 214 detects the tilt of the moving body with respect to the traveling plane based on the posture of the camera estimated by the 6-DOF estimation unit 212. Alternatively, a gravity sensor 29 such as an acceleration sensor or a gyro sensor may be used to detect the inclination of the moving body in the gravity direction.

The output unit 221 issues a warning to the user when the inclination detection unit 214 detects an inclination of a moving body larger than a predetermined value. The output unit 221 is, for example, a speaker. Alternatively, a display that displays a warning may be used. By notifying the user that the moving body is tilted, it is possible to safely deal with the movement control of the moving body before it fails.

FIG. 9 shows a flowchart of the processing procedure when applied to the movement control of the moving body in the present embodiment. Since the processing of the same step number is the same as the processing of the first or second embodiment, the description thereof will be omitted.

In S2040, the inclination detection unit 314 detects the inclination θ of the moving body with respect to the traveling plane. First, the tilt angle of the moving body is calculated. The inclination reference is the normal vector nref of the traveling plane in the camera coordinate system at the time of data acquisition for plane estimation in S3010. nref = (Rvi) -1nv. The nref may be calculated by determining one Rvi out of N Rvi, or the nref may be calculated from each Rvi and averaged. When the 3 × 3 rotation matrix representing the posture estimated by the 6-DOF estimation unit 312 is Rv, the normal vector of the traveling plane in the camera coordinate system is calculated as ncur = (Rv) -1nv. The inclination θ of the moving body with respect to the traveling plane is the angle formed by nref and ncur.

In S2045, the output unit 321 determines that the moving body is tilted if the inclination θ of the moving body with respect to the traveling plane is equal to or greater than a predetermined threshold value, and notifies the user of a warning. The warning to the user is given, for example, by emitting a warning sound from the speaker of the output unit 321.

<Embodiment 3>
In the present embodiment, a case where the present invention is applied to the movement control of a moving body will be described. In the present embodiment, the moving body is assumed to travel on a plane (floor surface) on which a slope partially exists, and is estimated based on image information taken by an image sensor (camera) mounted on the moving body. The mileage of the moving body is calculated based on the position and orientation of 6 degrees of freedom. The position and posture of the moving body are estimated using the measurement results of the position and posture of 6 degrees of freedom. As a result, the position and posture of the moving body can be stably acquired.

The information processing device 31 mounted on the moving body 3 shown in FIG. 10 estimates the position and posture of the moving body based on an image captured around the moving body, and controls the movement of the moving body based on the estimation result. .. The information processing device 31 is connected to an image sensor 32, a distance sensor 33, a contact sensor 34, an actuator 35, an odometer 37, and a storage unit 38. Further, the mobile body 3 performs wireless communication with the upper management system 16 of FIG.

The image sensor 32 is a camera. The image sensor 32 is an image pickup device that images the surroundings of a moving body. It is assumed that the image sensor in the present embodiment is a stereo camera composed of two monochrome cameras having an angle of view in the horizontal and vertical directions. The stereo camera is attached to the moving body so that the optical axis faces slightly upward from the horizontal plane. It is assumed that the internal parameters (focal length, image center, distortion parameter) of the two cameras constituting the stereo camera and the external parameters (relative position / orientation) between the cameras are pre-calibrated by a known method. In the present embodiment, a three-dimensional coordinate system in which the optical center of the camera is the origin, the optical axis direction is the Z axis, the horizontal direction of the image is the X axis, and the vertical direction of the image is the Y axis is defined as the camera coordinate system. It is assumed that the camera coordinate system of the stereo camera is the camera coordinate system of the left camera (or the right camera). It is assumed that the mounting position of the image sensor 32 on the moving body is known by the design value.

The distance sensor 33 is a device that measures the distance information of the scene from the sensor. The distance sensor in this embodiment is 2D LiDAR. 2D LiDAR is a distance sensor that acquires distance information by scanning a laser beam on a plane. The distance sensor 33 is attached to the moving body so that the plane on which the laser beam is scanned is a substantially horizontal plane. It is assumed that the mounting position of the distance sensor 33 on the moving body is known by the design value. In the third embodiment, the distance sensor 33 is used as an obstacle sensor.

The contact sensor 34 is a safety sensor. When the contact sensor detects the contact between the moving body and another object, the moving body makes an emergency stop.

The actuator 35 controls the motor based on the instruction from the information processing device 31 and the output of the obstacle sensor and the safety sensor to move or stop the moving body.

The upper management system 16 is a system that manages the operation of a plurality of mobile objects. The host management system 16 transmits the target position / orientation to the information processing device 31 of each mobile body. In the present embodiment, the target position / posture of the moving body is a position / posture with three degrees of freedom on a plane. Communication between the host management system 16 and the information processing device 31 is performed by wireless communication such as Wifi, BlueTouch, or 5G.

The odometer 37 measures the mileage from the amount of rotation of the wheels of the moving body.

The storage unit 38 stores map information about the environment in which the moving body travels. In addition, it holds information necessary for the mileage of the moving body and movement control. Note that the storage unit 38 may be in a form other than that mounted on the moving body as shown in FIG. 10, and may be inside the upper management system 16.

(Details of Information Processing Device 31)
FIG. 10 shows a diagram showing a functional configuration example of the information processing apparatus according to the present embodiment. The information processing device 31 is composed of an image information input unit 311, a 6-DOF estimation unit 312, a conversion unit 313, a mileage calculation unit 314, a control unit 315, a map information acquisition unit 316, and a plane information acquisition unit 317. The information processing device 31 is, for example, a PC, an embedded system, or a PLC (Programmable Logical Controller).

The image information input unit 311 inputs image information from the image sensor 32 and sends the input image information to the 6-DOF estimation unit 312. Since the image sensor 32 in this embodiment is a stereo camera, the image information is a stereo image (two images taken by the left and right cameras). The image sensor 32 captures image information of a factory or a distribution warehouse in which a moving body travels. Therefore, the image information in the present embodiment refers to a stereo image obtained by capturing the traveling environment of the moving body by an imaging device mounted on the moving body.

The 6-DOF estimation unit 312 collates the image information (stereo image) input from the image information input unit 311 with the map information of the scene, so that the position and orientation of the camera in the 3D space (position 3 degrees of freedom) (position 3 freedom) Degree, posture 3 degrees of freedom) is estimated. The position / orientation with six degrees of freedom is a position / orientation based on the three-dimensional coordinate system (hereinafter referred to as the world coordinate system) defined in the map information of the scene described above. In this embodiment, the map information of the scene is generated and the position / orientation estimation of the camera with 6 degrees of freedom is performed by the method of Mur-Artal et al. (R. Mur-Artal et al., "ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras,", 2017.). The method of Mur-Artal et al. Is a method using feature points detected on an image. The map information of the scene holds the information of the three-dimensional point cloud in the scene in which the moving object travels, such as a factory or a distribution warehouse in which the moving object travels. Specifically, the information of the three-dimensional point cloud in the scene is the three-dimensional coordinates of the feature points in the world coordinate system. In the present embodiment, the map information of the scene is generated in advance by the method of Mur-Artal et al. described above based on the image taken while the moving body is running. It is assumed that the map information of the scene is stored in the storage unit 38. The method of Mur-Artal et al. Minimizes the difference (reprojection error) between the position of the feature point on the image and the position when the feature point on the map is projected on the image using the estimated position and orientation. Estimates the position and orientation of the camera.

The conversion unit 313 converts the position / orientation of the camera with 6 degrees of freedom estimated by the 6-DOF estimation unit 312 into the position / orientation of the moving body with 3 degrees of freedom so that the moving body can be controlled. If the position / posture of 3 degrees of freedom is also a moving body that can be estimated or measured, the position / posture estimation result of 3 degrees of freedom and the position / posture conversion result of 6 degrees of freedom are selectively used according to the inclination of the moving body. You may. The inclination of the moving body may be determined, for example, by the value (Z) in the height direction of the estimation result of the position and the posture with six degrees of freedom. For example, when Z = 0, it is considered that the moving body is traveling on a reference surface, and the moving body is converted into a position and a posture whose dimensions are reduced to three degrees of freedom. On the other hand, when Z> 0 (or Z <0), it is considered that the slope is climbing (or descending), so control is performed using the position and posture with 6 degrees of freedom. As described above, the position / orientation with three degrees of freedom is the position / orientation on the plane on which the moving body travels. Of the three degrees of freedom, two degrees of freedom represent the two-dimensional position on the plane, and the remaining one degree of freedom represents the posture (orientation on the plane) around the normal of the plane. The method of estimating the plane on which the moving body travels will be described later. It is assumed that the information on the plane on which the moving body travels is stored in the plane information acquisition unit 317.

The mileage calculation unit 314 calculates the mileage of the moving body by using the difference between the position of the camera estimated by the 6-DOF estimation unit 312 and the position measured last time.

The control unit 315 controls the moving body based on the position / orientation converted by the conversion unit 313. Specifically, the control unit 315 performs route calculation based on the difference between the preset target position / orientation (3 degrees of freedom) of the moving body and the position / orientation of the 3 degrees of freedom output by the conversion unit 312, and calculates the route. An instruction such as a rotation speed of the motor is sent to the actuator 35 so as to follow the set path. When the position / orientation of the moving body substantially matches the target position / posture, the moving body is stopped and waits for the next target position / posture to be sent from the upper management system 16. The route calculation may use an algorithm such as an A * algorithm or an RRT (Rapidly exploring random stream), or a machine learning method such as Q-learning or DQN.

The map information acquisition unit 316 acquires map information of three degrees of freedom in the environment in which the moving object travels from the storage unit 38. In addition, the target position and posture of the three degrees of freedom of the moving body set in advance are acquired.

The plane information acquisition unit 317 acquires data for plane estimation by running the moving body on a plane that serves as the same reference as during operation. The data for plane estimation is position / orientation data at a plurality of positions (N points) when the moving body travels. The position-posture data is the 6-DOF position-posture of the image sensor 312 estimated by the 6-DOF position-posture estimation. Even when traveling on an inclined surface inclined by Θ from the reference surface, the position and orientation of 6 degrees of freedom are estimated based on the image captured by the image pickup device so that the position and orientation of 3 degrees of freedom can be correctly estimated.

FIG. 11 shows a flowchart of a processing procedure when the present invention is applied to the movement control of a moving body. In the following description, the notation of the process (step) is omitted by adding S at the beginning of each process (step). However, the information processing device 31 does not necessarily have to perform all the steps described in this flowchart.

In S3010, the plane information acquisition unit 317 estimates the plane on which the moving body travels as a preliminary preparation. By using the estimated plane, it is possible to convert the 6-DOF position / posture estimated by the 6-DOF position estimation unit 312 into the 3-DOF position / posture on the plane. The plane on which the moving body travels is estimated by fitting the plane to the position estimated when the moving body is run on the plane. The details of the method of estimating the plane on which the moving body travels will be described later.

In S3020, the map information acquisition unit 316 acquires the target position / posture of the moving body from the upper management system 16. In the present embodiment, the target position / posture of the moving body is a position / posture with three degrees of freedom on a plane.

In S3030, the 6-DOF estimation unit 312 estimates the first information indicating the position and orientation of the 6-DOF of the image pickup device based on at least the image captured around the moving body. Specifically, the 6-DOF estimation unit 312 estimates the position and orientation of the camera based on the image information captured by the stereo camera. When estimating the position and orientation, the map information acquired in advance by the map information acquisition unit 316 is used. The method of Mur-Artal et al. described above is used to estimate the position and orientation of the camera with 6 degrees of freedom. Since the map has already been generated, the map information is not updated and only the position and orientation of the camera are estimated from the image information.

In S3045, the conversion unit 313 converts the position / posture of 6 degrees of freedom estimated in S3030 based on the estimation result of the plane parameter on which the moving body travels estimated in S3010, so that the moving body has 3 degrees of freedom. Determine the position and posture of. Further, the position and orientation of the camera are converted into the position and orientation of the moving body based on the mounting position of the image sensor 32 on the moving body.

In S3060, the position / posture of the moving body converted in S3045 is compared with the target position / posture of the moving body acquired in S3020, and it is determined whether the moving body has reached the target position / posture. In the present embodiment, threshold values are set for each of the difference between the current position and the target position and the difference between the current posture and the target posture, and when both the difference in position and the difference in posture are below the corresponding threshold values, the target is set. It is determined that the position has been reached. In the present embodiment, threshold values are set for each of the difference between the current position and the target position and the difference between the current posture and the target posture, and when both the difference in position and the difference in posture are below the corresponding threshold values, the target is set. It is determined that the position has been reached. When the target position is reached, the process returns to S3020 and a new target position / attitude is received from the upper management system.

In S3070, the control unit 315 calculates the route based on the difference between the position / orientation of the moving body converted in S3045 and the target position / orientation of the moving body transmitted from the upper management system 16, and follows the calculated route. Controls the actuator 35 of the moving body. Further, the mileage calculation unit 314 calculates the mileage D of the moving body based on the position of the camera estimated by the 6-DOF estimation unit 313. Specifically, the mileage D is set to zero when the moving body starts to operate, and the mileage D is based on the difference d between the position at the time of the previous estimation and the position estimated this time in this step. Is updated as D ← D + d.

In S3080, the control unit 315 determines the end of the movement control. When a termination command is sent from the higher management system 16, the movement control is terminated. Otherwise, it returns to S3030 and continues the movement control.

FIG. 5 is a flowchart showing a processing procedure of the method of estimating the plane on which the moving body travels in S3010. In S1100, the plane information acquisition unit 317 acquires data for plane estimation by running the moving body on the same plane as during operation. The data for plane estimation is position / orientation data at a plurality of positions (N points) when the moving body travels. The position-posture data is the 6-DOF position-posture of the image sensor 32 estimated by the 6-DOF position-posture estimation. Here, the 6-DOF position / orientation of the camera in the i-th (1 ≦ i ≦ N) position / orientation data is expressed as (Xvi, Yvi, Zvi, Wxvi, Wyvi, Wzvi). Xvi, Yvi, and Zvi represent the three-dimensional position of the camera in the three-dimensional space, and Wxvi, Wyvi, and Wzvi represent the three-dimensional posture. In this embodiment, a posture expression is used in which the norm of the three-dimensional vector (Wxvi, Wyvi, Wzvi) is the rotation angle and the direction is the rotation axis. Further, the 3 × 3 rotation matrix of the posture represented by Wxvi, Wyvi, and Wzvi is represented by Rvi, and the three-dimensional vector (Xvi, Yvi, Zvi) t is represented by tvi. When acquiring the plane estimation data, the moving body is moved so that the locus of the moving body draws a closed curve, for example, instead of a linear locus so that the plane parameters can be estimated from the moving locus of the moving body. It should be noted that the deviation from the plane due to the partial slope, the vibration of the moving body, the unevenness of the floor surface, etc. can be absorbed by the plane fitting.

In S1110, plane fitting is performed on N position data (Xvi, Yvi, Zvi) t having three degrees of freedom. The parameters (normal vector nv = (nvx, nvy, nvz) t, passing position cv = (cvx, cvy, cvz) t) of the plane on which the moving body traveled when acquiring the plane estimation data are estimated. It is assumed that the normal vector nv is a unit vector. The passing position cv is obtained by calculating the center of gravity of (Xvi, Yvi, Zvi) t. The normal vector nv is obtained by eigenvalue-decomposing a covariance matrix of N three-dimensional vectors (Xvi-cvx, Yvi-cvy, Zvi-cvz) t and obtaining it as an eigenvector corresponding to the minimum eigenvalue. The estimated plane parameters are stored in the plane information acquisition unit 317.

Next, the processing procedure of the method of converting the position / posture of 6 degrees of freedom into the position / posture of 3 degrees of freedom in S3045 will be described.

The 6-DOF position / orientation (Xv, Yv, Zv, Wxv, Wyv, Wzv) is projected on the plane estimated in S3010, and the position / orientation (xv, yv, θv) t with 3 degrees of freedom is converted (note that). Rv is a 3 × 3 rotation matrix with postures represented by Wxv, Wyv, and Wzv). Specifically, a new three-dimensional coordinate system (hereinafter referred to as a plane coordinate system) with the plane passing position cv as the origin and the plane normal vector nv as the Z axis is defined, and six degrees of freedom in the three-dimensional world coordinate system. Convert the position / orientation to the plane coordinate system. From the definition, in the three-dimensional world coordinate system, the origin (0,0,0) t of the plane coordinate system is cvt, and the Z axis (0,0,1) t is nv. From this, the three-dimensional coordinate Xp (= (Xp, Yp, Zp) in the plane coordinate system is converted into the three-dimensional coordinate Xv (= (Xv, Yv, Zv) in the three-dimensional world coordinate system as shown in the following equation. ..

Xv = RpvXp + cv
Here, Rpv is a 3 × 3 rotation matrix that transforms the Z-axis (0,0,1) t into a plane normal vector nv. Rpv is calculated with the rotation axis as the outer product of (0,0,1) t and nv, and the rotation angle as the inverse cosine (arccos) of the inner product of (0,0,1) t and nv. Therefore, the three-dimensional coordinate Xv in the three-dimensional world coordinate system is converted into the three-dimensional coordinate Xp in the plane coordinate system as shown in the following equation.

Xp = Rpv-1 (Xv-cv)
Calculate Xp when tv is input as Xv. Of the positions and orientations with three degrees of freedom, xv and yv correspond to the Xp component and the Yp component of the three-dimensional coordinates Xp. θv is the direction of the vector that projects the optical axis of the image sensor onto a plane. In the present embodiment, the optical axis of the camera on the left side of the stereo camera is the optical axis of the image sensor. Assuming that the three-dimensional vector representing the optical axis of the image sensor in the camera coordinate system defined by the image sensor is vc, the optical axis vo of the image sensor in the plane coordinate system is expressed by the following equation.

vo = Rpv-1Rvvc
As described above, in this embodiment, since the stereo camera is attached to the automatic guided vehicle so that the optical axis of the stereo camera faces slightly upward from the horizontal plane, the optical axis vo is projected onto the XY plane of the plane coordinate system to calculate θv. The optical axis vo'projected on the XY plane of the plane coordinate system is calculated by the following equation.

vo'= vo- (vo · (0,0,1) t) vo
In the present embodiment, the angle of vo'with respect to the X axis of the plane coordinate system is θv.

As described above, in the present embodiment, the mileage of the moving body is calculated based on the position / posture with 6 degrees of freedom estimated from the image information. As a result, the mileage can be calculated stably even if there is a slope on the floor on which the moving body travels.

(Modification example)
In the above-described embodiment, a stereo camera is used as an image sensor. However, the image sensor is not limited to this, and any camera may be used as long as it can obtain image information for estimating the position and orientation. For example, it may be a monocular camera or an RGBD camera capable of acquiring an RGB image and a distance image. Further, the image does not have to be monochrome, and may be a color image such as an RGB image.

In the present embodiment, an AGV that distributes goods in factories and warehouses is assumed as a moving body, but other robots may be used. For example, it can be applied to a vacuum cleaner robot that cleans a room.

In the above-described embodiment, the input unit inputs image information from the camera. However, inputting via the input unit is not limited to image information. For example, sensor measurement values from an inertial sensor (accelerometer, angular velocity sensor) or attitude sensor installed in the camera may be input.

In the above-described embodiment, the position and orientation of the camera are estimated by using a method of minimizing the reprojection error of the feature points detected on the image. However, the method for estimating the position and orientation of the camera is not limited to this. For example, a method for minimizing the difference in luminance values may be used, as in the method disclosed by Angel et al. (J. Angel, et al., "LSD-SLAM: Large-Scale Direct Monocular SLAM," 2014.). Further, not only the image but also a hybrid method with an inertial sensor such as an acceleration sensor or an angular velocity sensor may be used. Further, it may be a position / orientation estimation method using a neural network (trained model). A learned model that outputs the position and posture of the moving body with three degrees of freedom from the image of the surroundings of the moving body captured by the imaging device may be prepared in advance. The trained model trains the image with the measured value of the correct distance as the correct answer. Alternatively, for example, as in the method disclosed by Tateno et al., The position / attitude may be estimated based on the distance information estimated by CNN (Convolutional Neural Network). (K. Tateno et al., "CNN-SLAM: Real-time sense monocular SLAM with learned depth prediction,", 2017.).

In the above-described embodiment, the plane on which the moving body travels is obtained by fitting the plane to the traveling locus. However, the method for determining the plane on which the moving body travels is not limited to this. For example, a two-dimensional marker placed in the scene may be used. (For example, two-dimensional markers disclosed in Kato, M. Billinghurst, Asano, Tachibana, "Augmented Reality System Based on Marker Tracking and Its Calibration", 1999.). Specifically, a two-dimensional marker is placed on a plane on which the moving body travels to match the coordinate system of the marker with the plane, and the position and orientation of the camera with respect to the marker at the same point and the position and orientation of the camera by the 6-degree-of-freedom estimation unit The position and orientation may be converted based on the coordinate system of the marker from the relationship of. Further, the two-dimensional marker may be arranged perpendicular to the traveling plane of the moving body. Further, when the plane is a horizontal plane, an acceleration sensor may be installed on the moving body, and the direction of the gravity axis measured by the acceleration sensor may be used as the normal vector of the plane.

In the embodiment, the method of Grisette et al., Which uses a particle filter, is used as a method of estimating the position and orientation based on the distance information. However, the position / orientation estimation method based on the distance information is not limited to this, and may be, for example, a method using a Kalman filter. Further, it is not always necessary to use a time series filter such as a particle filter or a Kalman filter, and the position / orientation may be estimated for each frame by the ICP algorithm. (Y. Chen, G. Mediani, "Object modeling by reregistration of multiple range images," Image and Vision Computing, 1992.).

In the embodiment, when the position / posture estimated by using the image information and the position / posture to be output to the control unit 15 are determined from the position / posture estimated by using the distance information, when the position / posture estimation using the image information fails. The estimated position and attitude were determined using the distance information. However, the method for determining the position and orientation is not limited to this. For example, if the position / orientation estimation using the distance information fails, the estimated position / orientation may be determined using the image information. Further, the position / orientation estimated using the image information / distance information may be evaluated by a common scale, and the position / orientation may be determined based on the scale. For example, as disclosed in the method of Tateno et al., The likelihood of the estimated position and orientation can be calculated if the probability distribution of the error is known. (Tateno, Kotake, Uchiyama, "High-precision and highly stable model fitting method that integrates distance and shading images for bin picking", IEICE Journal, vol.J94-D, no.8, pp. 1410-1422, 2011.). Therefore, by obtaining the distribution of the detection error of the feature points on the image and the distribution of the measurement error of the distance in advance, and calculating and comparing the likelihood of each position and orientation estimated using the image information / distance information. It is also possible to determine the position and orientation.

In the above-described embodiment, the mileage is calculated and the inclination of the moving body is detected by using the position / posture with 6 degrees of freedom. However, the method of using the position / posture with 6 degrees of freedom is not limited to this. For example, the failure of the camera position / orientation estimation may be determined by using the prior knowledge that the moving body travels on a substantially flat surface. For example, if the estimated camera position is not on a plane, it may be determined that the camera position / orientation estimation has failed. Further, the target of failure determination is not limited to the position and orientation estimation of the camera. For example, the above-mentioned method of Mur-Artal et al. Is a method of simultaneously estimating the position and orientation of a camera and generating map information of a scene. Therefore, the failure determination of the generation of map information may be performed by utilizing the prior knowledge that the moving body travels on a substantially plane based on the estimated position and orientation of the camera.

In the above-described embodiment, the position / orientation with 6 degrees of freedom is estimated using the image information. However, the information used when estimating the position / orientation with 6 degrees of freedom is not limited to the image information, and other information may be used as long as the information can estimate the position / orientation with 6 degrees of freedom. For example, the position and orientation of 6 degrees of freedom may be estimated using the distance information obtained from a distance sensor such as a depth camera or 3D LiDAR. Further, the position and orientation with 6 degrees of freedom may be estimated by using the image information and the distance information together. Further, information from an inertial sensor such as an acceleration sensor or an angular velocity sensor may be used together.

The present invention is also realized by executing the following processing. That is, software (program) that realizes the functions of the above-described embodiment is supplied to the system or device via a network for data communication or various storage media. Then, the computer (or CPU, MPU, etc.) of the system or device reads and executes the program. Further, the program may be recorded and provided on a computer-readable recording medium.

1 Moving body 11 Information processing device 12 Image sensor 13 Distance sensor 14 Contact sensor 15 Actuator 17 Odometer 111 Input unit 112 6 Degree of freedom estimation unit 113 Conversion unit 115 Control unit 116 Map information acquisition unit 117 Plane information acquisition unit

Claims (18)

An information processing device that determines the position and orientation of a moving body equipped with a function to measure multiple positions and orientations.
The first acquisition means for acquiring the first information indicating the position and posture of the moving body with six degrees of freedom, and
A second acquisition means for acquiring the second information indicating the position and posture of the moving body having three degrees of freedom, and
Using the first information, an output means for outputting the third information indicating the position and posture of three degrees of freedom, and at least one of the second information and the third information are selectively selected according to the inclination of the moving body. An information processing apparatus characterized by having a determination means for determining a position and a posture of the moving body by using the information processing apparatus. The determining means uses the second information when the inclination of the moving body is smaller than a predetermined value, and the third information when the inclination of the moving body is larger than a predetermined value, respectively, of the moving body. The information processing apparatus according to claim 1, wherein the position and the posture are determined. The information processing apparatus according to claim 1 or 2, wherein the first acquisition means acquires the first information based on an image captured by the photographing apparatus at least around the moving body. The second acquisition means is based on two-dimensional map information indicating an environment in which the moving body travels and distance information obtained by measuring the distance between an object and the moving body in the environment in which the moving body travels. The information processing apparatus according to any one of claims 1 to 3, wherein the second information is acquired. Based on the position and posture of the moving body having three degrees of freedom determined by the determining means and the map information representing the environment in which the moving body travels, the moving body is moved to the target position indicated in the map information. The information processing apparatus according to any one of claims 1 to 4, further comprising a control means for moving. Further, it has a detecting means for detecting the inclination formed by the moving body and the reference surface.
When the inclination detected by the detecting means is smaller than the predetermined value, the determining means determines the position and posture of the moving body with three degrees of freedom based on the second information, and detects the position and the posture of the moving body by the detecting means. The invention according to any one of claims 1 to 5, wherein when the inclination is larger than the predetermined value, the position and the posture of the moving body having three degrees of freedom are determined based on the third information. Information processing equipment. The information processing device according to claim 6, wherein the detection means detects the inclination of the moving body based on the first information. The information processing device according to claim 6, wherein the detection means detects the inclination of the moving body based on a gravity sensor mounted on the moving body. The first acquisition means is a learned model that inputs an image of the surroundings of the moving body captured by an imaging device mounted on the moving body and outputs the position and posture of the moving body with three degrees of freedom. The information processing apparatus according to any one of claims 1 to 8, wherein the position and the posture of the moving body with three degrees of freedom are acquired based on the above. The information processing according to any one of claims 1 to 9, further comprising an output means for outputting a notification to the user when the inclination of the moving body is detected based on the first information. apparatus. When the inclination formed by the moving body and the reference surface is smaller than a predetermined value, the determining means determines the position and orientation of the moving body with three degrees of freedom based on the second information and the third information. The information processing apparatus according to any one of claims 1 to 10, wherein the information processing apparatus is determined. The information processing apparatus according to any one of claims 1 to 11, wherein the position and posture of the moving body having three degrees of freedom are a position of two degrees of freedom and a posture of one degree of freedom. The information processing apparatus according to any one of claims 1 to 12, wherein the position and posture of the moving body having 6 degrees of freedom are a position of 3 degrees of freedom and a posture of 3 degrees of freedom. The determination means determines the position and posture of the moving body having three degrees of freedom by using the third information when the state in which the inclination of the moving body is larger than the predetermined value continues for a predetermined time. The information processing apparatus according to any one of claims 1 to 13. When the inclination of the moving body is smaller than a predetermined value and the inclination of the moving body is smaller than a predetermined time for a longer period of time than a predetermined time, the determining means obtains the second information from the third information. The information processing apparatus according to any one of claims 1 to 14, wherein the position and orientation of the moving body are changed so as to determine the position and orientation of the moving body. An information processing device that estimates the position and orientation of a moving object equipped with an imaging device.
A six-DOF estimation means that estimates first information indicating the position and orientation of the six-DOF of the imaging device based on at least an image of the surroundings of the moving body.
When the inclination formed by the moving body and the reference surface is larger than a predetermined value, the position and posture of the moving body having three degrees of freedom are determined based on the first information, and the moving body and the reference surface are used as a reference. When the inclination formed by the surface is smaller than a predetermined value, the position and posture of the moving body with three degrees of freedom are determined based on the distance information obtained by measuring the distance between the object and the moving body in the environment in which the moving body travels. An information processing apparatus comprising: a determination means for determining a position and a posture of the moving body having three degrees of freedom based on the second information shown. 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 16. It is an information processing method that estimates the position and orientation of a moving object equipped with an image pickup device.
A 6-DOF estimation step for estimating the first information indicating the position and orientation of the 6-DOF of the imaging device, and
When the inclination formed by the moving body and the reference surface is larger than a predetermined value, the position and posture of the moving body having three degrees of freedom are determined based on the first information, and the moving body and the reference surface are used as a reference. When the inclination formed by the surface is smaller than a predetermined value, the determination step of determining the position and orientation of the three degrees of freedom of the moving body based on the second information indicating the position and orientation of the three degrees of freedom of the moving body. , An information processing method characterized by having.

Images