1  2  3  4  5  6  7  8  9  10  11  12  13  14  15  16  17  18  19  20  21  22  23  24  25  26  27  28  29  30  31  32  33  34  35  36  37  38  39  40  41  42  43  44  45  46  47  48  49  50  51  52  53  54  55  56  57  60  61  62  63  64  65  

Formation Control Implementation Using KobukiTurtleBots and Parrot Bebop Drone*

Nicolas Gallardo, Karthik Pai, Berat A. Erol, Patrick Benavidez and Mo JamshidiDepartment of Electrical and

Computer EngineeringThe University of Texas at San Antonio

San Antonio, TX 78249Email: hbq744@my.utsa.edu, dxq821@my.utsa.edu, berat.erol@utsa.edu, patrick.benavidez@utsa.edu, moj@wacong.org

Abstract—Formation control of a collection of vehicles isa topic that has generated a lot of interest in the researchcommunity. This interest primarily stems from the increasedperformance and robustness that is provided by a swarm ofagents as compared to an individual member. Formation controlcan be achieved through many approaches. The approach used bythis paper is based on a leader-follower premise. A network ofagents can be controlled by assigning a leader for each agentin the formation. The group as a whole will be capable offollowing either a Virtual Leader (VL) or an agent within thegroup. The algorithm applied to a test-bed consisting of threeKobuki TurtleBot2 robots. Each Turtlebot2 is programmed tofollow a pre-defined virtual point in the formation. The test spaceis monitored by a Parrot Bebop drone hovering overhead thatidentifies agents uniquely through image processing techniques.The agents can then move in the test space, based on the leader’sposition, while maintaining a formation.

Index Terms—formation control, virtual leader, robot, drone,UGV, UAV, turtlebot, parrot bebop

I. INTRODUCTION

Groups of autonomous vehicles provide additional perfor-mance and functionality beyond the capabilities of individuals.A group can solve problems and achieve objectives whichare difficult for individual robots to achieve in a reasonabletime frame. In a target identification scenario, for example,the probability of targets being missed in a highly clutteredenvironment is much greater for a single surveillance vehiclethan it is for a formation [1]. Environmental exploration isanother area in which a formation of agents will have increasedperformance as compared to a single agent. SimultaneousLocalization and Mapping (SLAM) is an algorithm usedin environmental exploration that uses a graph-based mapmethod. In [2], an implementation of SLAM is presented thatrepresents nodes as the current pose of a robot and graph edgesas constraints between each pose. In this example, a map of therobot’s environment according to its position and pose can beeasily communicated to collaborating agents. The additionalsources of data from the members of the formation will onlyhelp with accuracy of the generated maps. Incremental andpersonal map data from multiple agents can be merged to get

* This work was supported by Grant number FA8750-15-2-0116 from AirForce Research Laboratory and OSD through a contract with North CarolinaAgricultural and Technical State University.

a complete map of the whole area. New data only needs to beinserted into the graph by connecting them to the correct nodes[3]. In a formation case, agents require methods to isolatedetected features of their peers from that of the environmentallandmarks.

Distributed group motion and control was first identifiedby Craig Reynolds through his study on schooling fishesand flocking birds [4]. Since then, attempts have been madeto model swarm behavior based on rules of attraction andrepulsion between neighbors in the swarm [5]–[7]. Such rulesof attraction and repulsion can be used to guide groups ofautonomous vehicles together in a controlled formation.

We were inspired by the formation control approach out-lined in [8] mainly because the method has light computationalrequirements and has not yet been implemented on hardware.This approach requires that we know the current position of theagents, such that the attractive and repulsive forces betweenthem can be calculated real-time and an initial formationcan be maintained. Information regarding the current positionof the agents can be obtained by several different methods.Finding coordinates of the agents through GPS is an attractivesolution because of its high accuracy [9], so we wanted ourapproach to be expandable in order to make use of GPS infuture real-world applications. In our GPS-denied laboratorysetup, we use an overhead camera with color identificationand tracking as an efficient solution to obtaining the coordinatedata of each Unmanned Ground Vehicle (UGV) in the system.Color tracking is implemented using a video feed from adrone hovering overhead. UGV coordinate data is used tocalculate the forces applied to the UGV to maintain the desiredformation. The forces are applied as to modify the UGV’sspeed and orientation. This entire process is outlined in thefollowing sections.

The underlying software architecture is an important part ofour work. We use Robot Operating System (ROS), a robustmessage passing infrastructure, to pass all of the navigationdata mentioned above back and forth between each sub-process that may need this data. For example, the coordinatesof each UGV are obtained by a C++ program running thecolor identification and tracking algorithm. In parallel, anotherC++ program calculates all the forces needed to maintain

WAC 2016 1570250515

1

the formation according to the coordinate data from thecolor tracking program. This situation, where one programdepends on incoming data from another program, is a keystrength of ROS’s message passing architecture. It utilizesa publisher/subscriber relationship between parallel programsto share data. In the example above, once coordinate data ispublished as a topic in ROS, this topic is subscribed to by theprogram that needs this information. Not only is ROS a robustmessage passing platform, it is also open source, allowingcollaborative development and sharing of software packages.This minimizes the need to rewrite a software package forcommon functions that have already been addressed.

The rest of the paper will be organized as follows: Thearchitecture of the system will be highlighted in Section II,with the hardware setup in Section III, followed by a detailedexplanation of the software processes in Section IV. Finally,experimental data will be discussed in Section V along withconclusions drawn from this experiment in Section VI.

II. ARCHITECTURE OF THE SYSTEM

The ROS Master is the Central Command Station, it can beexecuted on any piece of hardware that is running ROS. Inour experiment we assigned the ROS Master role to a laptop.Each Agent and the Drone communicates with the ROS Masterthrough Wireless Networks. The ROS Master gets real-timeVideo from the Drone, identifies and tracks each agent fromthe video feed using the HSV Algorithm, and calculates theforces between the agents based on the Algorithm explainedin further sections. This Force value is then translated intolinear and Angular data to the agents, so that they can move information. This structure is shown in Fig. 1. The orientation ofeach agent is also collected real-time to keep track of error inmovement, so that further commands can be issued to maintainformation.

Fig. 1: System Architecture

III. EXPERIMENTAL SETUP

A. The Test Space

The Test Space consists of three Kobuki TurtleBot Un-manned Ground Vehicles, each distinctly identified throughan assigned color on the top of the TurtleBot, such that theyare in visual range of the Parrot Bebop UAV.

(a) Parrot Bebop UAV (b) Kobuki TurtleBotUGV

Fig. 2: UAV and UGVs used in the experimental test bed.

A Kobuki TurtleBot is a low-cost, open source differentialdrive robot. It consists of a mobile base, an RGB-D sensor andan Arduino processor making it a perfect entry-level mobilerobot platform. The Kobuki was chosen because it is an opensource UGV platform, making it perfect for research anddevelopment. The Kobuki SDK is based off ROS, which is thepreferred development platform for ACE researchers becauseof its intuitive publisher/subscriber message passing structurethat allows robust and simple communication within multiplefacets of a robotic system.

Fig. 3: Representation of the TurtleBot2 UGV in the Coordi-nate System

The Turtlebot uses a differential drive to steer, and Cookprovides a clear approach in order to obtain and simulatethe robots movement [10]. The following are the kinematicequations, as shown in the Figure 3, R is the instantaneousradius of curvature of the robot’s trajectory, and W stands forthe distance between the wheels.

vl = θ(R− W

2), vr = θ(R+

W

2) (1)

where:vl: velocity of the left wheel

2

vr: velocity of the right wheel

θ: angular rate

Next, the Angular Rate of the robot is calculated:

vr − vl = θ(R+W

2−R+

W

2) = θW

θ =vr − vlW

(2)

Next the instantaneous radius of curvature is calculatedusing (1):

R =vl

θ+W

2

Using (2) we have:

R =vl

vr−vlW

+W

2=

2Wvl +W (vr − vl)2(vr − vl)

R =W

2∗ vr + vlvr − vl

(3)

Velocity along the robot’s longitudinal axis is calulatedusing (2) and (3):

vy = θR =vr − vlW

∗ W2∗ vr + vlvr − vl

=vr + vl

2(4)

Representing the robot’s velocity on earth coordinates wehave:

x = −vycos(90− θ) = −vy(cos(90)cos(θ) + sin(90)sin(θ))

y = vycos(θ)

Using equation (4) we have:

x = −vr + vl2

sin(θ) (5)

y =vr + vl

2cos(θ) (6)

For this reason the control variables will be:

vr = u1

vl = u2

The colors for the UGV identification are distinct colorswith a white background with very different HSV windows,so that mis-identification is avoided.

The Parrot Bebop Drone is a lightweight yet robust quadcopter with a 1080p ”fish-eye” camera and 3-axes imagestabilization. This allows the drone to be able to pan its camerain-flight to be able to access the location of the agents at alltimes. For this test setup in lab, we fixed the drone in a static”hover position” on top of a long pole, in order to maximizethe battery life. The ROS-Bebop driver was used in order toobtain data from the Bebop and make it available in ROS.

B. Data from the UGV TurtleBots and the Bebop Drone

a) Bebop Drone: The Bebop drone primarily sends inthe video feed from the test space to the ROS master. TheHSV algorithm is run on this input video to detect objectswithin the specified HSV values for red, yellow and green.For the identified objects in the video feed (the mobileagents) the HSV algorithm calculates the coordinates of thegeometric center of each object in reference to the CameraFrame as shown in Fig. 4. From the coordinates obtainedby the HSV algorithm, we can calculate a Virtual Leaderposition for the formation. The Virtual Leader is essentially thegeometric center of the formation, calculated from the agent’scoordinates.

Fig. 4: The Test Space from the UAV’s perspective

b) UGV TurtleBots: The UGVs publish their currentheading and state to the ROS Master. This data is essential astheir current heading is with reference to their own body frame.So, as the formation control algorithm outputs commands tothe UGVs with reference to the world frame, the body framedata from the UGVs is required in order to obtain the rightoutput data after the necessary translation and rotation. TheROS topics of interest published by the UGVs are currentorientation, current linear velocity, and current angular velocitydata as feedback.

C. Color Identification and Tracking

The developed color tracking ROS software package is aROS compatible version of the open source C++ color trackingsoftware in [11]. This program uses OpenCV to detect theHSV values of objects in an image frame. Each UGV wasequipped with a color tag (see Fig. 6) and when viewed bythe UAV’s camera from above the color tracking software canextract both the color and the (x, y) coordinate of the centerpoint of the tag as per the algorithm shown in Fig. 5. Thecoordinate data is made available for all other ROS nodes touse. The formation control ROS package is our experimentalimplementation of the controller presented in [8]. Written inC++, it utilizes incoming UGV coordinate values to calculatea force magnitude that translates into the desired speed anddirection of the robot.

3

Fig. 5: Color Detection and Tracking

D. Formation Control Algorithm

In this process, potential functions between the agents areused to implement group cohesion and separation. With sucha methodology, navigation of the swarm is a process ofharnessing the various potentials between the agents. A lot ofresearch in Swarm behavior has been done proposing variousformation control and coordination techniques [12]. However,as the algorithms become more complex, computational re-quirements are increased and the communication demandsfrom each agent become very prohibitive. The implementedalgorithm is based off [8] and [13]. The methodology usesenergy potential between agents called ’Agent Forces’ andbetween an ’artificial’ virtual leader called ’Leader Forces’.This Virtual leader is used in order to advance the groupthrough its environment. The Virtual Leader is not one ofthe agents or a physical vehicle, but an imaginary pointcalculated from the agents’ positions. It is used as a guide formovement of the group. The goal of using potential energyfunctions between agents is to repel or attract vehicles fromand to each other as the Virtual leader is moved throughthe environment.The dependence on the Virtual leader motionenables us to plan only the motion of the virtual leader, therebyreducing the task planning requirements.

a) Virtual Leader Forces: Once the Agents are in theirinitial positions, an initial Virtual leader can be calculated byfinding the center of mass for the whole group.

xcm =1

N

N∑i=1

xi

ycm =1

N

N∑i=1

yi

where xcm and ycm are the coordinates of the center of mass,or the geometric center of the formation.

The initial distance between each agent determines theposition of the Virtual Leader and the distance between the

agents and the Virtual leader dV L0 is the equilibrium distance

between each agent and the Virtual leader. The Virtual leaderforces FV L can be defined as:

FV Lx = KV L[d

V Lx − dV L

x0 ]

FV Ly = KV L[d

V Ly − dV L

y0 ]

dV Lx = xV L − xidV Ly = yV L − yi

where KV L is the Gain Constant, which can be adjustedas per the required elasticity of the forces between the agentsand the virtual leader.

Fig. 6: The Formation forces

b) Inter-Agent Forces: Inter-Agent forces are required sothat the agents maintain their equilibrium distance dij0 fromeach other as they follow the Virtual Leader, and the formationis maintained. The inter-agent forces Fij can be defined as:

F ijx = Kij [d

ijx − d

ijx0]

F ijy = Kij [d

ijy − d

ijy0]

dijx = xij − xidijy = yij − yi

Fig. 6 shows a pictorial representation of the position of theVirtual leader and the various forces acting on the agents.

IV. OUTPUT DATA TO THE UGVS

After the algorithm computes the various forces acting onthe agents, a single ”Sum of all Forces” is sent as output tothe UGVs. As the Total force output to the UGV is a vector,both the magnitude and direction needs to be translated for theUGV. Therefore, from the algorithm described in the earliersection, the x-component of the Force and Y-Component of theForce are calculated separately using the X and Y distancesseparately. Once we know the values of Fx and Fy , we cantranslate them into commands that can be understood by theUGV, namely ”Linear Command” and ”Angular command”.

4

Fig. 7: Force Vector for the UGV

a) Inter-Agent Forces: The Formula for the direction ofthe forces is:

θ = tan−1Fy

Fx

The Magnitude of the force in the direction θ is:

|~F | = |Fx|cosθ

or |~F | = |Fy|sinθ

It is to be noted that the reference frames for the UAVand UGVs are different as can be seen from Fig. 4. Thatis one aspect of the translation that needs to be carried out.Also, the real-time orientation of the UGV is important tounderstand which quadrant the angle of the UGV is in, so thatthe necessary translation can be done as per Fig. 8. Specialcases such as when the θ is very close to 0 deg or 30 deg aretaken care of separately, in order to avoid unnecessary rotationof the UGVs.

Also, if the final target θ is smaller than 180 deg, the UGVrotates anti-clockwise towards its target, and if greater than 10deg, turns clockwise towards the target. This is essentially foroptimization of the movement of the UGVs.

Thus, the UGV knows where it is currently located andpointed, and the ”Sum of all Forces” from the algorithm isconverted to a linear force magnitude and the corrected angleof the force and they are given to the UGV as its linear andangular commands respectively.

V. EXPERIMENTAL RESULTS

The UGV’s successfully maintain formation for simpletrajectories. However, two major factors come into play inthe implementation of the algorithm. Disturbance-free colortracking in varied lighting conditions and the update rate ofthe commands sent to the UGV. We are working on finetuning the parameters to minimize the probability of unde-sirable movements of the UGV when for example, trackingof one UGV is suddenly lost during movement, or when theUGVs attain their formation but the feedback is lost due toupdate rate factors. Due to such factors, when we attemptto apply a more complex trajectory, the formation sometimes

Fig. 8: Angle Translation

exhibits undesirable behavior. Work is ongoing in this front tohave more predictable behavior under various circumstances.The following experimental plots show the behavior of theformation control algorithm. In Fig. 9 the Virtual Leader isvaried about the image frame, and the Inter-agent (Fig. 10) andAgent-Leader (Fig. 11) Equilibrium Error respond accordingly.The sudden changes seen in Fig. 11 correspond to the variationin the Virtual Leader’s position. The plots show that theformation control algorithm is capable of driving the errorback close to zero.

Fig. 9: Virtual Leader Position vs. Time

5

Fig. 10: Inter-agent Equilibrium Error vs. Time

Fig. 11: Leader-Agent Equilibrium Error vs. Time

VI. CONCLUSION

Formation control can be accomplished using a variety ofmethods, with each one having pros and cons compared tothe other. In our case, the controller we decided to implementwas both straightforward theoretically and programming wiseand was initially advertised as computationally lightweight.From the results obtained in Section V, it is reasonable to

say that the proposed controller is in fact implementable andcan be used to control a formation of N number of agents inthe system. Future planned work involves moving the decisionmaking process to the cloud so that a large number of agentscan successfully hold formation. Better methods of detectionand tracking of the agents are currently being researched aswell.

REFERENCES

[1] J. A. Sauter, R. S. Mathews, A. Yinger, J. S. Robinson, J. Moody,and S. Riddle, “Distributed pheromone-based swarming control ofunmanned air and ground vehicles for rsta,” in SPIE defense and securitysymposium. International Society for Optics and Photonics, 2008, pp.69 620C–69 620C.

[2] G. Grisetti, R. Kummerle, C. Stachniss, and W. Burgard, “A tutorial ongraph-based slam,” Intelligent Transportation Systems Magazine, IEEE,vol. 2, no. 4, pp. 31–43, 2010.

[3] M. Pfingsthorn, B. Slamet, and A. Visser, “A scalable hybrid multi-robotslam method for highly detailed maps,” in RoboCup 2007: Robot SoccerWorld Cup XI. Springer, 2008, pp. 457–464.

[4] C. W. Reynolds, “Flocks, herds and schools: A distributed behavioralmodel,” in ACM Siggraph Computer Graphics, vol. 21, no. 4. ACM,1987, pp. 25–34.

[5] A. Okubo, “Dynamical aspects of animal grouping: swarms, schools,flocks, and herds,” Advances in biophysics, vol. 22, pp. 1–94, 1986.

[6] K. Warburton and J. Lazarus, “Tendency-distance models of socialcohesion in animal groups,” Journal of Theoretical Biology, vol. 150,no. 4, pp. 473–488, 1991.

[7] G. Flierl, D. Grunbaum, S. Levins, and D. Olson, “From individuals toaggregations: the interplay between behavior and physics,” Journal ofTheoretical Biology, vol. 196, no. 4, pp. 397–454, 1999.

[8] G. H. Elkaim and R. J. Kelbley, “A lightweight formation controlmethodology for a swarm of non-holonomic vehicles,” in AerospaceConference, 2006 IEEE. IEEE, 2006, pp. 8–pp.

[9] A. R. Conway, “Autonomous control of an unstable model helicopterusing carrier phase gps only,” Ph.D. dissertation, stanford university,1995.

[10] G. Cook, Mobile robots: navigation, control and remote sensing. JohnWiley & Sons, 2011.

[11] A. Kaifi, H. Althobaiti, and Z. Rentiya,“akaifi/multiobjecttrackingbasedoncolor.” [Online]. Available:https://github.com/akaifi/MultiObjectTrackingBasedOnColor

[12] W. M. Spears, D. F. Spears, J. C. Hamann, and R. Heil, “Distributed,physics-based control of swarms of vehicles,” Autonomous Robots,vol. 17, no. 2-3, pp. 137–162, 2004.

[13] G. H. Elkaim and R. J. Kelbley, “Extension of a lightweight formationcontrol methodology to groups of autonomous vehicles,” in Proc.ISAIRAS, 2005, pp. 5–9.

6