A Distributed Coordination Model forMulti-Robot Box Pushing
Marin Lujak
Dip. Ingegneria dell’Impresa - University of Rome “Tor Vergata”,Italy (e-mail: lujak@ing.uniroma2.it).
Abstract:
 In this work, we approach the problem of a box transport by the robots that arerequested to bring a box from an arbitrary initial to some preassigned target position. Therobots are modeled as rational collaborative autonomous agents working on their movementcoordination in a discrete state space. The proposed method for coordinating the multi-robotsystem is a two-level control model in which on the upper level, a Virtual Structure (VS)agent calculates offline the box trajectory from the initial to the goal position with incompleteobstacles data and informs the robots of the nominal route. The robots on the lower level, onthe basis of the preassigned positions and the data about the unforeseen obstacles sensed bylimited range sensors and exchanged among all the robots, individually optimize their localmovement objective functions to mutually push the box following a preassigned trajectory in asafe manner. When the robots, due to the unforeseen obstacles, move from the trajectory definedby the VS agent too far away, more than a predefined distance, they contact the VS agent forthe recalculation of the same. The results of the proposed coordination model’s simulation in2D environment are described.
Keywords:
 Co-operative control, multi-agent systems, multi-objective optimization, robotcontrol.1. INTRODUCTIONIn this work, the problem of box pushing by collabora-tive autonomous robots is considered. This problem is aspecial instance of the formation motion planning prob-lem. The motion of robots in many existing formationmotion planning approaches is planned and executed byutilizing complete representations of a task environment,e.g., formation shape, obstacle positions, and completerobots configuration. Important issues are still how toacquire a collective goal-directed, task-driven behavior ina distributed way and how to effectively come up withthe coordinated motions for a formation of autonomousrobots when the information about the environment foreach robot is only partially available or is too costly toobtain. These issues are particularly relevant if we are todevelop robot formations that can work collectively andadaptively on a cooperative task, even though each robotcan only sense and hence react to its environment locally.Cooperative task is the task whose performance dependson the joint locations, roles and inputs of the agents withcentralized strategies depending on the location or role of all the agents in the task (see Murray (2007)).Our goal in this work is to allow each robot agent to planits motions nearly independently based on the preassignedformation trajectory and the exchange of the unpredictedobstacle data with other robots.The problem of formation coordination is presentedthrough the problem of a rigid box transport, where therobots need to bring the box from an arbitrary initial tosome predetermined target position by pushing it in acoordinated and collaborative manner. The pushing po-sitions of the robots on the box are preassigned and static,i.e., do not change in time.The key issues in coordinating the individual robots’ tra- jectories in the box movement are to avoid collisions be-tween the robots, the box, and the unpredicted obstacles,and to avoid deadlock, that is, situations where each robotis waiting for the other to proceed, see O’donnell andLozano-Periz (1989).We propose a two-level control model for coordinating themulti-robot system in which on the upper level inspiredby Lewis and Tan (1997), a fictitious Virtual Structure(VS) agent calculates offline the box trajectory from theinitial to the goal position with incomplete obstacles dataand informs the robots of the nominal route. The robotson the lower level inspired by Liu and Wu (2001), on thebasis of the preassigned positions and the data about theunforeseen obstacles sensed by limited range sensors inreal time and exchanged among all the robots, individu-ally optimize their local movement objective functions tomutually push the box. If there are unforeseen obstaclesin the way, the robots avoid them following as closely aspossible the preassigned trajectory received from the VSagent. When the robots find themselves too far away, morethan a predefined distance, from the trajectory definedby the VS agent, the VS agent recalculates the trajectoryfrom their momentary to the target position and informsthem of the same. In this way, we obtain an approach inwhich no centralized modeling and control are involvedexcept for a high level criterion for measuring the qualityof collective box pushing.
 
Primary contribution of this paper is the proposed modelwith an online distributed multi-agent solution to therobots’ box pushing coordination problem in an environ-ment with unpredicted obstacles.The paper is structured as follows: Section 2 providesthe related work. Section 3 gives a formal statement of the problem with the necessary definitions. Section 4presents the distributed algorithm for the coordinationof robot agents in a box pushing problem, and section 5describes our computer simulation setup and the resultsof a collective box-manipulating example, where in a 2Dworld, robots push a box to a goal destination. We closethe paper with the conclusion found in Section 6.2. RELATED WORKIn cooperative control, much attention has been givenlately to the multi-robot trajectory planning and motionexecution while the subjects as determining, coordinatingand executing a plan of action in response to real-timedata in the strategic decision making has not been thatexplored.Autonomous mobile robots can effectively perform certaintasks with their collective behaviors. A group of robots canbe used to carry out tasks that are difficult or not effectivefor a single robot to perform alone. In the literature,there have been roughly three major methods to formationcontrol of multiple robots: behavioral, leader following,and virtual structure.The behavioral approach (see, e.g. Balch and Arkin(1998)) is based on some desired behaviors for each robotand the coordination of the robots is achieved by weighingrelative importance of each behavior. The problem is thatin this way the group behavior cannot be explicitly defined,and the mathematical analysis of the solutions is difficultas well as the guaranty of the group stability.The disadvantage of the leader following approach (see,e.g. Fierro et al. (2001)) is that it assumes only onegroup leader with a unidirectional control and there is nofeedback from the followers to the leader of the group.Only the latter has the information regarding the grouptrajectory and the formation is built as a reaction of thefollowers to the motion of the group leader (see, e.g. Desaiet al. (1998)).In the virtual structure (VS) approach (see, e.g. Lewis andTan (1997)), the entire formation is treated as one agent. Itis a completely centralized approach where the full config-uration state of VS must be communicated to each robotin the formation. The advantage of VS for a box pushingproblem is maintaining of a rigid geometric relationshipamong multiple robots during different movements.Another important decentralized approach to formationcontrol is consensus algorithms (e.g. Olfati-Saber and Mur-ray (2007)) where rapid agreement of all the participants isreached by bringing their information states to a commonvalue and thus allowing effective task performance.The coordination of the robots’ movements in bringing abox toward a desired goal location was a subject of Liu andWu (2001), Chakraborty et al. (2008), and Mataric et al.(1995), among others. The difficulty of this task is that themovements of the robots with respect to the box cannotfollow an explicit, global plan and control command, due toplanning costs. In Chakraborty et al. (2008), a centralizedlocal planning scheme is adopted to determine the nextposition and turning angle of the box in an environmentwith a number of static obstacles. The box pushing prob-lem is resolved by formulating the box-shifting as a multi-objective optimization problem which is solved using theNondominated Sorting Genetic Algorithm-II (NSGA-II)proposed in Deb et al. (2002). In O’donnell and Lozano-Periz (1989), the coordination is achieved by introducingexplicit coordination commands into the paths. In thisway, the motions of each manipulator are planned nearlyindependently and allow the execution of the path seg-ments to be asynchronous.Mataric et al. (1995) deal with the box pushing problemby a method inspired from human coordination commonlyobserved when people move large pieces of furniture. If the people who are pushing or carrying cannot see wherethey are going, another person stands between the carriedobject and the goal and periodically directs them. This”watcher” can see both the current position of the objectand the goal, and thus, can compute an error signal,perhaps in the form of a correction angle, that can becommunicated to the ”pushers.”Otani and Koshino (2009) and Kamio and Iba (2005) pro-posed multi-agent cooperation path-planning algorithmsfor object transportation based on rapidly exploring ran-dom tree (RRT). The RRT is a data structure and al-gorithm that is designed for efficient searching and isconstructed incrementally in a way that quickly reducesthe expected distance of a randomly chosen point to thetree.Path following for a rigid formation of 
 
 dimensionlessmobile robots (e.g. Gentili and Martinelli (2001)) canbe accomplished by steering behaviors, (e.g., Reynolds(1999)). All steering behaviors are based on local informa-tion; therefore, the robots get easily stuck if they are lo-cated in cluttered environments, such as narrow passages.To avoid deadlocks in narrow passages, Li et al. (2005)presented a hybrid technique, combining local steeringbehavior and an efficient AI technique for decision makingand planning cooperative multi-agent dynamic systemscalled a Coordination Graph (CG).Kawakami and Namerikawa (2008) discuss a target-enclosing problem for a group of multiple nonholonomicagents in a plane and propose the target-enclosing controllaws based on the consensus algorithm applied to thevirtual agents. Their strategies guarantee that multipleagents coordination finally results in a circular formationenclosing the target object which moves in the plane byintroducing virtual agents for the feedback linearization of the real nonholonomic agents.In Li (2008), a group of agents is enclosed by a deformablerectangle (i.e., the group shape) and probabilistic roadmap(PRM) is used to plan the global motions of the groupshape. Local motions of the agents inside a deformablerectangle are determined by group potential fields. Sincethis method is based on PRM, it is not suitable for real-time applications.
 
The real-time method presented in Kamphuis and Over-mars (2004) finds first a path (called the backbone path)for a single agent. The backbone path is then extended toa corridor using the clearance along the path. The agentscan move freely inside this corridor. A group region is apart of the corridor in which all agents must remain.3. PROBLEM FORMULATION AND DEFINITIONSWe consider a problem of motion coordination for a forma-tion of 
 N 
 robots pushing a box from some arbitrary initialto a desired goal position, in a collaborative distributedway. The robots are modeled as rational collaborative au-tonomous agents working on their movement coordinationand motion planning in a discrete state space. The box’strajectory is predetermined a priori and is followed by therobots in a discrete state space. The robots need to pushthe box based on their maximum movement capabilitiesand its desired trajectory in each step
 t
 = (1
,...,T 
),where
 T 
 is the final step when the box is on the final posi-tion. The problem is thus to design a multi-robot systemmodel in which each robot can calculate autonomouslyits movement direction and position based on its localsensory readings and the information received from theother agents in the system so as to collaboratively pushthe box following as closely as possible its predeterminedconfiguration state in each step.It is assumed that the pushing positions on the box arepreassigned for all the robots, and the individual robots’information about the environment, such as the states of other robots and the positions of the obstacles, is onlypartially available. We distinguish two kinds of obstacles:the obstacles known a priori, offline in the phase of the calculation of the trajectory of the box movement,and the obstacles which are unforeseen at the trajectorycalculation time and appear in the time of the executionof the box movement.
3.1 Virtual Structur
Virtual structure (VS) agent represents a fictitious modelof the box with as many points as there are the robotsin the system. The virtual structure is by definition inLewis and Tan (1997) a collection of elements, e.g., robots,which maintain a (semi-)rigid geometric relationship toeach other and to a frame of reference. It is therefore acompletely centralized model which calculates the trajec-tory of the box from some initial to the final goal position.The robots in each step only have to follow the preassignedtrajectory and have no autonomy in deciding of theiractions while doing so.New mobile robots’ positions define new position andorientation of the virtual structure, and the relative errorof movement caused by the deviation from the trajectorydue to their velocity limits. The VS recalculates in eachstep the trajectory to the goal position and communicatesit to all the robots in the team. Therefore, this algorithmis a mutual adaptation between the robots that move tokeep up with the trajectory preassigned by the VS, and theVS, which finds a nominal route to goal from the robots’new positions (e.g. Lewis and Tan (1997)).We assume that a virtual structure consists of 
 
 points,whose positions in its reference polar coordinate frame arerepresented by vectors
 p
R
1
 ,..,p
R
.Let
 
R
 be a transformation function that maps thevectors
 p
R
1
 ,..,p
R
 to the vectors of the world coordinateframe
 p
1
 ,..,p
 . The rigid body model requires that thereal world Euclidean distance between any two coordi-nate locations remain unchanged by the transformation.Therefore, the robots’ movement is in perfect formation attime
 t
 if exists such a transformation matrix
 T 
R
 betweenthe points on the virtual structure and each robot that
 p
i
 (
t
) =
 r
i
 (
t
) for all
 N 
 robots, see Lewis and Tan (1997).We want to minimize the total distance
 d
() between theVS’ and the robots’ positions so as to align them together.The function which gives a total error of the actual robots’vs. VS’ positions is stated as follows:
(
d
R
 ) =
i
d
2
i
 ,
 (1)where,
d
2
i
 = (
 p
i
 
r
i
 )
(
 p
i
 
r
i
 )
 .
 (2)If the displaced points are within the movement capabili-ties of the mobile robots and there are no unforeseen ob-stacles, then there will be no errors. However, if the robotsdo not succeed in perfect following of the VS coordinates,the resulting distance between the current reached pointand the predetermined point communicated by the VSresults in a synchronization error
 
(
d
R
 ) (e.g. Ren andBeard (2004)) which measures the deviation of the VS vs.the robots actual positions. It is the sum of errors for allrobots from their assigned points in the virtual structure.It is assumed that each robot is capable of moving fromthe actual position to any position, determined by theVS agent’s trajectory, which is at the distance less thanthe maximum step distance
 dist
lim
 from the actual robotposition. To limit the upper bound on the maximummovement distances of the rigid virtual structure, i.e., themaximum distance of robot movement in each step whenthe distances in two consecutive steps are too high, themaximum possible Euclidean distances of movement of each robot
 i
 are scaled down in the following way:
dist
i
lim
 =
 dist
i
MAX 
(
|
dist
1
|
,...,
|
dist
|
)
dist
lim
 ,
 (3)where
 dist
i
lim
 is the scaled distance of each robot
 i
,
 dist
i
is the actual distance received from
 V
 for the robot
 i
, and
dist
lim
 is the maximum possible distance of movementin each step. Note that in the case where there is nomaximum passed distance limit, and no obstacles in theway, the error of the VS level will be zero since each robotis capable of reaching its preassigned target position.
3.2 Force model 
We describe the dynamics of the robot-box interaction byintroducing a notion of artificial repulsive force betweenthe robots and the box from Liu and Wu (2001). Theartificial repulsive force between robots and the box isbased on a Hooke’s law, spring-like model in the followingway: if a robot
 i
 moves inside an
 l
0
 radius region around
 
the box, it applies an artificial repulsive force
 −
 i
(
t
) on thebox.
 i
(
t
) =
η
·
(
l
0
 −
d
i
(
t
)) i
 d
i
(
t
)
 ≤
 l
0
,
 
t
0 otherwise,
 
t
 (4)where
 η
 is a positive coefficient, and
 d
i
(
t
) is the Euclideandistance between the robot and his manipulation positionof the box at the time of exerting a repulsive pushing force,(see Liu and Wu (2001)). If the robot is outside the
 l
0
 rangeof the box, the pushing force is zero. For the square boxeswith a diameter 2
l
0
, this is in line with the fact that thebox should be pushed on the sides and the maximum forceis found in the center of each side since there the difference
l
0
 −
 d
i
(
t
) is maximal. The direction of the pushing force
i
(
t
) is defined through the angle
 θ
i
(
t
) between the lineconnecting the robot
 i
 and certain reference point
 O
 onthe box, and the polar axis srcinated at
 O
 (the pole of the box polar system) and tangent to the trajectory of the box. We will assume that the pole
 O
 is positioned inthe box’s centroid. Thus, if robot
 i
 is at distance
 d
i
(
t
)from the pole
 O
 and its orientation is
 θ
i
(
t
) in this polarcoordinate system, then its relative spatial configurationcan be expressed as a vector
 −
r
 Oi
 (
t
):
r
 Oi
 (
t
) =
 d
i
(
t
)
·
e
i
.
 (5)Furthermore, the direction of a pushing force,
 
 i
(
t
) willbe
 e
i
(
t
)
, and the magnitude of this force will be:
 i
(
t
) =
 F 
i
(
t
)
·
e
i
.
 (6)The robots will collectively push the box through theirrepulsive forces if and only if 
 d
i
(
t
)
 
 l
0
. The direction of box displacement is equal to the direction of the collectivenet repulsive pushing force on the box,
 vector sum
(
 i
),and the magnitude of box displacement is proportional tothat of the net force.
3.3 Robots’ optimization problem 
The problem of the lower level is to find for eachrobot
 i
, the force
 
 i
(
t
), with the pushing angle
 θ
i
(
t
) ineach period
 t
 so as to push the box from the current[(
x
b
 (
t
)
,y
b
 (
t
))
,θ
b
 (
t
)] to the configuration state [(
x
b
 (
t
+1)
,y
b
 (
t
+1))
,θ
b
(
t
+1)] in the step (
t
+1) given the VS’spredetermined trajectory, the constraints on the maximalmovement distances of the robots, and the momentaryobstacles in each step. (
x
b
 (
t
+1)
,y
b
 (
t
+1) are the coordi-nates of the box’s centroid and
 θ
b
(
t
+1)] is the orientationof the box in the World’s coordinate frame in step
 t
+1. Ithe movement along the trajectory is not possible due tothe unforeseen obstacles in the way, then the scope is tofollow the trajectory of the box as closely as possible.We solve this problem by optimizing an objective function(7) which was inspired by the work in Liu and Wu (2001)concerning the agents pushing a box in the work spacewithout obstacles, and modify it adding the function thatconcerns the avoidance of unpredicted obstacles (11). In(7), a product is chosen instead of a sum since it representsa logical conjunction where all the equations (8) – (11)have to be satisfied in the same time for the box to bemoved in the desired direction avoiding the obstacles.
(
t
) =
 h
1
(
t
)
·
h
2
(
t
)
·
h
3
(
t
)
·
h
4
(
t
)
 ,
 (7)
h
1
(
t
) determines a total net force on the box which is tobe maximized by all the formation robots:
h
1
(
t
) =
 α
·
vector sum
 i
(
t
)
.
 (8)
h
2
 is a measure of how close the robots are to the box, toexert a frictional force defined by the Hooke’s law (4) onthe object :
h
2
 =
 κ
·
[
i
d
i
(
t
)]
1
.
 (9)
h
1
 and
 h
2
, together express the robots’ objective to sur-round the box as tightly as possible with a maximum netpushing force. The direction of this force should be alignedwith the tangent on the trajectory given by the VS agent,pointing towards the goal. This objective is presented by
h
3
:
h
3
 =
cos(
θ
db
(
t
)) i
 θ
b
 (
t
)
 ∈
 [
π/
2
,π/
2],0 otherwise , (10)where
 θ
db
(
t
) is an angle between the tangent on the actualmovement direction, and the positive axis of the box’spolar coordinate system, pointing towards a tangent onthe desired trajectory.The robots collaboratively avoid unpredicted obstacles inreal time.
 h
4
 is the factor representing the distance
 s
i
 fromthe obstacles for each robot
 i
:
h
4
 =
 ω
·
[
i
s
i
(
t
)
 ,
 
t
]
,
 (11)where,0
 ≤
 s
i
(
t
)
 ≤
 s
lim
 (12)
s
i
(
t
) = 0 if the robot is in front of the obstacle, while
s
i
(
t
) =
 s
lim
 if there are no obstacles in a way.
 s
lim
 in (12)is the maximum range limit of the robot’s sensors.The objective function
 
(
t
) defined in this way is madeup of two types of equations: (8), (9), and (11) correspondto the relative spatial configuration of individual robotsrelative to a box and obstacles, while (10) corresponds tothe global feedback information of the box to the mutualmovements of the robots.4. DISTRIBUTED MULTI-AGENT COORDINATIONALGORITHMIn the proposed two-level architecture, the VS agent onthe upper level determines the box trajectory based onthe robot team’s momentary position, their desired goalposition and the coordinates of the obstacles known apriori and communicates it to the robots on the lowerlevel. In each step the robots individually determine theirnecessary movements to follow the preassigned trajectoryavoiding unpredicted obstacles sensed online by the sen-sors of limited range.Individual movement selection is handled independentlyby each robot while the coordinated movement evaluationis handled by the whole group of robots in the formation.If the synchronization error, created by robots’ avoidingthe obstacles unpredicted by the VS agent, gets largerthan a predetermined threshold, the robots contact the
 
VS agent for the recalculation of the trajectory from themomentary to their goal position.At least one robot in the team must be able to communi-cate to a VS agent and the communication network mustbe such that the other robots can at least indirectly receivethe updated trajectory information from that robot. Thestability of the robot formation is guaranteed if the com-munication graph between the robots and the VS agentcontains a directed spanning tree rooted in VS agent atall times t (e.g., Shamma (2008)).In the following, the multi-robot box-pushing coordinationalgorithm is presented.(1) VS agent updates the environment information andaligns the VS with the current box position.(2) VS calculates the box’s trajectory from the initial(current) to the goal position and informs the robotsof the same.(3) Through the optimization of the objective function(7), the robots in each step compute their forces
i
 applied to the box and the angles of impact
 θ
i
following the desired box trajectory if possible, basedon the limited local range sensor obstacle readingsand the exchanged obstacles’ data among the robotsin the group. a) If possible, i.e., if no obstaclesfound in the way, the robots follow the instructionsof movement. b) If impossible, i.e., if unpredictedobstacles are found in the way, the robots avoidobstacles moving in the direction of the trajectory.(4) If a box moves too far away, more than a predefinedmaximal synchronization error, the robots inform theVS agent of the actual position and the obstaclessensed in the way.(5) Go to step 1.5. SIMULATION RESULTSThe approach is demonstrated with simulations in whichwe ignore modeling and control uncertainties. The algo-rithm is simulated in MatLab 7.9.0 running on an IntelCore 2 Duo CPU personal computer with a speed of 2,20GHz and 4 GB of RAM memory.We are not entering here into specific types of driveof the robots, but assume that the robots are movingin each step as dots in 2D space up to predeterminedmaximum movement distance. Each robot is assumed to beequipped with a camera and an array of ultrasonic sensors.The obstacle is in the sensor range if its distance fromthe sensor is less than the maximum sensor range, i.e.,
s
i
 
 s
lim
. Note that for safety reasons, the sensor rangeshould be proportionally increasing as a box’s maximummovement distance per step increases.The simulation experiments are based on three robots thatcollectively push cylindrical box in a 2D (128
×
128 pixels)environment, as seen in Figures 1 and 2. The rectangularmovement space is divided into free space and the spacewith obstacles of rectangular shape: obstacles known apriori by the VS agent (black rectangles in Fig. 1 and 2)and unpredicted dynamic obstacles sensed by the robots’limited range sensors (green rectangles).Fig. 1. Robots (3 black dots) pushing a box (blue circle),and a predetermined trajectory calculated offline (redcurve). Obstacles known a priori (in black), andthe one (green) that appeared during the real timeexecution.Directed by the VS’s trajectory of the box, robots movein each step towards the new position based on the indi-vidual sensor readings and optimized objective function(7). They individually calculate the distances from thepredetermined positions on the box, the needed forces,and the impact angle to push the box in real time. Therobots exchange the values of objective function (7) foreach direction calculated individually, and in each stepmove in the direction of the maximum distance from theobstacles in line with the trajectory if possible, as seen inthe Fig. 1, and 2. The sensor range in our example is setto the value of the double box’s diameter.6. CONCLUSIONIn this work, a two-level model for coordinating multiplerobots in box pushing based on Virtual Structures andmulti-objective optimization is proposed. We present adistributed solution for the case of the presence of unpre-dicted obstacles, where each robot is responsible of adapt-ing its trajectory to the momentary group situation. Thereis no central agent which decides for all the formation; thegroup decision is brought up collaboratively guided by theVS agent trajectory and through optimization of the groupoptimization function.The experimental results show that this method in thecases of convex sufficiently distant obstacles gives goodresults in terms of total distance and time traversed; withthese constraints, the goal reaching is always guaranteedin limited time.Depending on the maximal admissible synchronizationerror for reconnecting with the VS agent, and on thelayout of the environment, this method is not guaranteedto always bring the box to the goal position (e.g. in
of 6