Localization Assistance for Multiple Mobile Robots using Target Tracking Techniques

Transcription

1 Localization Assistance for Multiple Mobile Robots using Target Tracing Techniques Peshala Jayaseara a, Hidei Hashimoto b and Taashi Kubota c a Dept. of Electrical Engineering, University of Toyo, Toyo, Japan b Dept. of Electrical, Electronics and Communication Engineering, Chuo University, Toyo, Japan c Inst. of Space and Astronautical Science (ISAS), JAXA, Japan peshala@nnl.isas.jaxa.jp Abstract Developing autonomous mobile robots that can coexist with human in populated environments is still considered a big challenge. To address this problem the authors propose a novel scheme to assist mobile robots by providing localization information externally. In the proposed scheme, the environment is sensed using a laser range finder and camera based sensor unit. Using Rao- Blacwellized particle filter technique, the robots that need assistance are continuously traced. In contrast with conventional laser range finder based tracing systems, the placement of sensor is changed to a level above average human height and the mobile robots are modified by attaching a cylindrical pole. The experiments show the validity of the proposed scheme for simultaneous localization assistance for multiple mobile robots. Two mobile robots were simultaneously navigated in given trajectories using assistance data, successfully. Keywords Localization, Mobile Robots, Target Tracing, Particle Filter I. INTRODUCTION Daily human environments are dynamic, unpredictable and populated. These real environments pose many challenges for conventional self-contained autonomous mobile robots even if they are equipped with as many sensors as possible. Adaptability to real environments comes at a cost of the mobile robot s computational capability, which, due to its size and mobility, is limited. Thus, considering the state-of-art technologies, it is difficult to expect a capable mobile robot navigating all by itself in complex human environments even in near future. Humans tend to depend on some assistance to minimize both the effort and time put into navigation. This human behavior can be imitated in the field of autonomous navigation for mobile robots. This idea motivates the need for a proper assistance scheme for autonomous navigation and such an assistance system can act as the missing building bloc to provide fully autonomous capabilities for navigation in human environments. More specifically, the project objective is to develop a structured assistance system for mobile robots navigating in crowded human environments. The case where such a localization assistance system would not have any prior nowledge about the mobile robots (e.g. shape, features) is considered. Furthermore, the authors intend to reduce the on-board sensors, eliminate the need for any other tags and consider minimum environment modification. The mobile robots will therefore not be equipped with any tags, heading reference systems and rely on information it obtains from the assistance system. In order to realize a structured mobile robot assistance scheme, two important aspects should be considered; the nature of assistance and a suitable methodology to implement it. In this research, the authors have narrowed the scope of the assistance system to provision of only localization information. Conventional laser range finder (LRF) based scan matching and camera based localization techniques can easily fail in complex human environments due to inability to see landmars (occlusion), or complexity of the environment. Other localization systems [][2] needs the environment to be modified severely and to undergo time consuming calibration procedures. To implement the proposed assistance system, target tracing techniques have been utilized. There has been considerable research done on tracing moving targets [3]-[6]. However, in these wors, traced information is often used from the viewpoint of the tracer and seldom used for the benefit of the targets being traced. As described in [7] target tracing can be effectively used to extract localization of mobile robots and assist multiple mobile robots in autonomous navigation; the authors have named the system as intelligent assistance (IA). In this paper the authors present their latest results of the intelligent assistance system. II. RELATED WORK ispace, proposed by Hashimoto laboratory [8], is an ambient intelligent space with ubiquitous distributed sensory intelligence and actuators for manipulating the space. Robot localization using on-board and distributed sensors has been proposed for ispace in [9]. The proposed system follows the same idea of [9] and has been improvised. Autonomous City Explorer Project [0] attempts to address the problem of autonomous mobile

2 robot navigation in natural, populated environments with external human assistance. The ambiguity in extracted visual information can lead to inconsistencies in information perception. As the third option, mobile robots can see help from other robots in the environment using cooperative localization (CL) technique. Ref.[]-[3] provide some of the example implementations of CL. In all these approaches each mobile robot shares its belief with the other members of the group. Amount of transmitted information and computational complexity are the two major issues that cannot be overlooed for a mobile robot which has limited resources. Moreover, if the mobile robots do not have a protocol for communication with each other, then CL cannot be established to increase individual localization accuracy. III. SYSTEM ARCHITECTURE The schematic diagram of the proposed system is given in Fig.. neighbor classifier. LRF cluster means are then adjusted to estimate the center position of the mobile robot. B. Camera based Target Tracing Initialization The assistant system has no prior nowledge of the robots to be served. Therefore, there should be a mechanism to initialize the tracing of each mobile robot. It can be expected that the mobile robots be occluded in the populated environment and the assistant has to automatically find some clue to detect it. The proposed scheme for initial attachment is by using a blining signal light (red bulb), with a nown frequency of blining, attached to the cylindrical pole of the mobile robot. At initialization the mobile robot stays stationary and the blining frequency is agreed upon by both parties at the time of initialization. An AND operation is performed between the thresholded image of the current frame and the thresholded image of the (r/f) th frame before the current frame; the blining frequency f and the frame rate r of the camera are nown, and f is chosen such that (r/f) is an integer. If the current frame contains the lit signal light, then an AND operation is performed with a frame that also contained another lit signal light. This can isolate the position of the signal light as explained in Fig. 2. The angle of the signal light is computed and it is matched with the nearest angle of LRF cluster means. The selected LRF measurement is used as the initial estimate for initializing tracing for the particular mobile robot. This is illustrated in Fig. 3. Fig.. Schematic diagram of the proposed system. A SICK LMS 29 LRF and a Point Grey Dragonfly2 IEEE firewire camera based sensor unit is used to detect mobile robots. The need for fusion of camera with LRF arises because the differentiation between different targets using only LRF data cannot be done. In the initialization phase, the robot is detected by the tracing initializer of the IA vision subsystem and by LRF, simultaneously. After correctly associating camera observations with corresponding LRF observations, this fused information is used for tracing mobile robots by a particle filter based tracer. Output from the particle filter tracer will be fed-bac to mobile robots as assistance information. A. LRF Data Processing In LRF data, a bacground subtraction method is utilized, and mobile robots are detected as foreground objects. The LRF scan plane is changed (section IV), and at this scan level the bacground hardly changes. Thus, bacground subtraction method for detection is robust. The laser scan points are clustered using a nearest Fig. 2. Capturing the light source based on difference images and the blining frequency. Fig. 3. Detection of the angle of the blining light source of a mobile robot and the selection of the associated LRF measurement

3 Fig. 4. Illustration of LRF placement and robot modification In doing so, not only its position but its heading direction can be estimated. This is because, most robots are non-holonomic, using differential-drive systems or Acerman steered systems and for such robots, the nonholonomic constraints limit the robot s velocity in each configuration (x, y, θ). As a result, if the interval between two successive measurements is small, its heading angle can be computed using: y& θ = tan x& (2) except x& < threshold _ x& AND y& < threshold _ y& This is illustrated in Fig. 5. It is made sure θ is not calculated when the robot is stationary or rotating with small translational velocities, which in turn results in noisy θ calculations (discontinuity in θ calculation). IV. MOBILE ROBOT MODIFICATION Different mobile robots have different base shapes. And, the LRF cluster center varies depending on the mobile robot s base shape as well as its heading angle. To be able to detect mobile robots in crowded environments and to trac the same position of robots at all times the robot platform is modified by attaching a vertical cylindrical pole with its height greater than average human height. The LRF placement is also changed to an overhead level at 2m from ground. This is illustrated in Fig. 4. V. MULTIPLE TARGET TRACKING IN CLUTTER The problem of data association maes multiple target tracing (MTT) a much harder tas than single target tracing. In MTT, the algorithm has to estimate which targets produced the measurements, before it is able to use them in actual tracing. The Rao-Blacwellized Monte Carlo data association algorithm proposed by S ar a et al.[4] estimates data associations with a sequential importance resampling (SIR) particle filter and the other parts with a Kalman filter. This idea can be directly used in the MTT in clutter case. Due to the conditional independences between the targets, the full Kalman filter prediction and update steps for all targets can be reduced to independent single target predictions and updates. Because the targets are a priori independent, conditional on the data associations C, the targets will remain independent during tracing. A. State Space Model Target state should be chosen in such a way that localization information of a target could be obtained from its state. Target state X is represented using its (x,y) position and velocities ( x, y) in the two dimensional Cartesian coordinates: X = x y x& y& () [ ] T Fig. 5. Illustration of the validity of heading angle estimation approach A discretized Wiener process velocity model is used as the dynamic model of a target described as follows: X = A X + w + cv 0 T 0 (3) 0 0 T A = cv where A cv is the state-transition matrix, T is the sampling interval and w is process noise. The nonlinear measurement model is a range-bearing (r, θ) model of the form: Z = f ( X ) + v 2 2 x + y (4) r = y + v θ tan x where Z is measurement and v is measurement noise. B. Algorithm Implementation In accordance with the method proposed by Särä in [4], the final Rao-Blacwellized particle filter implementation is as follows: Perform KF predictions for the means m (i) - and the covariances P (i) - of particles i =,,N. m = A m (5) T P = A P A + Q

4 Using SIR particle filter draw new data association C for each particle in i =,,N from importance distribution π C Z, C p C Z C (6) ( ) ( ) : : = :, : Using Bayes rule, p ( C Z:, C : ) (7) p( Z C, Z:, C : ) p( C C : ) Therefore, instead of from (6), we can draw a new association from (7). Calculate new unnormalized weights as follows: * * p( Z Z:, C : ) p( C C : ) w = w (8) π( C Z:, C : ) Normalize the weights Perform the KF updates for each of the particles conditional on the drawn data association variable V = Z h( m ) S K m h = x = P = m m h x + K h x m T m [ S ] T [ K ] T P = P K S If the effective number of particles neff = N (0) 2 ( w ) P i= V + R is too low, perform resampling. After the set of particles have been acquired, the filtering distribution can be approximated as N p( X, λ Z ) w δ ( λ λ ) N( X m, P () : ) i= VI. ROBOT-SIDE IMPLEMENTATION Robot-side implementation to mae use of IA information is described, here. A. EKF based Sensor Fusion An EKF based sensor fusion mechanism is implemented at the robot-side to obtain its pose by fusing IA data with odometry measurements; no attitude, heading reference sensors are used. The need for data fusion arises because of the fact that heading angle estimation undergoes discontinuities in IA as explained in section V.A. System Model: a nonlinear system model for the state transition in the form: x = g( x, v, w ) + ε x x + v T cos( θ ) (2) y = y + v T θ sin( ) + ε θ θ + w T x represents the state vector, v, w represents control inputs and ε is a Gaussian random vector (9) that models uncertainty introduced by the state transition. Measurement Model: a linear measurement model in the form: z = Hx + δ 0 0 (3) H = Both assistance and odometry measurements are similar in structure to the state vector, and therefore, the same measurement model can be used to update the filter. Sensor fusion taes place at the Kalman update step, where the filter is updated sequentially using all the sensor measurements (Odometry and IA data) for that particular instance. However, the mismatch of timestamps of the two measurements prevents us from applying such an update straightaway. This is overcome by interpolating one measurement to the other measurements timestamp. As odometry data can drift arbitrary over time, the raw odometry cannot be used as it is in the EKF. The interpretation of odometry data is changed continuously so that the odometry error accumulated up to a given point of time is eliminated. The interpretation is changed based on the pose estimation output of the EKF. VII. EXPERIMENTS AND RESULTS A. Simulations of Multiple Target Tracing in the presence of Clutter Tracing of three targets in the presence of clutter is simulated using MATLAB software. The results are given in Fig. 6. It is observed that the results of the Rao- Blacwellized particle filter tracer show a very good accuracy compared to the real trajectories for all the three targets even in the presence of clutter (Fig. 6 (a)). For all three targets, the mean position estimation error is 2.5cm and the mean heading angle estimation error is 2 degrees. As the heading angle of a target cannot be directly obtained from LRF readings, it is important to examine the accuracy of the proposed method for heading angle estimation. The three targets were given a continuous change of heading angle maing them to move in curved trajectories. Fig. 6 (b), (c) and (d) compare the heading angles of target, target2 and target3 respectively to their estimated heading angles. In the simulations, it can be observed that the estimated values follow the real values and the obtained error is within acceptable ranges for a mobile robot. In each graph in Fig. 6 (b), (c) and (d) there is an initial overshoot in the estimated angle. This should not come at a surprise as this region is where the heading angle is not calculated as the velocity state variables are smaller than their thresholds as explained before in section V.A.

5 Fig. 6. Simulation Results (a) Position comparison (b) Heading angle comparison for target (c) Heading angle comparison for target2 (d) Heading angle comparison for target3 B. Assisted Navigation Experiments In the experimental setting, as shown in Fig. 7, the mobile robot(s) is given four sub-goals (in a rectangle) to navigate using IAs assistance. The mobile robot does not contain any sensors apart from its built-in odometry. Assisted navigation experiments are conducted for both single and dual robots and their raw odometry versus the EKF pose output is examined to identify how well the assistance information has helped the robots to minimize the localization error and how well it can navigate on a given trajectory using IA support. The zone positioning system (ZPS) system of Hashimoto laboratory is used to compare the estimations with the actual localization. The results are given in Fig. 8 and Fig. 9 for dual robot experiment. The reason for choosing such a rectangular path was to examine the behavior of robot pose estimation when the heading angle provided by the intelligent assistant is invalid; at sharp corners in the trajectory the robots will be maing pure rotations without any translation velocity and only the position information from IA will be valid. Nevertheless, it is evident from the two figures that the pose estimation is not severely affected because of the EKF based sensor fusion implementation at robot-side. It can be clearly observed that, even after one cycle of navigation via the four given sub-goals, the raw odometry had considerably deviated from its actual localization. However, by fusing intelligent assistance data with odometry and changing the odometry interpretation the mobile robots have managed to fulfill the navigation tas successfully. Fig. 7. Assisted navigation - snapshots of (a) single mobile robot (b) multiple mobile robots experiment Fig. 8. Comparison between real odometry data, robot EKF output and ZPS output for robot#

6 REFERENCES Fig. 9. Comparison between real odometry data, robot EKF output and ZPS output for robot#2 VIII. CONCLUSION By imitating how humans navigate in complex environments with the help of assistance from others, the same behavior was mimiced for mobile robots by introducing and developing a structured assistance scheme, named intelligent assistance. Mobile robots are detected using a laser range finder and a camera based sensor unit and traced by employing the particle filter technique. The experiment results show the validity and the applicability of IA for multiple robot assistance, with an IA throughput: 0Hz, position estimation error: 5.5cm and heading angle error: By incorporating localization assistance information with rather noisy odometry measurements and a proper implementation at robot-side, the self-localization uncertainty of mobile robots is reduced and thereby the robots could be navigated in expected trajectories, even without any heading reference system, with a minimized error. A. Future Directions In the proposed scheme, the heading angle estimation undergoes discontinuities. Incorporating camera images to determine the heading angle in such situations is a potential solution to this problem. In the current implementation, the sensor unit static is ept stationary. However, if the sensor unit is mobile, better detection as well as increased coverage is possible by optimal positioning of the sensor unit. The authors are currently investigating the possibilities of transforming the intelligent assistant into a mobile intelligent assistant (MIA) using a mobile platform. [] Nissana Bodhi Priyantha, The cricet indoor location system, PhD Thesis, Massachusetts Institute of Technology, [2] R. Orr and G. Abowd, The Smart Floor: A mechanism for natural user identification and tracing, in Proceedings of the Conference on Human Factors in Computing Systems (CHI 00), pp , [3] D. Brscic and H. Hashimoto, Tracing of humans inside intelligent space using static and mobile sensors, in 33rd Annual Conference of the IEEE Industrial Electronics Society IECON, pp. 0-5, [4] S. S ar a, A. Vehtari, and J. Lampinen, Rao- Blacwellized particle filter for multiple target tracing, Information Fusion Journal, vol. 8, no., pp. 2-5, [5] R. Kurazume, H. Yamada, K. Muraami, Y. Iwashita, and T. Hasegawa, Target tracing using SIR and MCMC particle filters by multiple cameras and laser range finders, in International Conference on Intelligent Robots and Systems, IROS, pp , [6] K. Naamura et al., Tracing pedestrians using multiple single-row laser range scanners and its reliability evaluation, Systems and computers in Japan, vol. 37, no. 7, pp. -, [7] P. G. Jayaseara, Y. E. Song, T. Sasai, and H. Hashimoto, Intelligent assistance in localization for mobile robots, in The 8th France-Japan and 6th Europe- Asia Congress on Mechatronics, pp , Yoohama, Japan, November 22-24, 200. [8] J. H. Lee and H. Hashimoto, Intelligent space - concept and contents, Advanced Robotics, vol. 6, no. 3, pp , [9] D. Brscic and H. Hashimoto, Model based robot localization using onboard and distributed laser range finders, in IEEE/RSJ International Conference on Intelligent Robots and Systems, pp , [0] G. Lidoris, F. Rohrmuller, D.Wollherr, and M. Buss, The autonomous city explorer (ACE) project-mobile robot navigation in highly populated urban environments, in IEEE International Conference on Robotics and Automation ICRA 09, pp , [] D. Fox, W. Burgard, H. Kruppa, and S. Thrun, A probabilistic approach to collaborative multi-robot localization, in Autonomous Robots: Special Issue on Heterogeneous Multi-Robot Systems, vol. 8, no. 3, pp , [2] A. Howard, M.J. Mataric, and G.S. Suhatme, Localization for mobile robot teams using maximum lielihood estimation, in International Conference on Intelligent Robot and Systems (IROS02), vol. 3, pp , [3] A. Howard, Multi-robot simultaneous localization and mapping using particle filters, in Proc. Robot. Sci. Syst., Cambridge, [4] S. S ar a, A. Vehtari, J. Lampinen, Rao-Blacwellized Monte Carlo data association for multiple target tracing, in Proceedings of the Seventh International Conference on Information Fusion, vol. I, pp , 2004.