Metric Localization Using a Single Artificial Landmark for Indoor ...
2005 IEEE/RSJ International Conference on Intelligent Robots and Systems
Metric Localization Using a Single Artificial Landmark for Indoor Mobile Robots Gijeong Jang, Sungho Kim, Jeongho Kim and Inso Kweon Department of Electrical Engineering & Computer Science Korea Advanced Institute of Science and Technology (KAIST) 373-1, Guseong-dong, Yuseong-gu, Daejeon, Korea [email protected], [email protected], [email protected], [email protected]
Abstract – We present an accurate metric localization method using a simple artificial landmark for the navigation of indoor mobile robots. The proposed landmark model is designed to have a three-dimensional, multi-colored structure and the projective distortion of the structure encodes the distance and heading of the robot with respect to the landmark. Catadioptric vision is adopted for the robust and easier acquisition of the bearing measurements for the landmark. We propose a practical EKF based self-localization method that uses a single artificial landmark and runs in real time. Index Terms – metric localization, artificial landmark, catadioptric vision, EKF
I. INTRODUCTION Many vision-based self-localization methods using artificial or natural landmarks have been proposed. Natural landmarks are appropriate for both indoor and outdoor environments. They are selected considering their geometrical or photometrical features. Mobile robots analyze their characteristics and find self-pose using them [1] [2]. However, it is a formidable task to extract consistent and reliable landmarks in a complex environment. On the other hand, an artificial landmark is a very simple and powerful tool for selflocalization in indoor environments [3]. Advantages of landmarks based on specific patterns include easier detection, identification, and tracking [4]. For example, self-similar gray pattern landmarks are used as navigation and localization aids [5]. The commonly used method for computing the pose estimates of the robot is based on the bearing measurements of more than three landmarks [6]. The general approach for the method starts from detecting, identifying, and tracking multiple landmarks spread over the workspace [7]. To mitigate the difficulty of managing multiple landmarks, some approaches employ a single landmark of a plane pattern [1] [8] [9]. The projective distortion of the pattern makes it possible to calculate the relative pose of the robot from the pattern. However, the sensitivity of projective distortion relative to the change of viewing direction is severely reduced from the frontal view. For example, a change of 10D in viewing direction from the frontal view introduces a projective distortion of only 1.5%. In this paper, we explore a global and incremental localization method for a mobile robot working in an indoor
0-7803-8912-3/05/$20.00 ©2005 IEEE.
environment. We propose a simple artificial landmark model and a practical localization algorithm that uses bearing measurements of a single landmark. The proposed geometric beacon is a special type of target that can be reliably observed in successive sensor measurements and that can be accurately described in concise geometric terms. The novel 3-D structure of the pattern makes it possible to accomplish accurate single view-based localization in real time. II. LANDMARK MODEL Fig. 1 shows the appearance of the proposed landmark. The color pattern is composed of two horizontally neighboring color patches. The color pattern subtends an angle of 45 D with respect to two supporting planes. The two supporting planes intersect at the right angle and form a black line. The relative position of the line with respect to the color pattern varies as a function of camera orientation.
Fig. 1 Structure of Landmark
The brightness variation caused by the illuminant change is reduced by the use of the chromaticity color space: [r b] = [R /( R + G + B ) B /( R + G + B )]
(1)
To make each landmark distinguishable from the environment, multiple colors that are distant in the chromaticity color space are selected.
3407
III. LANDMARK DETECTION AND TRACKING The catadioptric vision sensor (omnidirectional camera) has a very wide field of view [10]. Another advantage of this camera is that it provides direct reports on horizontal bearing measurements. These facts simplify the problem of bearingonly localization. As shown in Fig. 2, if the surface of the mirror is formed by revolving a hyperbola around the Z-axis, all the rays directed to the focal point of the mirror reflect on the mirror surface and turn to the principal point of the lens. Consequently, if a robot moves on a flat plane, the patterns on the height Z invariably appear on the same radius in the omnidirectional image as shown in Fig. 3. The black circle is the horizontal line that is the projection of the horizontal plane in the omnidirectional image. As the robot begins to move and takes an image sequence, the points on the horizontal plane move only along the horizontal line.
Because of the wide field of view, the landmark can be detected in any pose of the vehicle. Here, a simple correlationbased detector is used to detect the angular range of the color pattern with the bearing of the rear vertical black line in the horizontal line. azimuth
frame Fig. 4 Omnidirectional route panorama
IV. LOCALIZATION ALGORITHM In this section, we present an algorithm that allows the robot to verify its absolute position from a view of a single landmark in one image. Based on the assumption that an indoor mobile robot navigates on a flat floor, we only seek the robot’s two-dimensional location and orientation with respect to the landmark. We assume that the origin of the reference coordinate is set as the location of the landmark in the 2-D floor map for localization of the mobile robot. We also assume that the landmark is positioned on the level of the horizontal plane of the omnidirectional mirror. In previous approaches based on a single landmark with a plane pattern, the sensitivity of projective distortion relative to the change of viewing direction is very low from the frontal view. Assuming orthographic projection [13] and a small viewing angle, Fig. 5 shows the projection model of a plane pattern.
Fig. 2 Configurations of omnidirectional camera
Fig. 3 Horizontal line for omnidirectional image
Fig. 4 is a special case of an omnidirectional spatiotemporal image, an Omnidirectional Route Panorama (ORP), which is the sequential stacking of horizontal lines taken by the robot moving on a plane [11]. A simple edge operation with nonmaxima suppression on ORP makes feature point tracking extremely easy. For example, a Canny edge operation [12] finds the edge trace. This means that the data association problem in localization can be solved simply by tracing the edge of ORP. In addition, memory can be saved for scene description and real-time edge tracking is feasible.
3408
Fig. 5 Configuration of projection of plane landmark (top view)
The relation between α and θ is l ′ = l ⋅ cosα
θ ≈ k ⋅ l ⋅ cos α
(2) (k : const )
(3)
dθ ≈ −k ⋅ l ⋅ sin α . dα
A. Initial Pose Calculation (4)
From (4), we observe that the sensitivity of observation angle θ with respect to viewing direction α becomes very small from the frontal view when α is small. Localization by observing three known points can be accomplished simply by solving trigonometric equations. However, this method is inaccurate in the degenerative position. The modified version of the method [6] solves the problem and leads to an accurate solution. However, this is also a batch method that assumes very high measurement accuracy. For this reason, it is not adequate for robot applications that require real-time performance. The proposed localization method is based on Extended Kalman Filtering (EKF), which is a compact and fast state estimation algorithm for real-time applications.
EKF requires initialization of the state. To find the initial state, we calculate the pose of the vehicle with measurements for three known points. However, the equations generated from the geometric relations are liable to fail in the degenerate configuration. For this reason, the initial pose of the robot is calculated by applying simple trigonometry and some feasible assumptions. It is relatively accurate. Moreover, EKF will improve the accuracy gradually in the next step. With the proposed configuration, the depth variation of the landmark itself is much smaller than the distance between the camera and the landmarks. In addition, the portion of the viewing angle for the landmark is small at a reasonable distance. From these observations, we assume an orthographic projection model and an orthoperspective projection model, which has the characteristic of parallel projection and parallel to projective projection, as shown in Fig. 7 [13].
Fig. 6 Geometric configuration of the vehicle-landmark system
Fig. 6 shows the geometric relation between the proposed landmark and the vehicle pose. With the help of the catadioptric vision sensor, we obtain three bearings of a point P1 , P2 , P3 . We denote the position and orientation of the vehicle by the state vector s = ( xv , y v , φ ) T
and the error covariance of the vehicle state by ⎛ Pxx ⎜ P = ⎜ Pyx ⎜ ⎝ Pφx
Pxy Pyy Pφy
Pxφ ⎞ ⎟ Pyφ ⎟ ⎟ Pφφ ⎠
Fig. 7 Geometric configuration for initial pose calculation
The vehicle location is represented in polar coordinates by the distance from the origin d and the angle α as shown in Fig. 6. As auxiliary lines, we set two parallel lines— l1 and l2 that are orthogonal to the viewing direction to the origin from the vehicle: l1 intersects the origin P2 and l2 intersects the center point of P1 and P3 . d1 is the distance between l1 and l2 , and d 2 is the distance between the vehicle and l2 . Assuming orthographic projection and a small viewing angle
The measurement vector is composed of the measured bearings of the three points in the artificial landmark z t = (θ1 , θ 2 , θ 3 )T
⎛ θ 2 − θ1 ⎞ ⎟⎟ ⎝ θ3 − θ 2 ⎠
α = tan −1 ⎜⎜
(5)
Assuming orthoperspective projection
Using this relatively simple model of the system and sequential measurements of three bearings, we estimate the pose of the robot represented by the state s and the uncertainty of the pose represented by the covariance P .
3409
d = d1 + d 2
(6)
d1 =
p (cosα + sin α ) 2
(7)
d2 =
p(cosα + sin α ) (θ 3 − θ1 )
(8)
From the observation θ 2 and the calculated value d and α , the vehicle state s = ( xv , yv ,φ )T can be found as follows: xv = d cosα
(9)
y v = d sin α
(10)
φ = π + α − θ2
(11)
B. Incremental localization Given the previously estimated pose of the robot with a large initial error covariance, EKF gradually updates the vehicle pose by alternating the two phases – motion update and measurement update.
We let the process noise covariance Q (for moving distance and rotation angle) be diagonal and constant. The magnitude is set by observing the variance of unmodeled effects such as wheel slippage. 2) Measurement Update: The measurement model for the angle measurements is the function of vehicle state and measurement noise: z t = h(s t , v t )
v t : measurement noise
(20)
The feature points in the artificial landmark are transformed from the world coordinates to the vehicle coordinates (Fig. 8). Setting a vehicle pose, the predicted measurement angle is calculated.
1) Motion Update: The motion model for a differential-drive vehicle follows a nonlinear dynamics model, which is represented by a nonlinear function: s t = f (s t −1 , u t −1 , w t −1 )
(12)
where u denotes motion control for change of steering angle and moving distance and w is the corresponding noise. We assume this noise follows a zero mean Gaussian distribution. The robot’s motion in each time step is modeled as a simple form:
xv ,t = xv ,t −1 + cos(φt −1 + (u 2,t + w2,t ) / 2) ⋅ (u1,t + w1,t )
(13)
yv ,t = yv ,t −1 + sin(φt −1 + (u 2,t + w2,t ) / 2) ⋅ (u1,t + w1,t )
(14)
φt = φt −1 + (u 2,t + w2,t )
(15)
Temporal update using EKF requires prediction of the state and the covariance. First, we project the state ahead according to the motion model assuming no additive motion error. sˆ t− = f (sˆ t −1 , u t −1 ,0)
(16)
Fig. 8 illustration for measurement model
The position of the landmark in the robot frame is y iR = R RW (y Wi − xW )
Where, xW is the vehicle position in the world frame and R RW is the rotation matrix changing the frame from world to robot. That is ⎛ x′ ⎞ ⎛ cos φ ⎜⎜ ⎟⎟ = ⎜⎜ ⎝ y ′ ⎠ ⎝ − sin φ
Pt = At Pt −1 A + Wt Qt −1Wt T t
T
θ m = tan −1 ( y ′ / x′) (17)
Because the system is nonlinear, we should linearize the system dynamics at each given step. Linearization is accomplished by calculating the Jacobian matrix for the state transition: A[i , j ] =
W[i , j ] =
∂f[i ] ∂s[ j ]
(sˆ t −1 , u t −1 ,0)
∂f[i ] ∂w [ j ]
sin φ ⎞⎛ x − xv ⎞ ⎟ ⎟⎜ cosφ ⎟⎠⎜⎝ y − yv ⎟⎠
(18)
(23)
Linearization is accomplished by calculating the Jacobian matrix for the measurement matrix: H [i , j ] =
∂h[i ] ∂s[ j ]
(sˆ t− ,0)
(24)
In the measurement update stage, we compute the Kalman gain by K t = Pt − H tT ( H t Pt − H tT + Rt ) −1
(sˆ t −1 , u t −1 ,0)
(22)
Therefore, the measurement angle is
Second, we project the error covariance ahead. −
(21)
(19)
3410
(25)
The noise covariance Rt of the measurements is determined by considering the image resolution. In (25), ( H t Pt − H tT + Rt ) means the projected uncertainty in the measurement space. The state is updated by weighting between the measured and predicted state as sˆ t = sˆ t− + K t (z t − h(sˆ t− ,0))
(26)
The Kalman gain K is set to minimize the a posteriori estimate error covariance. Finally, we update the error covariance as Pt = ( I − K t H t ) Pt −
(27)
Fig. 10 Localization result using initial pose estimation method
This alternating recursive algorithm provides the smoothing effect for process and measurement noise. It enhances state estimation accuracy and informs uncertainty of the estimated state, which is useful in mobile robot localization. V. EXPERIMENTS We tested the proposed algorithm in indoor environments with a differential-drive mobile platform of Pioneer II (Fig. 9). It carries a laptop computer with a Pentium IV processor clocked at 1.6 GHz and an omnidirectional color camera capturing 30 frames/s with a resolution of 640 × 480 pixels. The artificial landmark was installed at the corners of the indoor environment. The length of equilateral sides of the landmark were set to be 10 cm and the red-blue colored patch was attached for easier detection and tracking. The robot moved at 10 cm/s and the experiment was conducted in real time.
Fig. 9 System configuration
The experimental results of the localization are shown in Figs. 10 and 11. A red line denotes the real path. The center of the route is (2, 2) and the radius is 0.9 m. The vehicle starts at the point (2, 1.1) and moves counterclockwise.
Fig. 11 Result of incremental localization based on EKF
The result using the initial pose calculation method explained in section IV-A is shown in Fig. 10. We can observe that the result is relatively accurate, but the longer the distance between the robot and the landmark, the larger the variance of the estimates. In the experiment, the localization accuracy is mainly dependant on three factors—distance from the landmark, image resolution, and landmark size. Fig. 11 shows the incremental localization result when EKF is adopted. There is a smoothing effect on the direct estimates. The estimation error is generated from systematic errors such as measurement bias, uneven floor, and misalignment between camera and omnidirectional mirror. This error negates the assumption of Gaussian noise. A simulation was conducted to observe the performance of the proposed method where the added noise is Gaussian. The path was the same as for Fig. 10 and noise with 0.3D standard deviation was applied to the measurements. Simulation results on state estimation error for each entry— x v , y v , φ —are shown in Fig. 12. The estimate uncertainties become gradually smaller and the estimate errors exist inside the 3σ boundary of the error covariance.
3411
REFERENCES
Fig. 12 The convergence of the state estimation
The statistical report on estimation error for 200 rotations is shown in TABLE I. The mean positional error is about 7 cm and is reasonable for indoor application. TABLE I ESTIMATION ERROR FOR VEHICLE STATE x(cm)
y(cm)
Theta(degree)
Mean
4.35
4.89
0.99
Maximum
23.83
20.67
4.75
VI. CONCLUSION We proposed a simple artificial landmark model and metric localization algorithm for indoor mobile robots. The proposed algorithm based on EKF makes use of a single artificial landmark. The single landmark based algorithm makes the measurement process simple and EKF gradually enhances the estimation accuracy in real time. A geometric method is also proposed in the paper to enable mobile robots to initialize their positions autonomously and to recalculate their pose in case they get lost. The experimental results show the practicality of the method. The experimental results are quite accurate, taking into account the use of only a small single passive landmark. The most deterministic factor in the proposed method is the accuracy of the extracted bearing information for a given image resolution and given setup of artificial landmark.
[1] V. Ayala, J. B. Hayet, F. Lerasle and M. Devy, “Visual Localization of a Mobile Robot in Indoor Environments using Planar Landmarks”, Proceedings of 2000 IEEE International Conference on Intelligent Robots and Systems. [2] M. Mata, J. M. Armingol, A. de la Escalera and M. A. Salichs, “A Visual Landmark Recognition System for Topological Navigation of Mobile Robots”, Proceedings of 2001 IEEE International Conference on Robotics & Automation. [3] Gijeong Jang, Sungho Kim, Wangheon Lee and Inso Kweon, "Color Landmark Based Self-Localization for Indoor Mobile Robots", Proceedings of 2002 IEEE International Conference on Robotics & Automation. [4] Sung Joon Ahn, W. Rauh and M. Recknagel, “Circular Coded Landmark for Optical 3D-Measurement and Robot Vision”, Proceedings of the 1999 IEEE/RSJ International Conference on Intelligent Robots and Systems. [5] Amy J. Briggs, D. Scharstein, D. Braziunas, C. Dima and P. Wall, “Mobile Robot Navigation Using Self-Similar Landmarks”, Proceedings of 2000 IEEE International Conference on Robotics & Automation. [6] I. Shimshoni, “On Mobile Robot Localization from Landmark Bearings”, IEEE Transactions on Robotics and Automation, vol. 18, no. 6, December 2002. [7] Huoshehg Hu and Dongbing Gu, "Landmark-based Navigation of Industrial Mobile Robots", International Journal of Industry Robot, vol. 27, no. 6, 2000, pp 458–467. [8] J. M. Buenaposada and L. Baumela, "Real-time Tracking and Estimation of Plane Pose", Proceedings of 2002 IEEE International Conference on Pattern Recognition. [9] G. Simon, A. W. Fitzgibbon and A. Zisserman, "Markerless Tracking using Planar Structures in the Scene", Proceedings of 2000 IEEE and ACM International Symposium on Augmented Reality. [10] S. Baker and S. K. Nayar, “A Theory of Single-Viewpoint Catadioptric Image Formation”, International Journal of Computer Vision, vol. 35, no. 2, pp 1735–196, 1999. [11] Yasushi Yagi, Kousuke Imai, Masahiko Yachida, “Iconic Memory-based Omnidirectional Route Panorama Navigation”, Proceedings of 2003 IEEE International Conference on Robotics & Automation. [12] J. Canny, “A Computational Approach to Edge Detection”, IEEE Transaction on Pattern Analysis & Machine Intelligence, vol. 8, pp 679– 698, 1986. [13] Gang Xu and Zengyou Zhang, "Epipolar Geometry in Stereo, Motion and Object Recognition", Kluwer Academic Publishers.
ACKNOWLEDGEMENT This research has been supported by the Korean Ministry of Science and Technology for National Research Laboratory Program (Grant number M1-0302-00-0064).
3412
Metric Localization Using a Single Artificial Landmark for Indoor Mobile Robots Gijeong Jang, Sungho Kim, Jeongho Kim and Inso Kweon Department of Electrical Engineering & Computer Science Korea Advanced Institute of Science and Technology (KAIST) 373-1, Guseong-dong, Yuseong-gu, Daejeon, Korea [email protected], [email protected], [email protected], [email protected]
Abstract – We present an accurate metric localization method using a simple artificial landmark for the navigation of indoor mobile robots. The proposed landmark model is designed to have a three-dimensional, multi-colored structure and the projective distortion of the structure encodes the distance and heading of the robot with respect to the landmark. Catadioptric vision is adopted for the robust and easier acquisition of the bearing measurements for the landmark. We propose a practical EKF based self-localization method that uses a single artificial landmark and runs in real time. Index Terms – metric localization, artificial landmark, catadioptric vision, EKF
I. INTRODUCTION Many vision-based self-localization methods using artificial or natural landmarks have been proposed. Natural landmarks are appropriate for both indoor and outdoor environments. They are selected considering their geometrical or photometrical features. Mobile robots analyze their characteristics and find self-pose using them [1] [2]. However, it is a formidable task to extract consistent and reliable landmarks in a complex environment. On the other hand, an artificial landmark is a very simple and powerful tool for selflocalization in indoor environments [3]. Advantages of landmarks based on specific patterns include easier detection, identification, and tracking [4]. For example, self-similar gray pattern landmarks are used as navigation and localization aids [5]. The commonly used method for computing the pose estimates of the robot is based on the bearing measurements of more than three landmarks [6]. The general approach for the method starts from detecting, identifying, and tracking multiple landmarks spread over the workspace [7]. To mitigate the difficulty of managing multiple landmarks, some approaches employ a single landmark of a plane pattern [1] [8] [9]. The projective distortion of the pattern makes it possible to calculate the relative pose of the robot from the pattern. However, the sensitivity of projective distortion relative to the change of viewing direction is severely reduced from the frontal view. For example, a change of 10D in viewing direction from the frontal view introduces a projective distortion of only 1.5%. In this paper, we explore a global and incremental localization method for a mobile robot working in an indoor
0-7803-8912-3/05/$20.00 ©2005 IEEE.
environment. We propose a simple artificial landmark model and a practical localization algorithm that uses bearing measurements of a single landmark. The proposed geometric beacon is a special type of target that can be reliably observed in successive sensor measurements and that can be accurately described in concise geometric terms. The novel 3-D structure of the pattern makes it possible to accomplish accurate single view-based localization in real time. II. LANDMARK MODEL Fig. 1 shows the appearance of the proposed landmark. The color pattern is composed of two horizontally neighboring color patches. The color pattern subtends an angle of 45 D with respect to two supporting planes. The two supporting planes intersect at the right angle and form a black line. The relative position of the line with respect to the color pattern varies as a function of camera orientation.
Fig. 1 Structure of Landmark
The brightness variation caused by the illuminant change is reduced by the use of the chromaticity color space: [r b] = [R /( R + G + B ) B /( R + G + B )]
(1)
To make each landmark distinguishable from the environment, multiple colors that are distant in the chromaticity color space are selected.
3407
III. LANDMARK DETECTION AND TRACKING The catadioptric vision sensor (omnidirectional camera) has a very wide field of view [10]. Another advantage of this camera is that it provides direct reports on horizontal bearing measurements. These facts simplify the problem of bearingonly localization. As shown in Fig. 2, if the surface of the mirror is formed by revolving a hyperbola around the Z-axis, all the rays directed to the focal point of the mirror reflect on the mirror surface and turn to the principal point of the lens. Consequently, if a robot moves on a flat plane, the patterns on the height Z invariably appear on the same radius in the omnidirectional image as shown in Fig. 3. The black circle is the horizontal line that is the projection of the horizontal plane in the omnidirectional image. As the robot begins to move and takes an image sequence, the points on the horizontal plane move only along the horizontal line.
Because of the wide field of view, the landmark can be detected in any pose of the vehicle. Here, a simple correlationbased detector is used to detect the angular range of the color pattern with the bearing of the rear vertical black line in the horizontal line. azimuth
frame Fig. 4 Omnidirectional route panorama
IV. LOCALIZATION ALGORITHM In this section, we present an algorithm that allows the robot to verify its absolute position from a view of a single landmark in one image. Based on the assumption that an indoor mobile robot navigates on a flat floor, we only seek the robot’s two-dimensional location and orientation with respect to the landmark. We assume that the origin of the reference coordinate is set as the location of the landmark in the 2-D floor map for localization of the mobile robot. We also assume that the landmark is positioned on the level of the horizontal plane of the omnidirectional mirror. In previous approaches based on a single landmark with a plane pattern, the sensitivity of projective distortion relative to the change of viewing direction is very low from the frontal view. Assuming orthographic projection [13] and a small viewing angle, Fig. 5 shows the projection model of a plane pattern.
Fig. 2 Configurations of omnidirectional camera
Fig. 3 Horizontal line for omnidirectional image
Fig. 4 is a special case of an omnidirectional spatiotemporal image, an Omnidirectional Route Panorama (ORP), which is the sequential stacking of horizontal lines taken by the robot moving on a plane [11]. A simple edge operation with nonmaxima suppression on ORP makes feature point tracking extremely easy. For example, a Canny edge operation [12] finds the edge trace. This means that the data association problem in localization can be solved simply by tracing the edge of ORP. In addition, memory can be saved for scene description and real-time edge tracking is feasible.
3408
Fig. 5 Configuration of projection of plane landmark (top view)
The relation between α and θ is l ′ = l ⋅ cosα
θ ≈ k ⋅ l ⋅ cos α
(2) (k : const )
(3)
dθ ≈ −k ⋅ l ⋅ sin α . dα
A. Initial Pose Calculation (4)
From (4), we observe that the sensitivity of observation angle θ with respect to viewing direction α becomes very small from the frontal view when α is small. Localization by observing three known points can be accomplished simply by solving trigonometric equations. However, this method is inaccurate in the degenerative position. The modified version of the method [6] solves the problem and leads to an accurate solution. However, this is also a batch method that assumes very high measurement accuracy. For this reason, it is not adequate for robot applications that require real-time performance. The proposed localization method is based on Extended Kalman Filtering (EKF), which is a compact and fast state estimation algorithm for real-time applications.
EKF requires initialization of the state. To find the initial state, we calculate the pose of the vehicle with measurements for three known points. However, the equations generated from the geometric relations are liable to fail in the degenerate configuration. For this reason, the initial pose of the robot is calculated by applying simple trigonometry and some feasible assumptions. It is relatively accurate. Moreover, EKF will improve the accuracy gradually in the next step. With the proposed configuration, the depth variation of the landmark itself is much smaller than the distance between the camera and the landmarks. In addition, the portion of the viewing angle for the landmark is small at a reasonable distance. From these observations, we assume an orthographic projection model and an orthoperspective projection model, which has the characteristic of parallel projection and parallel to projective projection, as shown in Fig. 7 [13].
Fig. 6 Geometric configuration of the vehicle-landmark system
Fig. 6 shows the geometric relation between the proposed landmark and the vehicle pose. With the help of the catadioptric vision sensor, we obtain three bearings of a point P1 , P2 , P3 . We denote the position and orientation of the vehicle by the state vector s = ( xv , y v , φ ) T
and the error covariance of the vehicle state by ⎛ Pxx ⎜ P = ⎜ Pyx ⎜ ⎝ Pφx
Pxy Pyy Pφy
Pxφ ⎞ ⎟ Pyφ ⎟ ⎟ Pφφ ⎠
Fig. 7 Geometric configuration for initial pose calculation
The vehicle location is represented in polar coordinates by the distance from the origin d and the angle α as shown in Fig. 6. As auxiliary lines, we set two parallel lines— l1 and l2 that are orthogonal to the viewing direction to the origin from the vehicle: l1 intersects the origin P2 and l2 intersects the center point of P1 and P3 . d1 is the distance between l1 and l2 , and d 2 is the distance between the vehicle and l2 . Assuming orthographic projection and a small viewing angle
The measurement vector is composed of the measured bearings of the three points in the artificial landmark z t = (θ1 , θ 2 , θ 3 )T
⎛ θ 2 − θ1 ⎞ ⎟⎟ ⎝ θ3 − θ 2 ⎠
α = tan −1 ⎜⎜
(5)
Assuming orthoperspective projection
Using this relatively simple model of the system and sequential measurements of three bearings, we estimate the pose of the robot represented by the state s and the uncertainty of the pose represented by the covariance P .
3409
d = d1 + d 2
(6)
d1 =
p (cosα + sin α ) 2
(7)
d2 =
p(cosα + sin α ) (θ 3 − θ1 )
(8)
From the observation θ 2 and the calculated value d and α , the vehicle state s = ( xv , yv ,φ )T can be found as follows: xv = d cosα
(9)
y v = d sin α
(10)
φ = π + α − θ2
(11)
B. Incremental localization Given the previously estimated pose of the robot with a large initial error covariance, EKF gradually updates the vehicle pose by alternating the two phases – motion update and measurement update.
We let the process noise covariance Q (for moving distance and rotation angle) be diagonal and constant. The magnitude is set by observing the variance of unmodeled effects such as wheel slippage. 2) Measurement Update: The measurement model for the angle measurements is the function of vehicle state and measurement noise: z t = h(s t , v t )
v t : measurement noise
(20)
The feature points in the artificial landmark are transformed from the world coordinates to the vehicle coordinates (Fig. 8). Setting a vehicle pose, the predicted measurement angle is calculated.
1) Motion Update: The motion model for a differential-drive vehicle follows a nonlinear dynamics model, which is represented by a nonlinear function: s t = f (s t −1 , u t −1 , w t −1 )
(12)
where u denotes motion control for change of steering angle and moving distance and w is the corresponding noise. We assume this noise follows a zero mean Gaussian distribution. The robot’s motion in each time step is modeled as a simple form:
xv ,t = xv ,t −1 + cos(φt −1 + (u 2,t + w2,t ) / 2) ⋅ (u1,t + w1,t )
(13)
yv ,t = yv ,t −1 + sin(φt −1 + (u 2,t + w2,t ) / 2) ⋅ (u1,t + w1,t )
(14)
φt = φt −1 + (u 2,t + w2,t )
(15)
Temporal update using EKF requires prediction of the state and the covariance. First, we project the state ahead according to the motion model assuming no additive motion error. sˆ t− = f (sˆ t −1 , u t −1 ,0)
(16)
Fig. 8 illustration for measurement model
The position of the landmark in the robot frame is y iR = R RW (y Wi − xW )
Where, xW is the vehicle position in the world frame and R RW is the rotation matrix changing the frame from world to robot. That is ⎛ x′ ⎞ ⎛ cos φ ⎜⎜ ⎟⎟ = ⎜⎜ ⎝ y ′ ⎠ ⎝ − sin φ
Pt = At Pt −1 A + Wt Qt −1Wt T t
T
θ m = tan −1 ( y ′ / x′) (17)
Because the system is nonlinear, we should linearize the system dynamics at each given step. Linearization is accomplished by calculating the Jacobian matrix for the state transition: A[i , j ] =
W[i , j ] =
∂f[i ] ∂s[ j ]
(sˆ t −1 , u t −1 ,0)
∂f[i ] ∂w [ j ]
sin φ ⎞⎛ x − xv ⎞ ⎟ ⎟⎜ cosφ ⎟⎠⎜⎝ y − yv ⎟⎠
(18)
(23)
Linearization is accomplished by calculating the Jacobian matrix for the measurement matrix: H [i , j ] =
∂h[i ] ∂s[ j ]
(sˆ t− ,0)
(24)
In the measurement update stage, we compute the Kalman gain by K t = Pt − H tT ( H t Pt − H tT + Rt ) −1
(sˆ t −1 , u t −1 ,0)
(22)
Therefore, the measurement angle is
Second, we project the error covariance ahead. −
(21)
(19)
3410
(25)
The noise covariance Rt of the measurements is determined by considering the image resolution. In (25), ( H t Pt − H tT + Rt ) means the projected uncertainty in the measurement space. The state is updated by weighting between the measured and predicted state as sˆ t = sˆ t− + K t (z t − h(sˆ t− ,0))
(26)
The Kalman gain K is set to minimize the a posteriori estimate error covariance. Finally, we update the error covariance as Pt = ( I − K t H t ) Pt −
(27)
Fig. 10 Localization result using initial pose estimation method
This alternating recursive algorithm provides the smoothing effect for process and measurement noise. It enhances state estimation accuracy and informs uncertainty of the estimated state, which is useful in mobile robot localization. V. EXPERIMENTS We tested the proposed algorithm in indoor environments with a differential-drive mobile platform of Pioneer II (Fig. 9). It carries a laptop computer with a Pentium IV processor clocked at 1.6 GHz and an omnidirectional color camera capturing 30 frames/s with a resolution of 640 × 480 pixels. The artificial landmark was installed at the corners of the indoor environment. The length of equilateral sides of the landmark were set to be 10 cm and the red-blue colored patch was attached for easier detection and tracking. The robot moved at 10 cm/s and the experiment was conducted in real time.
Fig. 9 System configuration
The experimental results of the localization are shown in Figs. 10 and 11. A red line denotes the real path. The center of the route is (2, 2) and the radius is 0.9 m. The vehicle starts at the point (2, 1.1) and moves counterclockwise.
Fig. 11 Result of incremental localization based on EKF
The result using the initial pose calculation method explained in section IV-A is shown in Fig. 10. We can observe that the result is relatively accurate, but the longer the distance between the robot and the landmark, the larger the variance of the estimates. In the experiment, the localization accuracy is mainly dependant on three factors—distance from the landmark, image resolution, and landmark size. Fig. 11 shows the incremental localization result when EKF is adopted. There is a smoothing effect on the direct estimates. The estimation error is generated from systematic errors such as measurement bias, uneven floor, and misalignment between camera and omnidirectional mirror. This error negates the assumption of Gaussian noise. A simulation was conducted to observe the performance of the proposed method where the added noise is Gaussian. The path was the same as for Fig. 10 and noise with 0.3D standard deviation was applied to the measurements. Simulation results on state estimation error for each entry— x v , y v , φ —are shown in Fig. 12. The estimate uncertainties become gradually smaller and the estimate errors exist inside the 3σ boundary of the error covariance.
3411
REFERENCES
Fig. 12 The convergence of the state estimation
The statistical report on estimation error for 200 rotations is shown in TABLE I. The mean positional error is about 7 cm and is reasonable for indoor application. TABLE I ESTIMATION ERROR FOR VEHICLE STATE x(cm)
y(cm)
Theta(degree)
Mean
4.35
4.89
0.99
Maximum
23.83
20.67
4.75
VI. CONCLUSION We proposed a simple artificial landmark model and metric localization algorithm for indoor mobile robots. The proposed algorithm based on EKF makes use of a single artificial landmark. The single landmark based algorithm makes the measurement process simple and EKF gradually enhances the estimation accuracy in real time. A geometric method is also proposed in the paper to enable mobile robots to initialize their positions autonomously and to recalculate their pose in case they get lost. The experimental results show the practicality of the method. The experimental results are quite accurate, taking into account the use of only a small single passive landmark. The most deterministic factor in the proposed method is the accuracy of the extracted bearing information for a given image resolution and given setup of artificial landmark.
[1] V. Ayala, J. B. Hayet, F. Lerasle and M. Devy, “Visual Localization of a Mobile Robot in Indoor Environments using Planar Landmarks”, Proceedings of 2000 IEEE International Conference on Intelligent Robots and Systems. [2] M. Mata, J. M. Armingol, A. de la Escalera and M. A. Salichs, “A Visual Landmark Recognition System for Topological Navigation of Mobile Robots”, Proceedings of 2001 IEEE International Conference on Robotics & Automation. [3] Gijeong Jang, Sungho Kim, Wangheon Lee and Inso Kweon, "Color Landmark Based Self-Localization for Indoor Mobile Robots", Proceedings of 2002 IEEE International Conference on Robotics & Automation. [4] Sung Joon Ahn, W. Rauh and M. Recknagel, “Circular Coded Landmark for Optical 3D-Measurement and Robot Vision”, Proceedings of the 1999 IEEE/RSJ International Conference on Intelligent Robots and Systems. [5] Amy J. Briggs, D. Scharstein, D. Braziunas, C. Dima and P. Wall, “Mobile Robot Navigation Using Self-Similar Landmarks”, Proceedings of 2000 IEEE International Conference on Robotics & Automation. [6] I. Shimshoni, “On Mobile Robot Localization from Landmark Bearings”, IEEE Transactions on Robotics and Automation, vol. 18, no. 6, December 2002. [7] Huoshehg Hu and Dongbing Gu, "Landmark-based Navigation of Industrial Mobile Robots", International Journal of Industry Robot, vol. 27, no. 6, 2000, pp 458–467. [8] J. M. Buenaposada and L. Baumela, "Real-time Tracking and Estimation of Plane Pose", Proceedings of 2002 IEEE International Conference on Pattern Recognition. [9] G. Simon, A. W. Fitzgibbon and A. Zisserman, "Markerless Tracking using Planar Structures in the Scene", Proceedings of 2000 IEEE and ACM International Symposium on Augmented Reality. [10] S. Baker and S. K. Nayar, “A Theory of Single-Viewpoint Catadioptric Image Formation”, International Journal of Computer Vision, vol. 35, no. 2, pp 1735–196, 1999. [11] Yasushi Yagi, Kousuke Imai, Masahiko Yachida, “Iconic Memory-based Omnidirectional Route Panorama Navigation”, Proceedings of 2003 IEEE International Conference on Robotics & Automation. [12] J. Canny, “A Computational Approach to Edge Detection”, IEEE Transaction on Pattern Analysis & Machine Intelligence, vol. 8, pp 679– 698, 1986. [13] Gang Xu and Zengyou Zhang, "Epipolar Geometry in Stereo, Motion and Object Recognition", Kluwer Academic Publishers.
ACKNOWLEDGEMENT This research has been supported by the Korean Ministry of Science and Technology for National Research Laboratory Program (Grant number M1-0302-00-0064).
3412
