JP2839282B2 - Mobile object self-localization method - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION Object of the Invention; (Industrial application field) The present invention moves a mobile object such as a mobile robot on a predetermined route by using the next estimated point while self-localizing to find an error range. In particular, when a starting point and a destination are given, a moving route is generated from the environment model built in the moving object, and the correspondence between the input information from the visual sensor and the environment model is obtained. The position of the mobile,
The present invention relates to a self-localization method that calculates a posture and moves.

(Prior Art) A moving body that moves on the ground usually measures changes in its own moving distance and direction by detecting the rotation and steering angle of the wheels and performing cumulative calculation. This is called dead reckoning, which is fundamental for a mobile body to know its current position, but has a fatal problem that errors accumulate.

For this reason, it is necessary for the moving body to measure the environment in some way to know the current position, and to correct the measurement value by the dead reckoning. As means for this, there are a method of installing a mark and a lighthouse for a moving object on the environment (ground) side, and a method of using a feature point existing in the environment as a mark. However, in the former method, it is particularly complicated to install a mark or a lighthouse, and the latter method is desirable.

(Problems to be Solved by the Invention) In order to adopt the latter method, it is necessary to input a large amount of information to a moving body by a visual sensor or the like, process the information, and extract information necessary for position confirmation. is there. As such a self-localization method, there is “Kalman Filter Position Estimation of Mobile Robot” (Tatsuya Nakamura, Minoru Ueda) presented at the 25th Automatic Control Alliance Lecture Meeting (November 1982). Equations and observation equations are used, and these two equations are modeled, and there are too many gaps with the real ones to apply.
In addition, the 4th Robotics Society of Japan academic lecture (1986)
"A positioning problem for a monocular autonomous robot" (December 15-17) (Atsuyoshi Sugihara) focuses only on combinations,
Extraction from the image of the imaging means is unresolved, and explosion of combinations may occur.

The present invention has been made in view of the circumstances described above, and an object of the present invention is to express an environment in which a moving object moves by using a model based on computer graphics and obtain information from a sensor attached to wheels of the moving object. The position and orientation of the camera viewpoint are constrained to generate prediction information using a computer graphics method, image processing is performed using the generated prediction information, an error range is determined, and the viewpoint in the next self-localization is calculated. It is an object of the present invention to provide a method for self-localizing a moving object, wherein the method is used for restraining a moving object and an error does not accumulate even when moving on a predetermined route.

SUMMARY OF THE INVENTION (Means for Solving the Problems) The present invention relates to a method for self-localizing a moving object such as a mobile robot. An object of the present invention is to store an environment model file in which the positions of obstacles are stored in advance. Prepared, when the moving body has traveled a predetermined distance, the point is estimated by the output of the moving amount detection means provided in the moving body, an area of a certain error range is set from the estimated point, and each within the area is set. From the point, a predicted image that is to be captured by the imaging means provided on the moving body is generated from the environment model file using the computer graphics technique, and an obstacle of the environment model is generated on each generated predicted image. A marker which is a characteristic element is extracted to obtain a region where the marker is present, and the obtained region is applied to an input image from the imaging means, and the actual marker is obtained only from that region. The self-localization is performed by associating the markers of the environment model with the extracted markers, and the self-localization is evaluated in consideration of the resolution of the imaging unit and the error of marker detection to obtain an error range. This is achieved by using the error range for setting the error range at the next estimated point.

(Operation) As a function of the mobile robot, a function of identifying its own position and posture in the environment is essential. In a method of identifying a position and a posture using a sensor such as a gas rate gyro or a rotary encoder, an appropriate correction is necessary because errors accumulate. For that, input image information from the environment,
It is effective to identify the position and posture of the robot by comparing some characteristics with an environment model built in the robot. Here, as the environment in which the robot moves, (1) the environment is a known and polyhedral world,
(2) The floor is assumed to be flat, and in an artificial environment, the above assumptions (1) and (2) do not greatly limit the practicality. Under the above assumptions, in the present invention, the environment in which the robot moves is represented by a polyhedron, and markers (for example, vertical edges) are used as markers for self-localization.

According to the present invention, knowledge about the space in which the robot moves is incorporated as an environment model, and the position and orientation are calculated from three markers. The problem here is the extraction of the marker from the input image and the correspondence between the extracted marker and the marker in the environment model. In the present invention, the camera viewpoint is determined by the output of the rotary encoder attached to the robot. This is solved by constraining the position and posture of the camera within a certain range. In other words, by constraining the position and orientation of the viewpoint within a certain range, a region (hereinafter, referred to as a window) where the marker exists in the input image is generated by a computer graphics technique, and the input image is generated using this window. Is processed to extract a marker. The feature in this case is top-down image processing. By using a window, it can be assumed that one marker always exists in the window, so that image processing can be performed top-down and marker extraction becomes easy. Another feature is a simple association, and the association with the environment model is performed at the stage when the window is generated. When the windows for the respective markers do not overlap, the extracted markers uniquely correspond to the markers in the environment model. When the windows overlap, a plurality of markers in the environment model correspond to the extracted markers. Regarding a plurality of correspondence candidates for the extracted marker, erroneous correspondence is easily prevented by verification.

In addition, the self-localization is evaluated and the error range is obtained again, and is used for setting the error range area at the next estimated point.

(Embodiment) In the present invention, as shown in FIG. 1, one CCD camera (image sensor) 1 is mounted as input means of environmental information, and the amount of movement can be measured by a rotary encoder attached to wheels 2, and self-localization can be performed. Consider the mobile robot 3 that reaches the destination while repeating the movement. The mobile robot 3 inputs image information from the environment via the CCD camera 1 as shown in FIG. In this case, the moving floor is flat, and the movement is limited to the translation components in the X and Y directions and the rotation component around the Z axis as shown in FIG. The environment where the robot 3 moves is the fourth
As shown in the figure, it is a known and polyhedral world. As shown in FIG. 5, a route from the starting point SP to the destination GP is automatically generated, and the robot 3 is localized at the starting point SP. Robot 3 proceeds to the destination GP while performing a self-localization in the subgoal P ₁ ~P ₅ on the route, to generate a route so as to avoid the obstacle 4, 5 and 6.

The obstacle means an object existing in the environment.

Here, the basic concept of self-localization will be described. For simplicity, self-localization and movement are repeated as shown in FIG.
Assuming a robot that moves from the starting point S to the destination G in the dimensional space, an arbitrary point P and an arbitrary point Q during the movement are considered. First, self-localization is performed at a point P. The robot actually exists in the section Sp due to an error in self-localization. Then, the robot moves from the point P to the point Q, and the robot is likely to be present in the section Sq ′ due to the error of the self-localization at the point P and the measurement error of the moving amount obtained by the rotary encoder. . When self-localization is performed at the point Q, the robot actually exists in the section Sq due to an error in self-localization.
If the section Sq 'can be set at the point Q in this way, self-localization can be performed in the section Sq by constraint of the viewpoint (described later), and the measurement error of the rotary encoder can be cleared. Therefore, the measurement error of the movement amount does not accumulate, and the vehicle can reach the destination G with the self-localization error.

Next, the constraint of the viewpoint will be described. As shown in FIG. 3, the position and orientation of the robot 3 are represented by the translation components x, y and Z in the X and Y directions.
Expressed as (x, y, θ) with the rotational component θ around the axis,
The translation component dx, dy in the X and Y directions and the rotation component dθ about the Z axis are expressed as (dx, dy, dθ). The localization error and the measurement error for restricting the viewpoint are defined as follows. First, when the result of self-localization of the robot 3 at a certain point (x, y.θ) is (x ′, y ′, θ ′), the localization error δ of each component
x, δy, δθ are defined by the following equation (1).

Then, the robot 3 moves from a certain point by (dx, dy, dθ) and the amount of movement obtained from the rotary encoder is (dx
x ′, dy ′, dθ ′), the measurement error Δx,
Δy and Δθ are defined by the following equation (2).

As shown in FIG. 7, the robot 3 moves the point P (x _p , y _p ,
θ _p ), move by (dx, dy, dθ) after self-localization,
It is assumed that the vehicle has reached the point Q (x _q , y _q , θ _q ). The self-localization result at the point P and the movement amount (d
x ', dy', dθ '), it can be derived from equations (1) and (2) that the robot exists in the range shown in the following equation (3).

here, It is.

Here, the robot before the movement position determining result _{(x p ', y p'} , θ p ') and the moving amount (dx', dy ', dθ ') because it constantly monitors the true location (x _q , y _q , θ _q ) can be restricted by equation (3), that is, the range of the viewpoint can be predicted, and this is called constraint of the viewpoint.

Next, an algorithm for generating a window will be described.

First, the position and orientation of the viewpoint are constrained from the result of the self-localization before the movement and the measured value of the movement amount using the above equation (3).
After constraining the position and orientation of the viewpoint as shown in the area VA in FIG. 8A, the area is discretized (actually, for example, a circle VC
The square VA circumscribing the square is also obtained using θ _R , and each point in the square is discretized including the periphery. Since the number of discretized points is fixed, if the size of the square changes, the pitch of each point changes. ), A viewpoint VP is set at each of the discretized points, and a predicted image calculated as to how the environment looks at the viewpoint VP is calculated using a computer graphics technique as shown in FIG. Generated every time. In FIG. 8A, W indicates a world (absolute) coordinate system, and R indicates a robot coordinate system. Next, a window is formed by focusing on a marker (for example, a vertical edge) in the predicted image at each viewpoint VP. 'When, marker L ₁₁ in the predicted image example of FIG. 8 (B)' markers in the environment model file L, and markers on the predicted image L ... L ₁₄ '
... L _1n 'and marker _{L 21' ... L 24 '...} L 2n' and is present, the window W formed by the marker _{L 11 '... L 14' ...} L 1n '
₁ is as shown in FIG. Marker L ₂₁ '... L _24' ...
A similar window W ₂ (not shown) is formed for L _2n ′. When the windows of the markers overlap, as shown in FIG. 9, the windows are integrated and then the section is subdivided and processed. 'And windows W ₁ formed by the marker L _2' Figure 9 is a marker L ₁ and window W ₂ formed by 'the window W ₃ formed by the marker L _4' marker L ₃ are formed by Window W
_4, and the window W ₅ which is formed by the marker L ₅ 'overlap shows how to form a single window W. For the markers extracted from the window, a plurality of markers in the environment model are defined in a corresponding table as shown in FIG. 10 (A).
Correspond with S _{1 to} S ₅ . Then, window sorting is performed to determine the extraction order of the markers from the actual image. The sorting conditions are, in order of high priority, a) a marker having a size equal to or larger than a threshold, b) a marker having a large change in appearance with respect to a change in viewpoint position and posture, c) a marker having a short distance, and d) an image. Will be the marker at the end of.

Next, extraction of markers will be described. The following processing is performed on each of the generated windows until the number of extracted markers reaches a predetermined value.

First edge preserving smoothing filter to act twice by smoothing as FIG. 11 (A) for each pixel in the window W _i, Sobel conversion on individual pixels in the window W _i (edge enhancement processing ) To perform binarization from the edge strength and edge direction of the Sobel transform as shown in FIGS. In the case of the edge direction, the binarization condition is set to “1” when the edge direction is within a certain range (for example, 90 ° ± 10 °) as shown in FIG. The intensity at which the edge intensity is equal to or larger than the threshold value as in (1) is set to “1”. In this way, Figure 11 performs Hough transform as in FIG (E) for each pixel of the binarized in the window W _i as (D), FIG from the peak point PP (F) The marker M is extracted by finding the equation of a straight line on the image as shown by. Based on the association list as shown in FIG. 10A, the extracted markers are associated with the markers in the environment model as shown in FIGS.

On the other hand, the position and posture of the robot are determined from the markers A ', B', and C 'extracted from the real image shown in FIG. 12 as shown in FIG.
The angles α and β between AB and BC are calculated, and are obtained by solving the following equation (4). This will be described later.

Whether the position and orientation calculated as described above are correct is determined by creating a computer graphics image at the position and orientation and determining whether the marker on the computer graphics image matches the marker extracted from the input image. You can find out by examining it. Since the markers A ', B', and C 'used in the calculation of the position and orientation have camera parameters and errors at the time of marker extraction, the calculated position and orientation become the shaded area LA shown in FIG. . At that time, the range in which the marker D to be verified appears on the input image can be calculated. Therefore, the verification of the marker D may be performed by checking whether the marker is detected within the range. Here, the marker A ′,
Since the extraction errors of B 'and C' cannot be set in advance, the extraction errors of the markers A ', B' and C 'are expanded by one pixel until the verification of the marker D' is successful or the marker D 'has a certain size. Do. In other words, the robot existence region (error range) LA shown in FIG. 14 is expanded, and the verification range of the marker D is also expanded.

From the markers extracted in this way, the position of the robot,
The above-described verification is performed for all combinations of the three markers used in the calculation of the posture, and the position and the posture that minimize the error range are set as the results of the self-localization.

FIG. 15 shows an operation example of the present invention. The mobile robot 3 shown in FIG. 1 has a route and subgoals (P ₁ , P ₁₎ from a predetermined starting point SP to a destination GP as shown in FIG. P _2, ... according to), to move while avoiding obstacles (step S1), the subgoal (P _1, P _2, and determines at all times whether the come ...) (step S2), and when reaching subgoals (Step S3). In this stopped subgoal P _i , as shown in FIG.
_(i-1) error r _(i-1) at, theta _(i-1) to the current computer graphics images would appear from each point in the region plus error e _i of the rotary encoder according to the movement prepared by method determines a window W _i is the range of the image in the presence of markers of environmental model (step S4). next,
It is determined whether or not a marker exists only in the marker existing area obtained as described above in an actual image, and if a marker exists, it is extracted (step S5). Then, each of the extracted three or more markers is checked one by one to determine which one of the markers of the environment model corresponds to each other, and the final position and orientation are determined. At the same time, the errors r _i and θ _i at that time are stored and the next position and orientation are stored. Subgoal P
Prepare for _{(i + 1)} (step S6). Move on the route by repeating the above operation to the destination GP (step S
7).

Next, the self-localization method will be described with reference to the flowchart in FIG. That is, the details of steps S4 to S6 in FIG. 15 will be described.

First, the area of the viewpoint is defined by two errors, ie, the errors r _(i-1) and θ _{(i-1) of} the previous subgoal P _(i-1) and the error e _i of the rotary encoder of the current subgoal P _i , as shown in FIG. Determined as (step
S10) The area VA is discretized as shown in FIG. 8A to determine each viewpoint VP (step S11). Each viewpoint VP the predicted image may try a visible from the viewpoint Figure 8 for generating by a technique of the computer graphics as (B), determining the window W _i is the presence range of markers environment model (Step S12) . At this time, the posture error θ
Considering also _(i-1) , it is next determined whether or not the windows that are the marker existence ranges overlap (step S13). If the windows overlap, the windows are integrated (step S1).
4), a marker candidate list as shown in FIG. 10 is created (step S15). Next, markers are found and extracted only from the window range obtained from the actual image (input image) (step S16), and it is determined whether or not there are three or more extracted markers (step S17). In the following cases, the processing becomes impossible and the operation stops (step S18).

If there are three or more markers extracted in step S17, three markers are selected from all the extracted markers (step S20), and the three markers are selected as shown in FIG. 10 (1).
Are temporarily associated with the markers in the environmental model candidate list as shown at the top (L ₁ ′, L ₂ ′, L ₄ ′) (step S 21), and the extraction errors of the three markers are set in the area LA in FIG. (Step S22). Then, then as shown in Figure 18 to calculate the error range Q ₁ to Q ₈ of the self-localization (step S23). First
9 points Q _{0 to} Q ₈ of the error range LA in FIG. The error r _i , θ _i is obtained as follows, where 1 ≦ k ≦ 8.

Then, an area where a marker to be verified is present in the input image within the error range LA is calculated using a computer graphics technique (step S24). It is checked whether or not there is a marker to be verified in the calculated area (step S25).
It determines whether or higher) (Step S30), in some cases less than the ratio spread pixel extraction error D _e (step S3
4), D _e is returned to step S23 is smaller than a predetermined value (e.g., 3) (step S35). If the verification rate is higher than a certain ratio, the area of the error range is calculated (step S31), and it is determined whether the area is minimum (step S32). If the area is minimum, the area and error r _i , θ _i is updated (step S33). Figure No. 19 (A) is the window with D _e = 1 w
1 'shows an example of a validation failure because do not overlap, and FIG. (B) also D _e = 2 markers in the window w2 in D' marker D shows an example of a validation failure since no Omora, FIG ( C) is _De
In this example, the marker D 'overlaps the window w3 and the verification is successful. In this case, the verification rate is 100%.
That is, 0% or 100 when there is only one area to be verified
%. Then, it is determined whether or not the association with all the markers in the candidate list has been completed (step S3).
6) If it has been completed, it is determined whether all three markers have been selected or not (step S37). Step S
If all the associations have not been completed in 36, step S21
Return to

Next, the error range of the self-localization described in FIGS. 14 and 18 will be described.
The actual relationship with the image sensor 10 is as shown in Fig. 20, but since the top, bottom, left and right are reversed, the image sensor 10
Considering this, a configuration as shown in FIG. 21 is considered. Its coordinates relationship becomes as shown in FIG. 22, O _c represents an lens center coordinates. A method for obtaining the angles α and β will be described below. The input images A ', B', C 'on the image sensor 10 are associated with A, B, C of the environment model 20. Since it is known at which pixel of the image A 'the image sensor 10 appears, the 23rd
An image A 'as shown in FIG distance l ₁ between the optical axis Motomari (pixel spacing known), also the focal length f of the lens is known. Therefore, the angle α ₁ is obtained by α ₁ = tan ⁻¹ (l ₁ / f), the angle α ₂ is similarly obtained, and the angle α is finally obtained by α = α ₁ −α ₂ . Similarly, the angle β is obtained. Meanwhile, the 14th
Figure and 24 three points A, as shown in FIG, B, the center of the circle passing through O _c
When K _1, ∠RAB = 90 °, since the line segment ▲ ▼ length of is obtained as environment model 20, sinα = ▲ ▼ / 2r 1 radius r ₁ of the circle is obtained from. Since A from the environment model 20, the coordinates of B is known, if the radius r ₁ is Motomare center K ₁ coordinates (a _1,
b ₁ ) is found. Similarly, the center K _{2 of a} circle passing through three points B, C, and O _c
The coordinates (a ₂ , b ₂ ) of are also obtained. Therefore, the (4) of the point O _c from equation coordinates (x, y) is determined. This is defined as Q ₀ .

Next, we describe how to determine the orientation theta (angle between the camera optical axis and X _w). As shown in FIG. 25, the line segment Angle alpha ₁ of the optical axis has already been determined to. On the other hand, the coordinates of the point A is found from the environment model 20, since been obtained also the coordinates of the point O _c, segment The axis X each formed of _w theta 'is obtained by the calculation, the orientation theta theta =
the θ'-α _1.

Next, a case will be described in which one pixel which is a sampling error of the image sensor 10 is considered as described with reference to FIG. As shown in FIG. 26, the angle α _max when the images A ′ and B ′ are each expanded by one pixel is obtained in the same manner as the previous time.
As shown in FIG. 27, the angle α _min when each of the images A ′ and B ′ is narrowed by one pixel is obtained in the same manner as the previous time. Similarly, angles β _max and β _min are obtained. Therefore, point O
_c (Q ₀₎ Similarly coordinates of the point Q ₁ to Q ₈ and (X, Y, theta) is calculated as (5) of the formula.

In step S17, when there are four or more markers, an area where the markers other than the three used this time are present in the input image is calculated. This is from Q ₀ (X ₀ , Y ₀ , θ ₀ ) to Q ₈ (X ₈ , Y
₈ , θ ₈ ), a predicted image viewed from each point is calculated by a computer graphics method, and a window D ′ where three markers other than the three used this time are present is obtained. Then, it is checked whether or not the marker is actually present in the window D 'of the input image. If the verification rate is 80% or more, it is determined to be OK. When there are a plurality of regions, for example, when there are five regions and four or more regions have corresponding markers, respectively, 4/5 × 100
= 80% and OK. Next, calculate the shaded portion area surrounded by points Q ₁ to Q _8, a minimum if the area and the error r _i, and updates and stores the theta _i. If the verification rate is less than 80%, the second
As shown in FIG. 26, one more pixel of the image sensor is extended to obtain α _max and β _max (this is two pixels larger than α and β). Similarly, as shown in FIG. Then, α _min and β _min (that is, two pixels smaller than α and β) are obtained, and the same processing is performed. Next, the same combination is performed after changing the combination of associations. When all the combinations of associations are completed, the same processing is performed by selecting the three markers of the next combination, and eventually the area area of Q _{1 to} Q ₈ is obtained. Will be self-localized at the minimum. Error at that time
r _i and θ _i are used as a reference for the next error.

Effects of the Invention As described above, according to the present invention, image processing is performed on an area where a marker is likely to be present, so that the efficiency is high, and since the area is divided, the extraction of the marker from the image is stable. Further, since the extraction of extra lines is prevented, the efficiency of associating the extracted markers with the markers in the environment model is high. Further, since the self-localization error range is obtained again and used for setting the error range area at the next estimated point, it is possible to move on a predetermined route without accumulation of the error range.

[Brief description of the drawings]

FIG. 1 is a view showing an external configuration of a mobile robot according to the present invention, FIG. 2 is a view showing an example of an input image of an environment by an imaging means, and FIG.
FIG. 4 is a diagram for explaining a coordinate system of the robot, FIG. 4 is a diagram showing an example of an environment in which the robot moves, FIG. 5 is a diagram showing how a route is generated for the environment in FIG. 4, and FIG. FIG. 7 is a schematic diagram for explaining self-localization, FIG. 7 is a diagram for explaining constraint of a viewpoint, FIG. 8 is a diagram for explaining discretization of a viewpoint region and generation of a window, and FIG. FIG. 10 is a diagram for explaining the combination of the extracted markers, FIG. 11 is a flowchart showing the extraction of the markers from the window, and FIG. 12 is an example of the markers in the input image. Fig. 13 is a diagram for explaining the principle of calculating the position and orientation of the robot, Fig. 14 is a diagram for explaining the error of self-localization, and Fig. 15 is a diagram showing the robot movement of the present invention. The flowchart shown in Fig. 16 moves while self-stopping at the subgoal. Illustration illustrating, FIG. 17 is a flowchart showing an operation example of a self-orienting according to the invention, the 18
The figure is a diagram for explaining the error range of the robot position and posture, FIG. 19 is a diagram showing an example of verification of the position and posture,
20 to 25 are diagrams for explaining the calculation of the error range of the position and orientation, and FIGS. 26 and 27 are diagrams for explaining the change of the image input range. 1 ... CCD camera, 2 ... Wheels, 3 ... Mobile robot, 1
0 ... Image sensor, 20 ... Environment model.

──────────────────────────────────────────────────続 き Continuation of the front page (72) Inventor Kazuhiko Onishi 1-33-1 Shimoteno, Himeji-shi, Hyogo Glorry Industry Co., Ltd. (56) References JP-A-60-217413 (JP, A) (58) ) Surveyed field (Int.Cl. ⁶ , DB name) G05D 1/02 G06F 15/62 380

Claims (1)

(57) [Claims] An environment model file in which the position of an obstacle is stored in advance is prepared, and when the moving body has traveled a predetermined distance, the point is estimated by the output of a moving amount detecting means provided on the moving body, A region of an error range is set from the estimated point, and a predicted image that is to be captured by the imaging unit provided on the moving body from each point in the region is calculated from the environment model file using a computer graphics method. On each of the generated predicted images, a marker which is a feature element of an obstacle of the environment model is extracted to obtain a region where the marker is present, and the obtained region is applied to an input image from the imaging means. Then, actual markers are extracted only from that area, self-localization is performed by associating the markers of the environment model with the extracted markers, and the resolution and Taking into account the error detection again seek error range evaluated the self-localization position determining method for a mobile body, characterized in that so as to utilize the area setting error range in the next estimation point.