- Email: [email protected]
DYNAMIC LOCALIZATION OF A MOBILE ROBOT WITH ACTIVE BEACON SENSORS
DYNAMIC LOCALIZATION OF A MOBILE ROBOT WITH ACTIVE BEACON SENSORS SungBu Kim1, I. O. Lee2, D. I. Cho2, and JangMyung Lee1,* 1: Pusan National University, http://robotics.ee.pusan.ac.kr 2: Ninety System, Seoul, Korea
Abstract: With the development of service robots and with the emerging concept of ubiquitous world, localization of a mobile robot has become a popular issue. Even though several localization schemes are introduced previously, none of them is free from the economical constraints to be used for commercial robots. To implement a real time commercial localization system, a new absolute position estimation method for a mobile robot in indoor environment is proposed in this paper. Design and implementation of the localization system comes from the usage of active beacon systems each of which is composed of an RFID receiver and an ultra-sonic transmitter. The RFID receiver gets the synchronization signal from the mobile robot and the ultrasonic transmitter sends out a short-term signal to be used for measuring the distance from the beacon to the mobile robot. Position of a mobile robot in a three dimensional space can be calculated basically from the distance information from three beacons which have their own absolute position information. One of the interesting problems comes from the static localization scheme. That is, the collision avoidance against surrounding objects can be planned and controlled efficiently if the velocity of the moving object can be estimated. Main contribution of this paper is developing a dynamic localization scheme to be used for the moving objects, which is a new application field of active beacon sensors. In the dynamic localization scheme, sensor fusion techniques and extended Kalman filter have been used to improve the estimation accuracy. Experimental results demonstrate the effectiveness and feasibility of this scheme. Keywords: dynamic localization, active beacon system, extended Kalman Filter, mobile robot.
1. INTRODUCTION Robot positioning, or the process of a robot to find its own position in an unknown environment, is a major concern in mobile robot navigation. This process is crucial for a home or service robot because most of robot operations require precise control of the positions. As a kind of robot, a mobile robot can be applied for the various fields from home and office to the industry because they can conduct various missions with the two degrees of freedom motions. Particularly, mobile robots are essential for the tasks in dangerous and hazardous environments such as space and atomic plant. However in many tasks assigned to mobile robots, the environment is partially known or inherently uncertain, which limits the applications of mobile robot to well-structured environments. This uncertainty also arises in modeling and planning of the mobile robot itself as well as the environment. Accurate position estimation is essential for a mobile robot, especially under unknown environment. Conventional dead-reckoning method has the shortage of accumulating wheel slippage error.
Therefore this method is not suitable for the object carrying operations where a precise absolute position is required. Therefore, to prevent the error from being accumulated, many researchers have felt the necessity of the absolute position estimation. Accurate position estimation is essential for a mobile robot, especially under unknown environment. Conventional dead-reckoning method has the shortage of accumulating wheel slippage error. Therefore this method is not suitable for the object carrying operations where a precise absolute position is required. Therefore, to prevent the error from being accumulated, many researchers have felt the necessity of the absolute position estimation. Robot positioning can be classified into two fundamental methods: absolute and relative positioning schemes. Relative positioning is usually based on the dead reckoning (i.e., measuring the wheel rotation angle to compute the offset from a known starting position). Dead reckoning is simple, inexpensive, and easy to accomplish in real time. The disadvantage of dead reckoning is its unbounded accumulation of errors. Absolute positioning schemes usually rely on navigation beacons, active or passive
921
landmarks, map matching, navigation signals [1].
or
satellite
based
When a robot does not know its initial position and orientation, a cost to find autonomously the position and orientation is very high. GPS (Global Positioning System) is one of the economical solutions for this problem [2]. But GPS is inaccurate for navigating mobile robots and is not accessible in the rooms. An indoor GPS system is introduced, which consists of pseudo satellites and receivers to sense the relative position and attitude of a vehicle in a laboratory environment [3]. This system is too complex and expensive to be used for robotic applications. Instead of pseudo satellites, beacon systems provide an economical solution for indoor environment [4,5]. In this paper, as a kind of active beacon, it is proposed to use Active Beacon System (ABS) that consists of a radio frequency (RF) receiver and an ultrasonic transmitter. A mobile robot can select a specific beacon which has its own ID and position information during the navigation by sending a desired beacon code in RF. When a beacon receives its own ID from the robot, it sends out ultrasonic wave to the robot to measure the distance from the beacon to the robot based on the time of freight. Using the relative position information from the known beacons, the robot can estimate its own position. As there are so many error resources in measuring the position of a moving mobile robot, the Kalman filter is needed to obtain a more accurate estimated position of a mobile robot.
2. DISTANCE MEASUREMENT BY ULTRASONIC TRANSMITTER/RECEIVER TIMING It is possible to use different operating frequencies of ultrasonic generators in order to distinguish each signal for ultrasonic receiver modules. However in general it is not feasible since the total system cost becomes very high when each of the ultrasonic transmitter/receiver sensors has a different operating frequency. In this paper, a simple RF receiver module is added to each ultrasonic generator in order to solve the problem. That is, a simple RF receiver module is added to distinguish each ultrasonic generator. An RF transmitter module is installed on a mobile robot to invoke one of ultrasonic generators as desired. Figure 2 illustrates the localization procedure for the mobile robot. To initiate the localization process, a mobile robot transmits an RF signal to beacon #1 at time t0. When the beacon #1 receives the RF signal corresponding to its own ID, it sends out an ultrasonic signal to the mobile robot, which reaches the robot at time t1. Using the known transmission speed of the ultrasonic signal and the transmission time, t1-t0, the distance from the beacon #1 to the mobile robot can be calculated, which can be denoted as d1. While the mobile robot is moving in the room, the distance values, d1, d2, d3, and d4, are measured continuously. Notice that the localization error will be increased with the speed of the mobile robot.
Fig. 2. Ultrasonic sensor and RFID transmitter timing.
Fig. 1. Localization system flow. Figure 1 summarizes the localization process. To verify the objectives of our research experimentally, a robot positioning system has been designed, which has the following properties: (1) It is very fast and works in real time without any extra operation to scan the environment. (2) It is possible to find the heading of robot without compass or gyroscope. (3) It can be implemented economically. (4) The reflected beams of ultrasonic waves are canceled out efficiently to get more reliable data. Composition of this paper is as follows: First, section 2 explains for composition of ABS system. In section 3, it is explained about the Kalman filter algorithm for positioning estimation of a mobile robot in the system. And in section 4, computer simulations and experimental results to verify efficiency of system are shown. Finally, section 5 concludes this paper.
The localization error can be defined in terms of mobile robot and sensor parameters as follows
(x, y ) = f (t0 , t1, t2 , t3 , t4 , t5 , t6 , t7 ) v v em = f e (v ,3P, s1, , s2 , s3 )
(1) (2)
r where, s1 = t1 − t 0 , s 2 = t 3 − t 2 , s3 = t 5 − t 4 , s 4 = t 7 − t 6 , em i s position estimation error of the mobile robot, and r v (m/sec) is the speed of mobile robot. To r r reduce em , v and P should be decreased; s1 , s 2 , and s3 should be increased. To simplify and improve the filtering process, out-ofrange data are pre-filtered out based on the following heuristics: (1) When d i > d max or d i < d min , discard the d i , where d max and d min is the possible maximum and
922
minimum distances, respectively, which can be estimated for the given room. (2) The distance between two consecutive ultrasonic signal is less than d min , discard the later one that is assumed to be a reflected one.
A mobile robot state vector, r , consists of the 2 dimensional position, ( x, y ) , and a heading angle, θ . And, the state vector of a well-known point model can be defined as follows:
rk +1
⎡ xk + Tk cosθ + w1,k ⎤ ⎢ ⎥ = ⎢ y k + Tk sin θ + w2,k ⎥ ⎢⎣ θ k + Δθ k + w3,k ⎥⎦
(4)
T is a state vector, r = [x, y,θ ] T is Gaussian noise with the 0 mean wk = [w1, k , w2, k , w3, k ]
where
and Q dispersion, that is, wk ~ N (0, Q ) , and the subscript k means time-step. With this state definition, a control input is defined as uk = [Tk , Δθ k ]T . Now, the ultrasonic observation equation is defined as:
z k = h(rk , nk )
Fig. 3. Ultrasonic Receivers in a Robot.
(5)
A direction estimate like Fig. 3 in a mobile robot. Two ultrasonic receiver sensors installs in a mobile robot direction straight line position, p f and p g . And θ means a
where the measurement noise, nk , is modelled as Gaussian, that is nk ~ N (0, G ) and the function
direction angle of a mobile robot in Fig. 3 and l means with distance between the central point and ultrasonic receiver sensors of a mobile robot. The position vectors, p f = [x f , y f , z r ], p g = [x g , y g , z r ] is follow
h(rk , nk ) = {( x − xi )2 + ( y − yi )2 + ( z − zi )2}1 / 2 + nk
⎡ x f − xg y f , Pr = ⎢ ⎣ 2 ⎛ x − xg θ = cos −1 ⎜⎜ f ⎝ 2l
− yg
⎤ , zr ⎥ 2 ⎦ y − yg ⎞ ⎛ ⎟⎟ = sin −1 ⎜⎜ f ⎠ ⎝ 2l
(3) ⎞ ⎟⎟ ⎠
3.POSITION ESTIMATION USING EXTENDED KALMAN FILTER
h(rk , nk ) is described as follows:
(6)
where the subscript, i , implies an ultrasonic generator invoked at k th time-step by the mobile robot. Now, we can describe an extended Kalman filter algorithm to estimate the state vector of a mobile robot as follows: − rˆk +1 = f (rˆk , u k ,0 ) −
(7)
Pk +1 = Ak Pk A + Q t k
−
t
( (z
−
t
K k = Pk H k H k Pk H k + G − rˆk = rˆk + K k
k
(
−
− h rˆk ,0
Pk = (I − K k H k )Pk
))
)
−1
(8)
−
where rˆk − and rˆk represent state values of a mobile robot before and after correction, respectively, and K k is a Kalman filter gain. Also Pk − and Pk are error
Fig. 4. Extended Kalman filter cycle. When there are noises in a measured signal, a Kalman filter is a general tool for estimating a true value of a signal [6]. Notice that there is a nonlinear relation between the measured distance and global coordination of a mobile robot in this ABS system. Therefore, an extended Kalman filter for the nonlinear system is applied for this system to improve the estimation accuracy [7,8].
covariance matrices defined as follows: t Pk− = E[(rk − rˆk− )(rk − rˆk− ) ] Pk = E[(rk − rˆk )(rk − rˆk ) ]
(9)
t
Matrix Ak and H k are derived as a Jacobian matrix about the state vector f ( ⋅ ) and h( ⋅ ) :
r of each nonlinear function
∂f (rˆk , u k ,0) ∂r ⎡1 0 − Tk sin θ k ⎤ = ⎢⎢0 1 Tk cos θ k ⎥⎥ 1 ⎣⎢0 0 ⎦⎥
Ak =
(10)
923
∂h (rˆk ,0) ∂r ⎡ x − xi =⎢ ⎣ Di
Hk =
y − yi Di
(11) Ci ⎤ ⎥ Di ⎦
where Ci and Di are obtained as Ci = −(x − xi )l sin θ + ( y − yi )l cos θ D = {(x − x ) + ( y − y ) + (z 2
i
i
2
i
−z )}. i
1 2 2
(12)
The subscript about time, k , is omitted in each variable for the simplicity. Fig. 6. Linear incremental algorithm. 4.EXAMPLE By the real experiments, it is intended to show the possibility of dynamic localization. Assuming that the only one beacon datum is reliable, the localization scheme is derived. This actually represents the dynamic situation where multiple distance data can not be used for the localization since the each datum is obtained at a different location. For the real experiments, a mobile robot is developed using DSP2406 as shown in Fig. 5.
⎡ xn ⎤ ⎡d n cos θ n ⎤ ⎢ y ⎥ = ⎢ d sin θ ⎥ n⎦ ⎣ n⎦ ⎣ n
(13)
Differentiating both sides w.r.t time, the velocity can be represented as
⎡ x& n ⎤ ⎡cos θ n ⎢ y& ⎥ = ⎢ sin θ n ⎣ n⎦ ⎣
− d n sin θ n ⎤ ⎡d& ⎤ ⎢ ⎥ d n cos θ n ⎥⎦ ⎣θ& ⎦
(14)
By multiplying the inverse of the first matrix at the right side at both sides, we have
cos θ n ⎡d& ⎤ ⎡⎢ ⎢ & ⎥ = ⎢− 1 sin θ n ⎣θ ⎦ ⎣ d n
sin θ n ⎤ & ⎥ ⎡ xn ⎤ 1 cos θ n ⎥ ⎢⎣ y& n ⎥⎦ dn ⎦
(15)
This equation can be transformed into a discrete system equation if we replace d& as Δd / T . ⎡ cosθ n 1 ⎡ Δd ⎤ ⎢ 1 = T ⎢⎣Δθ ⎥⎦ ⎢− sin θ n d ⎣ n
Fig. 5. Experimental mobile robot. When the mobile robot is stationary and there is no obstacles blocking the ultrasonics signals between beacons and the mobile robot, the localization process can be done properly and accurately. However, in the real situations, there are several obstacles in the room, which disables the mobile robot to receive the beacon signal. And also when the mobile robot is moving fast, the sequential operation of receiving the ultrasonic signals from the beacons one by one is not fast enough to localize the mobile robot. As a main contribution of this paper, a dynamic localization scheme is proposed. In Fig. 6, the initial location of the mobile robot is represented as, P = [ xn , y n ,θ n ]t . And the velocity of the mobile robot, v, is defined as v = [ x& n , y& n ] . Note that in determining the location, the distance from the mobile robot to the beacon, dn, is known a priori. Therefore, the position of the mobile robot can be represented as
sin θ n ⎤ & ⎥ ⎡ xn ⎤ 1 cosθ n ⎥ ⎢ y& n ⎥ ⎣ ⎦ dn ⎦
(16)
where Δd = d n+1 − d n can be obtained by the distance measurement to the beacon and T represents the sampling period for the distance measurement. Plugging the known d n and θ n into Eq. (16), Δθ can be obtained. Therefore, the new location can be estimated as ⎡d n +1 ⎤ ⎡d n + Δd ⎤ ⎢θ ⎥ = ⎢θ + Δθ ⎥ ⎣ n +1 ⎦ ⎣ n ⎦
(17)
Using the equation (13), x n +1 and y n+1 are obtained. This derivation process shows that when the robot starts from an initial known position, the dynamic location—localization using only one beacon- is possible. Figure 7(a) illustrates a trajectory of the mobile robot. In the middle of the trajectory, region B, there was only one beacon was effective for the localization.
924
REFERENCES
Fig. 7 (a). Trajectory of a mobile robot.
Fig. 7 (b). Error between real value and estimate value of a mobile robot. Upper one of Fig. 7(b) shows the position estimation error without Kalman filtering. It shows that the estimation error increases rapidly with the time. However as it is shown in Fig. 7(b), by applying the Kalman filtering, the estimation error can be kept small –less than 5 mm—for more than 25 sampling periods.
J. Borenstein and L. Feng, “UMBmark – A Method for Measuring, Comparing, and Correcting Deadreckoning Errors in Mobile Robots,” The University of Michigan, 1994, Technical Report UM-MEAM-94-22. Iowa State University GPS page. Web site at http://www.cnde.iastate.edu/gps.html. Zimmerman, et al. “Experimental Development of an Indoor GPS Based sensing system for Robotic Applications,” Navigation, 1997, vol 43, No. 4, pp. 375-395. L. Kleeman, “Optimal estimation of position and heading for mobile robots using ultrasonic beacons and dead reckoning,” IEEE International conference on Robotics and Automation, May 1992, pp. 2582-2587. Jae. H. Kim and Hyung S. Cho, “Real time determination of a mobile robot`s position by linear scanning of a landmark,” Robotica, 1992, vol 10, pp. 309-319, Cambridge university press. Kyoung C. Koh, Jae S. Kim and Hyung S. Cho, “A position estimation system for mobile robots using a monocular image of a 3-D landmark,” Robotica, 1994, vol 12, pp. 431-441, Cambridge university press. T. Arai and E. Nakano, “Development of measuring equipment for location and direction (MELODI) using ultrasonic waves,” Trans. ASME, Journal of dynamic systems, Measurement and control, vol 105, pp. 152-156, 1983. Soo-Yeong Yi, Jae-Ho Jin, “Self-localization of a Mobile Robot using Global Ultrasonic Sensor System,” Journal of Control, Automation and Systems Engineering, 2003, vol. 9, no 2, pp. 145151.
5. CONCLUSION A new localization scheme for a mobile robot that is navigating indoor environments has been developed, which utilizes ultrasonic signal for distance measurement and RF signals for synchronization. The general structure of the equipment and the algorithm for calculating a position have been described and experimentally proved. Characteristics of the error sources have been closely analyzed to achieve a cheap and accurate localization system. The motion of the mobile robot, which is one of the main error sources for the localization, has been estimated and utilized to overcome the error causing properties. This enables the dynamic localization of the mobile robot, which is definitely required for the practical mobile robot whose speed becomes higher and higher. ACKNOWLEDGEMENT This work was supported in part by MIC & IITA through IT Leading R&D Support Project.
925
Abstract: With the development of service robots and with the emerging concept of ubiquitous world, localization of a mobile robot has become a popular issue. Even though several localization schemes are introduced previously, none of them is free from the economical constraints to be used for commercial robots. To implement a real time commercial localization system, a new absolute position estimation method for a mobile robot in indoor environment is proposed in this paper. Design and implementation of the localization system comes from the usage of active beacon systems each of which is composed of an RFID receiver and an ultra-sonic transmitter. The RFID receiver gets the synchronization signal from the mobile robot and the ultrasonic transmitter sends out a short-term signal to be used for measuring the distance from the beacon to the mobile robot. Position of a mobile robot in a three dimensional space can be calculated basically from the distance information from three beacons which have their own absolute position information. One of the interesting problems comes from the static localization scheme. That is, the collision avoidance against surrounding objects can be planned and controlled efficiently if the velocity of the moving object can be estimated. Main contribution of this paper is developing a dynamic localization scheme to be used for the moving objects, which is a new application field of active beacon sensors. In the dynamic localization scheme, sensor fusion techniques and extended Kalman filter have been used to improve the estimation accuracy. Experimental results demonstrate the effectiveness and feasibility of this scheme. Keywords: dynamic localization, active beacon system, extended Kalman Filter, mobile robot.
1. INTRODUCTION Robot positioning, or the process of a robot to find its own position in an unknown environment, is a major concern in mobile robot navigation. This process is crucial for a home or service robot because most of robot operations require precise control of the positions. As a kind of robot, a mobile robot can be applied for the various fields from home and office to the industry because they can conduct various missions with the two degrees of freedom motions. Particularly, mobile robots are essential for the tasks in dangerous and hazardous environments such as space and atomic plant. However in many tasks assigned to mobile robots, the environment is partially known or inherently uncertain, which limits the applications of mobile robot to well-structured environments. This uncertainty also arises in modeling and planning of the mobile robot itself as well as the environment. Accurate position estimation is essential for a mobile robot, especially under unknown environment. Conventional dead-reckoning method has the shortage of accumulating wheel slippage error.
Therefore this method is not suitable for the object carrying operations where a precise absolute position is required. Therefore, to prevent the error from being accumulated, many researchers have felt the necessity of the absolute position estimation. Accurate position estimation is essential for a mobile robot, especially under unknown environment. Conventional dead-reckoning method has the shortage of accumulating wheel slippage error. Therefore this method is not suitable for the object carrying operations where a precise absolute position is required. Therefore, to prevent the error from being accumulated, many researchers have felt the necessity of the absolute position estimation. Robot positioning can be classified into two fundamental methods: absolute and relative positioning schemes. Relative positioning is usually based on the dead reckoning (i.e., measuring the wheel rotation angle to compute the offset from a known starting position). Dead reckoning is simple, inexpensive, and easy to accomplish in real time. The disadvantage of dead reckoning is its unbounded accumulation of errors. Absolute positioning schemes usually rely on navigation beacons, active or passive
921
landmarks, map matching, navigation signals [1].
or
satellite
based
When a robot does not know its initial position and orientation, a cost to find autonomously the position and orientation is very high. GPS (Global Positioning System) is one of the economical solutions for this problem [2]. But GPS is inaccurate for navigating mobile robots and is not accessible in the rooms. An indoor GPS system is introduced, which consists of pseudo satellites and receivers to sense the relative position and attitude of a vehicle in a laboratory environment [3]. This system is too complex and expensive to be used for robotic applications. Instead of pseudo satellites, beacon systems provide an economical solution for indoor environment [4,5]. In this paper, as a kind of active beacon, it is proposed to use Active Beacon System (ABS) that consists of a radio frequency (RF) receiver and an ultrasonic transmitter. A mobile robot can select a specific beacon which has its own ID and position information during the navigation by sending a desired beacon code in RF. When a beacon receives its own ID from the robot, it sends out ultrasonic wave to the robot to measure the distance from the beacon to the robot based on the time of freight. Using the relative position information from the known beacons, the robot can estimate its own position. As there are so many error resources in measuring the position of a moving mobile robot, the Kalman filter is needed to obtain a more accurate estimated position of a mobile robot.
2. DISTANCE MEASUREMENT BY ULTRASONIC TRANSMITTER/RECEIVER TIMING It is possible to use different operating frequencies of ultrasonic generators in order to distinguish each signal for ultrasonic receiver modules. However in general it is not feasible since the total system cost becomes very high when each of the ultrasonic transmitter/receiver sensors has a different operating frequency. In this paper, a simple RF receiver module is added to each ultrasonic generator in order to solve the problem. That is, a simple RF receiver module is added to distinguish each ultrasonic generator. An RF transmitter module is installed on a mobile robot to invoke one of ultrasonic generators as desired. Figure 2 illustrates the localization procedure for the mobile robot. To initiate the localization process, a mobile robot transmits an RF signal to beacon #1 at time t0. When the beacon #1 receives the RF signal corresponding to its own ID, it sends out an ultrasonic signal to the mobile robot, which reaches the robot at time t1. Using the known transmission speed of the ultrasonic signal and the transmission time, t1-t0, the distance from the beacon #1 to the mobile robot can be calculated, which can be denoted as d1. While the mobile robot is moving in the room, the distance values, d1, d2, d3, and d4, are measured continuously. Notice that the localization error will be increased with the speed of the mobile robot.
Fig. 2. Ultrasonic sensor and RFID transmitter timing.
Fig. 1. Localization system flow. Figure 1 summarizes the localization process. To verify the objectives of our research experimentally, a robot positioning system has been designed, which has the following properties: (1) It is very fast and works in real time without any extra operation to scan the environment. (2) It is possible to find the heading of robot without compass or gyroscope. (3) It can be implemented economically. (4) The reflected beams of ultrasonic waves are canceled out efficiently to get more reliable data. Composition of this paper is as follows: First, section 2 explains for composition of ABS system. In section 3, it is explained about the Kalman filter algorithm for positioning estimation of a mobile robot in the system. And in section 4, computer simulations and experimental results to verify efficiency of system are shown. Finally, section 5 concludes this paper.
The localization error can be defined in terms of mobile robot and sensor parameters as follows
(x, y ) = f (t0 , t1, t2 , t3 , t4 , t5 , t6 , t7 ) v v em = f e (v ,3P, s1, , s2 , s3 )
(1) (2)
r where, s1 = t1 − t 0 , s 2 = t 3 − t 2 , s3 = t 5 − t 4 , s 4 = t 7 − t 6 , em i s position estimation error of the mobile robot, and r v (m/sec) is the speed of mobile robot. To r r reduce em , v and P should be decreased; s1 , s 2 , and s3 should be increased. To simplify and improve the filtering process, out-ofrange data are pre-filtered out based on the following heuristics: (1) When d i > d max or d i < d min , discard the d i , where d max and d min is the possible maximum and
922
minimum distances, respectively, which can be estimated for the given room. (2) The distance between two consecutive ultrasonic signal is less than d min , discard the later one that is assumed to be a reflected one.
A mobile robot state vector, r , consists of the 2 dimensional position, ( x, y ) , and a heading angle, θ . And, the state vector of a well-known point model can be defined as follows:
rk +1
⎡ xk + Tk cosθ + w1,k ⎤ ⎢ ⎥ = ⎢ y k + Tk sin θ + w2,k ⎥ ⎢⎣ θ k + Δθ k + w3,k ⎥⎦
(4)
T is a state vector, r = [x, y,θ ] T is Gaussian noise with the 0 mean wk = [w1, k , w2, k , w3, k ]
where
and Q dispersion, that is, wk ~ N (0, Q ) , and the subscript k means time-step. With this state definition, a control input is defined as uk = [Tk , Δθ k ]T . Now, the ultrasonic observation equation is defined as:
z k = h(rk , nk )
Fig. 3. Ultrasonic Receivers in a Robot.
(5)
A direction estimate like Fig. 3 in a mobile robot. Two ultrasonic receiver sensors installs in a mobile robot direction straight line position, p f and p g . And θ means a
where the measurement noise, nk , is modelled as Gaussian, that is nk ~ N (0, G ) and the function
direction angle of a mobile robot in Fig. 3 and l means with distance between the central point and ultrasonic receiver sensors of a mobile robot. The position vectors, p f = [x f , y f , z r ], p g = [x g , y g , z r ] is follow
h(rk , nk ) = {( x − xi )2 + ( y − yi )2 + ( z − zi )2}1 / 2 + nk
⎡ x f − xg y f , Pr = ⎢ ⎣ 2 ⎛ x − xg θ = cos −1 ⎜⎜ f ⎝ 2l
− yg
⎤ , zr ⎥ 2 ⎦ y − yg ⎞ ⎛ ⎟⎟ = sin −1 ⎜⎜ f ⎠ ⎝ 2l
(3) ⎞ ⎟⎟ ⎠
3.POSITION ESTIMATION USING EXTENDED KALMAN FILTER
h(rk , nk ) is described as follows:
(6)
where the subscript, i , implies an ultrasonic generator invoked at k th time-step by the mobile robot. Now, we can describe an extended Kalman filter algorithm to estimate the state vector of a mobile robot as follows: − rˆk +1 = f (rˆk , u k ,0 ) −
(7)
Pk +1 = Ak Pk A + Q t k
−
t
( (z
−
t
K k = Pk H k H k Pk H k + G − rˆk = rˆk + K k
k
(
−
− h rˆk ,0
Pk = (I − K k H k )Pk
))
)
−1
(8)
−
where rˆk − and rˆk represent state values of a mobile robot before and after correction, respectively, and K k is a Kalman filter gain. Also Pk − and Pk are error
Fig. 4. Extended Kalman filter cycle. When there are noises in a measured signal, a Kalman filter is a general tool for estimating a true value of a signal [6]. Notice that there is a nonlinear relation between the measured distance and global coordination of a mobile robot in this ABS system. Therefore, an extended Kalman filter for the nonlinear system is applied for this system to improve the estimation accuracy [7,8].
covariance matrices defined as follows: t Pk− = E[(rk − rˆk− )(rk − rˆk− ) ] Pk = E[(rk − rˆk )(rk − rˆk ) ]
(9)
t
Matrix Ak and H k are derived as a Jacobian matrix about the state vector f ( ⋅ ) and h( ⋅ ) :
r of each nonlinear function
∂f (rˆk , u k ,0) ∂r ⎡1 0 − Tk sin θ k ⎤ = ⎢⎢0 1 Tk cos θ k ⎥⎥ 1 ⎣⎢0 0 ⎦⎥
Ak =
(10)
923
∂h (rˆk ,0) ∂r ⎡ x − xi =⎢ ⎣ Di
Hk =
y − yi Di
(11) Ci ⎤ ⎥ Di ⎦
where Ci and Di are obtained as Ci = −(x − xi )l sin θ + ( y − yi )l cos θ D = {(x − x ) + ( y − y ) + (z 2
i
i
2
i
−z )}. i
1 2 2
(12)
The subscript about time, k , is omitted in each variable for the simplicity. Fig. 6. Linear incremental algorithm. 4.EXAMPLE By the real experiments, it is intended to show the possibility of dynamic localization. Assuming that the only one beacon datum is reliable, the localization scheme is derived. This actually represents the dynamic situation where multiple distance data can not be used for the localization since the each datum is obtained at a different location. For the real experiments, a mobile robot is developed using DSP2406 as shown in Fig. 5.
⎡ xn ⎤ ⎡d n cos θ n ⎤ ⎢ y ⎥ = ⎢ d sin θ ⎥ n⎦ ⎣ n⎦ ⎣ n
(13)
Differentiating both sides w.r.t time, the velocity can be represented as
⎡ x& n ⎤ ⎡cos θ n ⎢ y& ⎥ = ⎢ sin θ n ⎣ n⎦ ⎣
− d n sin θ n ⎤ ⎡d& ⎤ ⎢ ⎥ d n cos θ n ⎥⎦ ⎣θ& ⎦
(14)
By multiplying the inverse of the first matrix at the right side at both sides, we have
cos θ n ⎡d& ⎤ ⎡⎢ ⎢ & ⎥ = ⎢− 1 sin θ n ⎣θ ⎦ ⎣ d n
sin θ n ⎤ & ⎥ ⎡ xn ⎤ 1 cos θ n ⎥ ⎢⎣ y& n ⎥⎦ dn ⎦
(15)
This equation can be transformed into a discrete system equation if we replace d& as Δd / T . ⎡ cosθ n 1 ⎡ Δd ⎤ ⎢ 1 = T ⎢⎣Δθ ⎥⎦ ⎢− sin θ n d ⎣ n
Fig. 5. Experimental mobile robot. When the mobile robot is stationary and there is no obstacles blocking the ultrasonics signals between beacons and the mobile robot, the localization process can be done properly and accurately. However, in the real situations, there are several obstacles in the room, which disables the mobile robot to receive the beacon signal. And also when the mobile robot is moving fast, the sequential operation of receiving the ultrasonic signals from the beacons one by one is not fast enough to localize the mobile robot. As a main contribution of this paper, a dynamic localization scheme is proposed. In Fig. 6, the initial location of the mobile robot is represented as, P = [ xn , y n ,θ n ]t . And the velocity of the mobile robot, v, is defined as v = [ x& n , y& n ] . Note that in determining the location, the distance from the mobile robot to the beacon, dn, is known a priori. Therefore, the position of the mobile robot can be represented as
sin θ n ⎤ & ⎥ ⎡ xn ⎤ 1 cosθ n ⎥ ⎢ y& n ⎥ ⎣ ⎦ dn ⎦
(16)
where Δd = d n+1 − d n can be obtained by the distance measurement to the beacon and T represents the sampling period for the distance measurement. Plugging the known d n and θ n into Eq. (16), Δθ can be obtained. Therefore, the new location can be estimated as ⎡d n +1 ⎤ ⎡d n + Δd ⎤ ⎢θ ⎥ = ⎢θ + Δθ ⎥ ⎣ n +1 ⎦ ⎣ n ⎦
(17)
Using the equation (13), x n +1 and y n+1 are obtained. This derivation process shows that when the robot starts from an initial known position, the dynamic location—localization using only one beacon- is possible. Figure 7(a) illustrates a trajectory of the mobile robot. In the middle of the trajectory, region B, there was only one beacon was effective for the localization.
924
REFERENCES
Fig. 7 (a). Trajectory of a mobile robot.
Fig. 7 (b). Error between real value and estimate value of a mobile robot. Upper one of Fig. 7(b) shows the position estimation error without Kalman filtering. It shows that the estimation error increases rapidly with the time. However as it is shown in Fig. 7(b), by applying the Kalman filtering, the estimation error can be kept small –less than 5 mm—for more than 25 sampling periods.
J. Borenstein and L. Feng, “UMBmark – A Method for Measuring, Comparing, and Correcting Deadreckoning Errors in Mobile Robots,” The University of Michigan, 1994, Technical Report UM-MEAM-94-22. Iowa State University GPS page. Web site at http://www.cnde.iastate.edu/gps.html. Zimmerman, et al. “Experimental Development of an Indoor GPS Based sensing system for Robotic Applications,” Navigation, 1997, vol 43, No. 4, pp. 375-395. L. Kleeman, “Optimal estimation of position and heading for mobile robots using ultrasonic beacons and dead reckoning,” IEEE International conference on Robotics and Automation, May 1992, pp. 2582-2587. Jae. H. Kim and Hyung S. Cho, “Real time determination of a mobile robot`s position by linear scanning of a landmark,” Robotica, 1992, vol 10, pp. 309-319, Cambridge university press. Kyoung C. Koh, Jae S. Kim and Hyung S. Cho, “A position estimation system for mobile robots using a monocular image of a 3-D landmark,” Robotica, 1994, vol 12, pp. 431-441, Cambridge university press. T. Arai and E. Nakano, “Development of measuring equipment for location and direction (MELODI) using ultrasonic waves,” Trans. ASME, Journal of dynamic systems, Measurement and control, vol 105, pp. 152-156, 1983. Soo-Yeong Yi, Jae-Ho Jin, “Self-localization of a Mobile Robot using Global Ultrasonic Sensor System,” Journal of Control, Automation and Systems Engineering, 2003, vol. 9, no 2, pp. 145151.
5. CONCLUSION A new localization scheme for a mobile robot that is navigating indoor environments has been developed, which utilizes ultrasonic signal for distance measurement and RF signals for synchronization. The general structure of the equipment and the algorithm for calculating a position have been described and experimentally proved. Characteristics of the error sources have been closely analyzed to achieve a cheap and accurate localization system. The motion of the mobile robot, which is one of the main error sources for the localization, has been estimated and utilized to overcome the error causing properties. This enables the dynamic localization of the mobile robot, which is definitely required for the practical mobile robot whose speed becomes higher and higher. ACKNOWLEDGEMENT This work was supported in part by MIC & IITA through IT Leading R&D Support Project.
925