THE control of robot formation is an important topic

Transcription

1 Wireless Reception Signal Strength for Relative Positioning in a Vehicle Robot Formation Arturo Gil-Pinto, Philippe Fraisse and Rene Zapata Department of Robotics LRMM, University of Montpellier II 6 Rue Ada Montpellier, France {gilpinto,fraisse,zapata}@lirmm.fr Abstract An integration of the wireless network signal strength data with the vehicle sensors information by means of a Kalman filter is proposed to estimated the relative position of each vehicle in a robot formation set. Vehicle sensors consist of wheel speed and steering angle, the WiFi data consist of reception signal strength (RSS) and the angle of the maximal RSS with respect to the robot orientation. A nonholonomic nonlinear model vehicle is considered, due to this nonlinearities a extended Kalman filter EKF is used. Simulation and experimental results of the proposed estimation strategy are presented. I. INTRODUCTION THE control of robot formation is an important topic in the robotic research community. A robot formation of simple robots can perform task more efficiently and less costly than a single complex robot, i.e. tasks like rescue, mine search or exploration of dangerous areas can be performed by cooperative robots formation. The cooperation between the robots requires the transmission of information. The wireless network system is the most appropriate technique in this cases and almost every robot cooperation tasks are executed by mean of a wireless network infrastructure. Using the wireless network not only to transmit information between the robots but also as an instrument to positioning them can add security and reduce cost in the cooperative robots application, i.e. we can use the communication structure to secure the positioning when the conventional positioning systems like the Global Positioning System (GPS) is not available [1]. The GPS it is a extended method for outdoor location in robotics applications. The GPS method is based on frequencies of Hz, that requires a line sight between the sensor and the satellites. If no line sight is available, like indoor or any other foiled situation like urban areas, the GPS location will be not accessible or an erroneous measurement will be deployed by the sensor. The use of reception signal strength (RSS) of a wireless networks is a field of study in purposes in environmental monitoring, structural monitoring, and military battleground and public safety applications [2], i.e. in the United States of North America has required cellular operator to estimate the position of an emergency caller with error of less than 100m [3]. The positioning by the signal strength is supported A. Gil-Pinto is also with the Department of Mechanical Engineering of the Central University of Venezuela, Caracas VE 1041-A /06/$20.00 c 2006 IEEE by the IEEE standard., and will be the key feature for de outgoing IEEE a standard project [4]. In the robotic field the RSS of WiFi networks has been proposed to positioning robotic systems in indoor applications [5], [6]. The conventional techniques for localization by using radio signals are based in statistical models, i.e. could be considered the variation of the RSS individual measurement around de mean value has a normal distribution in db [2], [7], [8]. We propose to fuse the WiFi signal measurements with the vehicle sensor values to perform a decentralized estimation in real time of the relative position in the formation of each vehicle. A popular data fusion method is the extended Kalman filter (EKF). The EKF is a variation of the Kalman filter [9] to solve nonlinear problems. Other variations of the Kalman filter has been used as data fusion method for vehicles positioning like the unscented Kalman filter (UKF) [10], a comparative study of the EKF and the UKF in positioning problems is showed in [11]. In [12] a Kalman filter approach is used in multiple robots localisation. In vehicles positioning problem the EKF has been used to integrate the differential global positioning system (DGPS) signals and the vehicle sensors [13]. In this work we use the EKF to estimate de relative position of vehicles of a formation by fusing the WiFi network and vehicle sensors. The EKF is based on the a kinematical nonlinear model of a four wheel car-like vehicle. Each vehicle measures the strength and the direction of maximal reception of the received wireless signal from its nearest neighbor in the formation, also each robot measures its wheel speed and steering angle. Fusing these signals the robot is able to estimate its relative position with respect to its neighbor to be used to feedback the robot formation. II. ROBOT AND FORMATION MODEL We consider the problem of control a robot formation. The control is performed by using a decentralized leader follower approach [14]. Under this strategy each robot follower is relative positioned with respect to an assigned leader by the relative distance and angle between them. The formation structure can be then designed by defining the relatives angles and distances and the relations leader-follower in the set of robots. An absolute leader is assigned, for this robot there 100

2 is no leader to follow, that is its trajectory is defined in an exogenous way i.e. an human operator. A. Vehicle Model All robots are rear drove and front steered wheels car-like vehicles, each i vehicle is modeled as: ẋ i = v i cos(θ i ) ẏ i = v i sin(θ i ) θ i = v i tan(ξ i )/L, (1) ξ i = ω i { vi =(u v i v i)/τ v ω i =(u ω i ω i /R)/τ ω, (2) Where (x i,y i,θ i,ξ i ) are respectively the Cartesian coordinates of the vehicle, its orientation with respect to the inertial coordinate frame and its front wheels steering angle; (v i,ω i ), the linear and angular velocities, (R,L,τ v,τ ω ), are respectively the wheel radius, the length, and the motors time constants of the i vehicle and (u v i,uω i ) the controls. B. Decentralized Follower to Leader Model The relation between the follower robot j and its leader i can be modeled from (1) by: l ij v j cos(γ ij ) γ ij θ j = v j (sin(γ ij )/l ij tan(ξ j )/L) v j tan(ξ j )/L ξ j ω j cos(θ i + β ij ) +v i sin(θ i + β ij )/l ij 0, (3) 0 where l ij and γ ij are the relative distance and angle between the robots and β ij = θ ij γ ij, (Fig.1). The control strategy is to control the l ij distance and the γ ij angle by using the local controls u v j and uω j to compensate the perturbation created by movement of the leader i. We can express in a general form each robot subsystem (3) as: ẋ j = f j (x j,u j )+ f j (x j,x i,u i )+w j (t), (4) for j = 1,2,...N, where N is the number of followers in the formation, the state x j =(l ij,γ ij,θ j,ξ j,v j,ω j ) T, u j =(u v j,uω j )T ; the function f j describes the dynamic of the subsystem or robot j, f j the interaction of the robot j with its leader i, and w j the process noise vector. Fig. 1. Leader-Follower relative positioning We assume that the information available for the jth robot is represented by: I j (t)= { y j (t),y i (t),u j (t) }, (5) where y i and y j are respectively the outputs of the subsystems i and j. The robot i output y i is broadcast to the robot j by using a wireless network. The desired robot formation pattern is defined by setting all the relative distances and angles between the followers and its designated leaders. We use a decentralized approach as control: u j = F j (I j (t),t), (6) that is each robot j computes its control vector u j based on the available local information I j in order to compensate the perturbation f j in (4) produced by the movement of the leader robot i. For more on decentralized cooperative control see [15], [16] III. MEASUREMENT SYSTEM The measurement equation for the system (3) can be written by using the measurement equations (8-13): y j = h j (x j,t)+v j (t), (7) [ ] T with y j = R r γ ij θ j ξ j ṽ j ω j, being Rr the measurement of the RSS of the wireless network, γ ij the angle of arrival of the wireless signal and ( θ j, ξ j,ṽ j, ω j ), the orientation and velocities measurements. The vector v j is the measurement noise vector. A. Wireless RSS To obtain the feedback control strategy (6), the relative distance and angle between every follower and its leader have to be estimated. There exist many approaches to estimate the distance between the robots in decentralized formation applications, like vision based systems [14], or GPS localization [15]. In addition to the robot speed and direction sensors, we propose to use the RSS of the wireless communication network like a positioning sensing tool. Each subsystem or follower robot j is able to measure the RSS of the wireless communication channel with its leader i. The RSS can be related with the distance through the narrowband radio propagation pathloss model [2]: R p = R α log 10 (l)+ν, (8) where R 0 is the signal power loss in dbm at one meter of distance, R p is the signal loss in dbm at a distance of l > 1 meter, α the exponential path loss coefficient and ν is a Gaussian representing lognormal shadow fading effects in multipath environments. The random variable ν can be considered a zero-mean variable ν N(0,σn 2 ), with a standard variation σ n that depends on the characteristic of the multipath environment. If the transmission power R t of the WiFi network is known, we can determine the R p at the robot j by: R p = R t R r, where R r is the measured power at the reception node (robot j) [17]. 101

3 B. Wireless Direction In order to determine at the node j the direction γ ij of the source (node i) of the WiFi signal, we propose to use a steerable directional antenna [18]. Being directional it receives signal over a very narrow bean width. Hence, a maximum value for the RSS R r is achieved when the antenna is pointed directly to the source j. The direction γ ij can be express: γ ij = γ ij φ ij + ρ, (9) where γ ij is the measured angle, φ ij is an angle difference due to the non-line of sight environments. 1,[7], ρ the noise in the measurement and γ ij is the line of sight angle between the source i and the node j. C. Speed and Orientation Measurements Each robot j has an odometric speed v j sensor, an electronic compass for orientation θ j measurement and a potentiometer as steering angle ξ j sensor. The measurements can be modeled as: ṽ i = v i + η, (10) ω i = ω i + λ, (11) θ i = θ i + ι, (12) ξ i = ξ i + κ, (13) where η, λ ι and κ, are respectively, the noise in the linear speed, steering angular velocity, orientation and steer angle sensors. IV. EXTENDED KALMAN FILTER For a sampling time T s the ZOH discrete approximation of the model 4, and measurements 7, can be expressed at time t k+1,as: x j (t k+1 )=x j (t k )+[f j (x j (t k ),u j (t k )) +... f j (x j (t k ),x i (t k ),u i (t k ))] T s + w j (t k ), (14) y j (t k )=h j (x j (t k )) + v j (t k ), (15) The process noise w j and the measurement noise v j are assumed to be zero-mean, white noise with covariance properties as follows: 1 In this work we deal only with the line of sight environments, φ ij = 0. for all k and j. { E[w(k)w T Q(k), k = j ( j)] =, (16) 0, k j { E[v(k)v T R(k), k = j ( j)] =, (17) 0, k j E[w(k)v T ( j)] = 0, (18) The extended Kalman filter is based in two main steps. A time update projects the current state estimate in time, and a measurement update adjusts the projected estimated by an actual measurement at that time [19]. For the linear approximation of the process and measurement models(14) and (15), the EKF algorithm can be defined by: The time update is obtained by: ˆx j (t k+1)=ˆx j (t k )+[f j (ˆx j (t k ),u j (t k )) +... f j (ˆx j (t k ), ˆx i (t k ),u i (t k ))] T s, (19) P k = A kp k 1 A T k + Q k 1, (20) and the measurement update is given by: K k = P k HT k ( Hk Pk HT k + R ) 1 k, (21) ( )) ˆx j (t k )=ˆx j (t k ) + K k y j (t k ) h j (ˆx j (t k), (22) being the jacobian: and, A k[m,n] = ( f j[m] + f j[m] ) P k =(I K k H k )P k, (23) x j[n] (ˆx j (t k 1 ),u j (t k ),u i (t k ))), H k[m,n] = h j[m] x j[n] (ˆx j (t k)), where P k, is the error covariance and K k,the Kalman filter gain matrix. The discrete EKF algorithm (equations (19) to (23)) is a recursive process. As such it requires initialization prior to starting the recursion. If we assume that the first measurement occurs at t 1, the initialized state estimate and error covariance ˆx j (t 0 ) and P 0 should be given. The EKF is tuned by choosing the noise covariance matrixes Q k and R k. 102

4 Fig. 2. Robot trajectories Fig. 4. Leader-Follower-1 relative angle γ Fig. 3. Leader-Follower-1 RSS estimation V. SIMULATIONS The figures, (2,3,4,5) show respectively, the trajectories, the RSS, the distance, the relative angle and the distance estimation error for a robot formation of three vehicles. For the equation (8) we use a exponential path loss coefficient α = 2, and the Gaussian variable ν is supposed to have a normal variance of 3dBm. VI. EXPERIMENTAL SETUP The experimental vehicle (fig. 6) is equipped with: a PC, an odometric speed sensor for the linear velocity, an electronic compass for robot orientation, and the steering angle sensor for the front wheels. The WiFi network is constituted by an IEEE g wireless router as access point, and a USB wireless adapter for the robot node. Fig. 5. Leader-Follower-1 distance estimation error VII. EXPERIMENTAL RESULTS In order to get a first validation of the proposed estimation strategy we performed a simple experience where the robot Fig. 6. Experimental vehicle 103

5 Fig. 7. Experimental setup sketch Fig. 9. RSS estimation Fig. 8. Experimental setup is displaced with a constant angle γ = 0 with respect to the access point (fig. 7 and 8). First we determined experimentally the α and R 0 values of equation (8). Then the real time estimator for the distance between the robot and the access point was implemented. The figures (9) and (10) show respectively the estimations for the RSS and distance. Tne the RSS and distance estimations figures (9,10) we see how the random RSS signal is reduced by the EKF. A difference between the prediction and estimation is observed. This difference could be produced by a dynamic model mismatch. VIII. CONCLUSIONS AND FUTURE WORKS A. Conclusions A real time estimator of the relative position between carlike vehicles in a robotic formation was proposed in this work. We used a extended Kalman filter (EKF) to fuse the wireless network measurements and the vehicles sensors outputs to estimate the relative distance and angle between robots. The wireless network measurements are, the reception signal strength (RSS) and the angle were the maximal RSS is Fig. 10. Distance estimation reached. We validated the filter implementation for a robotic formation and we applied the filter in an experimental vehicle. Then the WiFi network can be also used as a positioning tool in cooperative robotic applications. B. Future Works We expect to validate the whole algorithm in the robot formation for the next autumn by using three experimental robots and by including directional motorized antennas for the vehicles. The variations in the WiFi RSS due to obstacles will be proposed to be studied in future works. 104

6 REFERENCES [1] P. Fraisse, R. Zapata, W. Zarrad, and D. Andreu, Remote secure decentralized control strategy for mobile robots, Adv. Robot, vol. 19, pp , [2] X. Li, Performance study of rss-based location estimation techniques for wireless sensor networks, IEEE Military Communications Conference, vol. 1, pp. 1 5, [3] J. H. Reed, K. J. Krizman, B. D. Woerner, and T. S. Rappaport, An overview of the challenges and progress in meeting the e-911 requirement for location service, IEEE Commun. Mag., vol. 36, pp , [4] R. Roberts, IEEE P ranging subcommitee final report, [Online]. Available: a-ranging-subcommittee-final-report.doc [5] V. Matelln, J. M. Caas, and O. Serrano, Wifi localization methods for autonomous robots, Robotica, vol. 24, pp , [6] K. Pahlavan, X. Li, and J. P. Makela, Indoor geolocation science and technology, IEEE Commun. Mag., vol. 40, pp , [7] Q. Yihong, H. Kobayashi, and H. Suda, Analysis of wireless geolocation in a non-line-of-sight environment, IEEE Trans. Wireless Commun., vol. 5, pp , [8] P. Krishnamurthy, J. Beneat, M. Marku, and K. Pahlavan, Modeling of the wideband indoor radio channel for geolocation applications in residential areas, IEEE Vehicular Technology Conference, vol. 1, pp , Jul [9] R. E. Kalman, A new approach to linear filtering and prediction problems, Trans. ASME Basic Engineering, vol. 82, pp , [10] E. A. Wan and R. V. D. Merwe, The unscented kalman filter for nonlinear estimation, IEEE Adaptive Systems for Signal Processing, Communications, and Control Symposium, pp , [11] M. St-Pierre and D. Gingras, Comparison between the unscented kalman filter and the extended kalman filter for the position estimation module of an integrated navigation information system, IEEE Intelligent Vehicles Symposium, pp , [12] S. I. Roumeliotis and G. A. Bekey, Distributed multirobot localization, IEEE Trans. Robot. Automat., vol. 18, no. 5, pp , [13] S. Rezaei and R. Sengupta, Kalman filter based integration of DGPS and vehicle sensors for localization, IEEE Mechatronics and Automation Conf., vol. 1, pp , [14] A. K. Das, R. Fierro, V. Kumar, J. P. Ostrowski, J. Spletzer, and C. J. Taylor, A vision-based formation control framework, IEEE Trans. Robot. Automat., vol. 18, pp , [15] J. T. Feddema, C. Lewis, and D. A. Schoenwald, Decentralized control of cooperative robotic vehicles: theory and application, IEEE Trans. Robot. Automat., vol. 18, no. 5, pp , [16] Z. Gong and M. Aldeen, Stabilization of decentralized control systems, Journal of Mathematical Systems, Estimation, and Control, vol. 7, pp. 1 16, [17] J. Cheng, Y. Lu, E. R. Thomas, and J. A. Farrell, Auonomous Mobile Robots. CRC Press, 2006, ch. Data Fusion via Kalman Filter: GPS and INS, pp [18] Circular waveguide antenna for 2.45 ghz / b / wifi / wlan. [Online]. Available: [19] G. Whelch and G. Bishop, An introduction to the kalman filter, [Online]. Available: 105