Self-position estimation device

Abstract

To provide a self-localization device that can identify and remove point cloud data generated by reflection as noise, thereby improving the accuracy of self-localization estimation. [Solution] The self-position estimation device 10 includes a storage means 4 for storing a point cloud map containing three-dimensional point cloud data to which reflection attributes have been assigned; a point cloud detection means 5 for acquiring point cloud data representing the three-dimensional position and shape of an object; a self-position prediction means 6 for predicting the current self-position of the mobile robot 1; a noise reduction means 7 for determining that point cloud data generated by reflection is noise and removing it from the point cloud data acquired by the point cloud detection means 5 based on the point cloud map stored in the storage means 4 and the current self-position of the mobile robot 1 predicted by the self-position prediction means 6; and a self-position estimation means 8 for estimating the self-position of the mobile robot 1 based on the point cloud data from which noise has been removed by the noise reduction means 7.

Description

Technical Field

[0001] The present invention relates to a self-position estimation device for a moving object.

Background Art

[0002] Conventionally, in order for a mobile object capable of autonomous driving to reliably perform autonomous driving to a destination, a technique called self-position estimation has been used. Self-position estimation estimates the current position of a moving object by comparing an environment map prepared in advance with data on the external world obtained by sensors. As the environment map, an occupancy grid map or a point cloud map created based on the presence or absence of objects in the moving range of the moving object may be used (see, for example, Patent Documents 1 and 2). The occupancy grid map divides and stores a plane or space of the moving range into a plurality of sections (cells), and cell values corresponding to the presence or absence of objects are assigned to the divided sections. The point cloud map represents objects existing in the plane or space of the moving range as discrete points (coordinates) for each minute area, and the point cloud data, which is a set thereof, is used as a map.

Prior Art Documents

Patent Documents

[0003]

Patent Document 1

Patent Document 2

Summary of the Invention

Problems to be Solved by the Invention

[0004] Incidentally, external data to be compared with environmental maps can be obtained, for example, using LIDAR (Light Detection and Ranging). LIDAR measures the distance to an object by detecting when the emitted laser beam reflects off the object's surface or the road surface and returns, and acquires point cloud data that shows the object's three-dimensional shape. However, if there are highly reflective objects in the environment, erroneous data may be obtained from LIDAR. Examples of highly reflective objects (reflective objects) include glass, mirrors, glossy resins, and metals. If there are reflective objects such as glass in the environment, the LIDAR's laser beam may mistakenly detect the object reflected off the reflective object as a real object. This leads to a problem where the accuracy of self-localization is reduced due to the point cloud data becoming noisy as a result of the reflection from the reflective object.

[0005] The objective of the present invention is to provide a self-localization device that can determine and remove point cloud data generated by reflection as noise, thereby improving the accuracy of self-localization. [Means for solving the problem]

[0006] The present invention provides a self-position estimation device for estimating the self-position of a moving object, comprising: a storage means for storing a point cloud map including three-dimensional point cloud data to which reflection attributes have been assigned; a point cloud detection means for detecting the distance to objects around the moving object and acquiring point cloud data representing the three-dimensional position and shape of the objects; a self-position prediction means for predicting the amount of movement from the past self-position of the moving object to the current self-position of the moving object and predicting the current self-position of the moving object; a noise reduction means for determining that point cloud data generated by reflection is noise and removing it from the point cloud data acquired by the point cloud detection means based on the point cloud map stored in the storage means and the current self-position of the moving object predicted by the self-position prediction means; and a self-position estimation means for estimating the self-position of the moving object based on the current self-position of the moving object predicted by the self-position prediction means, the point cloud data from which noise has been removed by the noise reduction means, and the point cloud map stored in the storage means.

[0007] With this configuration, the noise reduction means determines that point cloud data generated by reflection is noise and removes it from the point cloud data acquired by the point cloud detection means, based on the point cloud map stored in the storage means and the current self-position of the moving object predicted by the self-position prediction means. Then, the self-position estimation means estimates the self-position of the moving object based on the current self-position of the moving object predicted by the self-position prediction means, the point cloud data from which noise has been removed by the noise reduction means, and the point cloud map stored in the storage means. Thus, the self-position estimation device can improve the accuracy of self-position estimation.

[0008] In the present invention, it is preferable that the noise reduction means determines whether a point is noise caused by reflection based on the dot product of the normal vector of the reflective object surface defined based on the reflection attributes of the point cloud map stored in the storage means and the perpendicular vector drawn from each point of the point cloud data acquired by the point cloud detection means perpendicular to the reflective object surface.

[0009] Here, the dot product of the normal vector of the reflective object surface, defined based on the reflection attributes of the point cloud map stored in the memory means, and the perpendicular vector drawn perpendicularly from each point in the point cloud data acquired by the point cloud detection means to the reflective object surface, will be positive if the directions of the normal vector and the perpendicular vector are the same, and negative if they are different. Therefore, for example, if the direction of the normal vector is set to the inside of the reflective object surface (the side where the moving object is), the dot product will be positive for points relating to an object inside the reflective object surface, and negative for points relating to an object outside the reflective object surface.

[0010] According to the present invention, the noise reduction means determines whether a point is noise caused by reflection based on the dot product of the normal vector of the reflective object surface defined based on the reflection attributes of the point cloud map stored in the storage means and the perpendicular vector drawn perpendicularly from each point of the point cloud data acquired by the point cloud detection means to the reflective object surface. Therefore, points relating to objects outside the reflective object surface can be determined to be noise and removed.

[0011] In the present invention, it is preferable that the noise reduction means determines that a point is noise caused by reflection and removes it when the intersection point of the reflective object surface defined based on the reflection attributes of the point cloud map stored in the storage means and the straight line extended from the center of the sensor mounted on the moving body to each point of the point cloud data acquired by the point cloud detection means intersects within the region of the reflective object surface.

[0012] With this configuration, the noise reduction means determines that a point is noise caused by reflection and removes it when the reflective object surface defined based on the reflection attributes of the point cloud map stored in the storage means and the straight lines extended from the center of the sensor mounted on the moving body to each point of the point cloud data acquired by the point cloud detection means intersect within the region of the reflective object surface. Thus, it is possible to determine and remove points acquired by a beam line incident on the reflective object surface that relate to an object outside the reflective object surface as noise.

[0013] In the present invention, it is preferable that the noise reduction means determines that a point is noise caused by reflection and removes it when the distance between a reflective object surface defined based on the reflection attributes of the point cloud map stored in the storage means and each point of the point cloud data acquired by the point cloud detection means is greater than a predetermined threshold.

[0014] Here, if the distance between the reflective object surface defined based on the reflection attributes of the point cloud map stored in the memory means and each point in the point cloud data acquired by the point cloud detection means is short, then this point may be an object such as a curtain located near the reflective object surface. Such objects, like walls, are useful when estimating the self-position of a moving object. Therefore, if the distance between the reflective object surface and this point is short, it is considered inappropriate to judge this point as noise and remove it.

[0015] According to the present invention, the noise reduction means determines that a point is noise caused by reflection and removes it when the distance between the reflective object surface and each point in the point cloud data is greater than a predetermined threshold. Therefore, it does not remove points related to objects such as curtains that are located near the reflective object surface. Accordingly, the self-localization device can further improve the accuracy of its self-localization. [Brief explanation of the drawing]

[0016] [Figure 1] This figure shows a schematic configuration of a mobile robot equipped with a self-position estimation device according to one embodiment of the present invention. [Figure 2] Plan view of point cloud data acquired by point cloud detection means [Figure 3] Side view of point cloud data acquired by point cloud detection means [Figure 4] This diagram shows the relationship between the normal vector of the glass surface and the perpendicular vector drawn from each point in the point cloud data perpendicular to the glass surface. [Figure 5] This diagram shows the relationship between the glass surface and the straight lines drawn from the center of the sensor mounted on the mobile robot to each point in the point cloud data acquired by the point cloud detection means. [Modes for carrying out the invention]

[0017] One embodiment of the present invention will be described below with reference to the drawings. Figure 1 shows a schematic configuration of a mobile robot equipped with a self-position estimation device according to one embodiment of the present invention. As shown in Figure 1, the mobile robot 1 (mobile body) comprises a control means 2, a movement means 3, a storage means 4, a point cloud detection means 5, a self-position prediction means 6, a noise reduction means 7, and a self-position estimation means 8. The control means 2, storage means 4, point cloud detection means 5, self-position prediction means 6, noise reduction means 7, and self-position estimation means 8 constitute a self-position estimation device 10 that estimates the self-position of the mobile robot 1.

[0018] The control means 2 includes computing means such as a CPU (Central Processing Unit) and controls the operation of the mobile robot 1. It executes information processing according to a predetermined program stored in a storage means 4 such as a ROM (Read Only Memory) or a RAM (Random Access Memory). This control means 2 includes a movement control unit 21 that controls the movement means 3 and an estimation control unit 22 that controls the self-position estimation device 10.

[0019] The movement means 3 has a drive unit 31 such as a motor and wheels 32 that are rotationally driven by the drive unit 31. By being controlled by the movement control unit 21, the mobile robot 1 is autonomously driven. The storage means 4 stores various data for controlling the operation of the mobile robot 1. This storage means 4 stores a point cloud map including three-dimensional point cloud data with a reflection attribute indicating that it is data based on an object with a high reflectivity (reflective object).

[0020] Here, the point cloud map stores point cloud data representing the three-dimensional positions and shapes of objects in the environment where the mobile robot 1 moves as map information. This point cloud map may be created and stored in advance based on CAD (Computer Aided Design) data of the target environment, or may be created and stored in advance by SLAM (Simultaneous Localization and Mapping). In addition, in order to distinguish it from the point cloud data acquired by the point cloud detection means 5 described later, the point cloud data of the point cloud map is referred to as three-dimensional point cloud data. Also, when assigning a reflection attribute to the three-dimensional point cloud data, if it is a point cloud map based on CAD data, the reflection attribute determined by the CAD design information may be assigned to the corresponding point cloud. If it is a point cloud map created by SLAM, an operator may use a GUI (Graphical User Interface) tool or the like to assign the reflection attribute to the corresponding point cloud.

[0021] Figure 2 is a plan view of the point cloud data acquired by the point cloud detection means. Figure 3 is a side view of the point cloud data acquired by the point cloud detection means. As shown in Figure 1, the point cloud detection means 5 has a LIDAR 51, which, as shown in Figures 2 and 3, is controlled by the estimation control unit 22 to detect the presence or absence of objects by irradiating a beam B around the mobile robot 1.

[0022] The self-position prediction means 6, controlled by the estimation control unit 22, predicts the amount of movement from the past self-position of the mobile robot 1 to the current self-position of the mobile robot 1, and predicts the current self-position of the mobile robot 1. Here, the amount of movement from the past self-position of the mobile robot 1 to the current self-position of the mobile robot 1 can be predicted using various techniques, such as wheel odometry, which determines the amount of movement from the rotation speed of the wheels 32, or LIDAR odometry, which determines the amount of movement from LIDAR data, without using the point cloud map stored in the storage means 4.

[0023] The LIDAR 51 used in the point cloud detection means 5 detects the distance to an object by irradiating the surroundings with a beam B, such as an infrared laser. This LIDAR 51 is mounted on a mobile robot 1, and the relative position of the robot center, which is the reference for the mobile robot 1's own position, and the sensor center of the LIDAR 51 are known. As shown in Figures 2 and 3, the LIDAR 51 uses its sensor center and orientation as a reference position, and based on the distance from the sensor center, the angle (direction) around the sensor center, and the height (elevation or depression) from the sensor center, it detects the distance to an object around the mobile robot 1 for each irradiated beam B and acquires point cloud data P that represents the three-dimensional position and shape of the object.

[0024] As shown in Figure 1, the noise reduction means 7, controlled by the estimation control unit 22, determines that point cloud data P generated by reflection from reflective objects is noise and removes it from the point cloud data P acquired by the point cloud detection means 5, based on the point cloud map stored in the storage means 4 and the current self-position of the mobile robot 1 predicted by the self-position prediction means 6. Specifically, the noise reduction means 7 converts the point cloud data P acquired by the point cloud detection means 5 into the coordinate system of the point cloud map stored in the storage means 4, based on the current self-position of the mobile robot 1 predicted by the self-position prediction means 6. Then, the noise reduction means 7 determines that point cloud data P generated by reflection is noise and removes it from the point cloud map stored in the storage means 4 and the point cloud data P converted into the coordinate system of the point cloud map. In the following explanation, for the sake of clarity, we will use glass as a representative example of a reflective object, but the same principles apply to other reflective objects such as mirrors, glossy resins, and metals.

[0025] This noise reduction means 7 includes an inner product determination unit 71, an intersection determination unit 72, and a threshold determination unit 73. The dot product determination unit 71 determines whether a point is noise caused by reflection based on the dot product of the normal vector of the glass surface as a reflective object surface defined based on the reflection attributes of the point cloud map stored in the storage means 4, and the perpendicular vector drawn perpendicularly from each point of the point cloud data P acquired by the point cloud detection means 5 to the glass surface.

[0026] Figure 4 shows the relationship between the normal vector of the glass surface and the perpendicular vector drawn from each point in the point cloud data perpendicular to the glass surface. As shown in Figure 4, the dot product determination unit 71 defines the glass surface GS as the equation of a plane based on the reflection attributes of the point cloud map stored in the storage means 4. Here, the dot product of the normal vector n of the glass surface GS and the perpendicular vectors v1 and v2 drawn perpendicularly from each point p1 and p2 in the point cloud data P acquired by the point cloud detection means 5 to the glass surface GS will be positive if the directions of the normal vector n and the perpendicular vectors v1 and v2 are the same, and negative if they are different. In the example in Figure 4, the direction of the normal vector is set to the inside of the glass surface GS (the side where the mobile robot 1 is located), so if point p2 relates to an object inside the glass surface GS, the dot product will be positive, and if point p1 relates to an object outside the glass surface GS, the dot product will be negative. Therefore, in the example in Figure 4, the dot product determination unit 71 determines that point p1, where the dot product is negative, is noise caused by reflection (point p1 is the reflection point of point rp1) and removes it, while determining that point p2, where the dot product is positive, is not noise caused by reflection and retains it.

[0027] The intersection determination unit 72 determines that a point is noise caused by reflection and removes it if the glass surface defined based on the reflection attributes of the point cloud map stored in the storage means 4 and the straight lines extended from the sensor center of the LIDAR 51 mounted on the mobile robot 1 to each point of the point cloud data P acquired by the point cloud detection means 5 intersect within the area of ​​the glass surface.

[0028] Figure 5 shows the relationship between the glass surface and the straight lines drawn from the center of the sensor mounted on the mobile robot to each point of the point cloud data acquired by the point cloud detection means. As shown in Figures 5(A) and (B), the intersection determination unit 72 defines the glass surface GS as a plane equation based on the reflection attributes of the point cloud map stored in the storage means 4.

[0029] Here, Figures 5(A) and (B) show point p1 relating to an object that is outside the glass surface GS but inside the wall surface WS. In the example shown in Figure 5(A), the intersection determination unit 72 determines that the glass surface GS and the straight lines (beam lines B) extended from the sensor center of the LIDAR 51 mounted on the mobile robot 1 to each point of the point cloud data P acquired by the point cloud detection means 5 intersect at point C, and therefore determines that point p1 is noise caused by reflection (point p1 is the reflection point of point rp1) and removes it. In contrast, in the example of Figure 5(B), the intersection determination unit 72 determines that point p1 is not noise caused by reflection and maintains it because the glass surface GS and the beam lines B extended from the sensor center of the LIDAR 51 mounted on the mobile robot 1 to each point of the point cloud data P acquired by the point cloud detection means 5 do not intersect.

[0030] Furthermore, the aforementioned dot product determination unit 71 will determine that point p1 is noise caused by reflection and remove it in both examples shown in Figures 5(A) and (B). However, in the example shown in Figure 5(B), point p1 is point cloud data P relating to an object on the side where the mobile robot 1 is located, and therefore is not point cloud data P that should be determined to be noise caused by reflection and removed. In other words, the dot product determination unit 71 cannot correctly determine whether the noise is caused by reflection in a situation like the example in Figure 5(B). This is because the equation of the plane defining the glass surface GS alone cannot specify the region (range) of the glass surface GS. Even if the intersection point is found from the plane equation of the glass surface GS and the linear equation of the beam line B, this alone does not allow for the determination of whether that intersection point lies within the region of the glass surface GS. In contrast, the intersection determination unit 72 determines whether the glass surface GS and the beam lines B extended from the sensor center of the LIDAR 51 mounted on the mobile robot 1 to each point of the point cloud data P acquired by the point cloud detection means 5 intersect within the area of ​​the glass surface GS. Therefore, it can correctly determine whether or not the noise is caused by reflection, provided that the point cloud data P is based on the beam lines B irradiated onto the glass surface GS.

[0031] To determine whether the intersection point between the glass surface GS and the beam line B lies within the region of the glass surface GS, one can first find the intersection point of the plane equation of the glass surface GS and the linear equation of the beam line B, and then check if the three-dimensional point cloud data corresponding to the glass surface GS exists in the vicinity of this intersection point. If the three-dimensional point cloud data corresponding to the glass surface GS exists in the vicinity of this intersection point (for example, within a few centimeters), then this intersection point can be considered to be within the region of the glass surface GS. A data structure suitable for spatial data retrieval, such as a kd tree, can be used to search for neighboring points. Alternatively, as another method of intersection determination, the glass surface GS can be represented as a set of triangular patches, and if any of these triangular patches have an intersection point with the beam line B within their interior, then it can be determined that the glass surface GS and the beam line B have an intersection point. Since the region of the triangular patch is defined, it is possible to analytically determine whether an intersection point exists within it.

[0032] The threshold determination unit 73 determines that a point is noise caused by reflection and removes it if the distance between the glass surface, defined based on the reflection attributes of the point cloud map stored in the storage means 4, and each point P of the point cloud data acquired by the point cloud detection means 5 is greater than a predetermined threshold.

[0033] In this embodiment, all determination units, including the dot product determination unit 71, the intersection determination unit 72, and the threshold determination unit 73, remove only the point cloud data P that is determined to be noise caused by reflection. However, both the dot product determination unit 71 and the intersection determination unit 72 may remove only the point cloud data P that is determined to be noise caused by reflection, or both the dot product determination unit 71 and the threshold determination unit 73 may remove only the point cloud data P that is determined to be noise caused by reflection.

[0034] As shown in Figure 1, the self-position estimation means 8 is controlled by the estimation control unit 22 and estimates the self-position of the mobile robot 1 based on the current self-position of the mobile robot 1 predicted by the self-position prediction means 6, the point cloud data P from which noise has been removed by the noise removal means 7, and the point cloud map stored in the storage means 4, using techniques such as scan matching, Kalman filtering, or particle filtering.

[0035] According to this embodiment, the following actions and effects can be achieved. (1) The noise reduction means 7 determines that point cloud data P generated by reflection is noise from the point cloud data acquired by the point cloud detection means 5, based on the point cloud map stored in the storage means 4 and the current self-position of the mobile robot 1 predicted by the self-position prediction means 6. Then, the self-position estimation means 8 estimates the self-position of the mobile robot 1 based on the current self-position of the mobile robot 1 predicted by the self-position prediction means 6, the point cloud data P from which noise has been removed by the noise reduction means 7, and the point cloud map stored in the storage means 4. Thus, the self-position estimation device 10 can improve the accuracy of self-position estimation.

[0036] (2) The noise reduction means 7 determines whether a point is noise caused by reflection based on the dot product of the normal vector n of the glass surface GS, which is defined based on the reflection attributes of the point cloud map stored in the storage means 4, and the perpendicular vectors v1 and v2 drawn perpendicularly from each point of the point cloud data P acquired by the point cloud detection means 5 to the glass surface GS. Therefore, it is possible to determine that a point relating to an object outside the glass surface GS is noise and remove it.

[0037] (3) The noise reduction means 7 determines that a point is noise caused by reflection and removes it when the glass surface GS defined based on the reflection attributes of the point cloud map stored in the storage means 4 and the beam lines B extended from the sensor center of the LIDAR 51 mounted on the mobile robot 1 to each point of the point cloud data P acquired by the point cloud detection means 5 intersect within the area of ​​the glass surface GS. Thus, it is possible to determine that a point acquired by the beam line B incident on the glass surface GS, but relating to an object outside the glass surface GS, is noise and remove it.

[0038] (4) The noise reduction means 7 determines that a point is noise caused by reflection and removes it when the distance between the glass surface GS and each point in the point cloud data P is greater than a predetermined threshold, so it does not remove points related to objects such as curtains that are in the vicinity of the glass surface GS. Therefore, the self-position estimation device 10 can further improve the accuracy of self-position estimation.

[0039] [Variations of the Embodiment] Furthermore, the present invention is not limited to the embodiments described above, and any modifications, improvements, etc., that can achieve the objectives of the present invention are included within the scope of the present invention. For example, although no specific example was given for the mobile robot 1 in the above embodiment, the mobile robot 1 could be a service robot or a home robot, and more specifically, a cleaning robot, security robot, transport robot, and guidance robot. Furthermore, the self-position estimation device 10 of the present invention is not limited to use in the mobile robot 1, but can be used in a variety of mobile objects such as vehicles such as autonomous vehicles and work vehicles, and flying objects such as legged robots and drones. Furthermore, in addition to the LIDAR 51, other devices such as a monocular camera, stereo camera, or depth image camera can also be used as the point cloud detection means 5. When using a monocular camera or stereo camera, the same processing as with the LIDAR 51 should be performed on the point cloud data reconstructed from the image based on parallax. With a depth image camera, point cloud data can be obtained directly, so the same processing as with the LIDAR 51 should be performed.

[0040] Furthermore, although the above embodiment described the case where a point cloud map is used, the case where an occupied grid map is used can also be processed in the same way as the point cloud map case by treating each cell occupied by the object as a point. In particular, since the occupied grid map has the function of searching for cells in the vicinity of any point, it is possible to efficiently determine whether the intersection point of the glass surface GS and the beam line B is within the region of the glass surface GS by checking whether there are any cells of the glass surface GS in the vicinity of this intersection point. [Industrial applicability]

[0041] As described above, the present invention can be suitably used in a self-position estimation device for a moving object. [Explanation of Symbols]

[0042] 1. Mobile robot (mobile body) 2. Control means 3. Means of transportation 4 Memory means 5. Point cloud detection means 6 Self-position prediction means 7. Noise reduction means 8 Self-position estimation means 10 Self-position estimation device 21 Movement Control Unit 22 Estimation Control Unit 31 Drive unit 32 wheels 51 LIDAR 71 Inner product determination unit 72 Intersection determination section 73 Threshold determination unit

Claims

1. A self-position estimation device for estimating the self-position of a moving object, A storage means for storing a point cloud map containing three-dimensional point cloud data to which reflection attributes have been assigned, A point cloud detection means detects the distance to objects around the moving body and acquires point cloud data representing the three-dimensional position and shape of the objects. A self-position prediction means predicts the amount of movement from the past self-position of the moving object to the current self-position of the moving object, and predicts the current self-position of the moving object. Based on the point cloud map stored in the storage means and the current self-position of the moving object predicted by the self-position prediction means, a noise reduction means determines that point cloud data generated by reflection is noise from the point cloud data acquired by the point cloud detection means and removes it; A self-position estimation device comprising a self-position estimation means for estimating the self-position of a moving object based on the current self-position of the moving object predicted by the self-position prediction means, point cloud data from which noise has been removed by the noise removal means, and a point cloud map stored in the storage means.

2. In the self-position estimation device described in claim 1, The noise reduction means is A self-position estimation device characterized by determining whether a point is noise caused by reflection based on the dot product of the normal vector of the reflective object surface defined based on the reflection attributes of the point cloud map stored in the storage means and the perpendicular vector drawn perpendicularly from each point of the point cloud data acquired by the point cloud detection means to the reflective object surface.

3. In the self-position estimation device described in claim 2, The noise reduction means is A self-position estimation device characterized in that, when a reflective object surface defined based on the reflection attributes of a point cloud map stored in the storage means and a straight line extended from the center of a sensor mounted on the moving body to each point of the point cloud data acquired by the point cloud detection means intersects within the region of the reflective object surface, the device determines that the point is noise caused by reflection and removes it.

4. In the self-position estimation device described in claim 2 or claim 3, The noise reduction means is A self-position estimation device characterized in that, when the distance between a reflective object surface defined based on the reflection attributes of a point cloud map stored in the storage means and each point of the point cloud data acquired by the point cloud detection means is greater than a predetermined threshold, the device determines that a point is noise caused by reflection and removes it.

Images