Wireless Localization for Mobile Robots via Particle Filter and Sensor Fusion
2. RSSI M ODELING
2.1.TRILATERATION LOCALIZATION
The mobile robot used in this study was mounted on a mesh-based topology for the 802.15.4 ZigBee sensor network. A built-in received signal strength indicator was adopted to estimate the location of the mobile robot by applying the trilateration algorithm. The trilateration algorithm in general requires three reference nodes; however, the undesirable estimation results in the present implementation can be filtered by applying the logic judgment.
2 12th Mechatronics Forum Biennial International Conference
Therefore, only two reference nodes were required for localization as shown in Figure 1. The ZigBee modules used in this research were SmartRF04 and CC2430DB as shown in Figures 2 and 3, respectively. The SmartRF04 was used as a reference node, and CC2430DB was mounted on the mobile robot as a separate moving node.
Figure 1: Trilateration localization
Figure 2: SmartRF04 Figure 3: CC2430DB
2.2.THE ANTENNA ORIENTATION EFFECT
The SmartRF04 presents varied RSSI with different antenna orientations, and corresponding variation is even worse when CC2430DB is used. An obstacle-free indoor environment was set up to verify the antenna orientation effect, and the SmartRF04 was used as a receiver. The CC2430DB was used as a sender with a distance of 1 m between the two nodes. The signal strength was then measured with the changing antenna orientations from 0, 90, 180 and 270 degrees, as shown in Figure 4. The averaged results from 1000 trials in each direction were recorded and their histogram results are shown in Figure 5.
Figure 4: Different antenna orientations of SmartRF04
-49 -48 -47 -46 -45 -44 -43 -42 -41
Figure 5: Histogram on four orientations
RSSI, theoretically, should be -45 dbm at a distance of 1 m. The orientations both in 0 and 270 degrees are similar to the theoretical values, but those in 90 and 180 degrees present a relatively large error (Figure 5). The SmartRF04, with the same setup environment, also then becomes a sender, and CC2430DB becomes a receiver in test. Measurements generated from 1000 trials were also recorded and results indicate that the antenna orientation seriously affects RSSI.
2.3.SIGNAL FADING MODEL
RSSI is defined as a function of the transmitted power and the distance, between the sender and the receiver [14]. The received signal strength decreases with increasing distance as expressed in the following formula:
)
Mechatronics 2010, June 28 – 30, Swiss Federal Institute of Technology ETH, Zurich Switzerland 3
where n is signal propagation constant, also named the propagation exponent; D is the distance from the sender;
and A is the received signal strength at a distance of 1 m.
Experiments to estimate RSSI were conducted in an indoor obstacle-free environment. Both the SmartRF04 and CC2430DB sent a test package to record RSSI 10000 times with a fixed antenna orientation. The histogram results shown in Figure 5 indicate that almost all estimated RSSI are close to -45dbm as the distance approaches 1 m. The parameter A was then determined as -45, and the parameter n was determined by the averaged RSSI at different distances. Results shown in Figure 6 indicate that with the model n=3.5, the experimental results are consistent with the theoretical results.
0 5 10 15 20 25 30
3.1.PRINCIPLE OF THE PARTICLE FILTER
Both the information on electronic compass and the current state of the robot are also included with the RSSI-based localization; this is because the target node obtained simply from the RSSI-based localization renders primary and unreliable estimation results only [15].The state-space equation is given by:
1 1 zk is the observations at time instant k;
fx is the state transition function;
hk is the observation function;
Vk is the process noise;
Uk is the observation noise.
The particle filter is based on the Bayesian recursive filtering which uses a set of particles to approximate the system state [12, 13]. Each particle expresses a possible state space, and the weight of each particle expresses the truest of each particle as shown in (4) and (5) as
1 1
4 12th Mechatronics Forum Biennial International Conference
Its expectation value can be approximated as a weighted average:
0: 0: 0: 1: 0:
( ( k)) ( k) ( k | k) k
E g x
=∫ g x p x z dx
(7)The expectation in the discrete-time system can be rewritten as:
0: 0:
By applying the assumption of the first-order hidden Markov process, the weight of important sampling can be expressed as:
3.2.DESIGN OF THE PARTICLE FILTER FOR THE MOBILE ROBOT
The design flow of the particle filter can be considered as four steps:
Step 1: Particle generation. This phase involves the generation of N random particles expressing possible positions near the initial position of the robot. The weight of each particle expresses the possibility of true position as shown in Figure 7.
Step 2: Particle prediction. Each particle will predict its next position according to the motion model of the robot (Figure 8), and are derived through the following:
1 0 cos '
where Vmax is 0.0425 m/s, which is the maximum velocity of the robot; θ is the orientation of the robot obtained from the electronic compass; and w(t) and v(t) are noise.
.
Figure 7: The particle generation Figure 8: The Prediction
Step 3: Sampling and weight computation. The sensing data from environments were also obtained as the localization information for processing RSSI when each particle predicted its next position according to the motion model. The weighting function can be designed as the difference between the prediction position of particles and the RSSI-based localization results if the latter is accurate to a certain extent (Figure 9). The
Mechatronics 2010, June 28 – 30, Swiss Federal Institute of Technology ETH, Zurich Switzerland 5
following formula for the weighting function is used:
w = 1
distance. (13)
where the distance is the difference between the prediction position of particle and the RSSI based localization result.
Measurement likelihood
Figure 9: The distance weighting function
However, RSSI-based localization provides incorrect estimation results mainly because of the multi-path and the antenna orientation effects. Three physical constraints were considered here to filter incorrect localization results, and the most possible particle can then be found as the maximum velocity, angular velocity, and current state of the robot as detailed below.
a) The maximum velocity of the mobile robot. This is because localization results should fulfill the continuity in motion considering the maximum velocity of the mobile robot. Therefore, some localization results will be eliminated if the estimated motion is unreasonable. The weighting function is designed as the difference between the current velocity of the particle and the maximum velocity of the mobile robot (Figure 10) as expressed by:
1 Vmax-V',
(14)
where Vmax and V'represent the maximum and current velocities of the robot, respectively.
b) The angular velocity of the robot. Unreasonable localization information on angle difference will be neglected, in consideration of the angular velocity of the robot and orientation obtained from electronic compass. As shown in Figure 11, the weighting function can be determined as by:
( , 1)
P
k is the current position from RSSI based localization result;θ
p is the difference between current localization result and previous result;θ
k is the orientation of each particle; andθ
Δ is the difference between current orientation and previous orientation.
c) The current state of the robot. The antenna orientation will change when the robot rotates. The information on the current state of the robot will then be useful in filtering out incorrect estimation results from RSSI in order to reduce the antenna orientation effect. Three states are described below:
z Stop: Any localization error increases or position change leads to wrong RSSI estimation if the current state is stop.
z Forward or backward: Localization results should not change dramatically unless RSSI results are unreliable when the robot moves forward or backward.
z Rotation: When the robot rotates, the antenna orientation will also change. Otherwise, RSSI provides unreliable information.
6 12th Mechatronics Forum Biennial International Conference
The compensation method inverse to the current position of the RSSI value and compares the difference between the RSSI obtained from ZigBee as:
'
RSSI =RSSI−bias. (18)
The final weighting function can be described by applying the assumption of first-order hidden Markov system as:
P x x − is the probability of current state transfers to next state, compose by velocity weight function and angular velocity weighting function;
( k | ik)
P z x is the probability of observation obtained at the current state; and
1 i
wk− is the previous weight.
The weight of each particle is normalized as:
1
The estimated position is the expectation value of all possible particles and is derived as:
1:
Step 4: Resampling. This step will generate new particles according to the weight. It adds noise to keep the diversity of particles and, in the process, prevent the converging of all particles to one particle.
0 0.005 0.01 0.015 0.02 0.025 0.03 0.035 0.04 0
Figure 10: The velocity weighting function Figure 11: Angular velocity weighting function
4.EXPERIMENTAL RESULTS
The DSP microcontroller TI DSP 2812 was developed as the core of a multi-axis DC motor driver circuit combined with peripheral circuits particularly for the present chain-type robot motion. The distance between two reference nodes was 2 m, and the origin of the coordinates was the left reference node in the indoor obstacle-free environment. The robot began to move from position (1, 1) and recoded the localization error in four orientations, namely, 0, 90, 180 and 270 degrees, at each position. The particle number was 50. The circular points shown in Figure 12 show the positions where the robot will pass by.
The results applying the proposed particle filter fusion technique were compared with those applying the Kalman filter [16]. The localization results at each orientation by the proposed particle filter are significantly better than those generated when only the Kalman filter is applied (Figure 13). The localization failed particularly at 180 and 270 degrees, when either the Kalman filter or the uncompensated case is applied, respectively. Thus, the antenna orientation effects cannot be solved as shown in Figure 13. The results also indicate that the probability within 1 m is 27% for the uncompensated case, 35% for the Kalman filter, and 100% for the proposed particle filter, respectively. The summarized maximum average error in Table 1 also indicates that the estimation error decreases from 2.3982 to 0.5227 m by applying the proposed approach, representing a 73.9% improvement.
The continuous motion trajectory shown in Figures 14 and 15 indicates that the proposed approach achieves reliable results for the RSSI-based localization.
Mechatronics 2010, June 28 – 30, Swiss Federal Institute of Technology ETH, Zurich Switzerland 7
Figure 12: The experimental environment and trajectory Table 1: Comparison of localization error
Error(m) uncompensated（m） Kalman Filter（m） Particle Filter (m) Improvement (%)
AVG. 1.5080 1.5193 0.3665 73.9443%
0 0.5 1 1.5 2 2.5 3 3.5
Figure 13: Cumulative probability distribution of localization error
-2 -1 0 1 2 3 4
Figure 14: The uncompensated robot trajectory
-2 -1 0 1 2 3 4
Figure 15: The estimated robot trajectory by applying the particle filter
8 12th Mechatronics Forum Biennial International Conference
5.CONCLUSION
The measured RSSI varies when the antenna orientation is different even when the position is the same. The relative position between the target and reference nodes also plays an important role in achieving reliable results when applying the RSSI-based localization. Results further indicate that the localization error becomes smaller when the target node is between the two reference nodes. The particle filter used with fusion technique significantly reduced the effects of both multi-path and antenna orientation. Compared with the results produced when applying the Kalman filter only, the localization accuracy significantly improved by 73.9% when the proposed particle filter fusion technique is applied. Experimental results for the robot motion with a square contouring trajectory further indicate that the proposed method can reliably estimate real robot motion simply with the RSSI obtained from the ZigBee system.
Demo video website: http://lab816.cn.nctu.edu.tw/Demo/Demo_Video.htm .
ACKNOWLEDGEMENTS
This work was supported by National Science Council of the Republic of China under Contract NSC 97-2218-E-009-007.
REFERENCES
[1] N. B. Priyantha, A. Chakraborty, and H. Balakrishnan, "The Cricket Location-support System," Proceedings of the 6th annual international conference on Mobile computing and networking ,Boston, Massachusetts, United States, 2000.
[2] D. Niculescu and N. Badri, "Ad Hoc Positioning system (APS) Using AOA," Twenty-Second Annual Joint Conference of the IEEE Computer and Communications Societies, Vol. 3, pp. 1734-1743, 2003.
[3] C. Morelli, M. Nicoli, V. Rampa, U. Spagnolini, and C. Alippi, "Particle Filters for Rss-Based Localization in Wireless Sensor Networks: An Experimental Study," Acoustics, Speech and Signal Processing, pp. 4-4, 2006.
[4] "2.4 GHz Inverted F Antenna (Rev. B)", Texas Instruments, 2008.
[5] Z. Shah, R. A. Malaney, W. Xun, and K. Tai, "Experimental Deployment of Particle Filters in WiFi Networks,"
Communications, pp. 4692-4697, 2007.
[6] D. Niculescu and B. Nath, "Ad Hoc Positioning system (APS)," Global Telecommunications Conference, Vol. 5, pp.
2926-2931, 2001.
[7] L. Lazos and R. Poovendran, "SeRLoc: Secure Range-independent Localization for Wireless Sensor Networks,"
Proceedings of the 3rd ACM workshop on Wireless security Philadelphia, PA, USA, 2004.
[8] A. Ladd, K. Bekris, A. Rudys, L. Kavraki, and D. Wallach, "Robotics-based Location Sensing Using Wireless Ethernet,"
Wireless Networks, Vol. 11, pp. 189-204, 2005.
[9] H. S. Ahn, W. P. Yu, and Ieee, "Indoor Mobile Robot and Pedestrian Localization Techniques," International Conference on Control, Automation and Systems, Seoul, South Korea, pp. 1189-1193, 2007.
[10] A. Baggio and K. Langendoen, "Monte Carlo Localization for Mobile Wireless Sensor Networks," Ad Hoc Networks,Vol.
6, pp. 718-733, 2008.
[11] F. Caballero, L. Merino, I. Maza, and A. Ollero, "A Particle Filtering Method for Wireless Sensor Network Localization with an Aerial Robot Beacon," Robotics and Automation, pp. 596-601, 2008.
[12] B. Ristic and S. Arulampalam, Beyond the Kalman filter: Particle filters for tracking applications: Artech House, 2004.
[13] M. Nicoli, C. Morelli, and V. Rampa, "A Jump Markov Particle Filter for Localization of Moving Terminals in Multipath Indoor Scenarios," IEEE Transactions on Signal Processing, , Vol. 56, pp. 3801-3809, 2008.
[14] "CC2431 Location Engine", Texas Instruments, 2006.
[15] L. Klingbeil, T. Wark, and I. C. Soc, "A Wireless Sensor Network for Real-time Indoor Localization and Motion Monitoring," 7th International Symposium on Information Processing Sensor Networks, St Louis, MO, pp. 39-50, 2008.
[16] G. Welch and G. Bishop, An Introduction to the Kalman Filter, University of North Carolina H, Chapel Hill, NC, 1995.