JP2011008320A - Autonomous mobile unit, self position estimating device, and program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide an autonomous mobile unit, capable of making it difficult to be affected by a moving obstacle, when the autonomous mobile unit estimates self-position, and using measurement information of obstacles other than a flat wall surface for self-position estimation, and to provide a self-position estimating method thereof.SOLUTION: The autonomous mobile unit 100 includes a vision sensor 2 for measuring a moving environment; a three-dimensional environment data generating section 4 for generating environment data including a group of three-dimensional position data of the obstacles existed in the moving environment by using measurement results by the vision sensor 2; a wall-surface detecting section 6 for detecting flat wall surface existing in the moving environment by using the environment data; and a self-position estimating section 8 for estimating a self-position by collating position information of the obstacles and position information of the wall surface specified by position data belonging to a first height range among the group of position data contained in the environment data with a reference map indicating the position of a fixed obstacle that fixedly exists in the moving environment.

Description

  The present invention relates to an autonomous mobile body that moves autonomously and a self-position estimation method of the autonomous mobile body.

In recent years, autonomous moving bodies that move autonomously have been developed in consideration of the purpose of robots, vehicles, etc. themselves. The autonomous mobile body plans a movement route by referring to a reference map that it has, and moves according to the movement route. In an autonomous mobile body, the ability to recognize a surrounding environment and make a route plan and move accordingly is important.

Patent Document 1 discloses a technique in which an autonomous mobile robot measures a distance between its own position and a wall surface in a predetermined environment, estimates a current position, and heads for a destination. This document also discloses a recovery means when the self-position cannot be measured.
Further, Patent Document 2 discloses an apparatus that can create an environment map for determining whether or not movement is possible even in movement in a region having a step on the floor surface.
As described above, in recent years, a technique for allowing a mobile body to perform appropriate autonomous movement has been developed.

JP 2008-59218 A JP 2005-92820 A

The autonomous mobile body of patent document 1 measures the circumference | surroundings of self, and measures the distance of an own position and the surrounding wall surface. That is, the position of itself is estimated or corrected based on the wall surface in the predetermined environment.
However, the obstacle that is fixedly present in the predetermined environment is not limited to a flat wall surface (hereinafter referred to as a wall surface). For example, there may be a wall surface (hereinafter referred to as a curved wall surface) that draws an arc, a columnar column, a step difference in height from the floor surface in a predetermined environment, and the like.

The step may be a simple step or a moving obstacle. Examples of moving obstacles include humans, carts, and other moving objects. The pillars can also be moving obstacles. Since the autonomous mobile body estimates its own position on the basis of a fixed obstacle, there is a possibility that if the moving obstacle is recognized by mistake, the self-position is erroneously estimated.
Further, a curved wall surface and a columnar column cannot be detected by a plane detecting means or the like.

  The present invention has been made in order to solve the above-described problem, and makes it difficult for an autonomous mobile body to be affected by a moving obstacle when estimating its own position, and also measures measurement information of obstacles other than walls on its own. An object is to provide an autonomous mobile body and a self-position estimation program that can be used for position estimation.

The autonomous mobile body according to the present invention generates environmental data including a three-dimensional position data group related to an obstacle existing in the moving environment by using a visual sensor that measures the moving environment and a measurement result by the visual sensor. A three-dimensional environment data generating unit, a wall surface detecting unit for detecting a flat wall surface existing in the moving environment using the environment data, and a first high-level data among the position data group included in the environment data. By comparing the position information of the obstacle specified by the position data belonging to the range and the position information of the wall surface with a reference map indicating the position of the fixed obstacle that is fixedly present in the moving environment, A self-position estimation unit that estimates the position of
Moving obstacles such as humans, trolleys or other moving bodies are lower in height than fixed obstacles such as walls and pillars, and exist in a part of the three-dimensional space. Therefore, by collating with the reference map based on position information having position data higher than a specific height, the autonomous mobile body is less susceptible to moving obstacles when estimating its own position, and other than the wall surface Obstacle measurement information can also be used for self-position estimation, and accurate self-position estimation becomes possible.

  Further, the first height range may be determined based on a height distribution of the position data group. As described above, the tendency of a fixed obstacle or a moving obstacle can be determined depending on the height of the obstacle. With this configuration, it is possible to efficiently eliminate moving obstacles and collate with the reference map.

  Furthermore, it is preferable that the first height range is determined so as to include data having the highest height in the position data group. As mentioned above, moving obstacles are low in height compared to wall surfaces, so only the curved wall surfaces and cylindrical columns that are fixed obstacles are extracted without extracting moving obstacles that are not necessary for self-position estimation. It becomes possible to extract.

  Furthermore, the position data group may be a remaining data group obtained by removing the position data group corresponding to the wall surface from the position data group included in the environment data.

  Here, a reliability setting unit that gives reliability to the position data may be further provided, and the reliability setting unit is specified by position data belonging to a first height range in the position data group. The reliability set in the position information of the wall surface is made higher than the reliability set in the position information of the obstacle, and the self-position estimation unit preferentially matches the position information with the high reliability with the reference map. Used for. In this case, it is possible to separate the reliability of an object having a high height, such as a wall or a column, and an object having a low height, such as a step, so that a fixed obstacle and a moving obstacle can be distinguished. The reliability referred to in the present invention is a degree indicating whether walls, columns, steps, and other obstacles extracted from the three-dimensional environment data are moving obstacles, and the lower the reliability. It is determined as a moving obstacle, and self-location can be estimated without using the information.

  Further, the reliability setting unit uses the position data of the obstacle and the position information of the wall surface specified by the position data belonging to the first height range in the position data group included in the environment data as grid data. The degree of reliability may be set based on the number of the position data in each grid corresponding to the position data after conversion.

  The self-position estimation unit preferably calculates a rotation angle for superimposing the wall surface on the arrangement of the fixed obstacle specified by the reference map, using a normal vector of the wall surface. By obtaining a rotation angle at which the normal vectors of the respective wall surfaces coincide with each other, it is possible to perform accurate self-position estimation rather than matching each point and self-position estimation.

  On the other hand, the self-position estimation apparatus according to the present invention includes position data belonging to a first height range among the position data groups included in the environment data including three-dimensional position data groups related to obstacles existing in the moving environment. The position information of the obstacle specified by the position information and the position information of the flat wall surface detected using the environment data are collated with a reference map indicating the position of the fixed obstacle existing in the moving environment. Thus, it is characterized by estimating its own position.

  In addition, the self-position estimation program according to the present invention includes position data belonging to a first height range among the position data groups included in the environment data including three-dimensional position data groups related to obstacles existing in the moving environment. The position information of the obstacle specified by the position information and the position information of the flat wall surface detected using the environment data are collated with a reference map indicating the position of the fixed obstacle existing in the moving environment. Thus, there is provided a program for causing a computer to execute a self-position estimation process including a self-position estimation step for estimating the self-position.

  According to the present invention, when an autonomous mobile body estimates its own position, it becomes difficult to be affected by a moving obstacle, and measurement information of obstacles other than the wall surface can be used for self-position estimation. It is possible to provide an autonomous mobile body that can estimate the position of the vehicle and a self-position estimation program thereof.

FIG. 3 is a block diagram showing a control system for a moving body according to the first embodiment. The block diagram explaining a self-position estimation part. FIG. 3 is a perspective view of a predetermined environment according to the first embodiment. Explanatory drawing which illustrates in two dimensions the data acquired from environmental data. Explanatory drawing at the time of acquiring height range information. FIG. 6 is a block diagram of a matching unit according to the second embodiment. Explanatory drawing of an ICP algorithm. 3 is a flowchart of a self-position estimation process according to the first embodiment. FIG. 10 is a block diagram of a self-position estimation unit according to the second embodiment. Explanatory drawing which converts environmental data into a two-dimensional grid map. FIG. 10 is a block diagram of a matching unit according to the third embodiment.

  Hereinafter, specific embodiments to which the present invention is applied will be described in detail with reference to the drawings. In the drawings, the same elements are denoted by the same reference numerals, and redundant description is omitted as necessary for the sake of clarity.

<Embodiment 1 of the Invention>
FIG. 1 shows a configuration of autonomous mobile body (hereinafter referred to as mobile body) 100 according to Embodiment 1. The moving body 100 includes a visual sensor 2, a three-dimensional environment data generation unit 4, a wall surface detection unit 6, a self-position estimation unit 8, and the like.

  The visual sensor 2 has an active distance sensor such as a laser range finder. Then, the visual sensor 2 acquires distance image data of a predetermined environment. The range in which the visual sensor 2 performs measurement is defined by the performance of the visual sensor 2.

  The visual sensor 2 may include a plurality of cameras including an image sensor such as a charge coupled device (CCD) image sensor or a complementary metal oxide semiconductor (CMOS) image sensor. And the visual sensor 2 may produce | generate distance image data using the image data image | photographed with these several cameras. Specifically, the visual sensor 2 detects corresponding points from image data captured by a plurality of cameras. Next, the visual sensor 2 may restore the three-dimensional position of the corresponding point by stereo vision. Here, the search for the corresponding points in the plurality of captured images may be performed by applying a known method such as a gradient method or a correlation method using a self-space differential constraint formula for the plurality of captured images.

  The three-dimensional environment data generation unit 4 generates the environment data by performing coordinate conversion of the distance image data. The environmental data is a set of data in which position vectors of a large number of measurement points included in the image data are represented by a three-dimensional orthogonal coordinate system. Here, the environment data includes a position data group, and the coordinates of the position data group may be displayed in a device coordinate system fixed to the moving body 100 or displayed in a global coordinate system fixed to the environment. It may be.

The wall surface detection unit 6 detects a plane from the environment data, and detects a plane having a normal vector parallel to the floor surface as a wall surface among the detected planes. Moreover, the wall surface detection part 6 provides plane ID to the detected plane.
For the plane detection in the present embodiment, a method called a plane segment finder (PLS) may be used. In the plane segment finder, first, two points are randomly sampled from a three-dimensional point group. Next, an algorithm for obtaining the position and orientation of the plane by clustering the vectors of the two points.
Specifically, in the plane segment finder, first, a randomized three-dimensional Hough transform is performed on the environmental data to detect the inclination of the maximum plane. Next, the distance between the maximum plane and the origin is detected again using the randomized three-dimensional Hough transform. Then, a plane area (plane segment) is detected by extracting a point belonging to the maximum plane.

More specifically, in the plane segment finder, the plane is represented by the following formula (1).
ρ = (x ₀ cos (θ) + y ₀ sin (θ)) cos (φ) + z ₀ sin (φ)
(1)
In Equation (1), ρ is the length of a perpendicular line drawn from the plane to the origin, θ is an angle formed by a line obtained by projecting the perpendicular line on the xy plane and the x axis, and φ is an angle formed by the perpendicular line and the xy plane. is there.
Then, each measurement point (x ₀ , y ₀ , z ₀ ) in the environmental data is projected onto a (ρ, θ, φ) space, and a peak is detected to detect a plane. In the plane segment finder, even a plane divided into a plurality of planes can be detected. Further, only the measurement point of the predetermined portion of the environmental data can be projected onto the partial space of (ρ, θ, φ) space, and the detection range can be narrowed. As a result, the calculation cost can be significantly reduced.

  Furthermore, in order to detect a plane parameter (for example, a normal vector and a distance from the coordinate system origin of the normal vector) representing a plane equation from a large number of measurement points (environmental data), a Hough transform method or a random sampling method is used. Conventionally used. These conventionally known methods may be applied to the plane detection in the present embodiment. Thereby, the normal vector of the said plane can be obtained, and the plane which has a normal vector parallel to a floor surface can be recognized as a wall surface.

  In order to facilitate understanding of the invention, the self-position estimation unit 8 in the present embodiment will be described by dividing it into a wall surface position information acquisition unit 10, a height range information acquisition unit 12, and a matching unit 14, as shown in FIG. .

  The wall surface position information acquisition unit 10 acquires position information belonging to the wall surface detected by the wall surface detection unit 6. Further, the height range information acquisition unit 12 acquires position information of an obstacle specified by position data belonging to a height range determined based on a predetermined reference from environmental data. In the present embodiment, acquisition of environmental data in a three-dimensional space in a predetermined environment constituted by wall surfaces 41 to 43, a step 44, and a cylinder 45 as shown in FIG. 3 will be described.

  The height range to be acquired is determined based on the height distribution of the position data group. In addition, it is preferable that the position data exists in the environment data and is determined so as to include the position data having the highest height. As a result, a short moving obstacle can be easily excluded, and more accurate self-position estimation becomes possible. Further, the position data belonging to the height range is position data existing in the environment data, and may be determined from environment data excluding position data corresponding to the wall surface detected by the wall surface detection unit 6. .

The acquisition method will be described by illustrating the position information acquired by the wall surface position information acquisition unit 10 and the height range information acquisition unit 12 in a two-dimensional environment map as shown in FIG.
First, the wall surface position information acquisition unit 10 acquires a position data group included in the three-dimensional environment data of the wall surfaces 41 to 43 detected by the wall surface detection unit 6. Two-dimensional position information was acquired from the acquired position data group of the wall surfaces 41 to 43, and the position information was illustrated in a straight line on a two-dimensional environment map.
Next, as shown in FIG. 5, the height range information acquisition unit 12 acquires position data included in the data acquisition space 46 set in the height range in the environmental data. In the present embodiment, two-dimensional position information is acquired from the position data group of the cylinder 45 included in the data acquisition space 46, and the position information is illustrated by a circle of the two-dimensional environment map. The data acquisition space 46 may have a width, and the height width can be arbitrarily determined according to the number of position data included in the environmental data.
As a result, tall fixed obstacles such as pillars can be output to the map, and more accurate matching is possible.

  If self-position estimation cannot be performed appropriately, the height range for acquiring the position data group may be changed to a lower value. This may be changed repeatedly until the self-position estimation ends normally.

  The matching unit 14 matches the reference map around the self and the data acquired by each information acquisition unit, and estimates the self-position of the moving body 100. The matching will be described with reference to the block diagram of the matching unit 14 in FIG. In the present embodiment, matching using an ICP (Interactive Closest Point) algorithm is described.

  The initial position setting unit 16 determines the initial position of the data and reference map acquired by each of the information acquisition units 10 and 12 by the self-position estimation method using dead reckoning before starting the ICP algorithm.

The corresponding wall surface detection unit 18 uses the ICP algorithm to detect the wall surface included in the position information acquired by the wall surface position information acquisition unit 10 and the height range information acquisition unit 12 corresponding to the wall surface included in the reference map. .
Processing using the ICP algorithm will be described with reference to FIG. The ICP algorithm is used to match data from two different coordinate systems. In the present embodiment, the position information included in the reference map and the wall surface position information detected by the wall surface position information acquisition unit 10 and the height range information acquisition unit 12 are processed as data of the two different coordinate systems. . Pay attention to a point (indicated by a hatched circle in FIG. 7) included in the other nearest data from a point (indicated by a white circle in FIG. 7) included in one data, The plane ID of the plane to which the point included in the data belongs is detected. The corresponding wall surface detection unit 18 corresponds to the plane having the plane ID most detected as the plane ID to which the point included in the other data nearest to all the points belonging to the plane included in one data belongs. Detect as wall surface.

The rigid body transformation parameter calculation unit 20 uses a wall surface and a reference map detected by the corresponding wall surface detection unit 18 and a column acquired by the height range information acquisition unit 12, and the rotation R and the two data match. Find the translation T.
In order to simplify this coordinate conversion, the coordinate system of one shape is fixed and the other shape is coordinate-converted. A rotation R and a translation T that minimize the Euclidean distance defined between these corresponding points are calculated for the points of the entire data.
Note that although the technique using the ICP algorithm has been introduced for the matching in this embodiment, any technique may be used as long as each data can be matched. Further, a Weiss matching method or a matching method using a particle filter may be used instead of the ICP algorithm. By using a particle filter or the like, the rotation R and translation T are calculated by skipping the processing of the corresponding wall surface detection unit 18 even when there is no wall surface in the three-dimensional environment and there are columns or steps. It is also possible to do.

FIG. 8 is a flowchart showing a self-position estimation method of mobile unit 100 according to Embodiment 1. The three-dimensional measurement step (S101) is a step in which the moving body 100 measures surrounding environmental information using an information acquisition device such as a visual sensor. In the three-dimensional environment data generation step (S102), the surrounding environment information acquired in S101 is converted into environment data including a three-dimensional position data group. In the wall surface detection step (S103), the wall surface is detected from the environment data using a plane detection method. The wall surface position information acquisition step (S104) acquires wall surface position information belonging to the wall surface detected in S103. The height range information acquisition step (S105) acquires position information from the position data group included in the height range determined by a predetermined standard in the environmental data. In the matching step (S106), the mobile object 100 estimates its own position by matching the reference map around the user with the mobile object 100 and the position information acquired in each position information acquisition step.
When the wall surface is not detected in the wall surface detection step, the process proceeds to the height range information acquisition step. If matching is not properly performed, the process proceeds to the height range information acquisition step again, the height range for acquiring the position data group is changed, position information is acquired, and matching is performed again. . The matching may be repeated until it is properly performed or until all the height range information is acquired.

  The self-position estimation method by the self-position estimation unit 8 described above may be realized by causing a computer such as a microprocessor to execute a self-position estimation program in which the processing procedure shown in the flowchart of FIG. 8 is described.

  The self-position estimation program can be stored in various types of storage media, and can be transmitted via a communication medium. Here, examples of the storage medium include a flexible disk, a hard disk, a magnetic disk, a magneto-optical disk, a CD-ROM, a DVD, a ROM cartridge, and a nonvolatile RAM cartridge. In addition, the communication medium includes a wired communication medium such as a telephone line, a wireless communication medium such as a microwave line, and the Internet.

<Embodiment 2 of the Invention>
The second embodiment will be described with reference to FIG. The self-position estimation unit 8 of the moving body 100 may further include a reliability setting unit 22. The reliability setting unit 22 sets the reliability in the position information acquired by the wall surface position information acquisition unit 10 and the height range information acquisition unit 12. As described above, since the wall surface is detected by using the plane detection method, a plane perpendicular to the floor surface can be reliably detected, but the wall surface and the columnar column that draw an arc are detected by the plane detection means. I can't. Also, pillars and steps may be moving obstacles. Therefore, more accurate self-position estimation is possible by setting the reliability of obstacles such as curved walls, columns, or steps to be low. The timing for setting the reliability may be before the position information is acquired by the wall surface position information acquisition unit 10 and the height range information acquisition unit 12, and in that case, the reliability may be set for the environmental data. The reliability may be set for the position data.

Here, in the reliability setting method, the reliability of the wall surface may be set to 1, and the reliability of the curved wall surface, the step, or the column may be set by Expression (2).
(Reliability of level difference) = (Level of level difference) / (Height of wall surface) (2)

Further, as shown in FIG. 10, the reliability setting unit 22 may convert the position information acquired by the wall surface position information acquisition unit 10 and the height range information acquisition unit 12 into two-dimensional grid data. Alternatively, the reliability of each grid may be determined according to the number of position data projected onto each grid. Even a plane having a normal vector parallel to the floor may be a step instead of a wall. In the case of a step, since the number of position data is smaller than the wall surface, the reliability of the grid is reduced. Thereby, the wall surface and the step can be distinguished.
When the visual sensor 2 is tilted when generating environmental data including a three-dimensional position data group related to an obstacle present in the moving environment, the reliability may be set low. This is because accurate matching may not be possible.

  The self-position estimation unit 8 refers to the set reliability and performs matching using position information with reliability satisfying a specific threshold. However, when the matching with the reference map included in the moving body 100 is not properly performed, the threshold value can be changed.

<Third embodiment of the invention>
Next, a matching method different from the above matching will be described. In the third embodiment, matching is performed by paying attention to the normal vector of the wall surface data in the environmental data. In addition, the reference map around the self included in the moving body 100 holds the normal vector information of the wall surface in advance.

FIG. 11 is a block diagram of the matching unit 14 according to the third embodiment.
In the third embodiment, the initial position is set by the initial position setting unit 16 by the self-position estimation method using dead reckoning, as in the first embodiment. Also, the detection of the corresponding wall surface is performed in the corresponding wall surface detection unit 18 as in the first embodiment. Therefore, the description regarding these processes common to Embodiment 1 is omitted.

The rotation R calculation unit 24 calculates the rotation R that minimizes the difference between the normal vectors of the corresponding wall surfaces detected by the corresponding wall surface detection unit 18. The wall surface detected by the wall surface detection unit 6 is detected together with the normal vector, and the wall surface position information acquired by the wall surface position information acquisition unit 10 also includes the normal vector. The rotation R can be obtained using Equation (3). normal () represents a normal vector.
R = Min (R.normal (a) -normal (A) + R.normal (b) -normal (B) +...) (3)

  The translation T calculation unit 26 rotates the position information acquired by the wall surface position information acquisition unit 10 or the position information of the wall surface included in the reference map around itself, and performs the matching process again to calculate the translation T. Rather than obtaining the rotation R and translation T using each point included in the environmental data or the acquired position information, after obtaining the rotation R using the normal vector of each wall surface, matching processing is performed again to obtain the translation T. By obtaining the above, matching in a state where the points are miscorresponding to each other can be prevented, and more accurate matching can be performed. In the second matching, an ICP algorithm, a Weiss matching method, a matching method using a particle filter, or the like can be used.

DESCRIPTION OF SYMBOLS 100 ... Autonomous moving body 2 ... Visual sensor 4 ... Three-dimensional environment data generation part 41 ... Wall surface 42 ... Wall surface 43 ... Wall surface 44 ... Step 45 ... Cylinder 46・ ・ Data acquisition space 6 ・ ・ ・ Wall surface detection unit 8 ・ ・ ・ Self-position estimation unit 10 ・ ・ ・ Wall surface position information acquisition unit 12 ・ ・ ・ Height range information acquisition unit 14 ・ ・ ・ Matching unit 16 ・ ・ ・ Initial Position setting unit 18 ... corresponding wall surface detection unit 20 ... rigid body conversion parameter calculation unit 22 ... reliability setting unit 24 ... rotation R calculation unit 26 ... translation T calculation unit

Claims (9)

A visual sensor to measure the moving environment;
Using a measurement result of the visual sensor, a three-dimensional environment data generation unit that generates environment data including a three-dimensional position data group related to an obstacle existing in the moving environment;
Using the environmental data, a wall surface detection unit that detects a flat wall surface existing in the mobile environment,
Position information of an obstacle specified by position data belonging to a first height range in the position data group included in the environment data and position information of the wall surface are fixedly present in the moving environment. A self-position estimation unit that estimates the position of the self by collating with a reference map indicating the position of the fixed obstacle;
An autonomous mobile body comprising   The autonomous mobile body according to claim 1, wherein the first height range is determined based on a height distribution of the position data group.   The autonomous mobile body according to claim 1 or 2, wherein the first height range is determined so as to include data having the highest height in the position data group.   The autonomous mobile body according to claim 2 or 3, wherein the position data group is a remaining data group obtained by removing a position data group corresponding to the wall surface from the position data group included in the environment data. A reliability setting unit for adding reliability to the position data;
The reliability setting unit sets the reliability in the position information of the wall surface rather than the reliability set in the position information of the obstacle specified by the position data belonging to the first height range in the position data group. Increase
The autonomous mobile body according to any one of claims 1 to 4, wherein the self-position estimation unit preferentially uses the highly reliable position information for collation with a reference map. The reliability setting unit converts the position information of an obstacle and the position information of the wall surface specified by position data belonging to a first height range in the position data group included in the environment data into grid data. ,
The autonomous mobile body according to claim 5, wherein a reliability is set for each grid corresponding to each of the position data based on the number of the position data.   The said self-position estimation part calculates the rotation angle for superimposing the said wall surface on arrangement | positioning of the said fixed obstacle specified by the said reference map using the normal vector of the said wall surface. The autonomous mobile body according to claim 1.   The position information of the obstacle specified by the position data belonging to the first height range among the position data group included in the environment data including the three-dimensional position data group related to the obstacle present in the moving environment; Self-position estimation in which self-position estimation is performed by collating position information of a flat wall detected using environment data with a reference map indicating the position of a fixed obstacle that is fixedly present in the moving environment. apparatus. The position information of the obstacle specified by the position data belonging to the first height range among the position data group included in the environment data including the three-dimensional position data group related to the obstacle present in the moving environment; Self-position estimation in which self-position estimation is performed by collating position information of a flat wall detected using environment data with a reference map indicating the position of a fixed obstacle that is fixedly present in the moving environment. Comprising steps,
A self-position estimation program for causing a computer to execute self-position estimation processing.

Images