A Distributed Coordination Model forMultiRobot Box Pushing
Marin Lujak
Dip. Ingegneria dell’Impresa  University of Rome “Tor Vergata”,Italy (email: 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 multirobotsystem is a twolevel control model in which on the upper level, a Virtual Structure (VS)agent calculates oﬄine 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 deﬁnedby the VS agent too far away, more than a predeﬁned 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:
Cooperative control, multiagent systems, multiobjective optimization, robotcontrol.1. INTRODUCTIONIn this work, the problem of box pushing by collaborative autonomous robots is considered. This problem is aspecial instance of the formation motion planning problem. 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 conﬁguration. Important issues are still how toacquire a collective goaldirected, taskdriven behavior ina distributed way and how to eﬀectively 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 positions 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 between 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 andLozanoPeriz (1989).We propose a twolevel control model for coordinating themultirobot system in which on the upper level inspiredby Lewis and Tan (1997), a ﬁctitious Virtual Structure(VS) agent calculates oﬄine 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, individually 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 ﬁnd themselves too far away, morethan a predeﬁned distance, from the trajectory deﬁnedby 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 multiagent solution to therobots’ box pushing coordination problem in an environment 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 deﬁnitions. 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 boxmanipulating 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 multirobot trajectory planning and motionexecution while the subjects as determining, coordinatingand executing a plan of action in response to realtimedata in the strategic decision making has not been thatexplored.Autonomous mobile robots can eﬀectively perform certaintasks with their collective behaviors. A group of robots canbe used to carry out tasks that are diﬃcult or not eﬀectivefor 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 deﬁned,and the mathematical analysis of the solutions is diﬃcultas 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 conﬁguration 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 diﬀerent movements.Another important decentralized approach to formationcontrol is consensus algorithms (e.g. OlfatiSaber and Murray (2007)) where rapid agreement of all the participants isreached by bringing their information states to a commonvalue and thus allowing eﬀective 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 diﬃculty 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 problem is resolved by formulating the boxshifting as a multiobjective optimization problem which is solved using theNondominated Sorting Genetic AlgorithmII (NSGAII)proposed in Deb et al. (2002). In O’donnell and LozanoPeriz (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 segments 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) proposed multiagent cooperation pathplanning algorithmsfor object transportation based on rapidly exploring random tree (RRT). The RRT is a data structure and algorithm that is designed for eﬃcient 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
N
dimensionlessmobile robots (e.g. Gentili and Martinelli (2001)) canbe accomplished by steering behaviors, (e.g., Reynolds(1999)). All steering behaviors are based on local information; therefore, the robots get easily stuck if they are located 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 eﬃcient AI technique for decision makingand planning cooperative multiagent dynamic systemscalled a Coordination Graph (CG).Kawakami and Namerikawa (2008) discuss a targetenclosing problem for a group of multiple nonholonomicagents in a plane and propose the targetenclosing controllaws based on the consensus algorithm applied to thevirtual agents. Their strategies guarantee that multipleagents coordination ﬁnally 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 ﬁelds. Sincethis method is based on PRM, it is not suitable for realtime applications.
The realtime method presented in Kamphuis and Overmars (2004) ﬁnds ﬁrst 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 formation 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 autonomous 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 ﬁnal step when the box is on the ﬁnal position. The problem is thus to design a multirobot 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 predeterminedconﬁguration 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, oﬄine 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 Structure
Virtual structure (VS) agent represents a ﬁctitious modelof the box with as many points as there are the robotsin the system. The virtual structure is by deﬁnition 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 trajectory of the box from some initial to the ﬁnal 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 deﬁne 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 ﬁnds a nominal route to goal from the robots’new positions (e.g. Lewis and Tan (1997)).We assume that a virtual structure consists of
N
points,whose positions in its reference polar coordinate frame arerepresented by vectors
p
R
1
,..,p
RN
.Let
T
W R
be a transformation function that maps thevectors
p
R
1
,..,p
RN
to the vectors of the world coordinateframe
p
W
1
,..,p
W N
. The rigid body model requires that thereal world Euclidean distance between any two coordinate locations remain unchanged by the transformation.Therefore, the robots’ movement is in perfect formation attime
t
if exists such a transformation matrix
T
W R
betweenthe points on the virtual structure and each robot that
p
W i
(
t
) =
r
W 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:
E
(
d
W R
) =
i
d
2
i
,
(1)where,
d
2
i
= (
p
W i
−
r
W i
)
T
(
p
W i
−
r
W i
)
.
(2)If the displaced points are within the movement capabilities of the mobile robots and there are no unforeseen obstacles, 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
E
(
d
W 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
N

)
dist
lim
,
(3)where
dist
i
−
lim
is the scaled distance of each robot
i
,
dist
i
is the actual distance received from
VS
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 robotbox interaction byintroducing a notion of artiﬁcial repulsive force betweenthe robots and the box from Liu and Wu (2001). Theartiﬁcial repulsive force between robots and the box isbased on a Hooke’s law, springlike model in the followingway: if a robot
i
moves inside an
l
0
radius region around
the box, it applies an artiﬁcial repulsive force
−→
F
i
(
t
) on thebox.
−→
F
i
(
t
) =
η
·
(
l
0
−
d
i
(
t
)) if
d
i
(
t
)
≤
l
0
,
∀
t
0 otherwise,
∀
t
(4)where
η
is a positive coeﬃcient, 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 diﬀerence
l
0
−
d
i
(
t
) is maximal. The direction of the pushing force
F
i
(
t
) is deﬁned 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 conﬁgurationcan be expressed as a vector
−→
r
Oi
(
t
):
−→
r
Oi
(
t
) =
d
i
(
t
)
·
e
jθ
i
.
(5)Furthermore, the direction of a pushing force,
−→
F
i
(
t
) willbe
e
−
jθ
i
(
t
)
, and the magnitude of this force will be:
−→
F
i
(
t
) =
F
i
(
t
)
·
e
−
jθ
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
(
−→
F
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 ﬁnd for eachrobot
i
, the force
−→
F
i
(
t
), with the pushing angle
θ
i
(
t
) ineach period
t
so as to push the box from the current[(
x
W b
(
t
)
,y
W b
(
t
))
,θ
W b
(
t
)] to the conﬁguration state [(
x
W b
(
t
+1)
,y
W 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
W b
(
t
+1)
,y
W b
(
t
+1) are the coordinates of the box’s centroid and
θ
b
(
t
+1)] is the orientationof the box in the World’s coordinate frame in step
t
+1. If the 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 satisﬁed in the same time for the box to bemoved in the desired direction avoiding the obstacles.
H
(
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
−→
F
i
(
t
)
.
(8)
h
2
is a measure of how close the robots are to the box, toexert a frictional force deﬁned 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 surround 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
)) if
θ
W 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
H
(
t
) deﬁned in this way is madeup of two types of equations: (8), (9), and (11) correspondto the relative spatial conﬁguration 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 MULTIAGENT COORDINATIONALGORITHMIn the proposed twolevel 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 sensors 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 communicate 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 communication 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 multirobot boxpushing 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
F
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 predeﬁnedmaximal 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 algorithm 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 speciﬁc 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 oﬄine (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 individual 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 twolevel model for coordinating multiplerobots in box pushing based on Virtual Structures andmultiobjective optimization is proposed. We present adistributed solution for the case of the presence of unpredicted obstacles, where each robot is responsible of adapting 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 suﬃciently 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