JP2017146893A - Self-position estimation method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a self-position estimation method with which it is possible to suppress a reduction in the accuracy of self-position estimation when an environment at self-position estimation time has changed from an environment at map creation time.SOLUTION: The self-position estimation method estimates a self-position by matching of point groups obtained from a point group shown on environment map and a distance sensor. In this self-position estimation method, a degree of suitableness with respect to a point group obtained from the distance sensor that is based on a predetermined index is calculated for each measurement direction of the distance sensor, and a point group acquired from the distance sensor in a measurement direction in which is acquired a point group whose suitableness satisfies a predetermined criterion is used for matching with a point group shown on the environment map.SELECTED DRAWING: Figure 4

Description

  The present invention relates to a self-position estimation method, and more particularly, to a self-position estimation method for estimating a self-position by performing point cloud matching.

  An autonomous mobile robot that moves autonomously using a map is known. Such an autonomous mobile robot estimates its own position by matching information obtained from the distance sensor with information shown on the map. For example, Patent Document 1 searches for distance sum data approximated in map data for distance sum data obtained from the measurement results of a distance sensor, and based on the result, the position / posture of the autonomous mobile robot It is disclosed that a matching process for estimating is performed.

JP2013-020345A

  By the way, for example, when the object shown on the map moves at the time of self-position estimation, or the person who was not shown on the map appears at the time of self-position estimation, information on the environment shown on the map It can happen that the actual environment at the time of self-position estimation is different. This leads to a decrease in self-position estimation accuracy.

  The present invention has been made to solve such a problem, and suppresses a decrease in self-position estimation accuracy when the environment at the time of self-position estimation varies from the environment at the time of map creation. An object of the present invention is to provide a self-position estimation method capable of

A self-position estimation method according to an aspect of the present invention is a self-position estimation method for performing self-position estimation by matching a point group shown on an environment map and a point group obtained from a distance sensor, For each point direction obtained from the distance sensor for each measurement direction, a degree of suitability based on a predetermined index is calculated, and the point group is obtained such that the degree of suitability satisfies a predetermined standard. This is a method of using a point cloud acquired from the distance sensor in a direction for matching with a point cloud shown on the environmental map.
According to this method, when the environment has changed since the creation of the environment map, it is possible to perform self-position estimation by selecting data that is less affected by the change. For this reason, the fall of the precision of self-position estimation is suppressed.

  ADVANTAGE OF THE INVENTION According to this invention, when the environment at the time of self-position estimation has fluctuate | varied from the environment at the time of map preparation, the self-position estimation method which can suppress that self-position estimation precision falls can be provided.

It is a schematic diagram which shows the autonomous mobile robot concerning embodiment. It is a block diagram which shows an example of a function structure of the control apparatus concerning embodiment. It is a schematic diagram shown about each variable used for calculation of a suitable degree. It is a flowchart which shows an example of operation | movement of the autonomous mobile robot by the self-position estimation method concerning embodiment. It is a flowchart which shows a specific example of operation | movement of step 100 of the flowchart shown by FIG. 5 is a flowchart showing another specific example of the operation of Step 100 of the flowchart shown in FIG. 4. It is a schematic diagram explaining the self-position estimation method concerning a comparative example.

  Before describing the embodiment of the present invention, first, a comparative example examined by the inventor based on the self-position estimation method described in Patent Document 1 will be described.

  In this comparative example, when creating an environmental map, first, the point cloud acquired by the distance sensor, the distance sum that is the sum of the distances of the point cloud, and the self-position of the autonomous mobile body are linked. And save together. Then, when self-position estimation is performed, as shown in FIG. 7, the closest distance point group is extracted from the environment map with respect to the distance sum calculated from the point group acquired from the distance sensor. In the example shown in FIG. 7, the distance sum calculated from the distance sensor data is 10, and a distance point group having 11 which is the distance sum closest to the distance sum is extracted from the environment map. Subsequently, using the self-position corresponding to the extracted distance point group as an initial value, the point groups are matched to perform self-position estimation.

  The inventor has found that such a self-position estimation method has the following problems. For example, if the location of an object has changed compared to when the environment map was created, or if an object (such as a person) that did not exist when the environment map was created exists at the time of self-estimation, In the case where there is a change in the environment measured at the time of self-position estimation compared to the time when the position is created, the following problem occurs when the changed part occupies most of the distance sensor data. That is, if an initial value is obtained only by the sum of distances, it may match an incorrect point cloud and self-position estimation may fail. As a countermeasure to such a problem, a method of continuing the process until the self-position estimation is successful while changing the direction of the distance sensor is conceivable, but the accuracy of the self-position of the estimation result may be poor. In addition, since the true value of the position of the autonomous mobile body is not known and the part where the environment has changed is unknown, it is impossible to evaluate the accuracy of the self-position estimation result. This is because even if distance sensor data having a large proportion of the environment change portion is input, a self-position estimation result based on the input must be adopted.

  Therefore, if a standard that can evaluate whether the self-position estimation is good or bad is introduced when self-position estimation is performed, and the evaluation can be performed based on the standard, the possibility of the above problem occurring is low. Is expected to be.

Embodiment 1
Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. FIG. 1 shows an autonomous mobile robot 1 as an example of a self-position estimation apparatus according to an embodiment. The autonomous mobile robot 1 includes a distance sensor 11, a sensor movement control unit 12, a control device 13, and wheels 14, and moves by driving the wheels 14. In the present embodiment, the autonomous mobile robot 1 moves by driving the wheels 14, but the movement may be realized by other means.

  The distance sensor 11 detects distance information between the environment (object) in the three-dimensional detection region S around the autonomous mobile robot 1 and the autonomous mobile robot 1. For example, a laser range finder is used as the distance sensor 11, but the distance sensor 11 is not limited thereto, and may be an arbitrary distance sensor such as a depth sensor, an ultrasonic sensor, an infrared sensor, or a camera.

  The sensor movement control unit 12 changes the direction of the distance sensor 11 so as to change the viewpoint pattern of the distance sensor 11, that is, the measurement direction. The sensor movement control unit 12 moves the direction of the distance sensor 11 by driving an actuator based on the control signal, for example.

  The control device 13 includes, for example, a CPU (Central Processing Unit) that performs control processing, arithmetic processing, and the like, a ROM (Read Only Memory) and a RAM (Random) that store control programs, arithmetic programs, processing data, and the like executed by the CPU. Hardware is configured around a microcomputer composed of a storage device or the like composed of an Access Memory).

  The control device 13 controls the entire autonomous mobile robot 1. For example, the control device 13 performs various types of control such as control of the sensor movement control unit 12 and drive control of the wheels 14, and executes the following processing with the configuration shown in FIG.

  FIG. 2 is a block diagram illustrating an example of a functional configuration of the control device 13. 2 is realized by the CPU executing the control program or the arithmetic program, for example.

  The control device 13 includes an environment map storage unit 130, a viewpoint selection unit 131, a distance sensor data acquisition unit 132, and a self-position estimation unit 133. In the present embodiment, an index indicating whether or not the distance sensor data is suitable for self-position estimation (hereinafter referred to as “preferability”) is introduced, and each time the autonomous mobile robot 1 acquires the distance sensor data, this distance is calculated. A suitable degree corresponding to the sensor data is calculated. While the autonomous mobile robot 1 moves the distance sensor 11 under the control of the sensor movement control unit 12, the autonomous mobile robot 1 includes various attitudes including the attitude of the distance sensor 11, distance sensor data acquired from the distance sensor 11, and suitability for the distance sensor data. The combinations are collected, and the posture of the distance sensor 11 with the optimum degree of preference is selected. And the distance sensor data acquired from the attitude | position of the selected distance sensor 11 and the environmental map are collated, and a self-position is calculated. Hereinafter, each structure which implement | achieves this is demonstrated.

  The environment map storage unit 130 is realized by, for example, the above-described storage device, and stores an environment map that is map information about an environment in which the autonomous mobile robot 1 operates. This environment map is created in advance based on the detection result of the distance sensor 11 for the range in which the autonomous mobile robot 1 moves, and is stored in the environment map storage unit 130. This environment map includes point cloud information (point cloud coordinates and distances between points) acquired by the distance sensor 11 when the map is created.

  The viewpoint selection unit 131 moves the distance sensor 11 by instructing the sensor movement control unit 12, and a point group acquired from the distance sensor 11 is determined in advance for each viewpoint pattern (measurement direction) of the distance sensor 11. A suitable degree based on the index is calculated. Then, the viewpoint selection unit 131 selects a viewpoint from which a point group that satisfies a predetermined criterion for the degree of suitability is acquired. In addition, the viewpoint selection unit 131 directs the distance sensor 11 toward the selected viewpoint.

  The distance sensor data acquisition unit 132 acquires the distance sensor data acquired from the distance sensor 11 at the viewpoint selected by the viewpoint selection unit 131.

  The self-position estimating unit 133 compares the distance sensor data acquired by the distance sensor data acquiring unit 132 with the environment map, and estimates the self-position.

  Hereinafter, the suitability will be specifically described. In the present embodiment, the suitability for self-position estimation is defined only from the point cloud information obtained from the distance sensor 11. Assuming that the degree of preference is C and the matrix representing the point group distribution of n points obtained from the distance sensor 11 is P, the degree of preference is uniquely calculated using the function f as shown in the following equation (1). The matrix P is expressed as the following formula (2).

  Hereinafter, a specific example of the degree of preference C will be described. The problem with the conventional method occurs when the ratio of changed objects to the environment map is large. That is, by selecting data used for self-position estimation so that the ratio of changed objects becomes small, it is possible to suppress a decrease in self-position estimation accuracy. Therefore, it is only necessary to increase the value of the degree of suitability C in the case of such data. In order to reduce the ratio of the changing object, for example, it is considered preferable that the distance sensor 11 sees a point group having a wide distribution as far as possible from as far away as possible. This is because, by doing this, even if some objects move in the distribution, the ratio of the moved points in the entire distribution becomes small. The vertical direction is, for example, the vertical direction, and the horizontal direction is, for example, the horizontal direction.

When this idea is expressed in an equation, the degree of suitability C is expressed as the following equation (3), for example. In the following formula (3), as shown in FIG. 3, G is the center of gravity formed by the point group position of the distance sensor data in the coordinate system centered on the distance sensor 11, and _LG is the distance sensor 11. And V _X , V _Y , and V _Z are variances representing the degree of variation in the distribution of point groups centered on the center of gravity G. Here, V _X represents the vertical dispersion, V _Y represents the horizontal dispersion, and V _Z represents the depth dispersion. M and n are adjustment gains, respectively.

In the example shown in Expression (3), the degree of suitability C is an index that represents the distance of the point cloud distribution from the distance sensor 11 (hereinafter referred to as a distance index) and an index that represents the size of the vertical and horizontal spread of the point cloud distribution. (Hereinafter referred to as the dispersion index). The first term of the formula of the degree of preference C shown in the formula (3) indicates a component representing that the point cloud distribution is more suitable as it is farther from the distance sensor 11. The second term indicates a component representing that the distribution is more suitable as the degree of spread in the vertical and horizontal directions is larger. However, with respect to a distribution in which the distance from the distance sensor 11 is different but the spread in the vertical and horizontal directions is the same, the distribution needs to be relatively smaller as the distribution is farther from the distance sensor 11. Therefore, in the second term, the product of longitudinal dispersion and lateral dispersion, divided by the distance L _G to the center of gravity, so that to cancel the influence of the distance. It should be noted that by adjusting the values of the adjustment gains m and n, it is possible to adjust the contribution degree of the distance index and the contribution degree of the dispersion index in the appropriateness C.

  Next, an example of the operation by the self-position estimation method according to this embodiment will be described. FIG. 4 is a flowchart showing an example of the operation of the autonomous mobile robot 1 by the self-position estimation method according to the present embodiment. It is assumed that the environmental map is created in advance.

  In the example shown in FIG. 4, an operation for selecting an optimal viewpoint is performed in step 100 (S100) and step 101 (S101), and then, based on the data acquired in step 102 (S102), step 103 is performed. An operation of performing self-position estimation processing is performed by (S103) and step 104 (S104). Details will be described below.

  In step 100, the viewpoint selecting unit 131 selects a distance sensor viewpoint that is most suitable for self-position estimation. The viewpoint selection unit 131 moves the distance sensor 11, changes the viewpoint pattern, calculates the appropriateness for each viewpoint pattern, and selects the viewpoint with the highest appropriateness. The details of step 100 will be further described with reference to FIG.

  Next, in step 101, the sensor movement control unit 12 changes the position of the distance sensor 11 so that the distance sensor 11 is directed to the viewpoint selected in step 100.

  In step 102, the distance sensor data acquisition unit 132 acquires distance sensor data based on the viewpoint set in step 101, and outputs it to the self-position estimation unit 133.

  In step 103, the self-position estimation unit 133 matches the point group shown in the environmental map stored in the environmental map storage unit 130 with the point group shown in the distance sensor data acquired in step 102, and the distance sensor 3D conversion is performed to perform conversion between the position in the coordinate system fixed to 11 and the position in the coordinate system (world coordinate system) fixed in the movement space.

  In step 104, the self-position estimation unit 133 estimates the self-position on the environment map by three-dimensional conversion.

  As described above, according to the autonomous mobile robot 1 (self-position estimation apparatus) according to the present embodiment, the point group indicated by the environment map and the point group obtained from the distance sensor 11 are matched to perform self-position estimation. A self-position estimation method is provided that includes the following steps. That is, for each measurement direction of the distance sensor 11, for a point group acquired from the distance sensor 11, a step (step 100) for calculating the degree of preference based on a predetermined index and a criterion for which the degree of preference is predetermined There is provided a self-position estimation method including a step (step 103) of using a point cloud acquired from the distance sensor 11 for matching with a point cloud shown on the environment map in a measurement direction in which a satisfying point cloud is acquired. .

  Next, details of the operation in step 100 will be described. FIG. 5 is a flowchart showing a specific example of the operation in step 100 of the flowchart shown in FIG. In the example illustrated in FIG. 5, the viewpoint selection unit 131 determines a viewpoint pattern by searching for a viewpoint pattern with the highest degree of suitability C among all viewpoint patterns. Specifically, the following operations are performed.

  In step 111 (S111), the viewpoint selection unit 131 acquires distance sensor data (point cloud sensor data) from the distance sensor 11.

  In step 112 (S112), the viewpoint selection unit 131 calculates a suitableness value for the point group acquired in step 111, and temporarily stores the calculation result in a memory or the like.

  In step 113 (S113), the viewpoint selection unit 131 determines whether measurement has been completed for all viewpoint patterns. That is, when the appropriateness value is calculated for all viewpoint patterns, the process proceeds to step 115, and when the appropriateness value is not calculated for all viewpoint patterns, the process proceeds to step 114.

  In step 114 (S114), the viewpoint selecting unit 131 instructs the sensor movement control unit 12 to change to a viewpoint pattern for which a value of the suitability is not calculated. Thereby, the viewpoint of the distance sensor 11 is moved, and the process returns to Step 111 again.

  In step 115 (S115), the viewpoint selection unit 131 obtains the maximum value among the values of suitability for all viewpoint patterns. Specifically, the viewpoint selection unit 131 compares the values accumulated in step 112 to obtain the maximum value.

  In step 116 (S116), the viewpoint selection unit 131 outputs the sensor viewpoint that maximizes the value of suitability. Thereby, the viewpoint selection unit 131 completes the selection of the viewpoint.

  The first embodiment has been described above. According to the self-position estimation method according to the first embodiment, distance sensor data having the highest degree of suitability for self-position estimation is selected. For this reason, it is possible to perform self-position estimation by selecting data in which the ratio of the changed object is likely to be smaller than others among the acquired distance sensor data. That is, when the environment has changed since the creation of the environment map, it is possible to perform self-position estimation by selecting data that is less affected by the change. As a result, when the environment at the time of self-position estimation changes from the environment at the time of map creation, it can suppress that the success rate or precision of self-position estimation falls. It is also possible to calculate an index for judging the appropriateness of data using the environmental map and distance sensor data created in advance, but in such a method, when the environment measured by the distance sensor changes Also, matching with the environmental map does not work. On the other hand, according to the self-position estimation method according to the above embodiment, since the degree of suitability is calculated only from the distance sensor data, data obtained from a viewpoint that seems to have a small environmental change among a plurality of viewpoints. It can be selected and matched well with the environmental map.

  In addition, by using the favorableness degree for the calculation of the likelihood of self-position estimation, the higher processing system can know an index of how probable the estimated result is, and it can be used for subsequent processing. .

Embodiment 2
Next, a second embodiment will be described. In the first embodiment, as illustrated in FIG. 5, the viewpoint selection unit 131 selects a viewpoint pattern that maximizes the degree of suitability. On the other hand, in the present embodiment, the viewpoint selection unit 131 selects a viewpoint pattern whose suitability value exceeds a predetermined threshold value. The present embodiment is different in this point, and the other parts are the same as those in the first embodiment. Therefore, only different parts will be described, and description of overlapping points will be omitted.

  FIG. 6 is a flowchart showing another specific example of the operation in step 100 of the flowchart shown in FIG. In the example illustrated in FIG. 6, the viewpoint selection unit 131 determines a viewpoint pattern by searching for a viewpoint pattern whose suitability value exceeds a predetermined threshold. Specifically, the following operations are performed.

  In step 121 (S 121), the viewpoint selection unit 131 acquires distance sensor data (point cloud sensor data) from the distance sensor 11 as in step 111 of FIG.

  In step 122 (S122), the viewpoint selection unit 131 calculates a value of the suitability for the point group acquired in step 121. In step 123 (S123), the viewpoint selection unit 131 determines whether the calculated value exceeds a predetermined threshold. If the preference value exceeds the threshold value, the process proceeds to step 127. On the other hand, if the preference value does not exceed the threshold value, the process proceeds to step 124.

  In step 124 (S124), similarly to step 113 in FIG. 5, the viewpoint selection unit 131 determines whether or not measurement has been completed for all viewpoint patterns. If the preference value has been calculated for all viewpoint patterns, the process proceeds to step 125, and if the preference value has not been calculated for all viewpoint patterns, the process proceeds to step 126.

  In step 125 (S125), the viewpoint selection unit 131 notifies the failure of the process and ends the process, assuming that the value of the suitability is less than the threshold value in all viewpoint patterns. On the other hand, in step 126 (S126), as in step 114 of FIG. 5, the viewpoint selection unit 131 instructs the sensor movement control unit 12 to change to a viewpoint pattern for which the value of the suitability is not calculated. . As a result, the viewpoint of the distance sensor 11 is moved, and the process returns to step 121 again.

  In step 127 (S127), the viewpoint determined in step 123 that the value of the suitability exceeds the threshold value is output. Thereby, the viewpoint selection unit 131 completes the selection of the viewpoint.

  According to the self-position estimation method according to the present embodiment, distance sensor data whose degree of suitability for self-position estimation satisfies a predetermined criterion is selected. For this reason, it is possible to perform self-position estimation by selecting data in which the ratio of the changed object is equal to or less than the reference from the acquired distance sensor data. As a result, when the environment at the time of self-position estimation changes from the environment at the time of map creation, it can suppress that the success rate or precision of self-position estimation falls.

  Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention. For example, in the above-described embodiment, the degree of preference C is set as an example in which the ratio of the object that has changed with respect to the environmental map is reduced. However, the degree of preference is set according to various other purposes, It can be applied to self-position estimation as in the embodiment. For example, when the distance sensor 11 has a characteristic that the measurement accuracy of the distance in a specific range is better than the measurement accuracy of the distance in another range, the center of gravity of the point group acquired by the distance sensor 11 and the distance sensor 11 It is conceivable that the degree of preference is set so that the value of the degree of preference increases as the distance between the two is closer to the specific range.

DESCRIPTION OF SYMBOLS 1 Autonomous mobile robot 11 Distance sensor 12 Sensor movement control part 13 Control apparatus 14 Wheel 130 Environmental map memory | storage part 131 Viewpoint selection part 132 Distance sensor data acquisition part 133 Self-position estimation part

Claims (1)

A self-position estimation method for performing self-position estimation by matching a point cloud shown on an environmental map and a point cloud obtained from a distance sensor,
For each measurement direction of the distance sensor, for a point group acquired from the distance sensor, calculate a degree of suitability based on a predetermined index,
Self-position estimation method using a point cloud acquired from the distance sensor in matching with a point cloud shown in the environmental map in the measurement direction in which a point cloud with which the degree of preference satisfies a predetermined criterion is acquired .

Images