JP2017083663A - Coincidence evaluation device and coincidence evaluation method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a coincidence evaluation device that evaluates coincidence between a recording point of recording information and a measurement point more accurately.SOLUTION: A loop detection part 42 calculates each position evaluation value representing coincidence between position information on each plot on a reference map 53a and a position of a plot, calculated from measurement data, on the map. The loop detection part 42 further calculates each direction evaluation value representing coincidence between direction information on each plot on an azimuth map 53b and a measurement direction of the plot calculated from the measurement data. The loop detection part 42 furthermore evaluates whether the plot calculated from the measurement plot coincides with each plot on the reference map 53a based upon respective calculated position evaluation values and respective direction evaluation values.SELECTED DRAWING: Figure 1

Description

  The present invention relates to a consistency evaluation apparatus and a consistency evaluation method.

  Conventionally, an autonomous traveling body that autonomously travels within a limited area inside a building or outdoors is known. In recent years, an autonomous traveling body has been developed which is a region where the autonomous traveling body should move and has a function of creating a map according to the surrounding environment. SLAM (Simultaneous Localization and Mapping) is known as one of map creation methods by a robot having a distance sensor. Examples of technologies related to SLAM are disclosed in Non-Patent Documents 1 to 4 as described below.

  FIG. 17 is a diagram for explaining the technique of Non-Patent Document 1. Non-Patent Document 1 discloses a method of scan matching. In this method, a reference map shown in FIG. 17 is created using a reference scan. This reference map is composed of a plurality of grids. Each grid has a value representing the detection probability of an object (such as a wall) at the location of the grid in the reference map. In FIG. 17B, the detection probability is high at a white place and low at a black place.

  In the technique of Non-Patent Document 1, each measurement point included in the reference scan is projected onto the reference grid map. Then, a probability distribution is calculated isotropically (concentrically) over a predetermined blur range with the position of each projected point as the center. Here, from the viewpoint of calculation efficiency, the detection probability and the blurring range are represented not by theoretical values (rational numbers) but by integers within a specified range (for example, 0 to 255).

  After the generation of the reference map, a scan (hereinafter referred to as a reference scan) to be subjected to scan matching is projected onto the reference map. Next, the entire search range in the reference map is scanned by changing the x, y, and θ values related to the position of the reference scan within a predetermined search range. Thereby, a predetermined evaluation value is obtained. Finally, the maximum evaluation position (x, y, θ) is calculated by weighted averaging the evaluation values P [θ] [x] [y]. This position is determined as the match position of the target scan.

  Non-Patent Document 2 discloses an example of SLAM that uses the scan match disclosed in Non-Patent Document 1.

  FIG. 18 is a diagram for explaining the technique of Non-Patent Document 3. Non-Patent Document 3 discloses a method of scan matching. In this technique, each grid constituting the reference map has a predetermined azimuth value. The orientation value of each grid is calculated by averaging the measurement directions when the position of the corresponding grid is detected by scanning. Also, as shown in FIG. 18, for a thin wall, the grid has two orientation values, so that one grid represents the front and back surfaces of the wall.

  FIG. 19 is a diagram for explaining the technique of Non-Patent Document 4. Non-Patent Document 4 discloses a map generation method using scan data. In this technique, the direction of the wall is estimated based on the positional relationship between scan data and data adjacent thereto. And as shown in FIG. 19, based on this estimation result, the error of the measurement distance of each point contained in scan data is correct | amended.

Real-Time Correlative Scan Matching, Olson et al, Robotics and Automation, 2009. ICRA '09 .IEEE International Conference on Page (s): 4387-4393 Efficient Sparse Pose Adjustment for 2D Mapping, Konolige et al, Intelligent Robots and Systems (IROS), 2010 IEEE / RSJ International Conference on Page (s): 22-29 High Quality Pose Estimation by Aligning Multiple Scans to a Latent Map ”, Huang et al, Robotics and Automation (ICRA), 2010 IEEE International Conference on Page (s): 1353-1360 Highly Accurate Maximum Likelihood Laser Mapping by Jointly Optimizing Laser Points and Robot Pose, Ruhnke M. et al, Robotics and Automation (ICRA), 2011 IEEE International Conference on Page (s): 2812-2817

  In the technique of Non-Patent Document 1, each grid has detection probability information at a detection point, but does not have direction information. Therefore, the front and back of the wall cannot be distinguished. For this reason, there is a case where the scan match is not accurately performed, for example, the target scan and the reference scan are matched in a form in which the passage is shifted by one due to an error in matching the opposite walls in the scan match.

  Since the technique of Non-Patent Document 2 uses the scan match of Non-Patent Document 1, there is a problem similar to that of Non-Patent Document 1.

  In the technique of Patent Document 3, the grid has 1 or 2 orientation values. Therefore, the grid cannot correctly represent points (thin bars, etc.) measured from three or more surfaces. As a result, there is a problem that the evaluation result by the scan match goes up and down inappropriately. This technique also requires exception handling where the grid may have “two orientation values”. This exception processing is arranged in the innermost loop in the repeated loop processing in the scan match. As a result, there is also a problem that the calculation time required for the scan match becomes extremely long.

  Patent Document 4 merely discloses a technique for reducing the distance error for each scan, and does not show any device for applying this to scan matching.

  The present invention has been made in view of the above problems. An object of the present invention is to provide a consistency evaluation apparatus and a consistency evaluation method that can more accurately evaluate the consistency between a recorded point in recorded information and a measurement point.

  The coincidence evaluation apparatus according to an aspect of the present invention includes a recording unit that records position information and direction information on a map for at least one recording point as recording information, and a distance between a predetermined measurement position and surrounding measurement points. And an acquisition unit that acquires measurement data representing the measurement direction from the measurement position to the measurement point, and a calculation that calculates the position and measurement direction of the measurement point on the map based on the acquired measurement data Each position evaluation representing the degree of coincidence between the position information on the map of each recording point in the recording information and the position on the map of the measurement point calculated from the acquired measurement data A position evaluation unit that calculates a value, direction information of each recording point in the recording information, and measurement of the measurement point calculated from the acquired measurement data A direction evaluation unit for calculating each direction evaluation value representing a degree of coincidence with each direction, and each of the recording points in the recording information based on the calculated position evaluation value and each direction evaluation value. And a coincidence evaluation unit that performs an evaluation on whether or not they match.

  The coincidence evaluation method according to one aspect of the present invention includes a recording step of recording position information and direction information on a map for at least one recording point as recording information, and a distance between a predetermined measurement position and surrounding measurement points. And an acquisition step for acquiring measurement data representing the measurement direction from the measurement position to the measurement point, and a calculation for calculating the position and measurement direction of the measurement point on the map based on the acquired measurement data Each position evaluation representing the degree of coincidence between the position information on the map of each recording point in the recording information and the position on the map of the measurement point calculated from the acquired measurement data A position evaluation step for calculating a value, direction information of each recording point in the recording information, and the measurement calculated from the acquired measurement data A direction evaluation step for calculating each direction evaluation value representing a degree of coincidence with each measurement direction, and each of the measurement points in the record information based on the calculated position evaluation value and each direction evaluation value. And a coincidence evaluation step for evaluating whether or not the recording points coincide with each other.

  According to one aspect of the present invention, there is an effect that it is possible to more accurately evaluate the coincidence between a recording point in recording information and a measurement point.

It is a block diagram which shows the principal part structure of the robot which concerns on Embodiment 1 of this invention. It is a figure which shows the external appearance of the robot which concerns on Embodiment 1 of this invention. It is a figure explaining the robot which concerns on Embodiment 1 of this invention is provided. It is a figure explaining the outline | summary of the map creation in the robot which concerns on this embodiment. It is a figure for demonstrating the detail of the orientation division defined in the grid which comprises the orientation map in Embodiment 1 of this invention. It is a figure which shows the example of calculation of the azimuth | direction bit value in Embodiment 1 of this invention, and the example of an azimuth | direction map. It is a flowchart which shows the flow of a process by the node addition part in Embodiment 1 of this invention. It is a flowchart which shows the flow of the process by the loop detection part in Embodiment 1 of this invention. It is a figure which shows the process sequence for producing the reference | standard map in Embodiment 1 of this invention. It is a figure which shows the basic process sequence for the match position calculation in Embodiment 1 of this invention. It is a figure which shows the process sequence for producing the orientation map in Embodiment 1 of this invention. It is a figure which shows the additional process for the match position calculation which concerns on Embodiment 1 of this invention. It is a figure explaining an example of direction evaluation in Embodiment 1 of the present invention. It is a figure for demonstrating the detail of the orientation division defined in the grid which comprises the orientation map in Embodiment 2 of this invention. It is a figure which shows the example of a calculation of the direction bit value in Embodiment 2 of this invention, and the example of an orientation map. It is a block diagram which shows the principal part structure of the robot which concerns on Embodiment 3 of this invention. It is a figure for demonstrating the technique of a nonpatent literature 1. FIG. It is a figure for demonstrating the technique of a nonpatent literature 2. FIG. It is a figure for demonstrating the technique of a nonpatent literature 3. FIG.

Embodiment 1
A first embodiment of the present invention will be described below with reference to FIGS.

(Overview of robot 1)
The robot 1 (matching evaluation device) according to the present embodiment is a device that has a function of performing a cleaning operation and that can autonomously travel on a flat floor surface by left and right independent drive wheels. In this embodiment, the robot 1 is an autonomous traveling body that cleans the floor. Although not shown, the robot 1 includes a work unit for performing a cleaning work. The work unit includes a suction unit that sucks dust falling on the floor, a wet cleaning unit including a mop, a cleaning liquid tank, and a waste liquid tank. In addition, the function which a work unit has is not limited to cleaning, For example, the lawn mowing function which mows a lawn by having a rotating cutting blade may be sufficient.

  The robot 1 has a function of creating a map for specifying an area to be cleaned and a cleaning order according to an environment such as surrounding terrain and the like. The robot 1 has a function of creating a map by making the robot 1 autonomously travel as a function of creating a map.

(Coordinate system)
In the present embodiment, the robot 1a uses a plurality of coordinate systems that are distinguished from each other for map creation. Specifically, a vehicle body coordinate system, a sensor coordinate system, a world coordinate system, and a grid coordinate system are used.

  The vehicle body coordinate system is a coordinate system having a predetermined reference point in the robot 1a as an origin. The origin is at the center of each drive wheel 22 of the robot 1a. The forward direction of the robot 1a is defined as the x-axis plus direction, and the left direction is defined as the y-axis plus direction.

  The sensor coordinate system is a coordinate system viewed from the LRF 3 as shown in FIG. A sensor coordinate system is obtained by adding an offset corresponding to the attachment position of the LRF 3 to the vehicle body coordinate system.

  The world coordinate system is a coordinate system that is calculated inside the robot 1a and that has a predetermined external reference point as the origin. For example, the coordinates of the robot 1a when the robot 1a starts running are set as the origin. The world coordinate system is also called an odometry coordinate system.

  The grid coordinate system is a coordinate system that represents each coordinate by the number of grids in a map described later. The right direction in the map is the x-axis plus direction, and the upward direction is the y-axis plus direction. The origin in the grid coordinate system differs depending on the use situation of this coordinate system and is set as appropriate.

(Configuration of robot 1)
FIG. 1 is a block diagram showing a main configuration of a robot 1 according to Embodiment 1 of the present invention. As shown in this figure, the robot 1 includes a drive unit 2, an LRF (Laser Range Finder) 3, a control unit 4, and a storage unit 5 (recording unit).

(Driver 2)
The drive unit 2 includes two left and right motors 21, drive wheels 22, and an encoder 23. The motor 21 is connected to the drive wheel 22 and rotates the drive wheel 22 in accordance with control by the control unit 4. One drive wheel 22 is arranged on each of the left and right sides of the main body. The two drive wheels 22 are rotated by a motor 21 connected to itself. The encoder 23 is connected to each drive wheel 22. The number of rotations of each drive wheel 22 is detected and output to the control unit 4.

(LRF3)
The LRF 3 is a measuring instrument that measures a distance to a surrounding object in a scanning plane orthogonal to the rotation axis while rotating an optical distance sensor around the rotation axis. The LRF 3 includes a laser beam output unit 31, a laser beam receiver 32, and a scan data output unit 33. The laser beam output unit 31 and the laser beam receiver 32 are arranged on a scanning mechanism that rotates around a predetermined rotation axis in the LRF 3.

  FIG. 2 is a diagram illustrating an appearance of the robot 1 according to the first embodiment of the present invention. As shown in this figure, the LRF 3 is disposed on the front surface of the main body, which is the front surface of the robot 1 in the traveling direction. Furthermore, it may be arranged also on the rear surface of the main body that is opposite to the front surface of the main body. The LRF 3 measures the distance in the sensor coordinate system between the robot 1 and an object such as the wall 10 around the robot 1 by outputting the laser beam L radially and scanning the horizontal plane, and outputs it as scan data. Is.

  FIG. 3 is a diagram illustrating the LRF 3 provided in the robot 1 according to the first embodiment of the present invention. The LRF 3 detects the distance of the obstacle that has reflected the laser light L in the measurement direction at a certain time point based on the irradiation angle of the laser light L and the time required from the irradiation to the reflection. Obstacle detection (sensing) based on the so-called TOF (time of flight) principle. The LRF 3 is installed in the robot 1 so that its rotation axis is substantially vertical. The LRF 3 repeatedly measures the distance in each direction in a substantially horizontal plane by repeatedly transmitting the measurement light and receiving the reflected light while rotating the rotating shaft.

  As shown in FIG. 3, the field of view of the left and right direction of the LRF 3 is 100 degrees to the left and right, with the forward direction of the robot 1 being 0 degrees. The LRF 3 measures the distance every 1 degree. As described above, in the present embodiment, the forward direction of the robot 1 is defined as the x-axis plus direction in the sensor coordinate system. Further, the direction perpendicular to the x-axis is defined as the y-axis direction. Furthermore, when the robot 1 is viewed in plan, the counterclockwise direction from the x-axis is the plus direction of the angle, and the clockwise direction from the x-axis is the minus direction of the angle. Further, in the y-axis, the x-axis plus direction rotated 90 degrees counterclockwise (the leftward direction in FIG. 3) is defined as the y-axis plus direction, and the x-axis plus direction is rotated 90 degrees clockwise from the x-axis plus direction. The direction (the right direction in FIG. 3) is the y-axis minus direction.

  The scan data acquired by the LRF 3 is composed of a total of 201 measurement data of −100 degrees to 0 degrees to +100 degrees. The LRF 3 measures the surrounding measurement points at the current measurement position for each measurement cycle, and outputs the scan data acquired thereby to the control unit 4. The measurement period of LRF3 may be set arbitrarily, but is assumed to be 50 ms as an example.

  When the distance in a certain azimuth cannot be measured, the LRF 3 sets the distance data in that azimuth as a predetermined invalid value set in advance. The case where the distance could not be measured by this LRF3 means that the intensity of the reflected light of the laser light L is too small because the distance to the wall in that direction is too far or the reflectance is too low. The case where the part 32 cannot detect the reflected light or the case where the laser light receiving part 32 cannot identify the reflected light of the laser light L due to the light around the robot 1 being too strong can be cited.

  The invalid value is set to a value that is significantly larger than the rated distance defined as LRF3. As an example, when the rated distance of the LRF 3 is set to 10 m, the invalid value is set to 99 m. In this case, when the scan data measured by the LRF 3 is 99 m, it can be determined that the wall existing in that direction exists at a position far enough that the distance cannot be measured by the LRF 3.

(Control unit 4)
The control unit 4 includes a node addition unit 41 (acquisition unit, calculation unit, position evaluation unit, direction evaluation unit, consistency evaluation unit), loop detection unit 42 (acquisition unit, calculation unit, position evaluation unit, direction evaluation unit, coincidence) Sex evaluation unit) and a map generation unit 43. The node addition unit 41, the loop detection unit 42, and the map generation unit 43 are components that realize an SLAM function to be described later.

  The node addition unit 41 is called a node representing the distance and the self-position of an obstacle such as a wall in each direction from the distance information (scan data) output from the LRF 3 and the rotation speed information output from the encoder 23. Data at a predetermined timing is generated and stored in the storage unit 5 as node information 16c. For example, when the robot 1 moves a predetermined distance or a predetermined angle, the node adding unit 41 generates a node at every predetermined timing set in advance, and sequentially adds and stores the node as a node 51a in the storage unit 5.

  When connecting the nodes, the node adding unit 41 performs a so-called scan match process that finds a position where the positions of the obstacles in each node best overlap with each other centering on the self-position indicated by each node. Thus, the node adding unit 41 improves the accuracy when connecting the nodes.

  The map generation unit 43 draws the position of the obstacle at each node connected by the node addition unit 41 to an empty map prepared in advance having an area equivalent to the outer shape of the measurement range. Then, the map generation unit 43 specifies the inside of the position of the obstacle input in the sky map as a travelable area of the robot 1. Thereby, the map generation unit 43 creates a map, and stores the created map in the storage unit 5 as the output map 53c.

(Storage unit 5)
The storage unit 5 stores various data related to map creation in the robot 1. Specifically, the storage unit 5 stores a node group 51, a constraint group 52, and a map group 53.

  The node group 51 includes a plurality of nodes 51 a added by the node adding unit 41. Each node 51a is configured by a position 51b of the LRF 3 and scan data 51c acquired at the position 51b. Each node 51a is assigned a node number in the order of generation. The position 51b of the node 51a is a position on the world coordinate system.

  The constraint group 52 includes a plurality of constraints 52 a added by the node addition unit 41 or the loop detection unit 42. Each constraint 52a is data defining the positional relationship between two nodes. One is called a reference node and the other is called a target node. In each constraint 52a, the position of the target node in the sensor coordinate system of the reference node is defined in the vehicle body coordinate system.

  The map group 53 includes a reference map 53a (recording information), an orientation map 53b (recording information), and an output map 53c. The reference map 53a is created using at least one node 51a, and the presence or absence of a wall at the position of the plot (measurement point) calculated from each measurement data included in the scan data 51c included in each node 51a is defined. It is a map.

  The orientation map 53b is a map that is created using at least one node 51a and has information representing the orientation of each plot (measurement point) obtained from the distance data included in each scan data 51c.

  The output map 53c is a final map in which information of all added nodes 51a and all constraints 52a is reflected. The output map 53c is created by first plotting each node 51a on the map in the world coordinate system and further converting the position of each plot on the world coordinate system into a position on the grid coordinate system. The output map 53c is output to the robot 1a, for example, and touches the user's eyes. The output map 53c is also used for internal control of the robot 1 when the robot 1 performs route search or autonomous running, for example.

(Map creation)
Next, an outline of map creation in the present embodiment will be described mainly with reference to FIG.

  FIG. 4 is a diagram for explaining the outline of map creation in the robot 1a according to the present embodiment.

  The robot 1 creates a map by using a so-called SLAM (Simultaneous Localization and Mapping) process that creates a map while estimating its own position. As the robot 1 moves, the node addition unit 41, the loop detection unit 42, and the map generation unit 43 execute a predetermined process to create a map.

(Processing of the node addition unit 41)
1. Odometry Processing The node addition unit 41 first executes odometry processing. The encoder 23 outputs the rotational speed of the drive wheel 22 to the control unit 4 as rotational speed information. The scan data output unit 33 of the LRF 3 outputs scan data to the control unit 4 every predetermined measurement period (for example, 50 ms) set in advance. Then, the node adding unit 41 calculates the position (x, y, θ) of the robot 1 on the world coordinate system by calculating the moving distance and direction of the vehicle body from the rotational speed of the driving wheel 22 ( That is, odometry processing is performed). Thereby, the node adding unit 41 recognizes the self position of the robot 1 on the world coordinate system. Subsequently, the node adding unit 41 adds an offset for the attachment of the LRF 3 to its own position (x, y, θ) to thereby position the LRF 3 on the world coordinate system (x _s , y _s , θ _s ). Is calculated.

2. Node Addition Processing Next, each time the robot 1 moves to the extent that the distance or azimuth is equal to or greater than a predetermined value, the node addition unit 41 calculates the position (x _s , y _s , θ _s ) and scan data of the LRF 3 at that time And stored in the storage unit 5 as a node 51a.

  In each node, the node addition unit 41 generates the output from the LRF 3 at a timing suitable for SLAM processing. As an output from the LRF 3, a set of scan data is output to the control unit 4 at relatively short time intervals such as every 50 ms as an example. Therefore, if a node is generated for each output of the LRF 3, the amount of data becomes too large. For this reason, the node addition unit 41 generates and stores each node 51 a in the storage unit 5 at a time interval appropriately thinned out. In addition, what is necessary is just to set arbitrarily the timing which this node addition part 41 produces | generates the node 51a.

3. Constraint addition Next, the node addition unit 41 refers to each node information 51a stored in the storage unit 5, and calculates the positional relationship of each node. The node addition unit 41 performs so-called scan matching processing that finds the positional relationship in which the distance shapes at each node most closely overlap each other with (x _s , y _s , θ _s ) of the LRF3 on the plane coordinates by odometry processing as an initial value. And improve the accuracy of obstacles such as walls when connecting nodes together. The positional relationship between one node and another node is called constraint. That is, the node adding unit 41 creates a constraint between the nodes by the scan match, and stores the constraint information 52 a representing this in the storage unit 5.

(Processing of the loop detection unit 42)
The loop detection unit 42 executes a loop closing process. In this process, when there is a loop path in which the robot 1 passes through the same place twice on the trajectory of the robot 1, the first position and the last position in the loop path coincide with each other in order to correctly create a map. This is the process of transforming the map. In other words, a constraint in which the first position and the last position in the loop path coincide with each other is generated, and the position of the node is corrected based on the constraint. In the example of FIG. 4, the constraint from the node N2 to the node N1 corresponds to this.

  The loop closing process includes a loop detection process and a graph optimization process.

1. Loop Detection Processing In the example of FIG. 4, the loop detection unit 42 generates a constraint from the node N2 constituting the loop path to the node N1 by executing a scan match. At that time, the loop detection unit 42 generates a set of distance shapes by projecting the distance to the wall 10 in each azimuth on the planar coordinate system for the node that is the target of the constraint determination. For each node, the position of the wall 10 detected by the LRF 3 is represented by a distance shape including a plurality of points P in the control unit 4.

  FIG. 4B shows the distance shape at the node N1. FIG. 4C shows a distance shape at the node N2 after the robot 1 has moved through the movement route R from the node N1.

  The loop detection unit 42 generates a constraint between the node N1 and the node N2 based on the distance shape at the node N1 and the distance shape at the node N2. At that time, if the loop detection unit 42 can correctly calculate the positional relationship between the node N1 and the node N2 as shown in FIG. 4D, it can generate an appropriate constraint. On the other hand, if the positional relationship between the node N1 and the node N2 is erroneously calculated (outlier is calculated) as shown in FIG. 4E, an erroneous constraint is generated. In FIG. 4E, the wall in the distance shape of the node N1 and the wall in the distance shape of the node N2 are overlapped with the front and back of the wall being mistaken. In addition, outliers (Outliers) are generally terms that indicate data that is away from the group in statistics and the like, and originally have no meaning as wrong values, but in the SLAM field, Outliers are effectively used to mean “wrong values that contradict other data”.

  The novel scan match realized by the present invention is particularly used when a constraint relating to loop closure is generated by the loop detection unit 42. By this scan match, it is possible to prevent generation of an erroneous constraint as shown in FIG. 4E and to generate a correct constraint as shown in FIG. Note that the scan match of the present invention may be used during normal constraint generation by the node addition unit 41 described above.

2. Graph Optimization Processing The loop detection unit 42 stores the newly generated constraint in the storage unit 5. As a result, a new constraint is added to the reference map 53a. The loop detection unit 42 adjusts the positions of all the nodes in the reference map 53a so as to minimize the error of all the constraints in the reference map 53a including the added constraint (graph optimization). By this processing, the start position and the end position in the loop path coincide with each other. The loop detection unit 42 can use, for example, a conventional method described in Non-Patent Document 2 as specific graph optimization processing. Since this process is not an essential feature point in the present invention, further detailed description is omitted.

(Processing of the map generation unit 43)
The map generation unit 43 draws the position of each point P, which is the position of the obstacle at each node, on an empty map prepared in advance having an area corresponding to the outer shape of the measurement range. The map generation unit 43 first calculates the position coordinates of obstacles such as walls and pillars on the empty map, which is an unknown area, from the position of each point P representing the obstacle at each node, and inputs it to the empty map. To go. Then, the map generation unit 43 specifies the inside of the position of each point P input to this sky map as the travelable area of the robot 1. As a result, the map generation unit 43 creates an output map 53 c and stores it in the storage unit 5.

(Definition of orientation classification)
As will be described in detail later, the robot 1a of the present embodiment accurately executes a scan match by creating a predetermined orientation map 53b representing the orientation of the plot included in each reference scan in addition to the reference map 53a. . FIG. 5 is a diagram for explaining details of the azimuth divisions defined in the grid G constituting the azimuth map 53b in the first embodiment of the present invention. The orientation map 53b is composed of a plurality of grids G. Each grid G, when a wall is measured at that position, holds from which azimuth it is measured as an azimuth bit value (information on the measurement direction of a recording point) described below. Each grid G is defined with a plurality of azimuth sections in which the entire azimuth range is divided into predetermined azimuth ranges. As shown in FIG. 5, in this embodiment, there are eight types of azimuth divisions defined in each grid G in the azimuth map 53b, each having a different angle range of 45 degrees. Specifically:
Azimuth section 0 = 0 to 22.5 ° and 337.5 to 360 ° Azimuth section 1 = 22.5 to 67.5 ° Azimuth section 2 = 67.5 to 112.5 ° Azimuth section 3 = 112. Between 5 and 157.5 ° Orientation section 4 = Between 157.5 and 202.5 degrees Orientation section 5 = Between 202.5 and 247.5 degrees Orientation section 6 = Between 247.5 and 292.5 degrees Orientation section 7 = Between 292.5-337.5 °.

  As described above, the plurality of azimuth sections 0 to 7 are obtained by equally dividing the entire azimuth range in the grid G into the number of integer values. In the present embodiment, the x-axis plus direction in the orientation map 53b is defined as the 0 degree direction in the orientation map 53b.

  When creating the azimuth map 53b, the node adding unit 41 first calculates the azimuth classification to which the return beam azimuth of each plot in the scan data 51c belongs. The return beam azimuth is an angle obtained by viewing the direction of the beam from the grid G toward the LRF 3 in the azimuth map 53b.

  In the grid G, a predetermined adjacent width is defined in advance in addition to each orientation section. In this embodiment, the adjacent width is 1. The node adding unit 41 calculates an azimuth range as a numerical sequence corresponding to the return ray azimuth of the plot by expanding the azimuth section to which the return ray azimuth of the plot belongs by the adjacent width. The relationship between the azimuth classification to which the return beam azimuth belongs and the azimuth range calculated at that time is as follows.

When the direction division = 0, the direction range = (7, 0, 1)
When direction division = 1, direction range = (0, 1, 2)
When azimuth division = 2, azimuth range = (1,2,3)
When azimuth division = 3, azimuth range = (2, 3, 4)
When azimuth division = 4, azimuth range = (3, 4, 5)
When direction division = 5, direction range = (4, 5, 6)
When azimuth division = 6, azimuth range = (5, 6, 7)
When the orientation division = 7, the orientation range = (6, 7, 0).

  Here, each number in the azimuth range indicates a azimuth division number defined in the grid G.

  The node addition unit 41 calculates the azimuth bit value of the grid G corresponding to the plot in the azimuth map 53b using the azimuth range calculated for a certain plot. The azimuth bit value corresponds to the presence / absence of the numerical values of 0 to 7 included in the azimuth range with the true / false of the 1st to 8th bits (evaluation value indicating the presence of the measurement direction). It is a value (measurement direction information) calculated by OR operation. Specifically, the bit of the digit corresponding to the number included in the azimuth range is set to 1, and the bit of the digit corresponding to the number not included is set to 0, and the value of each bit is ORed. Thereby, the azimuth | direction bit value of this embodiment takes any value of 0-255. If the azimuth bit value is 0, the values of the azimuth sections in the grid G are all 0. On the other hand, if the azimuth bit value is 255, the values of the azimuth divisions in the grid G are all 1.

  FIG. 6 is a diagram illustrating a calculation example of the direction bit value and an example of the direction map 53b according to the first embodiment of the present invention. In (a) of this figure, one grid G is divided into nine sub-grids G of the same size, and for each of the eight sub-grids G excluding the center, the orientation sections 0 to 7 defined in FIG. Among these, the number of the direction division corresponding to the direction determined by the arrangement of the subgrid G is assigned.

As shown in FIG. 6A, when the return beam azimuth of the plot is included in the azimuth section 1, the azimuth sections 1 and 2 are adjacent to each other. Therefore, the azimuth range = (0, 1, 2). In this case, the orientation bit value of the grid G corresponding to this plot is 2 ^ 0 | 2 ^ 1 | 2 ^ 2 = 0000111b = 07h (hexadecimal number). When this is expressed in decimal, it is 2 ⁰ +2 ¹ +2 ² = 7.

The node adding unit 41 logically ORs the azimuth bit value calculated for the grid G corresponding to a certain plot with the existing value of the azimuth bit value of the grid G, thereby obtaining the azimuth bit value of the grid G. Update. As a result, the grid G can have azimuth bit values representing a plurality of azimuth sections calculated from a plurality of different plots. For example, if the azimuth bit value of the grid G is 15 (2 ⁰ +2 ¹ +2 ² +2 ³ ), it can be seen that in this grid G, the azimuth sections 0 to 3 are all “present”.

  FIG. 6B shows an example of the orientation map 53b in the present embodiment. In this example, an orientation map 53b created by combining two scan data is shown. When the scan data acquired at the node N11 is projected onto the orientation map 53b, two plots included in the scan data are directed to the grid G1 and the grid G2. Therefore, the azimuth range corresponding to the projection of these plots on the grid G1 and the grid G2 (the azimuth section to which the return beam azimuth belongs and its adjacent location) becomes “present” (shaded location in the figure).

  On the other hand, when the scan data acquired at the node N12 is projected onto the orientation map 53b, two plots included in the scan data are directed toward the grids G1 to G3. Therefore, the azimuth range corresponding to the projection of these plots on the grids G1 to G3 is “present” (shaded portion in the figure).

  In this embodiment, since each grid G has eight types of azimuth divisions, the azimuth bit value that each grid G can have takes a value of 8 bits (any one of 0 to 255). Therefore, the orientation of the plot in each grid G can be defined in detail.

(Process flow)
With reference to FIG. 7 and FIG. 8, the flow of processing in the node addition unit 41 and the flow of processing in the loop detection unit 42 will be described below. FIG. 7 is a flowchart showing a flow of processing by the node adding unit 41 according to the first embodiment of the present invention. FIG. 8 is a flowchart showing a flow of processing performed by the loop detection unit 42 according to the first embodiment of the present invention. In the robot 1, a series of processes by the node addition unit 41 and a series of processes by the loop detection unit 42 are executed in parallel.

  As shown in FIG. 7, the node adding unit 41 first executes odometry processing using the rotation amount information acquired from the encoder 23 and the scan data acquired from the scan data output unit 33 (S1). Next, the node adding unit 41 determines whether to add a node (S2). If S2 is NO, the process of FIG. 7 returns to the beginning. On the other hand, if S2 is YES, the node adding unit 41 generates a node and a constraint by executing a scan match (S3). Hereinafter, the latest node added together with the constraint at this time is called a target node, and the node added immediately before is called a reference node. The node adding unit 41 adds the generated node and constraint to the reference map 53a (S4, S5). Thereafter, the process of FIG. 7 returns to S1. That is, the node adding unit 41 repeatedly executes a series of processes shown in FIG.

  As shown in FIG. 8, the loop detection unit 42 first determines whether or not there is a remaining constraint candidate constituting the loop (S11). In the present embodiment, the constraint candidates are two nodes whose node numbers are separated by a predetermined value (for example, 10) or more. Of these two nodes, one is referred to as a reference node NR1 and the other as a target node NT. The loop detection unit 42 keeps a record of the combination of two nodes that have already been determined whether or not to be added as constraints. Then, two nodes of the recorded combination are excluded from the constraint candidates in S11. If S11 is NO, the loop detection unit 42 waits for a predetermined time (S12). Thereafter, the processing of FIG. 8 returns to S11.

  On the other hand, if S11 is YES, the loop detection unit 42 evaluates one constraint candidate (S13). Next, the loop detection unit 42 determines whether or not a constraint should be added (S14). Specifically, when the distance between the positions of the two nodes is smaller than a predetermined value (for example, 5 meters), the loop detection unit 42 determines that a constraint between the two nodes should be added. . If S14 is YES, the loop detection unit 42 executes a scan match (S15), and adds a constraint to the reference map 53a based on the result (S15). Thereafter, the loop detection unit 42 performs graph optimization (S16). Thereafter, the processing of FIG. 8 returns to S11. That is, the loop detection unit 42 repeatedly executes a series of processes shown in FIG.

(Details of scan match)
The details of the scan match in the present embodiment will be described with reference to FIGS. FIG. 9 is a diagram showing a processing procedure for creating the reference map 53a in the present embodiment. FIG. 10 is a diagram showing a basic processing procedure for calculating the match position in the present embodiment. A series of processes shown in FIG. 9 and FIG. 10 are the same as those in the prior art. On the other hand, a series of processes shown in FIGS. 11 and 12 are processes peculiar to the present embodiment, which are additionally performed on FIGS. 8 and 10, respectively.

  In the present embodiment, at least one of the node addition unit 41 and the loop detection unit 42 can perform the scan match process shown in FIGS. In the following description, the loop detection unit 42 first creates the reference map 53a and the orientation map 53b, and executes the match position calculation processing using these maps. Explain the details of the match.

(Creation of reference map 53a)
The loop detection unit 42 first prepares an empty reference map 53a divided by a plurality of grids (regions). The size (length of one side) of each grid is a fixed value, and is 50 mm in this embodiment. The size (length of one side) of the reference map 53a is a fixed value, and is 400 grids in this embodiment. This size is roughly determined by dividing LRF3 by twice the rated distance divided by the grid size. The origin of the reference map 53a is in the center. Each coordinate axis direction of the reference map 53a is created according to each coordinate axis direction in the sensor coordinate system of the reference node NR1. Each grid has a predetermined grid value (occupancy probability) at the position of the grid in the reference map 53a. This occupancy probability is the probability that an actual wall is at the position of the grid. A combination of coordinate information originally defined for each grid of the reference map 53a and a value of the grid corresponds to position information of a recording point or a measurement point. At the time of creating the empty reference map 53a, all grid values (occupancy probabilities) are predetermined minimum values to be described later.

  The loop detection unit 42 creates the reference map 53a using at least one scan. The at least one scan here is a predetermined number of nodes (for example, the past five) including the reference node NR1 included in the node group 51. Hereinafter, an example in which the loop detection unit 42 creates the reference map 53a using the five nodes NR1 to NR5 will be described.

  First, the loop detection unit 42 selects five nodes one by one as the target scan in the reference map 53a. Then, the position P [x, y] of each plot to be evaluated is calculated from each measurement data included in the scan data 51c in the target scan. The position P [x, y] is a grid coordinate (grid coordinate) with respect to the plot in the reference map 53a. Specifically, the node adding unit 41 calculates each distance to the wall 10 in each direction from −100 degrees to +100 degrees included in the scan data in the target node in the corresponding node NR in the sensor coordinate system of the reference node NR1. The position and orientation of each of the target plots, which are grid coordinates, are calculated by projecting the coordinates and the obtained coordinates onto the plane coordinate system and dividing the obtained coordinates by the grid size (measurement points). Is calculated). Thereby, 201 plot positions P [x, y] are calculated from one target scan.

In the process shown in FIG. 9, a predetermined blur width is determined in advance. This blur width can take a value in the range of −w to w (W is an integer) in both the x-axis direction and the y-direction. Loop detecting unit 42, a predetermined blur width x _d in the x-axis direction, for each combination of y-axis direction of the blur width y _d, it executes the 7 rows and 8 rows of the processing of FIG.

Loop detecting unit 42 First, the plot position P [x, y], and shifted by _{x d} in the x-axis direction, and, by shifted by _{y d} in the y-axis direction, the position _{P [x + x d, y} + x d] a calculate. Then, the occupancy probability (evaluation value indicating the presence of the measurement point) in the grid corresponding to P [x + x _d , y + x _d ] is calculated. Note that the loop detection unit 42 can calculate a normal distribution having P [x, y] as a vertex by executing this process on a combination of all xd and all yd.

Next, the loop detection unit 42 acquires the existing value of the occupation probability in the grid corresponding to P [x + x _d , y + x _d ] from the reference map 53a. The loop detection unit 42 compares the occupation probability calculated for P [x + x _d , y + x _d ] of the target plot with the existing value of the occupation probability in the grid corresponding to P [x + x _d , y + x _d ]. Then, the larger value is written in the grid corresponding to P [x + x _d , y + x _d ] in the reference map 53a. If the occupation probability calculated for P [x + x _d , y + x _d ] is larger than the existing value, the occupation probability of the grid corresponding to P [x + x _d , y + x _d ] is a new value (an evaluation indicating the existence of a recording point). Value).

  Through the above processing, the loop detection unit 42 creates the reference map 53a. The loop detection unit 42 recreates the reference map 53a every time a scan match is executed. This is because the node 51a (node range when a plurality of nodes 51a is used) used for creating the reference map 53a is different for each scan match. When the (occupancy probability) of each grid in the reference map 53a is imaged in correspondence with the luminance, a map as shown in (b) of FIG. 17 is obtained.

  In the present embodiment, when executing the series of processing shown in FIG. 9, the loop detection unit 42 also creates the orientation map 53b in addition to the reference map 53a by also executing the series of processing shown in FIG. . FIG. 11 is a diagram showing a processing procedure for creating the orientation map 53b in the present embodiment.

  When the loop detection unit 42 creates the empty reference map 53a by the process of the first line in FIG. 9, the loop detection unit 42 also creates the empty direction map 53b by the process of the 1.1 line of FIG. The orientation map 53b has the same size, grid size, and origin position as the empty reference map 53a. However, the grid of the orientation map 53b has not the occupation probability but the orientation bit value as a value, and the initial value of each grid is 0. A combination of coordinate information originally defined for each grid of the orientation map 53b and a value of the grid corresponds to direction information of a recording point or a measurement point.

(Creation of orientation map 53b)
When the loop detection unit 42 executes the process on the eighth line in FIG. 9, the loop detection unit 42 further executes the process shown on the 8.1th line to the 8.5th line in FIG. Update the value of each grid. Specifically, the loop detection unit 42 first acquires the occupancy probability of the grid corresponding to P [x + x _d , y + x _d ] in the reference map 53a, and determines whether or not it is zero. When occupancy probability is not 0, the loop detector 42, calculates the azimuth range of _{P [x + x d, y} + x d] Based on the orientation division and the adjacent width of the grid corresponding to, _{P [x + x d, y} + x d] (Line 8.2). Then the loop detector 42, the calculated _{P [x + x d, y} + x d] based on the orientation range, the azimuth is calculated bit value of _{P [x + x d, y} + x d] (8.3 lines). Finally the loop detector 42 in _{the, P [x + x d,} y + x d] and azimuth bit _{value, P [x + x d,} y + x d] in orientation map 53b to the existing value and the logical OR operation of the grid corresponding to. The loop detection unit 42 updates the grid existing value by writing the value obtained by this calculation into the grid in the orientation map 53b (line 8.4).

  Through the above processing, the loop detection unit 42 creates the orientation map 53b. The loop detection unit 42 recreates the orientation map 53b each time a scan match is executed. This is because the node 51a (node range when a plurality of nodes 51a is used) used for creating the orientation map 53b is different for each scan match.

(Match position calculation)
Subsequently, the loop detection unit 42 calculates the target scan match position in the reference map 53a by executing the processing shown in FIG. The loop detection unit 42 first calculates a predetermined search range when searching for a target scan in the reference map 53a. In the present embodiment, the loop detection unit 42 includes, as search ranges, a minimum value θ _{min, a} maximum value θ _max , and an interval θ _step , a minimum value x _min and x _max of x, and a minimum value y _min of y and The maximum value y _max is calculated. x _min , x _max , y _min , and y _max are all grid coordinates and take integer values. x _min , x _max , y _min , y _max , θ _min , and θ _max are determined by adding a predetermined search width to the search start position PS (x, y, θ), respectively. Specifically, the search start position PS is determined as the position of the target node NT viewed from the position of the reference node NR1, calculated in the sensor coordinate system at the node NR1, and then converted into grid coordinates. Further, x _min , x _max , y _min , y _max , θ _min, and θ _max are calculated as follows using the predetermined value w2 or wθ, respectively.

x _max = PS (x) + w2
x _min = PS (x) −w2
y _max = PS (y) + w2
y _min = PS (y) −w2
θ _max = PS (θ) + wθ
θ _min = PS (θ) −wθ
Next, the loop detecting unit 42, and either theta _o of theta _min through? _Max, and one of _{x o} of x min _{~x max,} and either _{y o} of y min _{~y max} For each of the different combinations, the processing of lines 5 to 7 in FIG. 10 is executed. At this time, the loop detection unit 42 sequentially increases θ by θ _{step and} sequentially increases x _o and _yo by one.

Specifically, the loop detection unit 42 first calculates a post-offset plot of the target scan (line 5). The post-offset plot here refers to each plot when the target scan is projected onto the offset coordinates (x _o , y _o , θ _o ) in the reference map 53a. Since the target scan includes a total of 201 plots, the plot after all offsets is an array of 201 having each coordinate (x, y) as an element.

Next, the loop detection unit 42 acquires the occupation probability of each grid corresponding to each post-offset plot in the reference map 53a (line 6). As a result, a total of 201 occupation probabilities (
An evaluation value indicating the presence of a recording point) is acquired.

The loop detection unit 42 calculates a predetermined offset probability at the current offset position (x _o , y _o , θ _o ) by integrating (accumulating) each occupation probability for the plots after all offsets (line 7). . Thereby, one offset probability is calculated based on 201 occupation probabilities. This offset probability is a position evaluation value indicating the degree of coincidence between the position of the recorded plot (measured wall) in the reference map 53a and the position of the plot calculated from the scan data 51c.

As a result of the above processing, the loop detection unit 42 has a total number of offsets NO = (2 × wθ ÷ θ _step +1) × (2 × w2 + 1) × (2 × w2 + 1) offset probabilities for one target scan. Is calculated. The loop detection unit 42 multiplies the offset probability and the offset amount (x _o , y _o , θ _o ) of all offset conditions calculated in this way, and the sum total of 201 each of x, y, and θ obtained as a result. Is divided by the number of offsets NO (ie, weighted average) to calculate the target scan match position (x _m , y _m , θ _m ) in the reference map 53a (line 11). Note that x _m , y _m , and θ _m are not integers but numbers having a value after the decimal point.

(Additional processing for calculating match position)
FIG. 12 is a diagram showing scan match addition processing executed by the loop detection unit 42 according to the present embodiment. When the loop detection unit 42 executes the process shown in line 5 of FIG. 10, it further executes the process shown in line 5.1 of FIG. Specifically, the loop detection unit 42 obtains an azimuth section to which the return beam azimuth belongs for each point of the calculated post-offset plot, and further calculates an azimuth bit value (a total of 201 cases) based thereon.

  Furthermore, when executing the process shown in line 6 of FIG. 10, the loop detection unit 42 additionally executes the processes shown in lines 6.1 to 6.4 of FIG. Specifically, the loop detection unit 42 acquires the current azimuth bit value (a total of 201) from each grid corresponding to each post-offset plot in the azimuth map 53b (line 6.1). Next, for each post-offset plot, the loop detection unit 42 ANDs the bit value of the corresponding plot orientation section and the current orientation bit value of the corresponding grid (line 6.2). In other words, this AND operation is performed on all the 201 bearing bit values. The calculation result obtained by the AND calculation is a direction evaluation value indicating the degree of coincidence between the measurement direction of the plot recorded in the orientation map 53b and the measurement direction of the plot calculated from the target scan.

  When the calculation result in a certain post-offset plot is false, the loop detection unit 42 determines that the azimuths do not match, and changes the occupation probability obtained from the grid corresponding to the offset plot in the azimuth map 53b to a predetermined minimum value. (Line 6.3). The minimum value here is a value sufficiently small relative to the maximum value of the occupation probability (the value of the vertex of the normal distribution at the time of creating the reference map 53a), for example, a value that is 1/100 of the maximum value. On the other hand, when the calculation result in a certain post-offset plot is true, it is determined that the azimuths match, and the occupation probability acquired from the grid corresponding to the post-offset plot in the reference map 53a is maintained at the current value. Thereby, each occupation probability of 201 cases is changed to the lowest value or remains at the current value. The more the orientations match, the lower the probability that each occupation probability will be changed to the lowest value, so the value of the offset probability calculated in line 7 in FIG. 10 becomes higher.

  As described above, in the present embodiment, each grid value (occupancy probability) is changed to the lowest value or maintained at the current value based on the direction evaluation value, and thus the position calculated from each grid value. As a result, it can be said that the evaluation value (offset probability) is a value including the direction evaluation value. This means that the evaluation based on the position evaluation value (offset probability) in this embodiment is synonymous with the evaluation based on both the position evaluation value and the direction evaluation value included therein. ,means.

  Note that the loop detection unit 42 may perform an evaluation on whether or not the plot of the target scan matches the recorded plot in the reference map 53a based on the calculated position evaluation value and direction evaluation value. At that time, the loop detection unit 42 measures a target scan of plots whose position evaluation value is within a predetermined range and whose direction evaluation value is within a predetermined range among the plots recorded in the reference map 53a. It can be determined as a matching point that matches the point or a candidate for the matching point.

(Example of orientation evaluation)
FIG. 13 is a diagram for explaining an example of orientation evaluation in the present embodiment. This figure shows a part of the orientation map 53b.

  In FIG. 13, when the offset position of the target scan becomes the position O21, the plots PA1 to PA3 projected on the orientation map 53b are directed to G21 to G23 in the orientation map 53b. Here, the azimuth sections (shaded portions in FIG. 12) calculated for the plots PA1 to PA3 overlap the true azimuth sections (shaded portions in FIG. 12) in the grids G21 to G23. As a result, if the AND bit values of the plots PA1 to PA3 and the existing values of the azimuth bit values of the grids G21 to G23 are ANDed, the result of the calculation becomes true. Accordingly, in all the three plots PA1 to PA3, the azimuth coincides with all the azimuths of the corresponding grids G21 to G23 in the reference map 53a, so that the offset probability of the target scan at the offset position O21 is high.

  On the other hand, in FIG. 13, when the offset position of the target scan becomes the position O22, the plots PB1 to PB3 projected on the orientation map 53b are directed to G31 to G33 in the orientation map 53b. Here, among the azimuth segments (hatched portions in FIG. 12) calculated for the plots PB1 to PB3, they overlap with the azimuth segments (shaded portions in FIG. 13) that are true in the grids G31 to G233. Only a plot PB3 is present. That is, the azimuth sections of the plots PB2 and PB3 overlap the false azimuth sections (blank portions in FIG. 13) in the corresponding grids G32 and G33. As a result, when the AND operation is performed on the azimuth bit value of the plot PB1 and the existing value of the azimuth bit value of the grid G33, the result becomes true. However, the azimuth bit values of the plots PB2 and PB3 and the azimuths of the grids G32 and G31 If each of the existing bit values is ANDed, the result will be false. Accordingly, only one of the three plots PB1 to PB3 has the same orientation as that of the corresponding grids G31 to G33 in the reference map 53a. Therefore, the offset probability of the target scan at the offset position O22 is Lower.

  As described above, in the example of FIG. 13, the offset probability at the offset position O21 is higher than the offset probability at the offset position O22. Accordingly, the offset position O21 having a higher offset probability is calculated as the match position of the target scan by the scan match. That is, the loop detection unit 42 can scan-match the target scan to a more correct position in the reference map 53a.

  On the other hand, in the conventional scan match, the calculation of the azimuth section (calculation of the azimuth bit value) to which the return beam azimuth of the plot belongs is not performed using the azimuth map 53b as in the present embodiment. Therefore, the offset probability of the offset position O22 is as high as the offset probability of the offset position O21. From this, if both the offset position O21 and the offset position O22 are included in the search range at the time of the scan match, a position closer to the offset position O22 is calculated as the match position of the target scan. Originally, the match position of this target scan should be calculated at a position closer to the offset position OA, so that the conventional technique results in an error in the match position.

  In particular, when the offset range O21 is not included in the search range at the time of the scan match but the offset position O22 is included, this error becomes extremely large. Specifically, out of a pair of walls defined by each plot included in the target scan, a wall on the opposite side is not a wall that should be matched correctly, but any of the walls in the reference map 53a. A defect that matches the wall defined by each plot included in the reference scan occurs (occurrence of an outlier as shown in FIG. 4E).

(Modification)
The origin, resolution, and coordinate direction of the reference map 53a are not limited to the example described above, and may be set to other values. The same applies to the orientation map 53b and the output map 53c.

The azimuth | direction division in the azimuth | direction map 53b is not restricted to eight types mentioned above, What is necessary is just arbitrary types. In view of the configuration of the computer, it is desirable that the types of orientation sections are 8 × 2 ⁿ (n is an integer of 0 or more).

  The correspondence relationship between the value of each azimuth section and the angle range is not limited to the above example, and can be changed to another relationship. For example, the azimuth section 0 may be a range other than the 0-22.5 ° and 337.5-360 ° ranges based on the x-axis direction.

  The adjacent width in each grid of the orientation map 53b may be a value other than 1, for example, 2 or 0.

  In the above example, the node adding unit 41 generates the orientation map 53b at the time of generating the reference map 53a, and calculates the orientation in each grid so that other orientations for the adjacent width are included. However, the present invention is not limited to this. For example, when the loop detection unit 42 calculates each azimuth bit value of the target scan (line 5.1 in FIG. 12), a bit adjacent thereto may be made true.

[Embodiment 2]
Embodiment 2 according to the present invention will be described below with reference to FIGS. Each member common to Embodiment 1 described above is denoted by the same reference numeral, and detailed description thereof is omitted.

(Definition of orientation classification)
FIG. 14 is a diagram for explaining the details of the orientation sections defined in the grid constituting the orientation map 53b according to the second embodiment of the present invention. As shown in this figure, in the present embodiment, there are four types of orientation categories defined in each grid in the orientation map 53b, each having a different angle range of 90 degrees. Specifically:
Azimuth section 0: Between 0 and 90 degrees Azimuth section 1: Between 90 and 180 degrees Azimuth section 2: Between 180 and 270 degrees Azimuth section 3: Between 270 and 360 degrees

  In the present embodiment, the adjacent width is zero.

The node adding unit 41 calculates the azimuth bit value of this plot as 2 ⁿ when the number of the azimuth section to which the return ray azimuth of a plot included in the scan data 51c belongs is n.

FIG. 15 is a diagram illustrating a calculation example of the direction bit value and an example of the direction map 53b according to the second embodiment of the present invention. As shown in (a) of this figure, when the return ray azimuth of the plot is included in the azimuth section 0, the azimuth bit value of the grid G corresponding to this plot is 2 ⁰ = 1. The value of the calculated azimuth bit value is 2 ⁰ = 1 (azimuth division 0), 2 ¹ = 2 (azimuth division 1), 2 ² = 4 (azimuth) according to the azimuth division to which the return beam azimuth of the plot belongs. Either division 2) or 2 ³ = 8 (azimuth division 4).

As in the first embodiment, the node adding unit 41 performs a logical OR operation on the azimuth bit value calculated for the grid G corresponding to a certain plot with the existing value of the azimuth bit value of the grid G. The direction bit value of the grid G is updated. As a result, the grid G can have an orientation bit value that represents the presence or absence of a plurality of different orientation sections. For example, if the azimuth bit value of the grid G is 3 (2 ⁰ +2 ¹ ), it can be seen that the azimuth section 0 and the azimuth section 1 are “present” in this grid G.

  FIG. 15B shows an example of the orientation map 53b in the present embodiment. In this example, an orientation map 53b created by combining two scan data is shown. When the scan data acquired at the node 21 is projected onto the orientation map 53b, two plots included in the scan data are directed toward the grid G41 and the grid G42. Therefore, the orientation division corresponding to the projection of these plots in the grid G41 and the grid G42 is “present” (shaded portion in the figure).

  On the other hand, when the scan data acquired at the node 22 is projected onto the orientation map 53b, three plots included in the scan data are directed toward the grids G41 to G43. Therefore, the orientation division corresponding to the projection of these plots in the grids G41 to G43 is “present” (shaded portion in the figure).

  In this embodiment, since each grid has four types of azimuth divisions, the azimuth bit value that each grid can have takes a value of 4 bits (any one of 0 to 15). Therefore, there is an advantage that the size of the orientation map 53b can be reduced compared to the first embodiment in which the grid orientation bit value is 8 bits (any one of 0 to 255).

[Embodiment 3]
A third embodiment according to the present invention will be described below with reference to FIG. Each member common to the above-described first or second embodiment is denoted by the same reference numeral, and detailed description thereof is omitted.

  FIG. 16 is a block diagram showing a main configuration of a robot 1a according to Embodiment 3 of the present invention. As shown in this figure, the robot 1a includes a drive unit 2, an LRF 3, a control unit 4a, a storage unit 5, and a display 6. The drive unit 2, the LRF 3, and the storage unit 5 are the same as those in the first and second embodiments. On the other hand, the control unit 4 further includes a map display unit 44 in addition to the members included in the robot 1 according to the first or second embodiment.

  In the present embodiment, the map generation unit 43 creates an orientation map equivalent to the orientation map 53b by calculating the orientation bit value of each grid constituting the output map 53c. The map generation unit 43 creates an output map 53c composed of a normal map composed of a grid that does not include an orientation bit value and an orientation map composed of a grid having an orientation bit value, and outputs the output map 53c to the map display unit 44. The map display unit 44 displays the input output map 53 c on the display 6.

  At that time, the map display unit 44 divides and displays each grid into sub-grids for each azimuth division, and further displays the true / false of the azimuth (whether or not wall detection is performed) in each sub-grid, as in FIG. . As a result, in addition to the normal map, the presence or absence of a wall for each direction is also displayed on the display 6. Therefore, the user of the robot 1a can visually recognize a more detailed map.

[Summary]
The coincidence evaluation apparatus according to aspect 1 of the present invention provides a distance between a recording unit that records position information and direction information on a map for at least one recording point as recording information, and a predetermined measurement position and surrounding measurement points. And an acquisition unit that acquires measurement data representing the measurement direction from the measurement position to the measurement point, and a calculation that calculates the position and measurement direction of the measurement point on the map based on the acquired measurement data Each position evaluation representing the degree of coincidence between the position information on the map of each recording point in the recording information and the position on the map of the measurement point calculated from the acquired measurement data A position evaluation unit that calculates a value, direction information of each recording point in the recording information, and measurement of the measurement point calculated from the acquired measurement data A direction evaluation unit for calculating each direction evaluation value representing a degree of coincidence with each direction, and each of the recording points in the recording information based on the calculated position evaluation value and each direction evaluation value. And a coincidence evaluation unit that performs an evaluation on whether or not they match.

  According to the above configuration, since the coincidence between the recording point and the measurement point is evaluated based on both the position evaluation value and the direction evaluation value, the measurement direction is different from the related art that does not consider the direction coincidence. It is prevented from being evaluated that different measurement points and recorded points coincide with each other. Therefore, it is possible to more accurately evaluate the coincidence between the measurement point and the recording point.

  The coincidence evaluation apparatus according to aspect 2 of the present invention is the above-described aspect 1, wherein the calculation unit is configured to calculate each of the above-described maps on the map for each measurement data based on the measurement data acquired at at least one of the measurement positions. The position of the measurement point and the measurement direction are calculated, and the recording unit calculates the s of the recording point on the map corresponding to the measurement point based on the calculated position and measurement direction for each of the measurement points. The position information and the direction information are recorded as the recording information.

  According to the above configuration, since the recording information can be generated using the measurement data, even if the recording information is not prepared in advance, the recording point and the measuring point in the recording information generated by the consistency evaluation device itself Can be evaluated.

  In the coincidence evaluation device according to aspect 3 of the present invention, in the aspect 1 or 2, the coincidence evaluation unit is configured such that the position evaluation value is a predetermined range among the recording points on the map in the recording information. And the recording point having the direction evaluation value within a predetermined range is determined as a matching point that matches the measurement point or a candidate for the matching point, or a matching degree is calculated. It is said.

  According to said structure, the coincidence point of a recording point and a measurement point or the candidate of the said coincidence point can be determined appropriately.

  In the coincidence evaluation device according to aspect 4 of the present invention, in any one of the above aspects 1 to 3, the coincidence evaluation unit records the measurement points on the basis of the position evaluation values and the direction evaluation values. One evaluation value regarding which of the recording points on the map in the information matches is calculated, and one of the recordings on the map in the recording information that matches the measurement point based on the one evaluation value It is characterized in that a point is determined or a matching position on the map is calculated.

  According to the above configuration, it is possible to appropriately determine one recording point that matches the measurement point, or to appropriately calculate the matching position between the recording point and the measurement point on the map.

  In the coincidence evaluation device according to aspect 5 of the present invention, in any one of the aspects 1 to 4, the position evaluation unit is configured to make a relative relationship between the position information of the recording point and the position of the measurement point in the map. The position evaluation value representing the degree of coincidence between the recording point and the measurement point is calculated based on a typical positional relationship.

  According to the above configuration, the coincidence between the measurement point and the recording point can be evaluated based on the relative positional relationship between the measurement point and the recording point.

  In the consistency evaluation apparatus according to Aspect 6 of the present invention, in any one of Aspects 1 to 5, the acquisition unit is configured to calculate the distance between the measurement position and a plurality of different measurement points, and the measurement position. A plurality of the measurement data representing each of the measurement directions to each of the plurality of measurement points is acquired, and the calculation unit is configured to map the map for each measurement data based on each of the acquired plurality of measurement data. By calculating the position and measurement direction of the measurement point above, the consistency evaluation unit combines the position evaluation value and the direction evaluation value calculated for each of the different measurement points, thereby obtaining a plurality of the above-described evaluation values. It is characterized in that a plurality of recording points that match each of the measurement points are determined, or one matching degree is calculated.

  According to the above configuration, the coincidence between the plurality of measurement points and the plurality of recording points is determined at one time, or one coincidence degree regarding the coincidence between the plurality of measurement points and the plurality of recording points is calculated. be able to.

  In the consistency evaluation device according to aspect 7 of the present invention, in any one of the above aspects 1 to 6, the map is divided into a plurality of regions, and the calculation unit performs the above operation for each region in the map. An evaluation value indicating the presence of the measurement point and a measurement direction of the measurement point are calculated based on the measurement data, and the recording unit calculates the evaluation value indicating the presence of the measurement point calculated for each region of the map and the Based on the measurement direction of the measurement point, an evaluation value indicating the existence of the recording point on the map corresponding to the measurement point and the direction information are recorded as the recording information.

  According to the above configuration, it is possible to record the recording information in which the evaluation value indicating the existence of the recording point and the direction information of the recording point are defined for each region.

  In the coincidence evaluation device according to aspect 8 of the present invention, in the aspect 6, in each of the areas, a plurality of azimuth sections divided for each predetermined azimuth range are defined. For each of the regions, information on the measurement direction is calculated by combining evaluation values indicating the presence of the measurement direction of the measurement points in the orientation sections of the region based on the measurement data.

  According to said structure, the recording information by which the presence or absence of the measurement direction from each direction of the recording point in each area | region was prescribed | regulated in detail can be recorded.

  The coincidence evaluation apparatus according to aspect 9 of the present invention is characterized in that, in the aspect 8, the plurality of azimuth divisions are obtained by equally dividing all azimuth ranges in the region into the number of integer values. .

  According to said structure, the recording information by which the presence or absence of the measurement direction of the recording point was evaluated equally for every azimuth | direction can be recorded.

  In the consistency evaluation device according to aspect 10 of the present invention, in the aspect 8 or 9, when the calculation unit calculates a measurement direction of the measurement point corresponding to one azimuth section among the plurality of azimuth sections, Calculating the measurement direction information by assigning the evaluation value indicating the existence of the measurement direction of the measurement point to the one azimuth section and the plurality of azimuth sections adjacent to the one azimuth section. It is characterized by.

  According to the above configuration, the presence / absence of the measurement direction of the recording point from a certain direction can be reflected more widely in the information on the measurement direction of the recording point in the recording information.

  The coincidence evaluation method according to aspect 11 of the present invention includes a recording step of recording position information and direction information on a map for at least one recording point as recording information, and a distance between a predetermined measurement position and surrounding measurement points. And an acquisition step for acquiring measurement data representing the measurement direction from the measurement position to the measurement point, and a calculation for calculating the position and measurement direction of the measurement point on the map based on the acquired measurement data Each position evaluation representing the degree of coincidence between the position information on the map of each recording point in the recording information and the position on the map of the measurement point calculated from the acquired measurement data A position evaluation step for calculating a value, direction information of each recording point in the recording information, and the measurement calculated from the acquired measurement data. Based on the direction evaluation step for calculating each direction evaluation value representing the degree of coincidence with the measurement direction of the point, and each position evaluation value and each direction evaluation value calculated, And a coincidence evaluation step for evaluating whether or not the recording points coincide with each other.

  According to said structure, the coincidence of a measurement point and a recording point can be evaluated more correctly.

[Example of software implementation]
The control block of the control unit 4 in the robot 1 (and the control unit 4a in the robot 1a) may be realized by a logic circuit (hardware) formed in an integrated circuit (IC chip) or the like, or a CPU (Central Processing Unit) ) May be implemented by software.

  In the latter case, the control unit 4 in the robot 1 (and the control unit 4a in the robot 1a) can read a program command, which is software that realizes each function, a program, and various data by a computer (or CPU). ROM (Read Only Memory) or a storage device (these are referred to as “recording media”), a RAM (Random Access Memory) for developing programs, and the like. And the objective of this invention is achieved when a computer (or CPU) reads a program from a recording medium and executes it. As the recording medium, a “non-temporary tangible medium” such as a tape, a disk, a card, a semiconductor memory, a programmable logic circuit, or the like can be used. The program may be supplied to the computer via any transmission medium (such as a communication network or a broadcast wave) that can transmit the program. Note that the present invention can also be realized in the form of a data signal embedded in a carrier wave in which the program is embodied by electronic transmission.

(Additional notes)
The present invention is not limited to the above-described embodiments, and various modifications can be made within the scope of the claims. Embodiments obtained by appropriately combining technical means disclosed in different embodiments are also included in the technical scope of the present invention. Furthermore, a new technical feature can be formed by combining the technical means disclosed in each embodiment.

  In each embodiment, a cleaning robot has been described as an example of an autonomous traveling body, but the present invention is not limited to a cleaning robot. For example, the present invention can be applied to air cleaning devices, photographing devices, and various robot devices (for example, security robots, housework support robots, animal type robots, etc.).

  The control unit 4 and the storage unit 5 in the robot 1 may be provided in another map generation device (computer) located away from the robot 1. In this case, the robot 1 and the map creation device constitute a robot system. The robot 1 includes a drive unit 2, an LRF 3, and a transmission unit (not shown). On the other hand, the map creation device includes a control unit 4, a storage unit 5, and a receiving unit (not shown). The robot 1 transmits the acquired rotation information and scan data to the map creation device through the communication unit. The map creation device receives rotation information and scan data through a receiving unit (not shown), and creates an output map using these information and data.

  The robot 1 may include an arbitrary array sensor (for example, an ultrasonic array sensor) that can measure the distance between the robot 1 and the environment (obstacle) instead of the LRF 3. In this case, the array sensor generates scan data or distance measurement data equivalent to the scan data and outputs it to the control unit 4.

The robot 1 may include an arbitrary three-dimensional measurement sensor that can measure three-dimensional distance data instead of the LRF 3. In this case, the three-dimensional measurement sensor acquires three-dimensional distance data and outputs it to the control unit 4. The control unit 4 creates various three-dimensional maps based on the three-dimensional distance data. In this aspect, it is preferable that the azimuth section is determined in correspondence with eight quadrants having a specific coordinate plane in the real world as a boundary. Specifically, with the upward direction of the robot 1 as the z-axis plus direction, each of the x-axis, y-axis, and z-axis plus / minus combinations (2 ³ = 8) is assigned to the azimuth divisions 0 to 7. Assign to. As a result, since there are eight types of azimuth divisions defined in each grid as in the first embodiment, the calculation is convenient.

1, 1a Robot 2 Drive unit 3 LRF
4, 4a Control unit (acquisition unit)
5. Storage unit (recording unit)
6 Display 21 Motor 22 Each driving wheel 22 Driving wheel 23 Encoder 31 Laser light output unit 32 Laser light receiving unit 33 Scan data output unit 41 Node addition unit (calculation unit, position evaluation unit, direction evaluation unit, consistency evaluation unit)
42 Loop detection unit (calculation unit, position evaluation unit, direction evaluation unit, consistency evaluation unit)
43 Map generation unit 44 Map display unit

Claims (11)

A recording unit for recording position information and direction information on a map for at least one recording point as recording information;
An acquisition unit for acquiring measurement data representing a distance between a predetermined measurement position and a surrounding measurement point and a measurement direction from the measurement position to the measurement point;
Based on the acquired measurement data, a calculation unit that calculates the position and measurement direction of the measurement point on the map;
Each position evaluation value representing the degree of coincidence between the position information on the map of each recording point in the recording information and the position on the map of the measurement point calculated from the acquired measurement data is calculated. A position evaluation unit to perform,
A direction evaluation unit that calculates each direction evaluation value representing the degree of coincidence between the direction information of each recording point in the recording information and the measurement direction of the measurement point calculated from the acquired measurement data;
A consistency evaluation unit that performs an evaluation on whether or not the measurement point matches each recording point in the recording information based on the calculated position evaluation value and each direction evaluation value. Characteristic consistency evaluation device. The calculation unit calculates a position and a measurement direction of each measurement point on the map for each measurement data based on the measurement data acquired at at least one measurement position,
The recording unit uses the position information and the direction information of each recording point on the map corresponding to each measurement point as the recording information based on the calculated position and measurement direction for each measurement point. The consistency evaluation apparatus according to claim 1, wherein the consistency evaluation apparatus records the consistency.   The coincidence evaluation unit includes the recording points in which the position evaluation value is within a predetermined range and the direction evaluation value is within a predetermined range among the recording points on the map in the recording information. The coincidence evaluation device according to claim 1, wherein the coincidence point is determined as a coincidence point or a candidate for the coincidence point, or the degree of coincidence is calculated.   The coincidence evaluation unit calculates one evaluation value related to which recording point on the map in the recording information matches the measurement point based on each position evaluation value and each direction evaluation value, Based on the one evaluation value, one recording point on the map in the recording information that matches the measurement point is determined, or one matching position on the map is calculated. The coincidence evaluation apparatus according to any one of claims 1 to 3.   The position evaluation unit represents the position evaluation representing a degree of coincidence between the recording point and the measurement point based on a relative positional relationship between the position information of the recording point and the position of the measurement point in the map. The consistency evaluation apparatus according to claim 1, wherein a value is calculated. The acquisition unit acquires a plurality of measurement data representing the distances between the measurement position and the plurality of different measurement points, and the measurement directions from the measurement position to the measurement points. And
The calculation unit calculates a position and a measurement direction of the measurement point on the map for each measurement data based on each of the acquired measurement data,
The coincidence evaluation unit determines a plurality of recording points that match each of the plurality of measurement points by combining the position evaluation values and the direction evaluation values calculated for the different measurement points. 6. The coincidence evaluation apparatus according to claim 1, wherein one coincidence degree is calculated. The map is divided into multiple areas,
The calculation unit calculates, for each region in the map, an evaluation value indicating the presence of the measurement point and a measurement direction of the measurement point based on the measurement data,
The recording unit records, for each region of the map, the evaluation value indicating the existence of the measurement point calculated and the measurement direction of the measurement point as the recording information with the region as the recording point. The coincidence evaluation apparatus according to any one of claims 1 to 6, characterized in that it is characterized in that: In each of the above-mentioned areas, a plurality of azimuth sections divided for each predetermined azimuth range are defined,
The calculation unit calculates, for each region in the map, information on the measurement direction by combining each evaluation value indicating the presence of the measurement direction of the measurement point in each azimuth section of the region based on the measurement data. The coincidence evaluation apparatus according to claim 7.   9. The coincidence evaluation apparatus according to claim 8, wherein the plurality of azimuth divisions are obtained by equally dividing all azimuth ranges in the region into integer numbers.   When the calculation unit calculates the measurement direction of the measurement point corresponding to one azimuth section among the plurality of azimuth sections, the one azimuth section and the plurality of azimuth sections adjacent to the one azimuth section The coincidence evaluation apparatus according to claim 8 or 9, wherein the information on the measurement direction is calculated by assigning the evaluation value indicating the existence of the measurement direction of the measurement point to each. A recording step of recording position information and direction information on the map for at least one recording point as recording information;
An acquisition step of acquiring measurement data representing a distance between a predetermined measurement position and a surrounding measurement point, and a measurement direction from the measurement position to the measurement point;
Based on the acquired measurement data, a calculation step for calculating the position and measurement direction of the measurement point on the map;
Each position evaluation value representing the degree of coincidence between the position information on the map of each recording point in the recording information and the position on the map of the measurement point calculated from the acquired measurement data is calculated. A position evaluation process to perform,
A direction evaluation step for calculating each direction evaluation value representing a degree of coincidence between the direction information of each recording point in the recording information and the measurement direction of the measurement point calculated from the acquired measurement data;
And a consistency evaluation step for evaluating whether or not the measurement point matches each recording point in the recording information based on the calculated position evaluation value and the direction evaluation value. Consistency evaluation method.

Images