JPH07200777A - Location estimating method in mobile object - Google Patents

Abstract

PURPOSE:To provide the location estimating method in a mobile object which is capable of obtaining the maximum estimation result of the estimated location of the mobile object with high accuracy based on a preliminarily imparted environmental map and the observed data by an environmental sensor. CONSTITUTION:When data where the distance up to the obstacles which are closest in each of the plural directions within environments is measured by an environmental map and an environmental sensor and area data where the configuration space in a mobile object is divided so that the correspondence relation of light ray and the obstacles may be equal within the same area is imparted, an error minimum configuration within an area where the matching error of measured data and the environmental map is minimized is calculated by each area unit and all the area error minimum configuration corresponding to the smallest value of these plural minimum configuration error values is determined (step 41 to step 60).

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a position estimation method for a mobile body, and in particular, estimates its own position based on a given environment map and measurement data obtained by the environment sensor.
The present invention relates to a position estimation method in a mobile body.

[0002]

2. Description of the Related Art Conventionally, navigation of a mobile body such as an autonomous mobile robot in a factory or a general office, an automatic mobile vacuum cleaner in a general household, an outdoor self-driving car, etc., which has a priori knowledge of an environment map, is realized. In order to do so, the basic problem is how to accurately estimate the position of the moving body from the observation data obtained by the environment sensor and the environment map given in advance. The environment of a mobile body is generally calculated by a geometrical method such as a visible graph or Voronoi diagram (for example, reference: Herbert E.
delsbrunner, “Algorithms in Combinatorial Geome
It is often described as a set of line segments in a two-dimensional plane for reasons such as ease of try "Springer-Verlag, 1987). On the other hand, an environment sensor has a plurality of rays in the radiation direction and each ray Usually, it can be modeled by a set of distance sensors that measure the distance to the nearest obstacle along the line, and the above-mentioned position estimation problem is that the error between the set of observation distances and the set of line segments of the environment map is This results in a pattern matching problem that seeks the position of the moving body and the direction of the sensor so that it becomes smaller.

[0003] With such matching, as a conventional representative method, for example, a reference: Haruo Takeda and Jean-
Claude Latombe, “Sensory Uncertainty Field f
or Mobile Robot Navigation, "IEEE Internati
onal Conference on Robotics and Automation, Nic
e, France, pp.2465-2472, 1992. This method first selects a subset of points that are considered to form a straight line from a sequence of observation points, applies a straight line to each subset using the least squares method, and further, each straight line matches the environment map without contradiction. This is a method for realizing matching by finding correspondence.

[0004]

The least squares method shown in the above prior art as a method for fitting a straight line to a point sequence is as follows.
When the error of observed data in the normal direction of a straight line follows a normal distribution, the maximum likelihood estimation line that maximizes the joint probability distribution of the observed values is given. However, in the conventional technique including the above-described two steps, the sum of squares of the vertical distance error between the estimated straight line and the observation point sequence considered in the previous step is used in the subsequent step to set a straight line and a set of environment map line segments. Could not be reflected in the matching error with. Further, it is difficult to reflect the matching error in the process of selecting a subset of points that form one straight line from the sequence of observation points. Therefore, the obtained estimated position of the moving body does not generally give the maximum likelihood estimation result, which hinders accurate navigation of the moving body. The present invention has been made in view of the above circumstances, and an object thereof is to solve the problems as described above in the related art and to obtain the maximum likelihood estimation result of the estimated position of the moving body, It is to provide a position estimation method in a mobile body.

[0005]

The above object of the present invention is to provide an environment map and data obtained by measuring distances to obstacles closest to a plurality of directions in the environment by an environment sensor.
When region data obtained by dividing the configuration space of the moving body is given so that the correspondence between the light rays and the obstacle becomes equal in the same region, the measurement data and the environment map are divided in units of each region. In the mobile object, which is characterized by calculating the minimum error configuration within the region where the matching error is the minimum, and obtaining the global minimum error configuration corresponding to the smallest error value of these multiple minimum configurations. It is achieved by the position estimation method.

[0006]

In the position estimating method for a moving body according to the present invention, when the moving body is provided with data obtained by measuring the distances to the obstacles closest to the plurality of directions in the environment by the environment sensor. , Matching with the environment map based on the measured data and the assumed configuration of the moving body in each divided area of the configuration space of the moving body so that the correspondence between the light beam and the obstacle is the same in the same area. The error is calculated, and the configuration of the moving body that minimizes the matching error described above is searched in each of the regions, and it corresponds to the smallest value among the plurality of region minimum error values thus obtained. The position of the moving body is estimated by selecting the minimum global error configuration for which the desired configuration is obtained.

[0007]

Embodiments of the present invention will now be described in detail with reference to the drawings. FIG. 1 shows an example of a moving body position estimation environment according to the present invention. In the figure, reference numeral 1 is a moving body, which is equipped with an environment sensor 2. Further, 3 to 7 are objects (obstacles) existing in the environment, and 8 is a target.
In such an environment, for example, the moving body 1 is moved to the objects 3 to 7
It is indispensable to accurately estimate the position of the moving body at each time point from the given sensor data in the case of leading to the target 8 without colliding with the vehicle and without losing sight of its own position. FIG. 2 shows a specific configuration example of the moving body 1 equipped with the sensor 2 as described above. In this example, the sensor 2 comprises a light source 2a which emits a horizontal plane ray to an object in the environment and a camera 2b for observing a line L which such a plane ray makes on the object.

In the visual field V picked up by the camera 2b,
By observing the relative value of the position of the straight line L with respect to the height H of the visual field, the absolute distance between the sensor 2 and the object can be calculated. Here, by performing such calculation for each direction discretized at a specific interval, a set of distance data in the radial direction can be obtained. As a distance sensor to the environment, a method of observing a reflected wave by ultrasonic waves, infrared rays, laser light, etc. is actually used in many cases. Such a sensor is normally arranged around the moving body with its optical axis in the radiation direction. In that case, the following description will be made on the assumption that the present invention is exactly the same as the camera sensor described above. I can continue.

FIG. 3 is an overall flowchart of a moving body position estimating method according to an embodiment of the present invention. This embodiment mainly comprises two processes. That is, steps 10 to 30 are preprocessing performed when the environment map is given, and steps 40 to 60 are position estimation processing performed when the sensor measurement data is actually given. Figure 3
In step 10, first, in step 10, the environment map is input to the information processing means of the mobile body. In this embodiment, the environment map is a set of line segments on a two-dimensional plane. For example, a list of two-dimensional coordinates of the start point and the end point of the line segment E _j = (Ex _1j ,
Ey _1j , Ex _2j , Ey _2j ), where j = 1, 2, ..., M
It is expressed as (M is the number of line segments). As a data input method, direct input by keyboard or communication, or reference to already defined data such as database,
Various forms such as input from other programs such as image recognition can be considered. In step 20, the position and direction of the discretized environment are updated.

The k-th position and direction will be written as configuration Q _k = (x _k , y _k , θ _k ) (where k = 1, 2, ..., K). Here, K is the number of discretized configurations in the environment, and the larger the discretization resolution is, the larger it is. x _k and y _k represent absolute coordinates of x and y in the environment, respectively, and θ _k represents a direction with reference to the coordinate axis fixed in the environment. In this step, for example, k is incremented by 1 with an initial value of 1. In step 21, the ray to be processed is updated and selected. In the following description, the relative direction of the i-th light ray with respect to the reference direction of the sensor is θ _i (where i = 1, 2, ..., N). In addition,
N represents the number of rays. In step 22, which line segment each ray of the sensor first intersects is calculated.

In this processing, the point (x _k , y _k ) is the starting point, and for all line segments that intersect the half line extending in the direction θ _k + t _i , the point of intersection is the most (x _k , y _k ). This can be achieved by selecting a line segment that is close. The above step 22 is repeated for all rays to find the correspondence between the rays and the line segments in the k-th configuration, and this process is repeated for all k to obtain the visible line segment for the configuration Q _k and the ray i. The function of number j, that is, j = f (k, i) can be obtained. In the final step 30 of the preprocessing, the configurations in which such correspondences are the same for all rays are connected, and the configuration space, that is, the three-dimensional space in the position direction is divided into regions.

[0012] As described in detail later, this time, QN in the vicinity of any two points Q _k on each area _{k = {k '| ‖Q k} ' -Q k ‖ ≦ ‖ (Dx, Dy, Dt )
‖, K '≠ k} (where Dx, Dy, and Dt are the resolutions of x, y, and θ, respectively, and ‖‖ is the maximum norm of the vector (‖ (x, y, z) ‖
= Max (| x |, | y |, | z |)), which can be reached by moving only to such a neighborhood) That is, each region is divided so as to be connected. Connected region labeling is a basic process of image processing, and its concrete algorithm is, for example, the document: Haruo Takeda "Image Connected Component Labeling from MR Codes" IEICE Transactions D-II, November 1993. It is described in.

Next, a method of estimating the position of the moving body by using the above-mentioned area division result when the actual sensor measurement data is given will be described. In step 40, the ray to be processed is updated and selected, and in step 41 the distance data is acquired by the information processing means. The distance data is represented by the distance r _i to the closest line segment in the direction of the i-th ray. This value includes an error. Hereinafter, the matching process between the distance data set and the line segment data will be described.
It is performed for each area previously obtained in advance. Specifically, first, in step 50, the area is updated. For this, for example, the label numbers of the selected areas may be sequentially updated in the order of the labels of the area labeling result.

Next, at step 51, as will be described in detail later, from the predetermined initial configuration, the configurations which are candidates for positioning are sequentially updated by a predetermined procedure. In step 52, the intersection of each ray and the visible line segment is found in the updated configuration in the same manner as in step 22, and the error between this and the observation data acquired in step 40 is found. The definition of the error will be described later in detail. In this way, steps 51 and 52 are repeated until the error becomes the minimum value in the area, and the position where the observation data and the environment best match is obtained in the area. This process is performed for all labeled regions, and in step 60, the smallest value among all the regions is selected, and the position data corresponding to this is selected as the global minimum value for this sensor position or moving object. It is estimated to be the position of. The above 40 to 60 steps can be repeated as many times as necessary.

The above processing will be described in more detail. FIG. 4 shows the definition of symbols necessary for the following description. In the figure, O represents the origin of the environment map, and (x, y) represents the sensor position. Also, R ₁ to R ₄ are light rays of the sensor,
Counterclockwise indexes 1-4 centered on (x, y) are attached. The number N of rays is 4 here. First, the direction of the ray R ₁ having the minimum index is set as the reference direction θ of the sensor. θ is the absolute coordinate system x, y defined in the environment
Absolute direction. The relative direction of the i-th ray R _{i with} respect to the reference ray R ₁ is t _i . Therefore, the absolute direction of the i-th ray is θ + t _i . In addition, the j-th line segment existing in the environment will be treated as a straight line (ρ _j , α _j ), unless the line segment is a problem for the convenience of the following description. Where ρ _j
Is the length of the vertical line from the origin O to the j-th line segment, and α _j is the direction of the vertical line with respect to the environment.

As is well known in elementary mathematics, line segment end points (Ex
_1j, Ey _1j) and (Ex _2j, during Ey _{2j) is, (Ey 2j -Ey 1j) sinα} j + (Ex 2j -Ex 1j) cosα j =
0 and ρ _j {(Ey _2j −Ey _1j ) cosα _j − (Ex _2j −Ex _1j ) sin
There is a relationship of α _j } = Ex _1j Ey _2j −Ex _2j Ey _1j . Next, regarding the division of the three-dimensional space of the position (x, y) and the direction θ performed in step 30,
explain. FIG. 5 shows a cut surface obtained by cutting the divided three-dimensional space at a certain plane θ = c (c is a constant). In this example, the environment is composed of five line segments E ₁ to E ₅ indicated by thick lines. The environment sensor consists of four rays R ₁ to R ₄ indicated by dashed lines. In particular, this figure shows a cross section of a space where θ = 0, that is, the direction of the light ray R ₁ which is the reference direction of the sensor coincides with the x axis.

For example, a sensor in such a posture is
When it exists at the position indicated by the circle in the figure, the ray R ₄ ~
From R ₁ , line segments E ₃ , E ₃ , E ₅ , and E ₂ become visible, respectively. This state is converted into a four-dimensional vector (3, 3, 5,
I will write 2). When the main space is divided into areas where the state vectors are the same, the area boundaries are shown by thin solid lines in the figure. The four-dimensional vector attached to each area indicates the correspondence between the ray of the sensor having the above-mentioned orientation and the line segment in the area. For example, the upper area (3, 3,
If the sensor with the above attitude is placed in (3, 2), line segments E ₃ , E ₃ , E ₃ , E ₂ are visible from the rays R ₄ to R ₁ regardless of the position in the area. . Such division can be obtained by starting a half line from the end point of each line segment in the direction in which each ray direction is inverted and extending it until it reaches any line segment.

Such division is actually performed by the above N in the discretized configuration space as described above.
It is obtained by labeling the connected region of the dimensional state vector. Further, in step 30, as a representative point of each area, the center of gravity of the connected areas having the same label is obtained. Here, x and y are average values, and θ is the direction of the sum of the unit direction vectors as the coordinates of the center of gravity. FIG. 6 shows the center of gravity of each region calculated in this way. In the example shown here, the start and end point coordinates are generated by random numbers in a quadrangle environment, an environment consisting of a total of 10 line segments is created, and the result of region division in that is shown. Each short vector in the figure indicates the position direction of the center of gravity. The vector is dense, that is, the region is finely divided at the position where the combination of visible line segments changes with respect to a small movement of the position or direction. These vectors are used as initial values in the position direction in step 51 in the area minimum value search.

Next, the method of calculating the matching error performed in step 52 will be described. The i-th ray of the sensor in the configuration (x, y, θ) is the number of the line segment seen in the area j _i , and the i-th ray when there is no sensor error.
When the sensor measurement distance of the light beam is r _0i , r _0i = | {ρ _j x cosα _j −y sinα _j } / cos (θ + t _i −α _j )} | ... (1) This is because the intersection of two straight lines x cos α + y sin α = ρ and y−b = (x−a) tan θ is x = {ρ−b sin α + a sin α tan θ} / {cos α + sin α tan θ} y = {ρ−b sin α− Since a cos α} tan θ / {cos α + sin α tan θ} + b, the distance between the point (a, b) and the intersection is | {ρ−a cos α−b sin α} / cos (θ−α) | I can lead you immediately.

Here, the sensor measurement distance of the i-th ray is r _i
The error between the measured distance and the true distance It is defined as This is the sum of squares of the difference between the measured distance and the true distance normalized by the true distance, which matches the characteristics of many sensors. By substituting (1) into the above equation, To get Given the region in step 50, j _i is uniquely determined, and α _j and ρ _j are determined from the environment map.

In step 51, the position (x, y) and the direction θ are given. Therefore, in step 52, the value of p can be calculated. FIG. 7 shows the distribution of error values. In the figure, four rays represent the true sensor configuration. Also, the broken line indicates the boundary of the region in the configuration space. The portion shown in dark gray shows a small error, and the portion shown in light gray shows a large error. The minimum value in the area of p is searched in step 51 by updating the position and direction, for example, as follows. The area centroid calculated in step 30 given the area in step 50.
(See FIG. 6) is used as an initial value to start the search. initial value
_{(x 0, y 0, θ} 0) with respect to _{p (x 0, y 0,} θ 0) with the value of the partial differential k = ∂p / ∂x at that point | x = x ₀ and h = ∂ Calculate the value of p / ∂y | y = y ₀ .

Next, at θ = θ ₀ , a two-dimensional vector
The boundaries Q _a0 and Q _b0 of the region are obtained in the (h, k) direction. This is, as shown in FIG.
The value of (x, y) when each ray is translated to the position passing through the end point of each visible line segment (in the figure, the section indicated by the pair (E _j , R _i ) of the line segment Ej and the ray Ri) Of the above), the innermost position (section hatched at the common part of each section) can be obtained. A one-dimensional minimum value search in the vector (h, k) direction having (x ₀ , y ₀ ) as an initial value is performed in the section between Q _a and Q _b obtained here.
As a minimum value search algorithm for a one-dimensional function, for example, the widely known Brent method (W. Pres, B.
Flannery, S. Teukolsky, W. Vetterling, “Numeri
cal Recipes in C ”Cambridge University Pres
s, pp.299-302, 1988).

Next, the minimum value (x ₁ , y ₁ ) obtained here is searched for the minimum value in the θ direction. First, the point (x ₁ , y ₁ )
In, the boundaries Qc and Qd of the region in the θ direction are obtained. As shown in FIG. 9, this is the value of θ when each light ray is rotationally moved to a position passing through the end point of each visible line segment in the region (in the figure, a combination of the line segment E _j and the light ray R _i ).
It can be obtained by finding the innermost position (section hatched at the common part of the above sections) among (indicated by the section on the circumference indicated by (E _j , R _i )). A one-dimensional minimum value search in the θ direction with θ ₁ = θ ₀ as the initial value is performed in the section between Qc and Qd obtained here. The same algorithm as above can be used for this one-dimensional search.

In step 51, the minimum value search in the (x, y) direction and the minimum value search in the θ direction are alternately repeated.
Find the minimum point in the region. In step 53,
In the subsequent two one-dimensional minimum value searches, if the moving distance from the initial value to the minimum value is smaller than a certain threshold value in any search, it is determined that the minimum value has been reached. FIG. 10 shows an example of the actual pattern matching process. The environment used is exactly the same as that shown in FIG. (a) is an example when the observation distance does not include any error. Q ₀ is the area centroid (initial value), Q ₁ is the first search processing result, and Q _G is the matching final result.
The state of four light rays in which the observation result is mapped to (x, y, θ) of the dimension search result is shown as the progress of matching.

It is shown that each observation point in the final result of QG completely matches with each line segment.
(b) shows an example of a matching result for observation data having an error. Data with error was generated by adding a normal random number with a standard deviation of 0.1 times the true length to the observation distance. This matching result can be said to be the optimal one that minimizes the criterion of (2). Further, (c) shows a case where there are a plurality of results, and shows that the search results performed in the two regions with respect to the two minimum points reach the same minimum value at different points. (d) shows a case where four light rays correspond to four different line segments.

According to the above-described embodiment, a point that minimizes the matching error is obtained for each configuration class unit in which the correspondence between the ray and the line segment is the same, and this is calculated for all possible ray rays. Since the minimum value over the entire area is obtained by performing the correspondence between the line segment and the line segment, it is possible to estimate the maximum likelihood of the moving body position that minimizes the joint probability distribution of the measurement error of each ray, and to estimate the moving body position with high accuracy. There is an effect that can be realized. It is needless to say that the above embodiment shows one example of the present invention, and the present invention should not be limited to this. For example, in the above-described embodiment, the divided data of the configuration space is automatically generated from the environment map. However, even if this is given from the keyboard by manual calculation from the environment map, the following position estimation processing is performed in exactly the same manner. It can be carried out.

The information processing means for performing the above processing is
It does not necessarily have to be physically mounted on the mobile unit, and may be a remote computer logically connected to the mobile unit itself by a wireless modem, for example. Further, in the above embodiment, a two-dimensional plane environment map,
Although a three-dimensional moving body configuration is assumed, it is also possible to apply this to a three-dimensional space environment map and a moving body that assumes a four-dimensional configuration or more. In this embodiment, the obstacle in the environment is limited to the line segment, but it can be extended to the obstacle having any shape. In the above embodiment, 3
The dimensional configuration space was divided by the procedure of integrating the labels of the points in the discretized space by the connected component labeling method. Conversely, this is done by issuing a dividing line from the end point of each line segment in the backward ray direction. , Can also be calculated analytically.

Further, even in a connected area, there are cases where a plurality of local minimum points are generated as shown in FIG.
It is possible to subdivide the region based on the determination result of the region shape and solve the special situation. Further, the representative point of the divided area, that is, the initial value in step 50 described above does not necessarily have to be the center of gravity of each area, and the same matching result can be obtained even if it is set to an arbitrary point in the area. be able to. Therefore, the representative point of the region is, by performing a search for a configuration capable of realizing each such state, for all correspondences between rays and line segments,
More complete area division can be performed. Further, in the present embodiment, the matching error is defined according to the equation (1). However, this is defined by the error used in the ordinary least squares method, that is, the sum of squares of the vertical distance from the line segment to the data point. Even in the same manner, the optimum matching result based on this criterion can be obtained.

Further, in the present embodiment, in the search for the minimum value, the boundary is obtained and checked in each one-dimensional search.
Depending on the setting method such as the search termination condition, it may be possible to improve the processing efficiency by obtaining the boundary in advance. Further, in the present embodiment, the regions are updated so as to search for all the regions in step 50. However, in actual navigation of the moving body, dead reckoning is performed by using past position / orientation history and control command information. In many cases, information about the position and direction can be obtained before the environmental sensor information by the method or the Kalman filter method. In such a case, it is possible to significantly reduce the calculation time by searching a region only around the specific position and direction information obtained.

[0030]

As described above in detail, according to the present invention, it is possible to realize a position estimation method in a moving body that can obtain the maximum likelihood estimation result of the estimated position of the moving body. Is played.

[Brief description of drawings]

FIG. 1 is a diagram showing an example of a moving body position estimation environment according to the present invention.

FIG. 2 is a diagram showing a specific configuration example of a moving body according to an embodiment.

FIG. 3 is an overall flowchart of a method for estimating a position of a moving body according to an embodiment.

FIG. 4 is a diagram showing definitions of symbols.

FIG. 5 is a diagram showing one cross section of three-dimensional region division.

FIG. 6 is a diagram showing an example of a region center of gravity.

FIG. 7 is a diagram showing a spatial distribution of an error function.

FIG. 8 is a diagram showing a region boundary search on an xy plane.

FIG. 9 is a diagram showing a region boundary search in the t direction.

FIG. 10 is a diagram showing an example of pattern matching.

FIG. 11 is a diagram showing an example in which a plurality of local minimum points exist.

[Explanation of symbols]

 DESCRIPTION OF SYMBOLS 1 Moving object 2 Environmental sensor 3-7 Object (obstacle) existing in the environment 8 Target 10 Environment map input means 22 Ray-line correspondence calculation means 30 Three-dimensional configuration space division means 41 Acquisition of distance data Means 52 Calculating means for matching error 60 Minimum value selecting means

─────────────────────────────────────────────────── ─── Continuation of the front page (51) Int.Cl. ⁶ Identification code Internal reference number FI Technical display location G05D 1/02 KJ G06T 7/60

Claims (3)

[Claims] 1. A position estimation method in a moving body, which estimates a position of a mobile body based on an environment map made up of a plurality of obstacles given in advance and measurement data obtained by an environment sensor using light rays. The map and the data obtained by measuring the distances to the obstacles closest to each other in a plurality of directions in the environment by the environment sensor, and the moving body so that the correspondence between the light beam and the obstacle is the same in the same region. When the region data obtained by dividing the configuration space of is given, the region error minimum configuration that minimizes the matching error between the measurement data and the environment map is calculated for each region unit, and these multiple minimum configurations are calculated. Of the minimum error value of the global error corresponding to the smallest global error configuration. Position estimation method for moving objects. 2. The environment map is defined by a two-dimensional plane, each obstacle in the environment map is represented by a line segment on the two-dimensional plane, and the configuration space is on the two-dimensional plane. Is defined in three dimensions of position and direction, the region data is a set of representative configurations of each region, and the minimum error within the region configuration is an initial value as a representative configuration of the region, and the region boundary is defined. The method for estimating a position in a moving body according to claim 1, wherein the configuration update is repeated in a direction to reduce the matching error while checking that the difference is not exceeded. 3. The method for estimating the position of a moving body according to claim 1, wherein within the same area, the area data is calculated from the environment map.