Experiments of Obstacles and Collision Avoidance with a Distributed ...

Experiments of Obstacles and Collision Avoidancewith a Distributed Multi-Robot SystemFilippo Arrichiello and Stefano ChiaveriniDepartment of Electrical and Information EngineeringUniversity of Cassino and Southern Lazio03043 Cassino, Italy{f.arrichiello, chiaverini}@unicas.itVaibhav Kumar MehtaL.N. Mittal Insititute of Information TechnologyJaipur, Indiavaibhavmehta@ieee.orgAbstract— In this paper we describe preliminary experiments ofobstacle and collision avoidance with a distributed multi-robotsystem composed of grounded mobile robots equipped with laserrange finder. We implemented a localization algorithm to makethe robots avail autonomous navigation in indoor environments,and then we apply a behavior based technique, namely the Null-Space based Behavioral control, to make the robots navigateavoiding collision among themselves and with obstacles detectedwith the laser. Several tests have been performed using a teamof up to four Khepera III mobile robots equipped with HokuyoURG laser range finder and communicating via ad-hoc wirelessnetwork.I. INTRODUCTIONMulti-robot systems received an increasing attention in thelast decades due to the advantages they present w.r.t. singlerobot in terms of fault tolerance, flexibility in mission execution,possibility to use distributed sensing and actuation, etc.The work [12] presents an overview on multi-robot systemsfocusing on aspects like system architecture, communication,task allocation, and application; while the work [11] dealswith the concepts of distributed intelligence and introducesa classification based on exhibited interactions.Recent researches in this field are mainly focused on distributedand cooperative robotic systems [4], i.e. systemswhere each robot interacts only with its neighbors and,together, they generate the global behavior of the team. Thework [8] presents an overview on networked robots, that ismultiple robots that cooperate by network communication,and explores the research challenges due to the interactionamong control, communication and perception. The book [9]provides an introduction to analysis and design of dynamicmulti agent networks focusing on graph theoretic methods.Several recent studies deal with the development of distributedcontrol approaches for multi-robot systems with theaim of achieving a global task (e.g., controlling the geometricalcentroid) by using distributed controllers. An aspect ofthe mentioned research poses the challenging problem of consensusfor multi-agent systems, i.e., reaching an agreementregarding a certain variable depending on the state of all theagents. In references [13] and [10] the consensus algorithmsare investigated with emphasis on robustness, time-delays andperformance bounds; while the results in [6] are related tothe stability analysis of several decentralized strategies thatachieve an emergent behavior. Networked robots give risealso to the problem of connectivity maintenance, as addressedin [7], that is the problem of how to ensure that the group ofrobots stay connected while accomplishing their task.Fig. 1.Team of Khepera III mobile robots.With the final aim of developing and testing distributedcontrol algorithms, we build up a completely distribute experimentalset-up composed of several Khepera III mobile robotsequipped with Laser Range Finder (LRF) and communicatingvia ad-hoc wireless network. Here we present preliminaryexperiments we performed with such platform. In particular,we at first implemented on board each robot a localization algorithmtaken from the literature [14] that allows the robots tonavigate in indoor environment (structured environment andcorridor). Specifically, the localization algorithm is a featurebasedExtended Kalman Filter (EKF) that uses odometrymeasurement for motion prediction and leaser informationfor measurement correction. Then, to control the robotsmotion, we refer to the Null-Space based Behavioral control,developed by some of the Authors of this paper [2]-[3], andwe use it to make the robots navigate towards waypoints,exchanging information via ad-hoc network, and avoidingcollision among themselves and with dynamic obstaclesdetected on their path. Results of tests with a single robotavoiding unpredicted obstacles and with four robots avoidingcollisions are presented.

II. EXPERIMENTAL PLATFORMFor the experiments presented here, we used a team of fourKhepera III mobile robots, produced by K-Team corporation,that are small size (12cm diameter) differential drive mobilerobots whose base is equipped with several proximity sensors(11 infrared sensors TCRT5000 Vishay Telefunkenand, 5ultrasound sensors 400ST/R100 Midas Components Ltd).The robots have been equipped with an extension board,namely the Korebot II, that is an embedded platform basedon the gumstix Verdex PRO with a Marvell PXA270 XScaleprocessor @ 600MHz, with 128MB RAM and 32MB Flashmemories. The Korebot II provides a standard embeddedLinux operating system (Angstrom distribution, kernel 2.6)and, to program it, we made use of the Khepera III toolboxdeveloped from the Distributed Intelligent Systems andAlgorithms Laboratory (DISAL) of EPFL.To allow robot-robot and, eventually, robot-pc communication,the robots have been equipped with an IEEE 802.11wireless card. In particular, the robots communicate througha wireless ad-hoc network built with the idea to performcompletely distributed experiments. In fact, the final objectiveis to perform experiments in (indoor) large environmentswhere the robots directly communicate only with their neighbors(i.e, other robots within the communication range)avoiding fixed access points. Apart from the configurationof ad-hoc network parameters, the communication strategyimplemented on board each robot exercises a multi-processarchitecture where each robot is able to broadcast informationusing UDP/IP protocol, and store information received fromits neighbors. Eventually, if a robot needs to send specificinformation to a not-directly connected robot, it can usemulti-hop communication. We have tested specific linuxfunctions as ip farwarding and olsrd daemon for the same.To improve the robots perception capabilities we equippedthem with Hokuyo LRF URG-04LX-UG01. The LRF isconnected to the Korebot II thorough USB but we provide itpower supply from external batteries. The Hokuyo LRF hasadequate performance in terms of weight, power consumption,range and accuracy, to properly fit with Khepera IIIrobots and allow them navigating in a indoor environment.III. EXTENDED KALMAN FILTER LOCALIZATIONMobile robot localization (often called as position estimationor position tracking) is the problem of determining the poseof a robot relative to a given map of the environment, and itis one of the most basic perceptual problem in robotics. Themaps are usually described in a global coordinate system,thus, localization is the process of establishing correspondencebetween the map coordinate system and the robotslocal coordinate system. Unfortunately, for indoor or GPSdenied environment, pose usually can not be directly sensed,thus it has to be inferred from data. However, a singlemeasurement is usually not sufficient to properly determinethe pose, and the robot has to integrate data over time. Thus,for the localization, the robot needs both a-priory informationabout the environment and information from exteroceptivesensors during the navigation.In this paper we refer to the approach presented in [14]-[5] and references therein; specifically we implemented afeature based Extended Kalman Filter (EKF) localizationalgorithm where the reference features are represented as linesegments, and data association and measurement correctionare performed in the Hough domain. Lines, as geometricfeature, provide strong and accurate information, but withfar less numbers than that of points. This solution properlyfits with our specifications since it works well for indoorenvironment, where the number of linear features is high(walls, doors, closets), it does not require high computation(which is of primary importance for the robotic platform weare using), it is robust to unpredicted obstacles and fit wellwith our obstacle avoidance strategy.In particular, robot localization is performed referring to adiscrete-time formulation of the EKF that, as well known,is an approximate estimator for the states and parameters ofnonlinear systems. Here, we assume that the state and themeasurement dynamics are respectively governed by:x k+1 = f(x k ,u k )+w k (1)y k = h(x k )+v k (2)where x k is the state (pose and orientation),u k is the controlinput, y k is the measurements, w k and v k are respectivelyprocess and measurement zero mean Gaussian noise withR wand R v covariance matrixes. Moreover we assume that wecan measure the true initial location x 0 of the robot withsome precision and we can use it to initialize the EKF withsome uncertainty. Then, the EKF implementation follows thetwo-step iterations:A. Prediction of state and estimation of error covarianceThe state prediction is performed on the base of the knowledgeof kinematic model parameters and input values. In thespecific case, it is performed using the input values u k =[s l s r ] T (i.e. the left and right wheels motion displacements,see fig. 2) and the knowledge of the odometric [ model ] parameters.The process covariance is R w = l 0 σ20 σr2 wherewe suppose a linear growth of the error model covarianceσ l = k l |s l |,σ r = k r |s r |. Thus, the system state is given byposition and orientation of the robotx k = [x k y k θ k ] T (3)and the state prediction update ˆx − k+1 = f(ˆx+ k ,ū k) is givenby:⎡ˆx + ⎤ ⎡⎤(s l,k +s r,k )ˆx − k+1 = k⎣ŷ + 2cos(ˆθ + k⎢+ s r,k−s l,k2b)(sk⎦+ ⎣ l,k +s r,k )2sin(ˆθ + k + s r,k−s l,k⎥2b) ⎦ (4)ˆθ + ks r,k −s l,kb

where b is the distance between the wheels. The estimationerror covariance is given byP − k+1= F k P + k FT k +G kR w G T k (5)where F k = ∂f∂x | x=ˆx + ku=ū kand G k = ∂f∂u | x=ˆx + ,u=ū k kthat, for the specific case are:⎡⎤⎢1 0 − (sl,k+s r,k )2sin(ˆθ + k + s r,k−s l,k2b)F k =(s⎣0 1 l,k +s r,k )2cos(ˆθ + k + s r,k−s l,k⎥2b) ⎦ (6)0 0 1⎡12G k = ⎣(cos(Θ)+Ssin(Θ)) 12 (cos(Θ)−Ssin(Θ))⎤12 (sin(Θ)−Scos(Θ)) 12 (sin(Θ)+Scos(Θ)) ⎦− 1 bwhere Θ = ˆθ + k + s r,k−s l,k2band S = (s l,k+s r,k )2b.s l,k1b(7)yy [m]1.510.50−0.5−1−1.5−2−1 0 1 2 3x [m]Fig. 3.ρ = xcos(α)+ysin(α)ρ 1α 1Line extraction from LRF scan.l 1l 1(α 1,ρ 1)ρFig. 2.x k+1 ,y k+1 ,θ k+1s r,kx k ,y k ,θ kEKF prediction based on odometric parameters.B. Measurement updates of state and estimation errorThe measurement update is performed using line informationfrom the LRF. Specifically, to extract line segments from theset of points of the LRF scan (see fig. 3), we use recursiveline splitting method[1], which is reliable and fast. Varioussteps of this method are described as follows:1) form a line from start and end points of the scan;2) find the point with the biggest distance to this line;3) if its distance is lower than a certain threshold, a linesegment is found;4) if not, split the point group at this point and repeat.After getting the line segments information, they were representedin the Hough Domain. The Hough transform allowsto represent a line represented in a Cartesian coordinateusing two parameters ρ and α that respectively represent thedistance of the origin from the line and the angle of thesegments starting from the origin and perpendicular to theline (see fig. 4). Thus, for each segments, we store ρ and αand the coordinates of the vertex points.Obtained the list of observed features, we have to performthe Data Association, that is, try to associate each observedfeature with a corresponding feature of the known map. Toallow the correspondence, both maps (a-priori and observed)were expressed in the same reference system (the robot localreference system), and the comparisons and the associationwere made in the Hough domain following the steps:αFig. 4.Transformation of line parameters in Hough domain.1) For all the observed landmark (NO), store in a list AHough coordinates and segments vertex coordinates;2) For each observed landmark in A find, if there exists,the features of the map with a certain threshold, i.e.the points in Hough domain with distance from thelandmark lower than a certain threshold; store them ina list B (see fig. 5);3) Among the different map features in the list B, find thesegment having mid point with the minimum distancefrom the observed segment midpoint;4) If the distance between the midpoints is lower thena certain threshold, then associate this feature to theobserved landmark.210−1−2−3−40.5 1 1.5 2 2.5ρxy [m]210−1−2−3 −2 −1 0 1 2 3x [m]Fig. 5. Data association between observed landmarks (red) and map (blue):correspondence in Hough domain and among segments.Indicating the NO observed landmarks with successful associationprocedure as:y k = [[r 1 ψ 1 ][r 2 ψ 2 ]···[r NO ψ NO ]] Tα

and indicating the associated map landmarks as:h(ˆx − k+1 ) = [[ρ 1α 1 ][ρ 2 α 2 ]···[ρ NO α NO ]] Tthen the measurement updates of state and estimation errorof the EKF has the form:K k+1 = P − k+1 HT k[H k P − k+1 HT k +R v ] −1 (8)ỹ k+1 = y k −h(ˆx − k+1 ) (9)ˆx + k+1 = ˆx − k+1 +K k+1ỹ k+1 (10)P + k+1= (I −K k+1 H k )P − k+1(11)where H k = ∂h ∂x | x=ˆx − = [ H T T1,k NO,k] ...HT andk+1[ ]−cos(αi ) −sin(αH i,k =i ) 0.0 0 −1IV. NULL-SPACE BASED BEHAVIORAL CONTROLTo manage robots’ motions we refer to the Null-Space basedBehavioral (NSB) control, that is a behavior based approachdeveloped by some of the Authors to control both singleand multiple robots (see e.g. [2]-[3]). This control approachallows to simultaneously manage multiple tasks arranged inpriority. The basic idea is that the lower priority tasks aresolved in the null-space of higher priority ones so that they donot effect them. We report the basic concepts in the following.Let define the position of the j th robot as p j = [ ] Tx j y jand the generic task variable to be controlled as σ ∈ R mσ = f(p j ) (12)The corresponding differential relationship is:˙σ = ∂f(p j )∂p jv j = J(p j )v j (13)where J ∈ R m×2 is the robot configuration-dependent taskJacobian matrix and v j ∈ R 2 is the robot velocity.The motion directives to the robot, i.e. velocity referencecommand, are elaborated as:v d,j = J † (˙σ d +Λ˜σ) (14)where J † = J T (JJ T ) −1 , Λ is a suitable constant positivedefinitematrix of gains and ˜σ = σ d −σ is the task error.Let us consider the mission for the j th robot composed bytwo elementary tasks. Using the subscript i referring to thei th task quantities for the j th robot, on the analogy of theabove equation, the i th task velocity is computed asv i = J † i (˙σ i,d +Λ i˜σ i ) (15)If the subscripti also denotes the degree of priority of the taskwith, e.g., Task 1 being the highest-priority one, accordingto [3] the final motion command to the robot is modified into:v f,j = v 1 +(I −J † 1 J 1)v 2 = v 1 +N 1 v 2 (16)whereI is the identity matrix of proper dimensions andN i isthe null-space projection matrix of the i th task. Remarkably,above eq. has a nice geometrical interpretation. Each taskvelocity is computed as if it were acting alone; then, beforeadding its contribution to the vehicle velocity, a lower-prioritytask is projected onto the null space of the immediately higherpriority task so as to remove those velocity components thatwould conflict with it.For the experiments presented in the next session we considereda multi-robot system where each robot accomplishesan individual task of moving toward assigned waypoints (orgoals) avoiding obstacles and collision with other vehicles.Thus, the mission of each robot is decomposed in twoelementary tasks:• Move to Goal. The move-to-goal task function is:σ g = p ∈ R 2 (17)while the desired values is σ g,d = p g where p g are thecoordinates of the goal. Since J g = J † g = I ∈ R 2∗2 ,the output velocity is given byv g = Λ g (p g −p) (18)that is a velocity in the goal direction proportional tothe distance from the goal p g .• Obstacles and collision avoidance. In presence of anobstacle in p o in the advancing direction, the aim ofthe task is to keep the robot at a safe distance from it.Therefore the task function isσ o = ||p−p o || ∈ R, (19)and σ o,d = S d where S d is the safety distance from theobstacle. Then, it holdsJ o = ˆr T ∈ R 1∗2 (20)where ˆr = p−p o||p−p o ||is the unit vector aligned with theobstacle-to-vehicle direction. Therefore, the task outputis a velocity, in the robot-obstacle direction, that keepsthe robot at a safe distance from the obstacle.v o = J † oλ o (S d −||p−p o ||). (21)while the task null-space matrix I − J † oJ o projectsvelocity vectors of lower priority task in the directiontangent to a circle centered in p o and passing for p.In presence of a linear obstacle, the task function isanalogous to the previous case with the difference thatp o is the point of the segment closest to p.The obstacle-avoidance is usually the highest priority tasksince its achievement is of crucial importance to preservethe integrity of the vehicle, however it is activated only inspecific conditions, that is when the robot is moving towardthe obstacle and when the distance is lower than a certainthreshold (see fig. 6). In this case, the implementation of

yyyythe NSB approach makes the robot moving toward the goalsliding around the (point or linear) obstacle keeping a certainsafety distance.GoalN 1v gv gv fv oLinear obstacleFig. 6. Example of velocity vector of a move to goal function in presenceof linear obstacles.It is worth noticing that the output of the NSB is a linearvelocity command elaborated considering the robot as amaterial point. To use the NSB to control a non-holonomicvehicle as the Khepera III, the output velocity of the NSBis passed to a Low-Level control that properly elaboratesangular and advancing velocity and, using odometric modelparameters, converts it to left/right wheels velocity command.In the implementation of the algorithms, a saturation isperformed on the resulting velocity in order to feed thesystem with limited amplitude signals.V. EXPERIMENTAL RESULTSIn this section we will illustrate two experiments, the firstdone with a single robot localizing using the EKF algorithmand avoiding obstacles using the NSB control, and the secondperformed with four robots avoiding avoiding collisions usingboth LRF and ad-hoc wireless communication.map of the environment available to the robot, while the blacksegments are the segments extracted from the LRF data; itis worth noticing that some of that segments are associatedto the map and allow the localization, while other segmentsare used for obstacle avoidance. The red triangle representsthe differential drive robot with the green vector representingthe v g and the blue vector representing the v f velocities forthe robot. The blue/magenta circles represent the referencevalues for the obstacle avoidance task, respectively thresholdfor detection of an obstacle and safety distance S d . Figure 8shows the sliding behavior of the robot around the blackcolored obstacle detected from the LRF. The experiment wasdone with the robot’s saturation velocity set to 20 cm/s, Λ gset to a diagonal matrix with elements 0.8, and λ o set to 1. Acomplete video of the experiment that shows the robustnessof the localization and obstacle avoidance algorithms evenin the case of a human entering the arena is available athttp://webuser.unicas.it/lai/robotica/video.html54.543.532.521.510.500 0.5 1 1.5 2 2.5 3 3.5 4x54.5454.543.532.521.510.500 0.5 1 1.5 2 2.5 3 3.5 4x54.543.53.5332.52.5221.51.511Fig. 7.Snapshot single robot case.In the first experiment the robot was commanded to iterativelymove between two waypoints whose coordinate areassigned relatively to the global map of the environment. Therobot had two tasks in hand: move towards the goal and avoiddynamic obstacles eventually detected in the environment.The obstacle avoidance, when active, was the highest prioritytask. Figure 7 shows a single snapshot of an iteration wherethe robot detects and avoids an obstacle not present inthe environment map. Figures 8 shows several snapshotsillustrating a reconstruction from experimental data of therobot behavior. In particular, the blu perimeter represents the0.5Fig. 8.00 0.5 1 1.5 2 2.5 3 3.5 4x0.500 0.5 1 1.5 2 2.5 3 3.5 4xSnapshot single robot case; reconstruction from experimental data.To test the robustness of the localization algorithm we alsoperform several tests in noisy indoor environment (a corridorwith open doors, closets with glass wall, people walkingaround); results are not reported here for space limitationbut reconstruction form experimental data is available at theprevious web-page.The second experiment was performed using four Khepera IIIrobots iteratively moving among waypoints. The robots wereinitially displaced in a cross configuration where each robot

yyyyhas to exchange its position with his diametral opposite robot.The robots are not coordinate but they exchange their positioninformation using wireless ad-hoc network to facilitate theobstacle avoidance. As for the previous experiment, thesaturation velocity is set to 20 cm/s, Λ g set to 0.8I 2 and λ oset to 1. Figure 9 shows a single snapshot of the experiment,while fig. 10 shows the reconstruction from the experimentaldata taken from one of the robot. As shown in the videoavailable at the previous web-page, the robots also avoidcollisions with unpredicted obstacle.four Khepera III mobile robots with LRF. We implementedand tested a feature-based EKF localization algorithm andwe used it together with a behavior-based motion control,the NSB, to allow the robots navigating in indoor environmentavoiding unpredicted obstacles and collisions amongthemselves. Despite in the performed experiments the cooperationamong robots is limited to the position informationexchange for obstacle avoidance, testing the robustness andthe performance of the system represented a preliminaryresult that allows to focus our future work on the developmentand testing of distributed coordination control algorithm formulti-robot systems.ACKNOWLEDGMENTSThe Authors would like to thank prof. Gianluca Antonelli for hisvaluable comments and suggestions for the realization of this work.The research leading to these results has received funding fromthe Italian Government, under Grant FIRB - Futuro in ricerca 2008n. RBFR08QWUV (NECTAR project), and under Grant PRIN 2008n. 2008EPKCHX (MEMONET project).54.543.532.521.510.54.53.52.51.50.5Fig. 9.00 0.5 1 1.5 2 2.5 3 3.5 4x54321Fig. 10.00 0.5 1 1.5 2 2.5 3 3.5 4xSnapshot multi-robot case.54.543.532.521.510.54.53.52.51.50.500 0.5 1 1.5 2 2.5 3 3.5 4x5432100 0.5 1 1.5 2 2.5 3 3.5 4xSnapshots multi-robot case; reconstruction from experimental data.VI. CONCLUSIONIn this paper we presents preliminary experiments we performedwith a distributed multi-robot system composed ofREFERENCES[1] G.C. Anousaki and K.J. Kyriakopoulos. Simultaneous localization andmap building of skid-steered robots. Robotics & Automation Magazine,IEEE, 14(1):79–89, 2007.[2] G. Antonelli, F. Arrichiello, and S. Chiaverini. The entrapment/escortingmission: An experimental study using a multirobotsystem. IEEE Robotics and Automation Magazine (RAM). SpecialIssues on Design, Control, and Applications of Real-World Multi-RobotSystems, 15(1):22–29, March 2008.[3] G. Antonelli, F. Arrichiello, and S. Chiaverini. The Null-SpacebasedBehavioral control for autonomous robotic systems. Journalof Intelligent Service Robotics, 1(1):27–39, Jan. 2008.[4] F. Bullo, J. Cortés, and S. Martínez. Distributed Control of RoboticNetworks. Applied Mathematics Series. Princeton University Press,2009.[5] W. Burgard, D. Fox, and S. Thrun. Probabilistic robotics, 2005.[6] A. Jadbabaie, J. Lin, and A.S. Morse. Coordination of groups of mobileautonomous agents using nearest neighbor rules. IEEE Transactionson Automatic Control, 48(6):988–1001, 2003.[7] M. Ji and M. Egerstedt. Distributed Coordination Control of MultiagentSystems While Preserving Connectedness. IEEE Transactions onRobotics, 23(4):693–703, 2007.[8] V. Kumar, D. Rus, and S. Sukhatme. Springer Handbook of Robotics,chapter Networked Robots, pages 943–958. B. Siciliano, O. Khatib,(Eds.), Springer-Verlag, Heidelberg, D, 2008.[9] M. Mesbahi and M. Egerstedt. Graph theoretic methods in multiagentnetworks. Princeton Univiversity Press, 2010.[10] R. Olfati-Saber, J.A. Fax, and R.M. Murray. Consensus and cooperationin networked multi-agent systems. Proceedings of the IEEE,95(1):215–233, Jan. 2007.[11] L.E. Parker. Distributed intelligence: Overview of the field and itsapplication in multi-robot systems. Journal of Physical Agents, 2(1):5,2008.[12] L.E. Parker. Springer Handbook of Robotics, chapter Multiple MobileRobot Systems, pages 921–941. B. Siciliano, O. Khatib, (Eds.),Springer-Verlag, Heidelberg, D, 2008.[13] W. Ren, R.W. Beard, and E.M. Atkins. Information consensus inmultivehicle cooperative control. IEEE Control Systems Magazine,27(2):71–82, Apr. 2007.[14] L. Teslić, I. Škrjanc, and G. Klančar. Ekf-based localization of awheeled mobile robot in structured environments. Journal of Intelligent& Robotic Systems, 62(2):187–203, 2011.