Self-position estimation device

Abstract

The present application provides a self-position estimating device, which can reduce the processing time when estimating the self-position in an environment with a large number of three-dimensional point group data. The self-position estimating device includes an estimating unit for estimating the self-position of a mobile robot. The estimating unit includes: a map generating unit for generating a map of the surroundings of the mobile robot based on the three-dimensional point group data detected by the detection unit; a geometric feature extraction unit for extracting geometric features from the current map and from the past map based on the map; a self-position calculating unit for selecting the geometric features extracted by the geometric feature extraction unit to form a group of geometric features, and calculating the self-position on the past map based on the group of geometric features; a self-position evaluating unit for evaluating the similarity between the current map and the past map for each group of geometric features based on the self-position calculated by the self-position calculating unit, and selecting the self-position with high similarity.

Description

Technical Field

[0001] The present invention relates to a device for estimating the own position of a mobile object. Background Art

[0002] In the past, the technology used for autonomous movement of mobile bodies such as mobile robots or self-driving cars used a technology called SLAM (Simultaneous Localization and Mapping) to estimate their own position and create an environmental map. As an environmental map, there are placeholder grid maps or point group maps that are created based on the presence or absence of objects in the moving range of the mobile body. The placeholder grid map divides the plane or space of the moving range into multiple partitions (cells) for storage, and gives each partitioned partition a cell value corresponding to the presence or absence of the object. The point group map represents the objects existing in the plane or space of the moving range as discrete points (coordinates) in each micro-area, and the collected point group data is used as a map.

[0003] When the mobile body moves autonomously to the destination, it finds the moving path from the current location to the destination and moves to the destination along the moving path. In order to move efficiently, the moving path is calculated using a pre-made map (existing map), and the mobile body moves while confirming (estimating) its own position on the existing map. The estimation of its own position on the existing map is an indispensable technology for the autonomous movement of the mobile body.

[0004] As a technique for estimating the self-position of a mobile body, a method is generally used to estimate the self-position by using the distance data obtained by the output of a distance sensor to match the position of the mobile body with a pre-prepared environment map. As a method for matching the position of a mobile body, for example, the ICP (Iterative Closest Point) method is used. In this method, the position of the mobile body is determined by comparing the characteristic parts of the environment map based on the geometric features of a set of points extracted from point group data (for example, refer to Patent Document 1). The self-position estimation method described in Patent Document 1 observes the surroundings through a distance sensor to obtain multiple three-dimensional points representing the surface of an object, and based on the observation results, extracts three-dimensional points at a short distance and three-dimensional points at a long distance whose feature value is greater than a specified threshold, and estimates the self-position by mapping the extracted multiple three-dimensional points with multiple three-dimensional points on the environment map.

[0005] Prior Art Literature

[0006] Patent Literature

[0007] Patent Document 1: Japanese Patent Application Publication No. JP2017-083230 Summary of the invention

[0008] Problems to be solved by the invention

[0009] However, in the invention of Patent Document 1, since the self-position is estimated by mapping multiple three-dimensional points, there is a problem of long processing time when estimating the self-position in an environment with a large number of three-dimensional points (three-dimensional point group data), such as an outdoor environment.

[0010] The present application aims to provide a self-position estimating device that can reduce processing time when estimating a self-position in an environment with a large amount of three-dimensional point group data, and can estimate the self-position with high accuracy.

[0011] Solutions to Solve Problems

[0012] The self-position estimating device of the present application is a self-position estimating device for estimating the self-position of a mobile body, and is characterized in that it comprises: a detection unit for detecting the distance from the mobile body to surrounding objects as three-dimensional point group data and an estimating unit for estimating the self-position of the mobile body, the estimating unit comprising: a map generating unit for generating a map of the surroundings of the mobile body based on the three-dimensional point group data detected by the detection unit; a storage unit for storing the map generated by the map generating unit; based on the map stored in the storage unit, extracting at least any one of three geometric features of a plane, a straight line and a sphere from a current map as a map of the surroundings of the current position of the mobile body as the current geometric feature, and a geometric feature extraction unit that extracts past geometric features from a past map of a map of the surrounding past positions of a moving body; a self-position calculation unit that selects two or three current geometric features from the current geometric features extracted by the geometric feature extraction unit, and selects a geometric feature group consisting of past geometric features of the same type and number as the selected current geometric features from the past geometric features extracted by the geometric feature extraction unit, and calculates a self-position on a past map based on the geometric feature group; and a self-position evaluation unit that evaluates the similarity between a current map and a past map for each geometric feature group based on the self-position calculated by the self-position calculation unit, and selects a self-position with a high similarity.

[0013] Therefore, since the self-position calculation unit uses geometric features that are significantly less than the number of three-dimensional points, its combination can also be realized in a small number. When using three-dimensional points to calculate the self-position, three three-dimensional points are selected from the current map and the past map respectively, and their group is used to calculate the self-position. Since the three-dimensional points have a large number (thousands to tens of thousands), their combinations are also large, and it takes a lot of processing time to calculate a group with high similarity. On the other hand, the geometric features of the present application are fewer in number (several to hundreds) than three-dimensional points, and the number of combinations can also be small. Therefore, the self-position estimation device can also reduce processing time when estimating the self-position in an environment with a large number of three-dimensional point group data.

[0014] In the present application, preferably, the own position evaluation unit extracts a sphere group from each of the current map and the past map, and based on the sphere group, evaluates the similarity between the current map and the past map for each group of geometric features.

[0015] Thus, since the own position evaluation unit extracts a sphere group from each of the current map and the past map, and evaluates the similarity between the current map and the past map for each group of geometric features based on the sphere group, the similarity between the current map and the past map can be evaluated for each group of geometric features based on a sphere group that is smaller than the number of three-dimensional point group data. Therefore, the own position estimation device can further reduce processing time.

[0016] In the present application, preferably, the self-position evaluation unit extracts self-positions having a similarity higher than a specified threshold by increasing the number of sphere groups from each of the current map and the past map, and re-evaluates the similarity between the current map and the past map based on the sphere group for each group of geometric features.

[0017] Thus, since the self-position evaluation unit increases the number of sphere groups from each of the current map and the past map for self-positions with a similarity higher than a predetermined threshold value and extracts them, and re-evaluates the similarity of the current map and the past map for each group of geometric features based on the sphere groups, it is possible to gradually lock (narrow the range) self-positions with a similarity higher than the predetermined threshold value while increasing the number of sphere groups in a stepwise manner. Therefore, the self-position estimation device can estimate the self-position with higher accuracy.

[0018] Preferably in the present application, the self-position evaluation unit evaluates the similarity between the current map and the past map based on the sphere group for each group of geometric features, selects the self-position with high similarity, and then further estimates the self-position based on the self-position in a manner such that the similarity between the three-dimensional point group data related to the current map and the three-dimensional point group data related to the past map becomes higher.

[0019] Therefore, since the self-position evaluation unit evaluates the similarity between the current map and the past map for each group of the geometric features based on the sphere group, after selecting the self-position with high similarity, the self-position is further estimated based on the self-position in a manner that the similarity between the three-dimensional point group data related to the current map and the three-dimensional point group data related to the past map becomes higher. Therefore, the self-position estimating device can reduce the processing time from the beginning compared to the case of estimating the self-position based on the three-dimensional point group data, and can estimate the self-position with higher accuracy.

[0020] Preferably in the present application, the self-position calculation unit determines whether there is consistency between the angle or distance between current geometric features included in the group of geometric features and the angle or distance between past geometric features included in the group of geometric features. If it is determined that there is no consistency, the self-position on the past map is not calculated based on the group of geometric features.

[0021] What is considered here is that since the consistency between the angle or distance between the current geometric features included in the group of geometric features and the angle or distance between the past geometric features included in the group of geometric features does not change even if a coordinate transformation is performed to calculate one's own position on the past map, when the similarity between the current map and the past map is evaluated based on one's own position calculated based on the group of inconsistent geometric features, the similarity is low.

[0022] According to the present application, since the self-position calculation unit determines whether there is consistency between the angle or distance between the current geometric features included in the group of geometric features and the angle or distance between the past geometric features included in the group of geometric features, if it is determined that there is no consistency, the self-position on the past map is not calculated based on the group of geometric features, so the self-position estimation device can further reduce the processing time.

[0023] In the present application, preferably, when the self-position calculation unit selects the current geometric features and the past geometric features to form a group of geometric features, the selection is made in such a way that the angles formed by the planes within each geometric feature, the angles formed by the straight lines, and the angles formed by the planes and the straight lines are greater than a specified angle.

[0024] Here, when the angles formed by the planes within each geometric feature, the angles formed by the straight lines, and the angles formed by the planes and the straight lines within each geometric feature are parallel and close, there is a situation where the self-position calculation unit cannot accurately calculate the self-position on the past map based on the group of geometric features.

[0025] According to the present application, the self-position calculation unit selects the current geometric features and the past geometric features to form a geometric feature group, and selects the angles formed by the planes in each geometric feature, the angles formed by the straight lines, and the angles formed by the plane and the straight line to form a predetermined angle or more. Therefore, the self-position calculation unit can properly calculate the self-position on the past map based on the geometric feature group. In addition, the predetermined angle is preferably an angle that clearly does not form a parallel angle, for example, preferably more than 30 degrees.

[0026] Preferably in the present application, the self-position calculation unit determines whether the current geometric features included in the group of geometric features and the past geometric features included in the group of geometric features include a ground plane which is a plane equivalent to the ground. If it is determined that the ground plane is included, the ground plane in the group of geometric features is corresponded to calculate the self-position on the past map.

[0027] Therefore, since the self-position calculation unit determines whether the current geometric features included in the group of geometric features and the past geometric features included in the group of geometric features include a ground plane that is a plane equivalent to the ground, when it is determined that the ground plane is included, the ground plane in the group of geometric features is made to correspond to calculate the self-position on the past map. Therefore, when the self-position estimation device includes multiple planes in the group of geometric features, it is possible to reduce the number of combinations of planes and further reduce the processing time.

[0028] Preferably in the present application, when the self-position calculation unit selects the current geometric features of the plane and the sphere and the past geometric features of the plane and the sphere to form a group of geometric features, it determines whether the current geometric features included in the group of geometric features and the past geometric features included in the group of geometric features include a ground plane which is a plane equivalent to the ground. When it is determined that the ground plane is included, only the sphere located at a specified height from the ground plane is selected.

[0029] Therefore, when the self-position calculation unit selects the current geometric features of the plane and the sphere and the past geometric features of the plane and the sphere to form a group of geometric features, it determines whether the current geometric features included in the group of geometric features and the past geometric features included in the group of geometric features include a ground plane that is a plane equivalent to the ground. When it is determined that the ground plane is included, only the sphere located at a specified height from the ground plane is selected. Therefore, when the self-position estimation device includes the ground plane in the group of geometric features, the number of spheres to be selected can be reduced, and the processing time can be further reduced. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] Figure 1 This is a diagram showing a configuration of a self-position estimating device of a mobile robot according to one embodiment of the present application.

[0031] Figure 2 It is a diagram that schematically represents the three geometric features of a plane, a straight line, and a sphere.

[0032] Figure 3 This is a schematic diagram showing the actual environment in which the mobile robot moves when viewed from vertically above.

[0033] Figure 4 This is a schematic diagram showing the entire map generated by stitching together scans during a single pass of the mobile robot using a position alignment method such as ICP.

[0034] Figure 5 This is a schematic diagram showing the current map and past maps.

[0035] Figure 6This is a schematic diagram showing the overall map that combines the current map with the past map by correcting the configuration of the current map using the own position with a high degree of similarity.

[0036] Figure 7 This is a flowchart showing the flow of a self-position estimation process performed by a self-position estimation device of a mobile robot.

[0037] Figure 8 A schematic diagram showing examples of angles or distances between geometric features. DETAILED DESCRIPTION

[0038] Hereinafter, one embodiment of the present application will be described with reference to the accompanying drawings.

[0039] Figure 1 It is a diagram showing the structure of a mobile robot self-position estimation device according to one embodiment of the present application. In this embodiment, the position (translation component) and direction (rotation component) of the mobile robot 1 in three-dimensional space are collectively referred to as self-position.

[0040] The mobile robot 1 is a mobile body capable of autonomous movement. Figure 1 As shown, it has: a self-position estimating device 2 for estimating the self-position of the mobile robot 1; a driving unit such as a motor (not shown in the figure) and wheels (not shown in the figure) driven by the driving unit; and a moving unit 3 that enables the mobile robot 1 to move autonomously.

[0041] The own position estimating device 2 includes a detecting unit 4 for detecting the distance between the mobile robot 1 and surrounding objects as three-dimensional point group data and an estimating unit 5 for estimating the own position of the mobile robot 1 .

[0042] The detection unit 4 has a three-dimensional laser scanner or LIDAR (Light Detection and Ranging) that detects the presence of an object by irradiating a laser beam around the mobile robot 1, and obtains three-dimensional point group data representing the surrounding shape. The three-dimensional laser scanner irradiates the surrounding area with a laser beam while rotating an infrared laser or other device (or a transmitting mirror) around the center of the sensor, and measures the presence and distance of an object for each specified angle. If there is an object, the position where the laser beam irradiates the object is obtained in the form of a distance and direction from the center of the sensor. Based on the distance and direction, a three-dimensional point (x, y, z) in the sensor coordinate system with the center of the sensor as the origin is obtained. T .

[0043] Therefore, by mounting multiple laser devices along the sensor center axis and rotating them, three-dimensional point group data can be obtained. In addition, when there is only one laser device, the laser beam can be scanned in both the vertical and horizontal directions to obtain three-dimensional point group data.

[0044] In the detection unit 4, the three-dimensional point group data obtained by one cycle of the laser scanner (for example, one rotation of 360°) is called scan data or simply scan, and is used as a processing unit.

[0045] The estimation unit 5 is composed of a CPU (Central Processing Unit) and a memory, and performs information processing according to a predetermined program stored in the memory. The estimation unit 5 includes a map generation unit 51, a storage unit 52, a geometric feature extraction unit 53, a self-position calculation unit 54, and a self-position evaluation unit 55.

[0046] The map generation unit 51 generates a map of the surroundings of the mobile robot 1 based on the three-dimensional point group data detected by the detection unit 4 .

[0047] The storage unit 52 stores the map generated by the map generating unit 51. In addition to the map generated by the map generating unit 51, the storage unit 52 may store the three-dimensional point group data detected by the detecting unit 4.

[0048] The geometric feature extraction unit 53 extracts at least one of the three geometric features of a plane, a straight line, and a sphere as a current geometric feature from a current map which is a map of the surrounding area of ​​the current position of the mobile robot 1, and extracts it as a past geometric feature from a past map which is a map of the surrounding area of ​​the past positions of the mobile robot 1, based on the map stored in the storage unit 52.

[0049] Figure 2 It is a diagram that schematically represents the three geometric features of a plane, a straight line, and a sphere.

[0050] In this embodiment, the geometric features are as follows Figure 2 As shown in FIG. 1 , it is a general term for figures (planes, straight lines, and spheres) having geometric features extracted from the three-dimensional point group data detected by the detection unit 4.

[0051] The geometrical characteristics (geometrical characteristics P) of a plane (P) are as follows Figure 2 As shown in P1 and P2 of FIG. 1 , the flat part is connected and extracted based on the three-dimensional point group data detected by the detection unit 4. The geometric features of the plane can be extracted using, for example, Hough transform, RANSAC (Random Sample Consensus), or principal component analysis.

[0052] The geometrical characteristics (geometrical characteristics L) of a straight line (L) are as follows Figure 2As shown in L1 and L2 of FIG, the straight line parts are connected and extracted based on the three-dimensional point group data detected by the detection unit 4. Similar to the geometric features of the plane, the geometric features of the straight line can be extracted using, for example, Hough transform, RANSAC, or principal component analysis.

[0053] The geometrical characteristics (geometrical characteristics B) of the sphere (B) are as follows Figure 2 As shown in B1 and B2, based on the three-dimensional point group data detected by the detection unit 4, the three-dimensional point group data contained in a sphere of radius R with an arbitrary three-dimensional point C as the center is extracted using the arbitrary point C as a representative. The geometric feature B is represented by the representative point C. In addition, the geometric feature B can also be extracted using the center of gravity of the three-dimensional point group contained inside the sphere of radius R as a representative. Furthermore, the sphere of the present application is not limited to a true sphere, but may also include an ellipsoid or a cube, etc. In other words, the geometric feature B represents a three-dimensional figure with a certain space with a representative point.

[0054] The self-position calculation unit 54 selects two or three current geometric features from the current geometric features extracted by the geometric feature extraction unit 53, and selects past geometric features of the same type and number as the selected current geometric features from the past geometric features extracted by the geometric feature extraction unit 53 to form a geometric feature group, and calculates the self-position on the past map based on the geometric feature group.

[0055] The own position evaluation unit 55 evaluates the similarity between the current map and the past map for each set of geometric features based on the own position calculated by the own position calculation unit 54, and selects the own position with high similarity.

[0056] Below, refer to Figure 3 to Figure 8 , explaining the concept that the own position of the mobile robot 1 is calculated by the own position calculation unit 54 and the own position evaluation unit 55 selects the own position with high similarity.

[0057] Figure 3 This is a schematic diagram showing the actual environment in which the mobile robot moves when viewed from vertically above.

[0058] The actual environment in which the mobile robot 1 moves is as follows Figure 3 As shown, there are a ground G, a wall W erected on the ground G, and objects OB1 to OB6 such as trees and obstacles existing on the ground G.

[0059] As the mobile robot 1 travels in the actual environment, the map generating unit 51 generates a map of the surroundings of the mobile robot 1 based on the three-dimensional point group data detected by the detecting unit 4 .

[0060] Figure 4This is a schematic diagram showing the overall map generated by combining scans using a position matching method such as ICP during a single movement of the mobile robot. Figure 4 The scissors represent the moving trajectory of the mobile robot.

[0061] In this example, the mobile robot passes through position PO2 and then position PO1, but because the two positions are close to each other, the same object can be measured by the 3D laser scanner. However, as the mobile robot moves, the position error accumulates, so the object measured at position PO2 and the object measured at position PO1 are the same object, but they are slightly deviated and recorded on the map.

[0062] In addition, the Figure 4 In the map, the deviation of objects is clearly drawn, but in reality, the three-dimensional point groups overlap multiple times, which will cause the map to appear skewed.

[0063] In order to eliminate such map distortion, a partial map extracted from the entire map is used. Here, the position PO1 is taken as the current position, and the partial map around the position PO1 is called the current map. Figure 4 In FIG. 1 , the dotted circle with PO1 as the center is the current map. Also, the position PO2 is a position that has been passed in the past, and the part of the map around the position PO2 is called the past map. Figure 4 In the figure, the dotted circle with PO2 as the center is the past map.

[0064] Figure 5 This is a schematic diagram showing the current map and past maps. Figure 5 (A) is from Figure 4 The overall map cuts out the current map around position PO1, Figure 5 (B) is from Figure 4 The entire map of the present invention cuts off the past map around the position PO2. In addition, the size of the range of the current map and the range of the past map are not limited to be the same. In addition, the coordinate system of the current map and the coordinate system of the past map are different from each other.

[0065] Next, to make the current map and the past map Figure 1 In this embodiment, the own position of the mobile robot 1 is calculated by the own position calculation unit 54, and the own position evaluation unit 55 selects the own position with high similarity between the current map and the past map.

[0066] Figure 6 This is a schematic diagram showing the overall map that combines the current map with the past map by correcting the configuration of the current map using the own position with a high degree of similarity.

[0067] Thus, in this embodiment, the estimation unit 5 is as follows Figure 6As shown, the own position of the mobile robot 1 is estimated so that the three-dimensional point group data of the current map and the past map overlap without deviation.

[0068] Figure 7 This is a flowchart showing the flow of a self-position estimation process performed by a self-position estimation device of a mobile robot.

[0069] In the self-position estimation process performed by the self-position estimation device 2 of the mobile robot 1, the estimation unit 5 is as follows: Figure 7 As shown, the processing of steps ST1 to ST7 is executed.

[0070] The map generation unit 51 generates a current map that is a map of the surrounding area of ​​the current position of the mobile robot 1 based on the three-dimensional point group data detected by the detection unit 4, and stores it in the storage unit 52 (step ST1: current map generation step).

[0071] Next, the estimating unit 5 obtains a past map, which is a map of the surrounding area of ​​the past position of the mobile robot 1 , from the storage unit 52 (step ST2 : past map obtaining step).

[0072] Here, the past map can, for example, follow the moving trajectory of the mobile robot 1, select a past position close to the current position, and obtain a map of the surrounding area of ​​the past position. It can also select an area of ​​a specified range based on the overall map created so far (an environmental map that integrates the past map and the current map) to obtain a map of the surrounding area of ​​the past position.

[0073] After the current map is generated in the current map generation step ST1 and the past map is acquired in the past map acquisition step ST2, the geometric feature extraction unit 53 extracts three geometric features of a plane (P), a straight line (L) and a sphere (B) from the current map as current geometric features, and extracts them from the past map as past geometric features based on the map stored in the storage unit 52 (step ST3: geometric feature extraction step).

[0074] In addition, in the present embodiment, the geometric feature extraction unit 53 extracts three geometric features as the current geometric features and the past geometric features, but may also extract one geometric feature or two geometric features. In short, the geometric feature extraction unit only needs to extract at least one of the three geometric features of a plane, a straight line, and a sphere as the current geometric feature and the past geometric feature.

[0075] The self-position calculation unit 54 selects two or three current geometric features from the current geometric features extracted by the geometric feature extraction unit 53, and selects past geometric features of the same type and number as the selected current geometric features from the past geometric features extracted by the geometric feature extraction unit 53, and creates a group of the current geometric features and the past geometric features (step ST4: geometric feature selection step). For example, geometric features PPL (two planes P and one line L) are extracted from the current map, and geometric features PPL are extracted from the past map to create a group consisting of two geometric features PPL.

[0076] Then, the self-position calculation unit 54 calculates the current self-position ( Figure 4 Where PO1) was located on the past map (step ST5: own position calculation step).

[0077] In this calculation, two or three current geometric features need to be accurately matched with the same type and number of past geometric features. However, which geometric features are matched is often unknown in advance, so the self-position is calculated for each combination of geometric features to verify the accuracy of the self-position.

[0078] For example, there are two combinations when calculating the position based on the current geometric features P1P2L1 and the past geometric features P3P4L2. One is to correspond P1 to P3, P2 to P4, and L1 to L2, and the other is to correspond P1 to P4, P2 to P3, and L1 to L2.

[0079] Here, when the current geometric feature and the past geometric feature constitute a geometric feature group, the self-position calculating unit 54 selects the angles formed by the planes in each geometric feature, the angles formed by the straight lines, and the angle formed by the plane and the straight line so as to form a predetermined angle or more (for example, 30 degrees or more). This is because, for example, taking the aforementioned geometric feature PPL as an example, when two planes are parallel or when a plane and a straight line are parallel, the self-position cannot be uniquely calculated from the geometric feature group.

[0080] In addition, in this embodiment, the current geometric features and past geometric features are selected in such a way that the angles formed by the planes within each geometric feature, the angles formed by the straight lines, and the angles formed by the plane and the straight line are greater than a specified angle, but such a restriction may not be set.

[0081] Furthermore, when the self-position calculation unit 54 selects a geometric feature group consisting of a current geometric feature including a plane and a past geometric feature including a plane, the self-position calculation unit 54 determines whether the current geometric feature included in the geometric feature group and the past geometric feature included in the geometric feature group include a ground plane that is a plane equivalent to the ground. Then, when it is determined that the ground plane is included, the self-position calculation unit 54 selects only a sphere located at a predetermined height from the ground plane.

[0082] Here, in the present embodiment, since the mobile robot 1 includes the mobile unit 3 having wheels, it is possible to easily determine whether the ground plane is included by using a sensor or the like.

[0083] Furthermore, in the present embodiment, when determining that the ground plane is included, the own position calculation unit 54 selects only the sphere located at a predetermined height from the ground plane, but such a restriction may not be provided.

[0084] There are various methods for calculating one's own position on the past map based on a combination of current geometric features and past geometric features, but the method of calculating by dividing it into rotational components and translational components is simple.

[0085] First, the rotation component is calculated based on a group of two or more direction vectors. Here, let the direction vector of the current geometric feature be d i r , and the direction vector of a past geometric feature corresponding to it is d i C , the matrix of the rotational component of the self-position to be obtained is R, and their relationship is as shown in the following equation (1). If there are two such equations, the rotational component can be calculated.

[0086] [Formula 1]

[0087]

[0088] Therefore, the rotation matrix R is calculated by taking into account the error of the three-dimensional point group data and minimizing C in the following equations (2) and (3).

[0089] [Formula 2]

[0090]

[0091]

[0092] Here, the direction vector uses the normal vector of the plane, the direction vector of the straight line, and the direction vector of the straight line connecting the two spheres. When the self-position calculation unit 54 selects, for example, the geometric feature PPP (three planes P), the geometric feature PPL (two planes P and one straight line L), and the geometric feature PLL (one plane P and two straight lines L), the rotation component is calculated using three direction vectors. In addition, when the self-position calculation unit 54 selects, for example, the geometric feature PBB (one plane P and two spheres B), the rotation component is calculated using two direction vectors (the normal vector of the plane and the direction vector of the straight line connecting the two spheres). In addition, when the geometric feature LB (one straight line L and one sphere B) is selected, the rotation component is calculated using two direction vectors (the direction vector of the straight line and the direction vector of the perpendicular line descending from the sphere in a straight line). In addition, m in equations (2) and (3) is the number of direction vectors.

[0093] Next, the translation component is calculated for the types of the current geometric features and the past geometric features. The direction vector of the current geometric features and the three-dimensional point group data on the current map are rotated in advance by the previously obtained rotation matrix R. As a result, the direction vectors of the corresponding geometric features are the same. Then, let the translation vector of the obtained own position be t, and calculate the translation component for each type of the current geometric features and the past geometric features based on the following three formulas.

[0094] In the case of the geometric features of a plane (P), let its normal vector be n i , the point on the plane of the current map is c i C , the point on the plane of the past map is c i r , then their relationship is as shown in equations (4) and (5). Here, P is an arbitrary point on the plane. And, equation (6) is obtained based on equations (4) and (5).

[0095] [Formula 3]

[0096]

[0097]

[0098]

[0099] In the case of the geometric characteristics of a straight line (L), let its direction vector be d i , let c be a point on the straight line of the current map i C , the point on the straight line of the past map is c i r, then their relationship is as shown in equations (7) and (8). Here, P is an arbitrary point on the straight line, and u and v are the parameters of the straight line equation. And, equation (9) is obtained based on equations (7) and (8).

[0100] [Formula 4]

[0101]

[0102]

[0103]

[0104] In the case of the geometric features of the sphere (B), let the representative position of the sphere in the current map be c i C , let c be the representative position of the sphere in the past map i r , then their relationship is as shown in formula (10).

[0105] [Formula 5]

[0106]

[0107] Then, the following equation (11) as a simultaneous equation for the translation vector t is obtained based on equations (6), (9), and (10) corresponding to the types of the current geometric features and the past geometric features.

[0108] [Formula 6]

[0109] At=q···(11)

[0110] Here, A is the stacked rows and columns on the left sides of equations (6), (9), and (10), and q is the stacked vectors on the right sides of equations (6), (9), and (10). This simultaneous equation can be solved for the three variables of the translation vector t = (x, y, z) by selecting two or three current geometric features and selecting a group of geometric features consisting of the same type and number of past geometric features as the selected current geometric features.

[0111] Here, the self-position calculation unit 54 determines whether there is consistency between the angle or distance between the current geometric features included in the group of geometric features and the angle or distance between the past geometric features included in the group of geometric features. Then, if it is determined that there is no consistency, the self-position calculation unit 54 does not calculate the self-position on the past map based on the group of geometric features.

[0112] Figure 8 A schematic diagram showing examples of angles or distances between geometric features.

[0113] Here, the angle or distance between the geometric features used to determine the consistency is as follows: Figure 8 As shown in (A), the angle formed by the plane P1 and the plane P2, the angle a1 formed by the plane P1 and the straight line L1, or the distance d1 between the plane P2 and the straight line L1 can be used.

[0114] Furthermore, when the geometric feature extraction unit 53 extracts the geometric feature PLL, the angle or distance between the geometric features used to determine the consistency is as follows: Figure 8 As shown in (B), the angle formed by the plane P3 and the straight lines L2 and L3 or the distance d2 between the straight lines L2 and L3 can be used.

[0115] Furthermore, when the geometric feature extraction unit 53 extracts the geometric feature PBB, the angle or distance between the geometric features used to determine the consistency is as follows: Figure 8 As shown in (C), the distances h1 and h2 between the plane P4 and the spheres B1 and B2 or the distance d3 between the sphere B1 and B2 can be used.

[0116] Further, the angle or distance between the geometric features used to determine consistency is as follows when the geometric feature extraction unit 53 extracts the geometric feature LB: Figure 8 As shown in (D), the distance d4 between the straight line L4 and the sphere B3 can be used.

[0117] Furthermore, in the present embodiment, when the self-position calculation unit 54 determines that there is no consistency between the angle or distance between the current geometric features included in the group of geometric features and the angle or distance between the past geometric features included in the group of geometric features, it does not calculate the self-position on the past map based on the group of geometric features, but may calculate the self-position on the past map based on the group of geometric features.

[0118] Furthermore, the self-position calculating unit 54 determines whether the current geometric features included in the group of geometric features and the past geometric features included in the group of geometric features include a ground plane that is a plane equivalent to the ground. Then, if it is determined that the ground plane is included, the self-position calculating unit 54 calculates the self-position on the past map corresponding to the ground plane in the group of geometric features.

[0119] In addition, in this embodiment, when it is determined that the ground plane is included, the self-position calculation unit 54 calculates the self-position on the past map corresponding to the ground plane in the group of geometric features, but it is also possible to calculate the self-position on the past map without corresponding to the ground plane.

[0120] In the self-position calculation step ST5, after calculating the self-position on the past map, the self-position evaluation unit 55 evaluates the similarity between the current map and the past map for each group of geometric features based on the self-position calculated by the self-position calculation unit 54, and selects the self-position with high similarity (step ST6: self-position evaluation step).

[0121] Specifically, the own position evaluation unit 55 extracts a sphere group from each of the current map and the past map, and evaluates the similarity between the current map and the past map for each set of geometric features based on the sphere group.

[0122] First, the own position evaluation unit 55 assumes that the own position (assumption) calculated by the own position calculation unit 54 is pose, and the sphere group of the current map is P c , the sphere group of the past map is P r .

[0123] Then, the similarity between the current map and the past map is evaluated for each set of geometric features in the following order (A1) to (A5).

[0124] (A1) Set the score S=0.

[0125] (A2) i C ∈P c After the coordinate transformation from pose, it becomes q i C .

[0126] (A3)q i C There exists P within the radius r r In the case of point , let S = S + 1.

[0127] (A4) P c Repeat the above-mentioned processes (A2) to (A3) for all points.

[0128] (A5) When the score S is greater than or equal to the threshold value sthre, the own position (hypothesis) is considered to be correct.

[0129] Here, pose replaces P i C In the case of coordinate transformation, the following equation (12) is used: R is the rotation matrix of pose, and t is the translation vector of pose.

[0130] [Formula 7]

[0131]

[0132] In this embodiment, the process of (A3) is performed by nearest neighbor search using a kd tree. In contrast, the self-position evaluation unit 55 may replace the process of (A3) by obtaining P, for example. r The nearest point to q i C Based on the distance from the point, update S.

[0133] Furthermore, the own position evaluation unit 55 increases the number of sphere groups extracted from each of the current map and the past map for the own position having a higher similarity than a predetermined threshold, and re-evaluates the similarity of the current map and the past map for each group of geometric features based on the sphere groups. In other words, the own position evaluation unit 55 gradually evaluates the similarity in stages while increasing the number of sphere groups according to the following sequence (B1) to (B4).

[0134] (B1) Let the number of sphere groups in the kth evaluation be m k , the threshold of score S is sthre k .

[0135] (B2) Perform the above processing (A1) to (A5), retaining the score S as the threshold sthre k The above is your own position (hypothesis).

[0136] (B3) Find the maximum value smax of the score S of the retained own position (hypothesis) k , delete the score S less than ratio×smax k own position (hypothesis).

[0137] (B4) Increase m k , increase sthre k , and returns to the process of (B2). Then, the processes of (B2) to (B4) are repeated a predetermined number of times.

[0138] In addition, in the present embodiment, 0.85 is set as the value of ratio, but it may be set to a different value.

[0139] Then, the own position evaluation unit 55 arranges the last remaining own positions (hypotheses) in the order of the score S, and selects the upper own position (hypothesis) as the own position with high similarity. In addition, the own position selected here can be one or more than two. When more than two own positions are selected, their own positions can be evaluated again in the self-position estimation step ST7 described later.

[0140] In the self-position evaluation step ST6, the self-position evaluation unit 55 evaluates the similarity between the current map and the past map for each group of geometric features based on the sphere group, selects the self-position with high similarity, and then further estimates the self-position based on the self-position in a manner that increases the similarity between the three-dimensional point group data related to the current map and the three-dimensional point group data related to the past map (step ST7: self-position estimation step).

[0141] Specifically, the own position evaluation unit 55 assumes that the own position (hypothesis) selected as the own position with high similarity in the own position evaluation step ST6 is denoted as pose, and the three-dimensional point group data of the current map is denoted as P. c , the three-dimensional point group data of the past map is P r .

[0142] Then, the own position evaluation unit 55 further estimates the own position by using a position matching method such as ICP so that the similarity between the three-dimensional point group data on the current map and the three-dimensional point group data on the past map becomes higher.

[0143] Furthermore, in the present embodiment, the own position evaluation unit 55 evaluates the similarity in stages while increasing the number of sphere groups in the order of (B1) to (B4) described above. However, the similarity may not be evaluated in stages.

[0144] Furthermore, in the present embodiment, in the self-position estimation step ST7, the self-position evaluation unit 55 further estimates the self-position in the order of (A1) to (A5) described above in such a way that the similarity between the three-dimensional point group data related to the current map and the three-dimensional point group data related to the past map becomes higher, but the self-position estimation step ST7 may be omitted. In this case, in the self-position evaluation step ST6, the self-position selected by the self-position evaluation unit 55 may be estimated as the self-position on the past map.

[0145] According to this embodiment, the following operations and effects can be achieved.

[0146] (1) Since the self-position calculation unit 54 uses geometric features that are much smaller in number than the number of three-dimensional points, the number of combinations can be small. When using three-dimensional points to calculate the self-position, three three-dimensional points are selected from the current map and the past map respectively, and their combination is used to calculate the self-position. Since the number of three-dimensional points is large (thousands to tens of thousands), the number of combinations is also large, and it takes a lot of processing time to calculate a group with high similarity. On the other hand, the geometric features of the present application are fewer in number (several to hundreds) than three-dimensional points, and the number of combinations can also be small. Therefore, the self-position estimation device 2 can also reduce processing time when estimating the self-position in an environment where the number of three-dimensional point group data is large.

[0147] (2) Since the own position evaluation unit 55 extracts a sphere group from each of the current map and the past map and evaluates the similarity between the current map and the past map for each group of geometric features based on the sphere group, the similarity between the current map and the past map can be evaluated for each group of geometric features based on a sphere group that is smaller in number than the three-dimensional point group data. Therefore, the own position estimation device 2 can further reduce the processing time.

[0148] (3) Since the self-position evaluation unit 55 increases the number of sphere groups from each of the current map and the past map for self-positions with a similarity higher than a predetermined threshold value and extracts them, and re-evaluates the similarity of the current map and the past map for each group of geometric features based on the sphere groups, it is possible to gradually lock (narrow the range) self-positions with a similarity higher than the predetermined threshold value while increasing the number of sphere groups in a stepwise manner. Therefore, the self-position estimation device 2 can estimate the self-position with higher accuracy.

[0149] (4) Since the self-position evaluation unit 55 evaluates the similarity between the current map and the past map based on the sphere group for each group of geometric features, after selecting the self-position with high similarity, the self-position is further estimated based on the self-position using a position matching method such as ICP in such a way that the similarity between the three-dimensional point group data related to the current map and the three-dimensional point group data related to the past map becomes higher. Therefore, the self-position estimating device 2 can reduce the processing time from the beginning compared with the case of estimating the self-position based on the three-dimensional point group data, and can estimate the self-position with higher accuracy.

[0150] (5) Since the self-position calculation unit 54 determines whether there is consistency between the angle or distance between the current geometric features included in the group of geometric features and the angle or distance between the past geometric features included in the group of geometric features, if it is determined that there is no consistency, the self-position on the past map is not calculated based on the group of geometric features. Therefore, the self-position estimation device 2 can further reduce the processing time.

[0151] (6) Since the self-position calculation unit 54 selects the current geometric features and the past geometric features to form a geometric feature group in such a way that the angles formed by the planes within each geometric feature, the angles formed by the straight lines, and the angle formed by the plane and the straight line are greater than a specified angle, the self-position calculation unit 54 can accurately calculate the self-position on the past map based on the group of geometric features.

[0152] (7) Since the self-position calculation unit 54 determines whether the current geometric features included in the group of geometric features and the past geometric features included in the group of geometric features include a ground plane that is a plane equivalent to the ground, when it is determined that the ground plane is included, the ground plane in the group of geometric features is made to correspond to calculate the self-position on the past map. Therefore, when the self-position estimation device 2 includes multiple planes in the group of geometric features, it is possible to reduce the number of combinations of planes, thereby further reducing the processing time.

[0153] (8) Since the self-position calculation unit 54 determines whether the current geometric features included in the group of geometric features and the past geometric features included in the group of geometric features include a ground plane that is a plane equivalent to the ground when selecting the current geometric features of the plane and the sphere and the past geometric features of the plane and the sphere to form a group of geometric features, and when it is determined that the ground plane is included, only the sphere located at a specified height from the ground plane is selected. Therefore, when the self-position estimation device 2 includes the ground plane in the group of geometric features, the number of spheres to be selected can be reduced, and the processing time can be further reduced.

[0154] [Variations of Embodiments]

[0155] In addition, the present application is not limited to the embodiments, and modifications and improvements within the scope that can achieve the purpose of the present application are included in the present application.

[0156] For example, in this embodiment, the mobile robot 1 as a mobile body is not specifically illustrated, but mobile robots such as service robots and household robots can be exemplified. More specifically, the mobile body can be exemplified as a sweeping robot, a security robot, a transport robot, a guide robot, etc. In addition, the mobile body can also be an autonomous vehicle or a work vehicle, etc.

[0157] In this embodiment, the mobile robot 1 includes the own position estimation device 2, and the own position estimation device 2 includes functions such as the map generation unit 51. In contrast, the mobile body may not include the own position estimation device. For example, the own position estimation device may be provided on another device so as to be communicable with the mobile body.

[0158] In this embodiment, the mobile robot 1 includes the moving unit 3, but it may not include this unit, and may be a cart equipped with a self-position estimating device that is moved manually by the user, for example.

[0159] Industrial Applicability

[0160] As described above, the present application can make good use of the own position estimation device in the mobile body.

[0161] Description of reference numerals:

[0162] 1 Mobile Robot

[0163] 2 Self-position estimation device

[0164] 3 Mobile Unit

[0165] 4 Detection unit

[0166] 5 Presumption Unit

[0167] 51 Map Generation Department

[0168] 52 Storage

[0169] 53 Geometric feature extraction unit

[0170] 54 Self-position calculation unit

[0171] 55 Self-position evaluation department

Claims

1. A self-position estimating device for estimating a self-position of a moving object, characterized in that: The self-position estimating device comprises: A detection unit for detecting the distance from the moving object to surrounding objects as three-dimensional point group data; and an estimating unit for estimating the own position of the mobile object, The presumption unit has: A map generating unit generating a map of the surroundings of the moving object based on the three-dimensional point group data detected by the detecting unit; a storage unit for storing the map generated by the map generating unit; A geometric feature extraction unit that extracts at least one of three geometric features of a plane, a straight line, and a sphere from a current map that is a map of the vicinity of a current position of the moving body as a current geometric feature, and extracts at least one of three geometric features of a plane, a straight line, and a sphere from a past map that is a map of the vicinity of a past position of the moving body as a past geometric feature, based on a map stored in the storage unit; A self-position calculating unit that selects two or three current geometric features from among the current geometric features extracted by the geometric feature extracting unit, selects past geometric features of the same type and number as the selected current geometric features from among the past geometric features extracted by the geometric feature extracting unit to form a geometric feature group, and calculates the self-position on the past map based on the geometric feature group; and The own position evaluation unit evaluates the similarity between the current map and the past map for each group of the geometric features based on the own position calculated by the own position calculation unit, and selects the own position with high similarity; in, The own position evaluation unit extracts a sphere group from each of the current map and the past map, and evaluates the similarity between the current map and the past map for each set of the geometric features based on the sphere group.

2. The self-position estimating device according to claim 1, wherein: The own position evaluation unit extracts the own positions having a similarity higher than a predetermined threshold from each of the current map and the past map by increasing the number of sphere groups, and re-evaluates the similarity between the current map and the past map based on the sphere groups for each group of the geometric features.

3. The self-position estimating device according to claim 1 or 2, wherein: The self-position evaluation unit evaluates the similarity between the current map and the past map for each group of the geometric features based on the sphere group, selects the self-position with high similarity, and then further estimates the self-position based on the self-position in a manner that increases the similarity between the three-dimensional point group data related to the current map and the three-dimensional point group data related to the past map.

4. The self-position estimating device according to any one of claims 1 to 2, wherein: The self-position calculation unit determines whether there is consistency between the angle or distance between the current geometric features included in the group of geometric features and the angle or distance between the past geometric features included in the group of geometric features. If it is determined that there is no consistency, the self-position on the past map is not calculated based on the group of geometric features.

5. The self-position estimating device according to any one of claims 1 to 2, wherein: When selecting the current geometric feature and the past geometric feature to form a group of geometric features, the self-position calculation unit makes the selection in such a way that the angles formed by the planes within each geometric feature, the angles formed by the straight lines, and the angle formed by the plane and the straight line are greater than a specified angle.

6. The self-position estimating device according to any one of claims 1 to 2, wherein: The self-position calculation unit determines whether the current geometric features included in the group of geometric features and the past geometric features included in the group of geometric features include a ground plane that is a plane equivalent to the ground. If it is determined that the ground plane is included, the self-position on the past map is calculated by making the ground plane in the group of geometric features correspond.

7. The self-position estimating device according to any one of claims 1 to 2, wherein: When the current geometric features of the plane and the sphere and the past geometric features of the plane and the sphere are selected to form a group of geometric features, the self-position calculation unit determines whether the current geometric features included in the group of geometric features and the past geometric features included in the group of geometric features include a ground plane that is a plane equivalent to the ground. When it is determined that the ground plane is included, only the sphere located at a specified height from the ground plane is selected.

Images