JPH02259913A - Self normal positioning method for mobile object - Google Patents

Abstract

PURPOSE:To prevent errors from being accumulated by expressing a moving environment with the use of a model by means of computer graphics, restricting the position and posture of a camera viewpoint based on the sensor information of the mobile object, generating predictive information, processing a picture with the use of the predictive information, and utilizing the processing for the restriction of the viewpoint in the next self-normal positioning. CONSTITUTION:A mobile robot 3 restricts the position and posture of a camera viewpoint in a certain range by the output of a rotary encoder attached to a wheel 2, generates a window where a marker exists in an input picture in the method of computer graphics, processes the input picture with the use of the window, and extracts the marker. Further the marker of the environmental model is allowed to correspond to the extracted marker, the self-normal positioning is settled, the resolution of an image pickup means 1 and the error of marker detection are taken into consideration, the self-normal positioning is evaluated, the error range is obtained again, and the corrected range is utilized for the area setting of the error range at a next presumed position. Thus the measuring errors of the amount of travel are not accumulated, and the mobile object can arrive at the target place only with the error of the self- normal positioning.

Description

[Detailed Description of the Invention] Purpose of the Invention; (Industrial Application Field) This invention enables a mobile object such as a mobile robot to self-localize and determine an error range while moving on a predetermined route using the next estimated point. Regarding the self-localization method when moving objects, especially when the starting point and destination are given, a B movement route is generated from the environmental model built in the mobile object, and the human power information from the visual sensor is correlated with the environmental model. This invention relates to a self-localization method that calculates the position and orientation of a moving object and moves it.

(Prior Art) A mobile object moving on the ground usually measures its own travel distance and changes in direction by detecting wheel rotation and steering angle and performing cumulative calculations. This is called dead reckoning, and is fundamental for a moving object to know its current location, but it has a fatal problem of accumulating errors.

For this reason, it is necessary for the moving body to measure the environment by some method to know its current position and to correct the measured value due to the dead reckoning described above. Methods for this include: ■ installing landmarks and lighthouses for moving objects on the environment (ground) side, ■
There is a method of using existing feature points in the environment as landmarks. However, the former method (2) is particularly complicated in that it requires the installation of landmarks and lighthouses, so the latter method (2) is preferable.

(Problem to be solved by the invention) In order to adopt the latter method, it is necessary to input a large amount of information to the mobile object using a visual sensor, etc., and process it to obtain the information necessary for position confirmation. need to be extracted. This self-localization method was presented at the 25th Automatic Control Association Conference (
``Position Estimation of Mobile Robots Using Kalman Filter'' (Tatsuya Nakamura, Volume 1), published in November 1982, uses a state equation and an observation equation related to deviation, and these two equations are used in the model. It has been converted into
There is a huge gap between this and reality, making it difficult to apply. Also, the 4th Academic Conference of the Robotics Society of Japan (Showa 6)
A certain positioning problem for a monocular autonomous robot (Atsukichi Sugihara) published by December 15-17, 2017 focuses only on combinations, and the extraction from the images of the imaging means is unsolved. Also, combinatorial explosions may occur.

This invention was made due to the circumstances mentioned above,
The purpose of this invention is to represent the environment in which a moving object moves using a computer graphics model, and to calculate the position of the camera viewpoint from information from sensors attached to the wheels of the moving object.
The posture is constrained and predicted information is generated using a computer graphics method, and the generated predicted information is used to perform image processing to determine the error range and used to constrain the viewpoint in the next self-localization. An object of the present invention is to provide a self-localization method for a moving object that prevents errors from accumulating even when moving along a predetermined route.

Structure of the Invention; (Means for Solving the Problems) The present invention relates to a self-localization method for a mobile object such as a mobile robot. When the moving object has traveled a predetermined distance, the point is estimated based on the output of the movement amount detection means provided on the moving object, an area within a certain error range is set from the estimated point, and each point within that area is A predicted image that would be captured by an imaging means provided on the moving body from a point is generated from the environment model file using a computer graphics method, and a range where a marker that is a characteristic element of an obstacle in the environment model exists is generated. , extracting actual markers only from the region within the determined range in the input image from the imaging means, and performing self-localization by correlating the markers of the environmental model with the extracted markers; This is achieved by evaluating the self-localization taking into account the resolution of the imaging means and the error in marker detection, recalculating the error range, and using it to set the error range at the next estimated point.

(Function) The function of a mobile robot is to identify its own position and posture in the environment. In methods of identifying position and orientation using sensors such as gas rate gyros and rotary encoders, errors accumulate and appropriate correction is required. For this purpose, it is effective to identify the robot's own position and posture by manually inputting image information from the environment and comparing certain characteristics with an environmental model built into the robot. Here, as the environment in which the robot moves, (1) the environment is known and is a polyhedral world; (2)
It is assumed that the floor surface is flat, and the above assumptions (1) and (2) do not greatly limit practicality in an artificial environment. Based on the above assumptions, in this invention, the environment in which the robot moves is expressed as a polyhedron, and markers (for example, vertical edges) are used as signs for self-orientation.

In this 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 markers from the input image and the correspondence between the extracted markers and the markers in the environment model, but in this invention, the output of the rotary encoder attached to the robot is used to This problem is solved by constraining the position and orientation of . In other words, by constraining the position and orientation of the viewpoint within a certain range, an area where the marker exists in the input image (hereinafter referred to as a window) is generated using computer graphics techniques, and this window is used to generate the input image. The marker is extracted by processing. A feature of this case is top-down image processing. By using a window, it can be assumed that one marker always exists within the window, so image processing can be performed in a top-down manner, making it easier to extract markers. Another feature is easy mapping, and the mapping with the environment model is done at the stage of window generation. If the windows for each marker do not overlap, the extracted marker and the marker in the environment model uniquely correspond. Furthermore, if the windows overlap, a plurality of markers in the environment model correspond to the extracted marker. A plurality of correspondence candidates for the extracted marker are verified to easily prevent incorrect correspondences.

In this invention, as shown in FIG.
Let us consider a mobile robot 3 that can measure quantities and reaches its destination while repeating self-orientation and movement. The mobile robot 3 manually obtains image information as shown in FIG. 2 from the environment via the CCD camera 1. In this case, the floor surface to be moved is flat, and the movement consists of only translational components in the X and Y directions and rotational components around the Z axis, as shown in FIG. The environment in which robot 3 moves is known as a polyhedral world as shown in Figure 4, and
As shown in the figure, a route from the starting point SP to the destination GP is automatically generated, and the robot 3 is oriented at the starting point SP. Robot 3 has subgoals P1 to P on the route,
Proceed to the destination GP while performing self-orientation, and generate a route while avoiding obstacles 4 and 5.6.

Note that an obstacle means an object existing in the environment.

Here, the basic concept of self-localization will be explained. For simplicity, assume a robot that moves from a starting point S to a destination G in a one-dimensional space by repeating self-orientation and movement as shown in Figure 6, and consider an arbitrary point P and an arbitrary point Q during the movement. . First, self-localization is performed at point P. Due to the self-localization error, the robot actually exists in the section Sp. Then, the robot moves from point P to point Q, but there is a high possibility that the robot exists in section Sq' due to an error in self-localization at point P and an error in measuring the amount of movement determined by the rotary encoder. When self-localizing at point Q, the robot actually exists in section Sq due to an error in self-localizing. If the section SQ' can be set at the point Q in this way, self-localization can be achieved in the section Sq by constraining the viewpoint (described later), and the measurement error of the rotary encoder can be cleared. Therefore, the measurement error of the amount of effort does not accumulate and the destination G can be reached with the error of self-orientation.

Next, to explain the constraint of the viewpoint, as shown in Fig. 3, the position and posture of the robot 3 are determined by
and rotation component θ around the Z axis, expressed as (x, y, θ),
The amount of movement is expressed as (dx, dy, de) by translational components dx and dy in the X and Y directions and rotational component dθ around the Z axis.

In order to constrain the viewpoint, localization error and measurement error are defined as follows. First, the point where robot 3 is (x, y,
If the result of self-localization is (x', y', θ'') at
) defined by the formula.

Then, from the point where robot 3 is (dx, dy, dθ
) and the amount of movement determined from the rotary encoder is (dx', dy', dθ°), then the measurement errors ΔX, Δy, and Δθ of each component are defined by the following equation (2).

Here, as shown in FIG.
Suppose that after self-localizing at θP), the robot moves by (dx, dy, dθ) and reaches point Q (Xq, 3'q, θ,). Based on the self-localization result at point P and the movement amount (dx', dy', dθ゛) found from the rotary encoder,
The robot exists within the range shown by the following equation (3),
It is derived from equations (1) and (2).

It is.

Here, the robot uses the self-localization result (Xp
', Vp', θ, °n and movement amount (dx', dy', dθ
”), the true current location (x,, 3
'(1+θq) can be restricted using equation (3), that is, the range of the viewpoint can be predicted, and this is called viewpoint constraint.

Next, the window generation algorithm will be explained.

First, the position and orientation of the viewpoint are constrained using equation (3) above from the result of self-orientation before movement and the measured value of the amount of movement. After constraining the position and orientation of the viewpoint as shown in the area VA in FIG.
A square VA circumscribing C is found using OR as well, and each point within the square is discretized including its surroundings. Since the number of points to be discretized is fixed, if the size of the square changes, the pitch of each point will change. ), a viewpoint VP is set at each discretized point, and a predicted image of how the environment will look at that viewpoint VP is calculated for each viewpoint using a computer graphics method, as shown in the same figure (B). Generated for each vp. Note that W in FIG. 8(A) indicates the world (absolute) coordinate system, and R indicates the robot coordinate system.

Next, a window is formed by focusing on markers (for example, vertical edges) in the predicted image at each viewpoint vP. If we set the marker in the environment model file and the marker on the predicted image as "," then in the predicted image example of FIG. 8(B), marker 3.'...114°...Llno and marker L21
°"'L24°"'L2n° exists, and the marker Lll
. . , and the window W formed by 4 . . . Llno is as shown in FIG. Marker L21
A similar window W2 (not shown) is formed for ゛...L24゛...L2□゛. If the windows of each marker overlap, the windows are integrated as shown in FIG. 9, and then the sections are subdivided and processed. FIG. 9 shows a window W1 formed by the marker Ll', a window W2 formed by the marker L2°, a window W formed by the marker L3°,
a window W4 formed by marker L4';
The window W5 formed by the marker L5" overlaps to form one window W. The marker extracted from the window includes multiple markers in the environment model as shown in Figure 1θ (^) Correspondence is made between sections 51 to S5 in the correspondence table as shown in FIG.

The sorting conditions are high (in descending order of priority): (a) markers whose size is larger than the threshold; (mouth) markers whose visibility changes greatly with changes in viewpoint position and posture; (viii) markers that are close in distance; ) serves as a marker at the edge of the image.

Next, explaining the extraction of markers, the following process is performed for each generated window until the number of extracted markers reaches a predetermined value.

First, as shown in FIG. 11(A), an edge preservation smoothing filter is applied twice to each pixel in the window W by smoothing, and each pixel in the window WI is subjected to 5obel conversion (edge enhancement processing). 5.
The same figure (B
) and (C). The conditions for binarization are:
In the case of the edge direction, as shown in the same figure (C), the edge direction within a certain range (for example, 90° ± 10") is set to "1," and in the case of the edge strength, as shown in the same figure (B), the error is small. The intensity that is greater than or equal to the threshold value is defined as 1''. In this way, the window W1 is binarized as shown in FIG. 11(D).
Hough as shown in the same figure (E) for each pixel in
After conversion, the marker M is extracted by determining a position equation on the image as shown in FIG. 3(F) from the peak point PP.

Based on the correspondence list shown in Figure 1θ (A), the extracted markers are added to the markers in the environment model (1) to (3) in the same figure.
Correlate as follows.

On the other hand, the position and posture of the robot are calculated from the markers A', B', and C' extracted from the real image shown in Fig. 12, as shown in Fig. 13. Next (4
) can be found by solving the equation. This will be discussed later.

To determine whether the position and orientation calculated as described above are correct, create a computer graphics image at that position and orientation, and check whether the markers on the computer graphics image match the markers extracted from the input image. You'll know if you look into it. Marker A used to calculate position and orientation
', B', and C' have errors in camera parameters and marker extraction, so the calculated position and orientation are the 14th
This becomes the shaded area LA shown in the figure. The range in which the marker D to be verified appears on the input image can then be calculated.

Therefore, marker D can be verified by checking whether the marker is detected within the range. Here, marker A'
, B', and C' cannot be set in advance, so the extraction errors of markers A', B', and C' are expanded pixel by pixel until verification of marker D° is successful or until a certain size is reached. Let's do it. In other words, the robot's existence area (error range) LA shown in FIG. 14 expands, and the marker D
The scope of verification will also expand.

The above verification is performed for all three marker combinations used to calculate the robot's position and orientation from the markers extracted in this way, and the position and orientation that minimizes the error range are taken as the self-localization results. .

(Example) FIG. 15 shows an example of the operation of the present invention, in which the mobile robot 3 shown in FIG. (PL, P2...), move while avoiding obstacles Step Sl), subgoal (Pl, P2...)
．． ...) is constantly determined (step S2), and once the subgoal is reached, the subgoal is temporarily stopped (step 53). In this stopped subgoal P, as shown in Fig. 16, the error in the previous subgoal P (r+1-11.0°-1) plus the error ei of the rotary encoder due to the current movement is calculated. An image that can be seen from each point is created using a computer graphics method, and a window W is created, which is the range on the image where markers of the environment model exist.

is determined (step S4). Next, in the actual image, it is determined whether or not a marker exists only within the marker existence area determined as described above, and if a marker exists, it is extracted (step S5), and the extracted three or more The final position and orientation are determined by sequentially checking which of the markers in the environment model the marker corresponds to, and at the same time, the error r1. θ1 is memorized and prepared for the next subgoal P (1 or 1.) (step S6). By repeating the above operation up to the destination GP, eight movements are made on the route (step S7).

Next, the self-localization method will be explained with reference to the flowchart in FIG. In other words, steps 54 to S in FIG.
6 will be explained in detail.

First, the error r(1-11 of the previous subgoal P(1-11)
+θ, t, ) and the error el of the rotary encoder of subgoal P1 this time to define the viewpoint area as VA in Figure 16.
(Step 510), the area VA is discretized as shown in FIG. 8(A), and each viewpoint vP is determined (Step 511). For each of the above-mentioned viewpoints vP, a predicted image that will be seen from that viewpoint is generated using a computer graphics method as shown in FIG. 8(B), and the window W, which is the existing range of the marker of the environment model, is determined (step 512). . Considering this sharpening posture error θth -1),
Next, it is determined whether or not the windows in the existing range of markers overlap (step 513), and if they overlap, the windows are integrated (step 514), and a marker candidate list as shown in FIG. 10 is created (step 513). 5
15). Next, from the actual image (input image), find and extract markers only from the window range obtained above (step 516), determine whether there are 3 or more extracted markers (step 517), 2 or less In this case, the process becomes impossible and stops (step 5ta).

If there are three or more markers extracted in step 517, select three markers from all the extracted markers (step 520), and select the three markers as shown in FIG. 10 (step 520).
1), the topmost part (Ll', L2°, L4°) is once associated with the marker in the candidate list of the environment model (step 521), and the extraction error of the three markers is calculated as the region L in Fig. 14.
Assume as A (step 522). Then, as shown in FIG. 18, the self-localization error range Q1 to Q8 is calculated (step 523). 9 points Q of the error range LA in FIG. −06 is expressed as , and the error “1.θ1 is determined as follows, assuming 1≦ and ≦8.

Then, the area where the marker to be verified exists in the input image within the error range LA is calculated using computer graphics techniques (step 524). Check whether the marker to be verified exists within the calculated area (
Step 525), then it is determined whether the verification rate is at least a certain percentage (for example, 80% or more) or not (Step 530), and if it is smaller than a certain percentage, the pixels of the extraction error D0 are expanded (
Steps 534), D, and h) If it is smaller than a predetermined value (for example, 3), the process returns to step S23 (step 535). In addition, if the verification rate is higher than a certain percentage, the area of the error range is calculated (step 531), and it is determined whether the area is the minimum (step 532). Update (step 533
). Figure 19 (8) shows an example in which verification fails because marker D° does not overlap window w1 at De-1, and (8) in the same figure also shows an example in which verification fails because marker D° does not overlap window w2 at D8-2. An example of verification failure is shown in (C) of the same figure.
3 shows an example in which the marker D° overlaps the window stomach 3 and the verification is successful. In this case, the verification rate is 100%. In other words, if there is only one area to verify, 0% or 1
00%. Then, in step 5, it is determined whether all the markers in the candidate list have been associated with each other.
36), if it has been completed, it is further determined whether or not all three marker selections have been completed (step 537). If all correspondences are not completed in step 53B, the process returns to step 521.

Next, to explain the self-localization error range explained in FIGS. 14 and 18, the actual relationship between the environment model 20 and the image sensor IO is as shown in FIG. Therefore, assuming that the image sensor IO is located at the opposite position 10^, consider a configuration as shown in FIG. 21. The coordinate relationship is as shown in FIG. 22, where OC indicates the lens center coordinates.

The method for determining the angles α and β will be explained below.

To the input image on the image sensor lO', B', C'
are associated with ^, B, and C of the environment model 20. Since we know which pixel of the image sensor 10 the image A is reflected in, we can find the distance C1 between the image A and the optical axis as shown in Figure 23 (pixel spacing is known), and The focal length f is known. Therefore, α,=tan-'(n
l/f), angle α1 is found, angle α2 is found in the same way, and finally angle α is found from α;α, -α2.

The angle β is found in the same way. On the other hand, Figures 14 and 24
As shown in the figure, the center of the circle passing through the three points ^, B, and Oc is K.

Then, since ZRAB=90° and the length of line segment AB can be found from the environmental model 20, the radius r of the circle can be found from sin α=A8;/2r+. Since the coordinates of one end of the environment model 20 are known, the radius rl is found, and the coordinates (a+, b+) of one at the center are found. Similarly, 3 points B, C,
The coordinates (az, b2) of 2 are also found at the center of the circle passing through Oe. Therefore, the coordinates (x, y) of the point Oc can be found from the above equation (4).

Let this be Qo.

Next, how to obtain the attitude θ (the angle between the camera optical axis and Xw) will be described. As shown in FIG. 25, while the angle α1 between the optical axis and the line segment has already been found, the coordinates of point A are known from the environment model 20, and the coordinates of point Oc have also been found. Therefore, each θ° between the line segment A"·Oc and the axis is calculated, and the attitude θ is θ°-α1.

Next, a case will be described in which one pixel, which is the sampling error of the image sensor 10, is considered as explained in FIG. 19. As shown in Figure 26, add 1 ' and B' to the image.
The angle α□ when the pixels are expanded is found in the same way as the previous time, and as shown in Figure 27, the images ^゛ and B° are each 1
The angle α1n when the pixels are narrowed is also found in the same way as the previous time. Similarly, the angle βIIIMM+β, . . . In is also determined. Therefore, similarly to point 0c (QO), points Q1 to q6
The coordinates (X, Y, θ) of are determined as in equation (5).

In step 517, if there are four or more markers, an area where markers other than the three used this time exist in the input image is calculated. This is Q. (XOYO+Qo)~
A predicted image visible from each point of Qa (Xs, Ya, θ8) is calculated using a computer graphics method, and a window D° in which markers other than the three used this time exist is determined. Then, it is checked whether a marker actually exists within the window D° of the input image, and if the verification rate is 80% or higher, it is determined to be OK. When there are multiple areas, for example, when there are five areas and four or more of the areas have corresponding markers, the value is 415 x 100-110%, which is set to 0. Next, the area of the shaded area surrounded by points Q and Q8 is calculated, and if it is the minimum, the area and errors rI and θ1 are stored and updated.

If the verification rate is less than 80%, expand the image sensor pixel by one more as shown in Figure 26.
.
III I n +βmin' (which means narrowing down by two pixels from α and β) is obtained, and the same processing is performed thereafter. Next, change the combination of correspondences and repeat the same process. Once all the combinations of correspondences have been completed, select the next combination of three markers and perform the same process. It will be self-localized based on the following. The error rl+01 at that time becomes the standard for the next error.

Effects of the Invention: As described above, according to the present invention, image processing is performed on areas where markers are likely to exist, which is efficient, and because the areas are divided, extraction of markers from images is stable.

Furthermore, since unnecessary lines are prevented from being extracted, the efficiency of associating extracted markers with markers in the environment model is high. Furthermore, since the self-localization error range is recalculated and used to set the error range at the next estimated point, the error range does not accumulate and it is possible to move along the predetermined route.

[Brief explanation of drawings]

FIG. 1 is an external configuration diagram of a mobile robot of the present invention, FIG. 2 is a diagram showing an example of an input image of the environment by an imaging means, and FIG.
The figure is a diagram for explaining the coordinate system of the robot, Figure 4 is a diagram showing an example of the environment in which the robot moves, and Figure 5 is the diagram for explaining the robot's coordinate system.
Figure 6 is a schematic diagram for explaining self-localization, Figure 7 is a diagram for explaining viewpoint constraint, and Figure 8 is a diagram for discretizing viewpoint area and Figure 9 is a diagram for explaining window generation, Figure 9 is a diagram for explaining window composition, Figure 1O is a diagram showing the correspondence of extracted markers, and Figure 11 is a diagram showing how markers are extracted from windows. 12 is a diagram showing an example of a marker in an input image, FIG. 13 is a diagram to explain the principle of calculating the robot's position and orientation, and FIG. 14 is a diagram to explain errors in self-localization. . Figure 15 is a flowchart showing how the robot moves according to the present invention.
Figure 6 is a diagram showing how the robot moves without self-stopping at a subgoal, Figure 17 is a flowchart showing an example of self-localization operation according to the present invention, and Figure 18 is a diagram for explaining the error range of robot position and posture. , FIG. 19 is a diagram showing an example of verification regarding position and orientation, and FIGS. 20 to 25 are positions,
FIGS. 26 and 27 are diagrams for explaining calculation of the posture error range, and diagrams for explaining changing the image input range. DESCRIPTION OF SYMBOLS 1... CCD camera, 2... Wheel, 3... Mobile robot, 10... Image sensor, 20... Environmental model.

Claims (1)

[Claims] 1. Prepare an environment model file in which the positions of obstacles are stored in advance, and when the moving object has progressed a predetermined distance, estimate the point based on the output of the movement amount detection means provided on the moving object, and calculate the estimated point. A region with a certain error range is set from the environment model file, and a predicted image that will be captured by the imaging means provided on the moving body is generated from each point within the region using a computer graphics method. The range in which a marker, which is a characteristic element of an obstacle in the environmental model, exists is determined, and the actual marker is extracted only from the region within the determined range in the input image from the imaging means, and the marker of the environmental model is extracted. The self-localization is performed by associating the image with the extracted marker, and the self-localization is evaluated taking into account the resolution of the imaging means and the error of marker detection, and the error range is recalculated, and the error range at the next estimated point is determined. A self-localization method for a moving object, characterized in that it is used for setting an area.