the null-space-based behavioral control for mobile robots...

Filippo ArrichielloStefano ChiaveriniDipartimento di Automazione,Elettromagnetismo,Ingegneria dell’Informazionee Matematica Industriale,Università degli Studi di Cassino,Via G. Di Biasio 43,03043 Cassino (FR),Italy{f.arrichiello,chiaverini}@unicas.it

Giovanni IndiveriPaola PedoneDipartimento Ingegneria dell’Innovazione,Università del Salento,via Monteroni,73100 Lecce,Italy{giovanni.indiveri, paola.pedone}@unisalento.it

The Null-space-basedBehavioral Control forMobile Robots withVelocity ActuatorSaturations

Abstract

In this paper we present the application of the null-space-based be-havioral (NSB) approach to the motion control of mobile robots withvelocity saturated actuators. The NSB is a behavior-based robot con-trol approach that uses a hierarchical organization of the tasks toguarantee that they are executed according to a desired priority: ituses a projection technique to avoid that, in the absence of actuatorsaturations, low-priority tasks could influence higher-priority tasks.The main contribution of this paper is the extension of the NSB ap-proach to the case where actuator velocity saturation bounds are ex-plicitly taken into account. The proposed solution dynamically scalestask velocity commands so that the hierarchy of task priorities is pre-served in spite of actuator velocity saturations. The approach hasbeen validated on two specific case studies. In the first case, the NSBelaborates the motion directives for a single mobile robot that hasto reach a target while avoiding a point obstacle� in this case, themission is composed of two tasks. In the second case, the NSB elab-orates the motion directives for a team of six mobile robots that has

The International Journal of Robotics ResearchVol. 00, No. 00, Xxxxxxxx 2009, pp. 000–000DOI: 10.1177/0278364909358788c� The Author(s), 2009. Reprints and permissions:http://www.sagepub.co.uk/journalsPermissions.navFigures 3–27 appear in color online: http://ijr.sagepub.com

to entrap and escort a target� in this case the mission is composed offour tasks. The approach is validated by numerical simulations andby experiments with real mobile robots.

KEY WORDS—

1. Introduction

Mobile robots have been object of widespread research in thelast few decades. Their applications span over service, indus-trial, military, and civil fields and involve missions such as ex-ploration, transportation, and mobile manipulation. In spite ofthe many advancements in the field of mobile robotics, a con-siderable number of challenging issues are still open.

With reference to motion control problems, a wide vari-ety of methods and techniques has been presented in the lit-erature including behavior-based approaches. These appear tobe appealing because they allow the system to navigate au-tonomously in complex environments, avoiding (or at leastlimiting) the need of off-line path planning by using sensors toobtain instantaneous information of the environment with anincreased overall flexibility. Behavior-based approaches have

1

2 THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / Xxxxxxxx 2009

Fig. 1. Sketch of the considered single-robot control architecture.

been shown to be particularly useful and successful for nav-igation tasks in unknown or dynamically changing environ-ments. Among the behavioral approaches, seminal works arereported by Brooks (1986) and Arkin (1989), while a com-prehensive overview is presented by Arkin (1998). Behavioralapproaches have been also applied to the formation controlof multi-robot systems as in Parker (1996), Mataric (1997),Balch and Arkin (1998) and Kumar and Stover (2000). Thebehavioral paradigm to design and analyze robot control sys-tems has been particularly successful within robot competitioninitiatives such as RoboCup1 where research focuses even-tually more on artificial intelligence (AI) and high-level rea-soning to achieve team cooperation rather than low-level mo-tion control. Indeed this line of research has some interest-ing overlap with multi-agent systems approaches, althoughmulti-robot and multi-agent systems do have distinct fea-tures (Farinelli et al. 2004). For a recent example of behavior-based robot control field experiences refer to Hurdus and Hong(2008).

By applying inverse kinematics techniques, typically usedfor industrial manipulators, to mobile robots, a new behav-ior based approach has been recently proposed: the null-space-based behavioral (NSB) control (Antonelli et al. 2008b).Specifically, the NSB approach is derived from work on task-based kinematical control for industrial manipulators (Naka-mura et al. 1987� Maciejewski 1988� Samson et al. 1991) byexploiting eventual kinematical redundancies to control morethan one motion task simultaneously. However, in the case ofconflicting tasks, it is necessary to devise singularity-robustalgorithms that ensure correct inverse velocity mapping (Chi-averini 1997). In Antonelli and Chiaverini (2003, 2004) thisapproach has been used in simulation to control a multi-robotteam performing a caging mission, subject to obstacle avoid-ance and the failure of one or more vehicles. A comparisonwith conventional behavior-based approaches is described byAntonelli et al. (2008b) and its experimental application tomulti-robot missions such as formation control, escort or en-trap an autonomous target are presented by Antonelli et al.(2009, 2008a).

1. See http://www.robocup.org/.

When applied to a single vehicle, the NSB solution maybe thought of as a standard guidance loop within a three-layercontrol architecture. In particular, the vehicle is assumed tohave a multi-layer control architecture made of three nestedcontrol loops (see Figure 1). The outermost loop implementsthe mission controller. It runs at the lowest frequency, com-pared with the other two, and substantially takes care of high-level scheduling, such as deciding which set of tasks shouldbe activated with what priority order. Eventually the missioncontroller can be replaced by a manual definition of the tasksto be performed in each mission, as in the experimental casesdescribed here. The second loop (the NSB in Figure 1) con-stitutes the kinematics control system. It runs at a faster ratethan the mission layer and at a lower rate with respect to theinnermost control loop. The NSB guidance controller gener-ates high-level kinematic commands assuming that the vehicleto be controlled is a material point, and it generates the de-sired velocity commands for such material point. The guidancecontroller, by design, ignores the specific dynamic model ofthe vehicle (differential drive wheeled robot, car-like wheeledrobot, Mecanum wheeled robot, marine vehicles etc.). Thelower-level controller (LLC in Figure 1) is the innermost loopand it runs at the highest frequency with respect to the otherloops. The LLC loop is in charge of mapping the NSB ve-locity reference commands in commands to the actuators ofthe robot. Indeed the LLC must be designed taking explicitlyinto account the nature of the vehicle to be used. The outputstage of the LLC usually accommodates the necessary actua-tor servo loops most often based on PI or PID architectures.Similar architectures exhibiting an internal joint velocity loopand an external kinematics loop are well known in industrialrobotics applications (Sciavicco and Siciliano 2000) and havebeen studied quite thoroughly (Kelly and Moreno 2005).

If the frequency separation among the control loops de-picted in Figure 1 is sufficiently wide, the three controllersmay be designed as if they were independent. This kind ofarchitecture is rather common for autonomous robots. The ar-chitecture sketched in Figure 1 can also be referred to as anavigation, guidance, and control (NGC) architecture (Fossen2002). The navigation system (not shown in Figure 1) providesposition, orientation, and velocity information to the motion

Arrichiello et al. / The Null-space-based Behavioral Control for Mobile Robots with Velocity Actuator Saturations 3

control systems. It is usually designed based on state observertheory and its outputs are fed to the guidance and low-levelcontrol loops. In the perspective of the NGC architecture, theNSB realizes a particular kind of guidance controller.

In this paper we show how the NSB approach can also beused to generate control commands for a team of cooperatingrobots. Indeed the NSB solution is specifically targeted at ex-ploiting redundancy: given a set of prioritized collective tasks,the NSB approach provides a natural and elegant way to gen-erate velocity commands for all of the vehicles in the team, inorder to efficiently fulfill the tasks. This is a centralized ap-proach: there is a single NSB module that collects positionand orientation feedback from all vehicles and, based uponthe tasks to be satisfied, generates velocity commands for themultiple LLC modules of all of the team members. In both thesingle- and multi-vehicle cases, the NSB algorithm generatesvelocity commands, implementing a kinematics-based controlloop.

An important implementation issue that has been oftenoverlooked, when designing kinematics based control laws, isrelated to actuator velocity saturation. The importance of cor-rectly dealing with actuator saturation issues is well known.Indeed there is an extensive bulk of research referring to linearcontrol systems (see Wu et al. (2007) and references therein).As for robotic systems, most results in the literature are relativeto torque saturations for manipulators: the problem of manipu-lator joint stabilization in an equilibrium posture with boundedtorque actuators, for example, is addressed in Zavala-Río andSantibáñez (2007). In particular, an extension of the classicalPD with gravity compensation (Sciavicco and Siciliano 2000)law is proposed able to guarantee, under mild feasibility as-sumptions, that the pose is stabilized to a constant desiredvalue without exceeding given actuator torque limits. The issueof trajectory tracking for manipulators with bounded torqueactuators is addressed, for example, by Dixon et al. (1999)and Aguiñaga-Ruiz et al. (2009) where a controller achiev-ing global uniform asymptotic stability of the tracking erroris presented (alternative methods usually guarantee only localstability). A completely different and very interesting perspec-tive to cope with velocity and acceleration saturations in ro-botic manipulators is presented in Omr�cen et al. (2007): theproposed solution applies to redundant systems. When somejoints have saturating velocities or accelerations during theexecution of a given task, an additional command (filteredthrough the null space of the task) is computed, if possible,so that the velocity in task space remains unchanged while thejoints do not saturate any longer. In principle, this approachcould be eventually also applied to the NSB architecture, yetthe additional command to prevent joint saturation should beaccounted for as an extra task, hence reducing the possibility ofperforming other tasks. Moreover, such additional commandcould be unfeasible according to the singularity conditions de-scribed in Omr�cen et al. (2007), whereas the method proposedin this paper always guarantees bounded velocities, eventu-

ally at the expense of scaling (even to zero) lower-prioritytasks.

Other recent results relative to bounded torque control ofwheeled mobile robots include Kim and Park (2008) and Chenet al. (2009). The first addresses the problem of trajectory gen-eration, rather than control, based on an elegant interpolationschema exploiting a principal component analysis (PCA) ofpre-computed torque-minimum solutions linking a set of ini-tial and final poses. Chen et al. (2009) describes the designof an H� controller for the problem of trajectory tracking onbehalf of a dynamical model of a wheeled mobile robot. Theproposed solution appears to be computationally demanding asa constraint optimization problem needs to be solved on line.

The above literature overview confirms that, in spite of thepractical relevance of the issue, little attention has been de-voted to the problem of actuator velocity saturation.

Within a kinematics task-based control architecture, if alow-priority task command induces even a single actuator tosaturate its velocity, the high-priority task output would be-come corrupted. The common work-around approach, i.e. scal-ing down all of the actuator speeds to avoid saturations, has thedrawback that higher-priority tasks are slowed down by lower-priority tasks. This violates the very spirit of prioritized taskexecution, namely that lower-priority tasks should not inter-fere with higher-priority tasks. To avoid these limitations, theNSB solution can be extended with velocity saturation man-agement techniques (Indiveri 2009� Arrichiello et al. 2009).The solution for the management of actuator velocity satura-tions presented in Indiveri (2009) and applied in Arrichiello etal. (2009), although effective, is conservative: to avoid satura-tions, task commands are scaled down on the basis of a worst-case condition that is not necessarily always met. In this pa-per we extend the velocity saturation management techniqueproposed in Indiveri (2009): our proposed solution, althoughmore complex from an algorithmic point of view, is never over-conservative and still simple enough to be implemented on-line. By adopting this solution in a single-vehicle NSB sce-nario, we derive a control architecture as depicted in Figure 2.

In Section 3 we describe this solution, and in Section 4 weapply it, both in simulation and experimentally, on two specificcases of motion control with velocity saturated actuators: a sin-gle mobile robot mission where a mobile robot has to reach atarget avoiding a point obstacle (Arrichiello et al. 2009), anda mission of a team of six mobile robots that have to entrapand escort a moving target (Antonelli et al. 2008a). The exper-imental platform adopted here is a team of Khepera II mobilerobots.

2. The Null-space-based Behavioral Control

The NSB control is a behavior-based technique to design mo-tion controllers for robotic systems (Antonelli et al. 2008b,a).


Fig. 2. Single-vehicle control architecture, with velocity saturation module.

In particular, the NSB uses a hierarchy-based structure to si-multaneously control multiple tasks, in which the componentsof the lower-priority tasks that conflict with higher-prioritytasks are suppressed via a projection technique. In line with theconventional behavior-based approaches and similar to task-based kinematic control approaches, the NSB technique re-quires a decomposition of the overall mission in elementarysub-problems (or tasks) that have to be managed simultane-ously. For each task we define a suitable function and elabo-rate motion control directives. Then, we prioritize the set oftasks that compose the overall mission. Finally, we determinethe robots’ global motion control directives by appropriatelycombining the motion directives of the individual tasks.

Specifically, by defining as �� m�1 the task variable tobe controlled and as p � �s�1 the system configuration, then

�� f�p� (1)

with the corresponding differential relationship:

�� f�p��p

�p � J�p�v� (2)

where m is the task function dimension, J � �m�s is the

configuration-dependent task Jacobian matrix, and

v :� �p � �s�1 (3)

is the system velocity. Note that, in case of a team of l planarrobots where pi � �2�1 is the position of the i th robot, thenp � [pT

1 � � � pTl ]T, that makes s � 2l. Also note that the only

case of interest is m � s� otherwise, if m � s, the task wouldeither be unfeasible or the null space of a full rank J�p� (i.e.rank�J�p�� s) would be empty thus preventing the possibil-ity of controlling any other task.

Consider a generic task k defined by the task variable �� k

having a desired value �� d�k and a Jacobian Jk : the velocity ref-erence for the system is computed starting from the desiredvalues �� d�k by solving the inverse kinematic problem at a dif-ferential level. In particular, being m � s it is possible to makeuse of the Moore–Penrose pseudo-inverse Jacobian of the taskfunction to choose the minimum-norm velocity that fulfills the

task. Thus, the velocity reference of the generic kth task canbe calculated as

vk � J†k� �� d�k k �� k�� (4)

where J†k � JT

k �JkJTk ��1 (when m � s and rank�Jk�p�� m),

k is a suitable constant positive-definite matrix of gains and�� k is the task error defined as �� k � �� d�k � �� k .

It is worth noticing that the term k �� k is added to coun-teract the numerical drift due to discrete-time integration. Adetailed discussion of the issues related to the case of rank-deficient Jacobian (i.e. rank�Jk�p�� m) goes beyond thescope of this paper. However, it should be noted that the right-inverse J†

k in Equation (4) can be computed through singular-ity robust techniques, such as singular value decomposition(SVD). Note that by replacing the command (4) in the cor-responding dynamic equation (2), the closed-loop task errordynamics results in the exponentially stable equation

�� k k �� k � 0� (5)

Moreover, in case the kth task should be a regulation task,i.e. �� d�k � 0, the corresponding error dynamics would stillbe exponentially stable even if the velocity command (4) wasscaled by any � � 0. Namely if �� d�k � 0, then

vk � �J†k�k �� k�� 0� (6)

which implies �� k �k �� k � 0� (7)

that is exponentially stable.When the mission is composed of multiple tasks, the over-

all system velocity is obtained by properly merging the out-puts of the individual tasks. A velocity vector for each task iscomputed as if it was acting alone� then, before adding the sin-gle contribution to the overall vehicle velocity, a lower-prioritytask is projected onto the null space of the immediately higher-priority task so as to remove those velocity components thatwould conflict with it. If the subscript k of Equation (4) alsodenotes the task priority (with task 1 being the highest-prioritytask) the overall robot velocity is derived as

vd � v1 N1�1v2 N1�2v3� (8)


Fig. 3. (a) Sketch of the projection mechanism in a two-task mission. (b) Saturation based on maximum 2-norm.

where N1�k is the projection matrix into the null space of thetasks from 1 to k. In particular, defining J1�k as

J1�k �

��

J1

J2

��

Jk

�� (9)

the null-space projection matrix N1�k is elaborated as

N1�k ��

I� J†1�kJ1�k

� (10)

Therefore, the generalization of Equation (8) can be writtenin the form:

vd �Ntaskk�1

�vk (11)

with

�vk ��

v1 if k � 1�

N1�k�1vk if k � 1�

A detailed convergence and stability analysis of the closed-loop task error dynamics is reported in Antonelli et al. (2008c)where the concepts of orthogonality and independency are in-troduced. In Antonelli et al. (2008c) it is shown that the so-lution given by Equations (9)–(11) gives rise to a stable andconvergent task error dynamics under very mild conditions onthe task Jacobians Jh and J1�k . Such conditions are always met

in the cases discussed in this paper. Moreover, Antonelli etal. (2008c) showed that under stronger assumptions related tothe orthogonality and independency of the task Jacobians in-volved, also the solution

�vk ��

v1 if k � 1��I� J†

1J1

� � ��

I� J†k�1Jk�1

vk if k � 1�

gives rise to a convergent and stable closed-loop task error dy-namics. It is worth noticing that such stronger assumptionsalso hold for all of the examples and case studies addressedin this paper.

With reference to Equation (11), lower-priority tasks do notconflict with higher-priority tasks because the correspondingcommands are filtered by the null space projectors of higher-priority tasks. Of course the lack of conflict can be guaranteedonly if the commands in Equation (11) are faithfully imple-mented by the robots. If a velocity command should exceedthe physical capability of a robot (thus, saturating to a max-imum value) it would eventually jeopardize the execution ofother tasks or modify the priority hierarchy.

For example, consider a two task mission in the situationwhere the first task is represented by a mono-dimensional

function whose Jacobian is�1 1

�, its null-space projection

matrix is I� J†1J1 � 1�2

�1 �1

�1 1

�and its velocity output is

the vector v1 � �1� 1�T. Moreover, suppose that the secondarytask output is v2 � ��1� 2�T (see the sketch of Figure 3(a)).


Fig. 4. Sketch of proposed saturation management techniquein the presence of 2-norm saturation.

Then

�v2 ��

I� J†1J1

v2 � ��1�5� 1�5�T� (12)

vd � v1 �v2 � ��0�5� 2�5�T� (13)

where vd 2 � 2�55. Assume now that the vehicle is such thatthe �x� y� velocity components, due to physical constraints, arebounded by the value max � 2: in this case, vd would notbe implemented, but rather v � ��0�5� 2�T would. Such acommand does not guarantee the correct execution of eithertask 1 or task 2. Similarly, if the vehicle should have an up-per bound on the 2-norm of the total velocity, say max v 2 � max � 2, the command vd would not be implemented, butrather the vehicle velocity would be v � max�vd� vd 2� ��0�39� 1�96�T. In this case, the NSB paradigm of project-ing lower-priority tasks in the null space of higher-prioritytasks would still be satisfied, but the projection in the higher-priority task direction is v1� max� vd 2� � �0�78� 0�78�T

rather than �1� 1�T (see the sketch of Figure 3(b)), namely morethan 20% slower than desired originally. This simple toy ex-ample shows that velocity saturations may have a significantimpact on the performance of the standard NSB algorithm.

In the next session we introduce a saturation managementtechnique to cope with this problem. In the previous exam-ple, this technique would compute the velocity vector as in thesketch of Figure 4. That is, the chosen velocity vector guaran-tees the correct execution of the velocity command relative tothe primary task while only the secondary task velocity com-mand is scaled down such that the overall vector satisfies the2-norm saturation constraint. The proposed velocity saturationmanagement solution extends a previously designed algorithmthat is briefly discussed in the following section.

3. The Saturation Management Technique

The solution discussed in Arrichiello et al. (2009) and Indiveri(2009) is briefly summarized below. Consider the function s :�� [0�� such that

s�x� c� �

��

0 if x � 0�

1 if 0 �x � c�

c��x� otherwise,

(14)

where the non-negative second argument c of s�x� c� will becalled the capacity of x . By definition s�x� c� is simply a non-negative scalar scaling factor such that xs�x� c� is “clipped”to c sign�x� whenever �x� should exceed the capacity c andis equal to x otherwise, i.e. xs�x� c� is simply the saturatedversion of x in the range [�c� c]. Also note that by its verydefinition

s�x� 0� � 0� for all x� (15)

namely if x should be assigned zero capacity, then xs�x� 0� �0 for any value of x . In order to take into account the upperbound on the norm of the vehicle’s velocity, Equation (11) ismodified as follows:

vd �Ntaskk�1

�vk s � �vk � ck� � (16)

where each task capacity is recursively and dynamically com-puted as

c1�t� � max �constant, i.e. �c1�t� � 0�

c2�t� � c1 � �v1 s � �v1 � c1�

c3�t� � c2�t�� v2 s � �v2 � c2�t��

�� (17)

cNtask�t� � cNtask�1�t��vNtask�1

�� s��vNtask�1

�� cNtask�1�t��

Note that by construction, all of the above task capacities arenonnegative, i.e. ck � 0 for all k � [1� Ntask], and that

ck � ck�1 for all k � [2� Ntask]�

ch � 0 �� ck � 0 for all k � h�

namely if a given task is assigned zero capacity, all of thelower-priority tasks will also automatically have zero capac-ity and all of their weights in the sum (16) will be zero. Thecapacity of task h can be viewed as the residual capacity afterthe higher-priority task h � 1 has been commanded� thus, byexample, c2 will be zero (and also ck for k � 2) if the task 1input �v1 is saturating all of its capacity c1. In words, each task


will be commanded with a non-null weight only if the higher-priority task have not saturated. The fact that c1 needs not toexceed max is due to the fact that task 1 alone should not satu-rate the actuator capacity max� moreover, given that ck1 � ck

for all k � [1� Ntask � 1], the condition c1 � max guaranteesthat each term in the sum Equation (16) will have Euclideannorm smaller or equal to the threshold max. Most important,following the same kind of proof reported in Indiveri (2009),it can be shown that also vd in Equation (16) has norm smallerthan or equal to max.

However, the solution described above has some limits:

(1) There is only one saturation threshold (or capacity) foreach task rather than for each actuator. This is a limitbecause, in general, a system may have actuators withdifferent saturation thresholds.

(2) The saturation is symmetric, namely the lower and upperlimit are equal in norm and opposite in sign. This doesnot allow to cope with actuators having asymmetricalsaturation thresholds.

(3) The decision on whether to scale a lower-priority ve-locity command or not is taken only based on the normof the higher-priority command. This corresponds to aworst-case decision as the sum of the two (non-scaled)commands, for example �vk�vk1, could be feasible evenif the norm of the higher-priority command exceeds itsthreshold, i.e. even if �vk � ck .

The solution to overcome these limitations is described inthe following. The novel approach will be described with ref-erence to a multi-robot system rather than a single robot.

3.1. A Novel Saturation Management Technique

The proposed velocity saturation management solution is de-signed at the interface between the NSB module and the lower-level actuator control module: this allows to design the NSBcontrol system in a transparent manner with respect to veloc-ity saturation issues. The adopted notation is introduced beforeproceeding to illustrate the new saturation management tech-nique.

3.1.1. Notation

Denoting by ui the actuator command input vector for roboti , the relationship between ui and the single vehicle’s velocity�pi � vi � �2�1 is assumed to be

ui � Di�zi �vi � (18)

�zi � fi �zi � gi �zi �ui � (19)

where zi is a state vector relative to the direct kinematics ofthe robot and Di �zi � is a well-posed square matrix of full rankfor any zi . As will be apparent in the following sections, Equa-tions (18)–(19) can be thought of as a generalization of thekinematics model of a wheeled robot. Note that, in general, thedynamic equation (19) does not need to be BIBO (bounded in-put bounded output) stable: the absence of finite escape timephenomena, for example, will be sufficient. Indeed if, for ex-ample, z is a position variable, a constant (bounded) actuatorspeed can drive it to infinity and this is not a problem.

Assume that there are l robots, each with ni actuators fori � 1� 2� � � � � l. The total number of actuators is n ��l

i�1 ni .Then the complete model linking the system’s control inputvector u � �n�1 to the system’s velocity v defined in Equa-tion (3) is given by

u � Dtot�z�v� (20)

�z � F�z�G�z�u� (21)

with

z � �zT1 � z

T2 � � � � � z

Tl �

T� (22)

Dtot�z� � diag�D1�z1�� Dl�zl�� (23)

F�z� � diag�f1�z1�� fl�zl�� (24)

G�z� � diag�g1�z1�� gl�zl�� (25)

Following the notation described in Section 2, any single reg-ulation task h (that is to bring �� h to its constant desired value�� h�d), that involves all of the l robots (global task), would havedynamics

�� h � Jh�ph�vh� (26)

where �� h � �mh�1, ph � �n�1, vh :� �ph and Jh � �mh�n .Note that the subscript h denotes the task and not a specificteam member.

According to Equation (4), the ideal robot team commandassociated with this single task is

uh � DtotJ†h

�h�� h�d � �� h�

�� (27)

If a task, the kth, was to involve only a subset of robots (localtask), its dynamics would have the same structure, but withJacobian, system velocity, and command vector of differentsizes.

In order to combine the commands of global and local tasks,it is necessary to construct the command vectors associatedwith local tasks with the same dimensions of the commandvectors associated with the global tasks. These vectors will beindicated with an asterisk *, for example v�k � �n�1. This vec-tor is obtained by putting the components of vk into the posi-tions in a column that are related to the corresponding veloci-ties of the global task and assigning the value zero to all othercomponents. In practice, for example,


v�k � �0� 0� � � � � k1� k2� � � � � 0� � � � � k��1�� k�� 0� � � ��T

where k j is the j th component of the vector vk (kth task) andthe vector v�k � �n�1 is ordered, in the sense that its pth com-ponent is always related to the same velocity component of thesame robot.

In the absence of saturation, the classical NSB solution cor-responding to Ntask tasks (ordered with decreasing priority)would produce an overall command utot � �n�1:

utot �Ntaskk�1

uk� (28)

uk :� Dtot �v�k � (29)

where uk � �n�1 for all tasks k and vectors �vk are definedaccording to Equation (11) with, eventually, the zero paddingconvention described above to make all vectors the same size(notice the � symbol on �vk in Equation (29)).

3.1.2. Saturation Management

The solution in Equation (28) does not account for the fact thatthe single actuators may be constrained to produce outputs ina limited range. In this respect, note that, by construction, ifa command uk ensures that the task error �� k�d � �� k� goes tozero, according to (6)–(7) the same is valid for any scaled com-mand �k uk with2 �k � �0� 1]. Based on this observation, theproposed saturation management technique consists of com-puting suitable command scaling factors such that saturationthresholds are satisfied while preserving the tasks hierarchy.Before analyzing the details of the this approach, note that ifmatrices Di �� in Equation (18) (and hence Dtot in (29)) havebounded norm, there is no conceptual difference in applyingthe saturation management (i.e. scaling schema) to signals �v�kor uk in Equation (29). Indeed either of the two signals can bethought of as the systems control inputs.

With reference to Figure 2 for robot i , this would basicallycorrespond to implementing the saturation block before or af-ter its direct kinematics model Di . As a consequence, for thesake of generality, the system’s control input will be denotedwith wk corresponding to either wk � �v�k or wk � uk accordingto the physical implementation of the solution. The two alter-natives allow to designer to focus on the low-level control sys-tem (wk � uk) or on the guidance level (wk � �v�k ) by viewingas inputs either the actuator output velocities or the vehicle’sspeed components. The description of the saturation manage-ment technique is thus elaborated in terms of the generic inputw.

2. This is also true for �k � 1, but this is excluded to avoid amplifying thecontrol effort.

With reference to the task 1 (task of highest priority), sup-pose that the j th control input should be confined in the inter-val [� j � � j ]: then the set of admissible task 1 commands canbe introduced as

B1 :� �w1 � �n�1 : �1 j � [� j � � j ] � j � 1� � � � � n��

Similarly, the set of scaling-admissible commands can bedefined as

S1 :� �w1 � �n�1 : � �1 � �0� 1]

�� 1�1 j � [� j � � j ] � j � 1� � � � � n��

If the set S1 is empty, this means that the highest-prioritytask is not compatible with the control constraints. Therefore,it can not be implemented even in a scaled form. In particular,when the set S1 is empty, given that task 1 cannot be realized,its corresponding command will need to be the null command,namely it can be written as �1w1 with �1 � 0. If the set S1

is not empty, the vector �1w1 can be commanded to be �1 �max�� 0� 1] : �w1 � B1�.

Once the vector �1 w1 has been commanded, a similar pro-cedure can be adopted for the task of priority 2, i.e.

B2 :� �w2 � �n�1 : �2 j � [c2 j � c2 j ] � j � 1� � � � � n��

S2 :� �w2 � �n�1 : � �2 � �0� 1]

�� 2�2 j � [c2 j � c2 j ] � j � 1� � � � � n��

where c2 j :� � j � �1�1 j and c2 j :� � j � �1�1 j .If the set S2 is empty, the task 2 is not commanded as it

is incompatible with the control constraints. In particular, thenull command �2 w2, with �2 � 0, can be associated with thetask 2. If the set S2 is not empty, the command vector �2 w2,with �2 � max�� 0� 1] : �w2 � B2�, can be associatedwith the task 2.

For tasks of order k � 3, we have

Bk :� �wk � �n�1 : �k j � [ckj � ckj ] � j � 1� � � � � n�� (30)

Sk :� �wk � �n�1 : � �k � �0� 1]

�� k �k j � [ckj � ckj ] � j � 1� � � � � n�� (31)

where

ckj :� c�k�1� j � ��k�1� ��k�1� j �

ckj :� c�k�1� j � ��k�1� ��k�1� j �

If the set Sk is empty, the task k is not commanded as it is in-compatible with the control constraints. In particular, in thiscase the null command �kwk , with �k � 0, can be associatedwith the task k. If the set Sk is not empty, the command vec-tor �k wk , with �k � max�� 0� 1] : �wk � Bk�, can beassociated with the task k.


In the presence of saturation, the final command to send tothe system appears to be

wtot �Ntaskk�1

�kwk� (32)

where the scale factors �k are null or in �0� 1], depending onwhether the corresponding sets Sk are empty or not.

Note that, by construction, Equation (32) guarantees that allcommand saturation constraints are always respected. More-over, in contrast to what occurs by implementing the solutionin Arrichiello et al. (2009) and Indiveri (2009), it may hap-pen that lower-priority tasks are executed while some of theirhigher-priority tasks are not because unfeasible with respect tothe control limits. In practice, this may occur because withinthe new proposed solution it may happen that �i � 0 and� j � 0 with j � i .

To implement the described procedure, it is necessary tocheck, for each task, if the corresponding classical NSB com-mand wk is admissible by scaling or not. This correspondsto checking whether the set Sk is empty or not. When Sk isnot empty, it is also necessary to calculate the greatest fac-tor � � �0� 1] for which �wk is still admissible. Given thedefinitions of the sets Sk and the thresholds ckj and ckj , theproblem associated with the generic task kth can be formulatedin recursive form as follows:

for all k � 1� 2� � � � � Ntask� for all j � 1� 2� � � � � n :

ckj � c�k�1� j � ��k�1� ��k�1� j �

ckj � c�k�1� j � ��k�1� ��k�1� j �

�k �� : ckh � � �kh � ckh

��

�k �n�

h�1

�k

��0� 1]�

�k ��

sup �k if �k �� 0 otherwise,

with c0 j � � j , c0 j � � j , �0 � 0 and w0 � 0.Note that, in the hypothesis that the classical NSB com-

mands are bounded, i.e. wk � � for all k, and that thecontrol signals do not have saturation thresholds, i.e. � j �� and � j � � for each j , there are �k � �0� 1] and�k � 1 for all k, thus, the commands resulting from the al-gorithm proposed coincide systematically with those of theclassical NSB. Also note that, by construction, the proposedmethod to scale the components of wk guarantees that theinfinity norm of the processed wk command is bounded bythe largest ckj in Equation (30). Should a 2-norm (or any otherwell-posed norm, rather than an infinity norm) threshold of wk

be of interest, this is by no means an issue: one would simplyneed to compute the scaling factor between the two norms andapply it to the command wk . Further details on this point areomitted for the sake of brevity.

4. Case Studies

In order to test the conjuncted action of the NSB approachwith the saturation management technique described in Sec-tion 3, two case studies are described in the following. In thefirst experiment, the NSB elaborates the motion directives fora single mobile robot that has to reach a target while avoidinga point obstacle� thus, the mission is composed by two elemen-tary tasks: avoid the obstacle and reach the target. In the sec-ond experiment, the NSB elaborates the motion directives fora team of six mobile robots that have to entrap and to escorta target� in this more complex case the mission is composedby four tasks. Thus, after the description of the robot kinemat-ical model, the two case studies will be discussed in order tovalidate the proposed approach both by simulations and exper-imentally with real robots.

4.1. Unicycle-like Kinematic Model

Assume that all vehicles to be controlled are differential drivewheeled mobile robots moving in a horizontal plane. In orderto implement the kinematics control solution described in sec-tion 2 on such a mobile robot, the systems’ velocity vd shouldbe assigned at each time instance. Since the NSB outputs alinear velocity for a material point, while the mobile robot ac-tuators are its wheels, it is necessary to convert the NSB outputvd to wheels’ desired velocities ��.

With reference to Figure 5 and the variables depicted there,the mapping between the wheels’ angular velocities3 ��l� �r �

T

and the velocity of point C (wheels axis center) is given by

�� C

�

�� r

2

�� 1 1

� 1b

1b

��l

�r

�� (33)

where C i1 and ��i1 � j1� are the linear and angular velocityvectors of point C (note also that � � �� ).

Yet given the non-holonomic nature of the mobile robot, thevelocity of point C cannot be arbitrarily assigned as it must bealways aligned with i1. To overcome this difficulty, the pro-posed control solution can be applied to a point P as depictedin Figure 5. Denoting by vP � � P�x�� P�y��T the velocity

3. Here �l j1 and �r j1 are the left and right wheels’ angular velocity vectors,respectively.


Fig. 5. Khepera II mobile robot: unicycle-like mobile robotmanufactured by K-Team and its kinematics model

of point P in the absolute frame �i0� j0�, standard kinematicsrelations allow to express wheels’ speeds in terms of vP as��l

�r

�� 1

r

��1 � b

�P

1 b�P

�� c� s�

�s� c�

�� P�x�

P�y�

�� (34)

where c� :� cos � and s� :� sin � . As for the choice of the pa-rameter �P , this should be small enough to let point P stay inthe footprint of the vehicle. The specific choice �P � b guar-antees isotropy in the sense that the norm of ��l� �r �

T does notdepend on the orientation of vP .

Equation (34) corresponds to Equation (18), namely thewheel angular velocities �l and �r are the components of thecommand vector u, the state variable z is the robots heading �and the matrix multiplying vP in (34) is D�z�:

u � ��l� �r �T� (35)

z :� �� (36)

D�z� � 1

r

��1 � b

�P

1 b�P

�� cz sz

�sz cz

�� (37)

�z � f gu� (38)

g :�� r

2b�

r

2b

� �1�2� (39)

f � 0� (40)

where Equation (38) (modeling the robot’s rotational motion)is simply the second component of Equation (33). This showsthat indeed Equations (18) and (19) generalize the kinematicsof wheeled differential drive robots. Note that D�z� in Equa-tion (37) is always well defined. As for the dynamic equa-tion (38), note that this is not BIBO stable: if, for example,

the robot was required to move always on a circle, u would beconstant and z�t� � ��t� would be a ramp. The only reason-able assumption on the dynamics of z is that it does not exhibitfinite escape time phenomena and this is certainly guaranteedby the simple integrator dynamics of Equation (38).

The specific implementation presented below refers to thealternative wk � �v�k , namely the control inputs processed bythe saturation management module are the velocity commandselaborated by the NSB module rather than the robot’s wheelvelocities. As discussed in Section 3.1.2, given that matrixD�z� in Equation (37) has bounded norm, there is no majordifference between this option and the alternative wk � uk

(wheel speeds). The motivation in choosing wk � �v�k is basi-cally related to the software architecture of the experimentalplatform for which this solution was simpler to realize.

4.2. First Case Study: Single Robot–Two Tasks Mission

In this section we will analyze the case study of a single non-holonomic mobile robot that has to perform an autonomousmission of reaching sequential targets while avoiding point ob-stacles. Thus, after the definition of the proper task functionsfor the NSB implementation, simulation and experimental re-sults will be discussed.

In particular, the robot mission is decomposed into two el-ementary tasks:

� Task 1: obstacle avoidance�

� Task 2: move to goal.

The obstacle avoidance is the highest-priority task becauseits achievement is of crucial importance to preserve the in-tegrity of the vehicle. However, this task is activated only whenrequired, i.e. when the robot is moving in the obstacle directionand when the robot–obstacle distance is lower than a safetyvalue.

The task functions are defined as follows:

� Obstacle avoidance. In the presence of an obstacle in theadvancing direction, the task aim is to keep the robot at asafety distance from the obstacle. Thus, its implementa-tion elaborates as output a velocity, in the robot–obstacledirection, that keeps the robot at a safe distance from theobstacle. Therefore, it is

� 1 � p� po � �� 1�d � d�

where po is the obstacle position and d is the safety dis-tance from the obstacle.


Table 1. Kinematic and Mission Parameters for the FirstCase Study

Kinematic parameters Mission parameters

r � 8 mm �1 � 2 � 1

b � 2�65 cm max vd � max � 7�5 cm s�1

�pb � 1 � 1�d � 40 cm

Fig. 6. Path followed by the robot during the simulation.

� Move to goal. The move-to-goal task generates a ve-locity command in the vehicle-to-goal direction propor-tional to the distance from the goal pg� namely,

�� 2 � p � �2�1�

�� 2�d � pg�

4.2.1. Simulation Results

The simulations have been performed via an ad-hoc softwarewritten in C . It should be noted that the velocity commandsgenerated by such code can be either fed into a custom-madevisualization software for simulation, or to the real experimen-tal set-up for physical tests. This allows the simulations to berun with exactly the same law used for the experimental tri-als. In particular, all of the mission parameters (ranging, byexample, from the sampling frequency, the control gains andthe robot parameters) used in simulation are the same of theexperimental tests. The chosen parameters are summarized inTable 1.

In the proposed mission, the robot, starting from a positionclose to a point obstacle, has to sequentially reach two targetconfigurations. Figure 6 shows the path followed by the robot(solid line) and the obstacle avoidance safety region (dottedline).

Fig. 7. Task velocity commands during the whole simulation.(a) Norm of �v1 in dotted line and norm of �1 �v1 in solid line.(b) Norm of �v2 in dotted line and norm of �2 �v2 in solid line.

Since the starting position is deep inside the obstacle-avoidance safety region, at the beginning of the mission theobstacle-avoidance task function saturates the velocity request.As a consequence, the move-to-goal task is scaled almost tozero and the robot starts its movement escaping along a radiusof the circle centered in the obstacle. Once sufficiently far fromthe obstacle, the vehicle heads toward the first goal along thetangent to the circle. After the achievement of goal 1, the ro-bot tries to reach the second goal, behind the obstacle. Again,during its mission the robot meets the obstacle and then turns,moving on the obstacle-avoidance safety region (i.e. sliding onthe null-space of the obstacle-avoidance task).

In Figure 7 the norms of velocity commands related to theobstacle-avoidance and move-to-goal tasks are shown. Fig-ure 7(a) exhibits the plots of the norm of �v1, elaborated bythe NSB as defined in Equation (11), before (dotted line) andafter (solid line) the multiplication by the scaling factors �1.Analogously, in Figure 7(b) the plots of the norm of �v2 (dottedline) and �2 �v2 (solid line) are depicted.

To properly evaluate the saturation mechanism technique,Figure 8 shows the main saturation parameters during the first6 seconds of the experiment, where their variation is most sig-nificant. Figures 8(a) and (b) respectively exhibit the norm of�v1 and �v2 to the left, �1 and �2 in the middle, and the normof �1 �v1 and �2 �v2 to the right. Figure 9 shows the norm of thesum of the two velocity commands plotted in Figure 8. To en-sure that the Euclidean norm of the overall velocity commandvd is bounded by max, the command is scaled by a proper fac-tor� thus, vd is always under the saturation threshold max.Finally, Figures 10(a) and (b) respectively display the veloci-ties �l and �r .

In order to comparatively evaluate the performance ofthe proposed velocity saturation management technique, the


Fig. 8. The saturation management technique during the first6 seconds of the simulation. (a) Behavior of the obstacle-avoidance task. (b) Behavior of the move-to-goal task. In boththe cases, the left, middle, and right plots respectively show thenorm of the velocity command �vk , the scaling factor �k and thenorm of the scaled velocity command �k �vk .

Fig. 9. Norm of the overall velocity command vd during thesimulation.

Fig. 10. Wheels’ angular velocities (a) �l and (b) �r duringthe simulation.

single-robot mission described above is executed with the fol-lowing four different methods (the control gains are alwaysthe same in all four cases):

(a) Classical NSB in the absence of saturation, namely anycommanded speed, no matter how large in norm, can beimplemented by the robot.

(b) Classical NSB where the norm of the velocity vector vd

is scaled to a maximum threshold in the eventuality itexceeds such a threshold.

(c) Classical NSB where the desired velocity vd (no matterhow large it is) is mapped to wheel speed commands.Then the wheel speed commands are saturated to a max-imum allowed threshold.

(d) NSB with proposed saturation scheme on the signalw � �v�d . In this case an additional scaling factor is alsocomputed such that the 2-norm (rather than the infinitynorm) of the scaled �v�d does not exceed a prescribed up-per bound.

The resulting paths of these four alternatives are depictedin Figure 11. To some extent case (a) can be thought of asthe ideal situation: yet such a case could never be realized inpractice given the extremely large speed of the robot (refer toFigure 12). Of course one could try to limit such speed val-ues by decreasing the control gains (equal in the four cases),but there would still always be different initial configurationsgiving rise to very large speeds. The paths of case (b) appearto be quite close to the “ideal” paths of case (a). Neverthe-less, the plot of the task errors in Figure 13 reveals that thesecondary task (move to goal 1) is actually perturbing the pri-mary task in the sense that a portion of the control effort thatshould be used for the primary task is actually sacrificed forthe sake of the secondary task. This is in deep conflict with thevery spirit of tasked based control as the lower-priority tasksshould be executed only to the extent that they do not perturbthe higher priority tasks. Similar considerations also hold forcase (c) that can be regarded as the worst case as far as the re-sulting paths are concerned. In contrast, case (d) results in taskerror convergence dynamics that are fully compliant with thetask-based control paradigm: namely, the primary task (beingalways feasible in this case) is always given highest priority inspite of saturations. Indeed the secondary task does not sub-tract control authority to the primary task: it is executed onlywhen the primary task error is sufficiently small. This behavioris reflected in the resulting paths showing that the robot firstmoves away from the obstacle and only when it is sufficientlyfar it uses its control authority to (also) implement the sec-ondary task.

The values of vd corresponding to the four cases are de-picted in Figure 12. The values reported for case (c) are com-puted through the inverse kinematics model based on the satu-rated wheel speeds. Note that quantization of the wheel speed


Fig. 11. Paths obtained in the four comparative cases described.

Fig. 12. 2-Norm of the vd values relative to the four comparative cases described.


Fig. 13. Task errors relative to the four comparative cases described: the primary task errors are depicted with a solid line (y-scaleon the left) and the secondary task errors with a dashed one (y-scale on the right).

commands is modeled in simulation in compliance to the ex-perimental platform. The effect of such quantization is visiblein the corresponding (case (c)) vd plot of Figure 12.

4.2.2. Experimental Results

The experimental set-up is based on a team of Khepera IImobile robots manufactured by K-team4 and available at theLAI (Laboratorio di Automazione Industriale) of the Univer-sità degli Studi di Cassino (see Figure 5). The Khepera II mo-bile robot is a differential-drive vehicle having a unicycle-likekinematics with an approximate dimension of 8 cm in diam-eter. The robot can communicate through a Bluetooth mod-ule with a remote Linux-based PC where the NSB has beenimplemented. To estimate the needed absolute position mea-surements we have developed a vision-based system usingtwo CCD cameras, a Matrox Meteor-II frame grabber (Ma-trox 2001) and self-developed C++ image-processing func-tions. The acquired images are 1�024�768 RGB bitmaps. Themeasurement error has an upper bound of � 0�5 cm and � 1�.The remote PC receives the position measurements at a sam-pling time of 70 ms and elaborates the NSB control. Once the

4. See http://www.k-team.com/.

Fig. 14. Path followed by the robot during the experiment.

NSB generates the desired linear velocities, the wheels’ de-sired velocities are computed through the kinematic model ofSection 4.1 and sent to the robot through the Bluetooth mod-ule. The wheels’ controller (onboard the robot) is a PID devel-oped by the manufacturer. The encoders resolution is such thata quantization of � 0�8 cm s�1 and � 9� s�1 is experienced.

Analogously to the simulative case study, Figure 14 showsthe paths of the mobile robot during the overall mission. Theexperiment is shown in Extension 1. The behaviors of the nu-


Fig. 15. The saturation management technique during the whole experiment. (a) Behavior of the obstacle-avoidance task. (b)Behavior of the move-to-goal task. In both cases, the left, middle, and right plots respectively show the norm of the velocitycommand �vk , the scaling factor �k , and the norm of the scaled velocity command �k �vk .

Fig. 16. Norm of the overall velocity command vd during theexperiment.

merical simulations and of the experiments are in excellentagreement� however, the noise and the slight irregularity ofthe robot path in Figure 14 is due to several experimental fac-tors including a relatively high sample time of the control al-gorithm execution (that is performed on a remote controller),the quantization due to the encoders resolutions and to hard-ware/software limits of the Khepera II mobile robot, and mea-surement noise of the vision system for position estimation.Moreover, the difference in the mission execution time is dueto the vehicle dynamics that in the simulation cases is ne-glected.

To evaluate the performance of the saturation manage-ment technique, Figure 15 shows the output of the obstacle-avoidance and move-to-goal task functions before (see the leftplots of Figure 15) and after (see the right plots of Figure 15)the application of the saturation management technique. It isworth noting that, at the very beginning of the experiment, theinfinity norm of the obstacle-avoidance task function output�v1 overtakes the threshold max, thus the scaling factor �1 islower than 1 (see the middle plots of Figure 15(a)) and it sat-urates the velocity command �v1, while the scaling factor �2

(see the middle plots of Figure 15(b)) saturates the secondarytask keeping the components of vd to the saturation thresholds.

Fig. 17. Wheels’ angular velocities (a) �l and (b) �r duringthe experiment.

When the component of the primary task output �v1 goes underthe threshold, the scaling factor �1 is constantly equal to 1.Figure 16 shows the norm of the overall velocity command vd ,calculated as in Equation (32). As in the simulative case, theEuclidean norm of vd is always under the Euclidean norm ofthe maximum possible velocity max � 7�5 cm s�1.

Finally, Figure 17 shows the wheels’ angular velocities �l

and �r during the experiment.

4.3. Second Case Study: Multi-robot–Four Tasks Mission

In this section, we analyze the case study of a team of sixmobile robots that has to escort an external target. The con-sidered mission is aimed at making the robots entrapping and


then escorting an external target by properly displacing aroundit, and by reducing its escape windows. The escorting missionhas some similarities with the caging mission, where a teamof robots is commanded to surround an object and transport it.The last mission has been recently studied for cooperative mo-bile robots (Fink et al. 2008� Murphey and Horowitz 2008)and marine vehicles (Esposito 2009), and it mainly focuseson the cooperative manipulation issues. In this paper, instead,we consider the escorting mission analyzed in Antonelli et al.(2008a), that does not consider any physical interaction withthe escorted object. We use such a mission as a specific casestudy to test our saturation management algorithm since it iscomposed of multiple elementary tasks, and it allows to testthe algorithm in complex scenarios. Thus, in the following, af-ter the definition of the tasks that compose the escorting mis-sion for the NSB implementation, simulation and experimentalresults with a team of six mobile robots will be discussed.

The escorting mission (Antonelli et al. 2008a) is decom-posed into four elementary tasks, namely:

� Task 1: obstacle avoidance�

� Task 2: distribution on a circumference�

� Task 3: centroid on the target�

� Task 4: polygon with equal edges�

where the task functions are defined as follows:

� Obstacle avoidance. The task function is defined as inSection 4.2� moreover, to avoid collisions among teammembers, each robot considers the neighboring robotsas obstacles. As in the previous case study, the obstacleavoidance, when active, is the highest-priority task.

� Distribution on a circumference. This task moves the ro-bots on a given circumference around the centroid. Therelative one-dimensional task function is

�� s ��

1

2�pi � c�T �pi � c� � � �

�T

used to keep each robot of the team at a given distance rfrom a point c � �2�1 by setting

�� s�d ��

r2

2� � �

�T

�

� Centroid on the target. This task commands the ro-bots’ centroid to be coincident with the target. The two-dimensional task function �� c is simply given by

�� c � fc�p1� � � � �pl� � 1

l

li�1

pi � �p�

Table 2. Kinematic and Mission Parameters for the SecondCase Study

Kinematic parameters Mission parameters

r � 8 mmmax vd � max vd � i�max � 15 cm s�1

b � 2�65 cm � o�d � 20 cm�pb � 1 o � 1, s � 0�5

c � 2, p � 3

� Polygon with equal edges. The polygon-with-equal-edges task distributes the robot at the vertices of a poly-gon with equal sides. This is achieved by simply impos-ing the same distance between adjacent vehicles.

According to Equation (4), each elementary behavior out-puts a velocity reference command to each robot of the team.To obtain the actual motion reference commands to the robots,the outputs that accomplish the single behaviors are merged byEquation (32) on the basis of the active behaviors and on theirpriority orders.

4.3.1. Simulation Results

In the proposed mission, a team of six robots has to entrap andescort a target that is a disk. The same mission parameters havebeen used in simulations and experiments, and are summarizedin Table 2.

Figure 18 shows the system evolution during the whole sim-ulation. In particular, at the beginning of the simulation, therobot 6 is deep inside the safety region (dotted line) of the diskplaced in the point of coordinates �80� 110�, while the othersfive robots are aligned parallel to the x-axis. After 5.65 sec-onds the six robots have entrapped the target. Then, the diskis moved in a straight line to the point of coordinates �76� 70�.Therefore, the team follows and escorts the disk until destina-tion.

To describe how the proposed approach operates, we focusthe attention on the behavior of the robot 4. In Figure 19 thenorm of the velocity commands for each task of the robot inthe absence (dotted line) and in the presence (solid line) ofsaturation is shown. In particular, Figure 19 exhibits the normof �v4k and �k �v4k for k � 1� � � � � 4. Figure 20 displays the normof the overall velocity command v4�d of the robot during thesimulation. It is worth noting that the Euclidean norm of v4�d

is always below the maximum threshold 4max � 15 cm s�1.Finally, Figures 21(a) and (b) respectively show the wheels’angular velocities �l and �r of the robot during the simulation.


Fig. 18. Evolution of the system during the whole simulation.

Fig. 19. Task velocity commands for the robot 4 during the whole simulation. Behavior of (a) obstacle-avoidance, (b) distribution-on-a-circumference, (c) centroid-on-the-target, and (d) polygon-with-equal-edges tasks. In every cases, there is the norm of �v4k

in dotted line and the norm of �k �v4k in solid line.

4.3.2. Experimental Results

The experimental trials relative to the multi-robot case havebeen performed at the LAI of the Università degli Studi diCassino. The team of robots is made of Khepera II mobile ro-bots, shown in Figure 22, manufactured by K-team5. Detailsabout these robots were given in Section 4.2.2.

5. See http://www.k-team.com/.

Analogously to the simulative case study, Figure 23 showsthe system evolution during the whole simulation. The experi-ment is shown in Extension 2. As for the single-robot case, thebehaviors of the numerical simulations and of the experimentsare in good agreement.

To properly evaluate the saturation mechanism technique,we focus attention on the behavior of the robot 6. Figure 24shows the main saturation parameters for all tasks of this robot


Fig. 20. Norm of the overall velocity command v4�d during thesimulation.

Fig. 21. Wheels’ angular velocities (a) �l and (b) �r of therobot 4 during the simulation.

Fig. 22. Team of Khepera II mobile robots used during theexperiments.

during the experiment. In particular, Figures 24(a)–(d) showthe norm of �v6k to the left, �k in the middle, and the norm of�k �v6k to the right for k � 1� � � � � 4, respectively. Figure 25shows the norm of the overall velocity command v6�d for therobot during experiment. Figure 26(a) and (b) shows, respec-tively, the wheels’ angular velocities �l and �r of the robotduring the experiment.

Finally, Figure 27 shows the task errors for the whole sys-tem during the experiment. The errors are first convergent to

Fig. 23. Evolution of the system during the whole experiment.

zero. Then, when the target is moved, the errors increase andfinally converge again at zero since the robots follow and es-cort the target to destination.

5. Conclusions

In this paper, the conjunct application of a behavior-basedtechnique, namely the NSB control (Antonelli et al. 2008b),with the new saturation management technique described inSection 3, has been investigated. The objective of this paperwas to extend the NSB control to the motion control of ro-botic systems with velocity saturated actuators, avoiding thatvelocity saturations induced by lower-priority tasks corrupt thehigher-priority tasks. In particular, the proposed solution aimsat managing actuator velocity saturations by dynamically scal-ing task velocity commands, so that the hierarchy of task prior-ities is preserved in spite of actuator velocity constraints, and


Fig. 24. The saturation management technique for the ro-bot 6 during the whole experiment. Behavior of (a) obstacle-avoidance, (b) distribution-on-a-circumference, (c) centroid-on-the-target, and (d) polygon-with-equal-edges tasks. Inevery case, the left, middle, and right plots respectively showthe norm of the velocity command �v6k , the scaling factor �k

and the norm of the scaled velocity command �k �v6k .

Fig. 25. Norm of the overall velocity command v6�d during theexperiment.

at overcoming the limits of the solution proposed by Indiveri(2009). The proposed approach has been first tested on the mo-tion control of a single robot, and then on the motion control ofa team of cooperative mobile robots. The first test aims at bet-ter explaining the approach characteristics in a simple scenariowith a single mobile robot achieving a two-task mission�more-over, this case study has been used for a comparative analysis

Fig. 26. Wheels’ angular velocities (a) �l and (b) �r of therobot 6 during the experiment.

Fig. 27. Errors during the experiment. Error of the (a) centroid-on-the-target, (b) obstacle-avoidance, (c) distribution-on-a-circumference, and (d) polygon-with-equal-edges task func-tions.

with different saturation management techniques in order tovalidated the effectiveness of the proposed method. The sec-ond test aims at validating the approach in a more complexsituation with a team of six mobile robots achieving a four-task mission. In both cases, the approach has been successfullyvalidated by numerical simulations and experimental tests withreal mobile robots.


Acknowledgment

The research leading to these results has received funding fromthe European Community’s Seventh Framework Programmeproject CHAT: “Control of Heterogeneous Automation Sys-tems: Technologies for scalability, reconfigurability and secu-rity”, Contract Number INFSO-ICT-224428.

Appendix: Index to Multimedia Extensions

The multimedia extension page is found at http://www.ijrr.org

Table of Multimedia Extensions

Extension Type Description

1 Video Experiment with a single mobile ro-bot and a two tasks mission.

2 Video Video of the experiment with a teamof cooperative mobile robots and afour-task mission.

References

Aguiñaga-Ruiz, E., Zavala-Río, A., Santibáñez, V. and Reyes,F. (2009). Global trajectory tracking through static feed-back for robot manipulators with bounded inputs. IEEETransactions on Control Systems Technology, 17(4): 934–944.

Antonelli, G., Arrichiello, F. and Chiaverini, S. (2008a). Theentrapment/escorting mission: an experimental study usinga multirobot system. IEEE Robotics and Automation Mag-azine, 15(1): 22–29.

Antonelli, G., Arrichiello, F. and Chiaverini, S. (2008b). TheNull-Space-based Behavioral control for autonomous ro-botic systems. Journal of Intelligent Service Robotics, 1(1):27–39.

Antonelli, G., Arrichiello, F. and Chiaverini, S. (2008c). Sta-bility analysis for the null-space-based behavioral controlfor multi-robot systems. 47th IEEE Conference on Deci-sion and Control and 8th European Control Conference,Cancun, Mexico.

Antonelli, G., Arrichiello, F. and Chiaverini, S. (2009). Exper-iments of formation control with multirobot systems usingthe null-space-based behavioral control. IEEE Transactionson Control Systems Technology, 17(5): 1173–1182.

Antonelli, G. and Chiaverini, S. (2003). Kinematic controlof a platoon of autonomous vehicles. Proceedings 2003IEEE International Conference on Robotics and Automa-tion, Taipei, Taiwan, pp. 1464–1469.

Antonelli, G. and Chiaverini, S. (2004). Fault tolerant kine-matic control of platoons of autonomous vehicles. Proceed-ings 2004 IEEE International Conference on Robotics andAutomation, New Orleans, LA, pp. 3313–3318.

Arkin, R. (1989). Motor schema based mobile robot naviga-tion. The International Journal of Robotics Research, 8(4):92–112.

Arkin, R. (1998). Behavior-based Robotics. Cambridge, MA,The MIT Press.

Arrichiello, F., Chiaverini, S., Pedone, P., Zizzari, A. and In-diveri, G. (2009). The null-space based behavioral controlfor non-holonomic mobile robots with actuators velocitysaturation. Proceedings 2009 IEEE International Confer-ence on Robotics and Automation, Kobe, Japan, pp. 4019–4024.

Balch, T. and Arkin, R. (1998). Behavior-based formation con-trol for multirobot teams. IEEE Transactions on Roboticsand Automation, 14(6): 926–939.

Brooks, R. (1986). A robust layered control system for a mo-bile robot. IEEE Journal of Robotics and Automation, 2:14–23.

Chen, H., Ma, M.-M., Wang, H., Liu, Z.-Y. and Cai, Z.-X.(2009). Moving horizon H� tracking control of wheeledmobile robots with actuator saturation. IEEE Transactionson Control Systems Technology, 17(2): 449–457.

Chiaverini, S. (1997). Singularity-robust task-priority redun-dancy resolution for real-time kinematic control of robotmanipulators. IEEE Transactions on Robotics and Automa-tion, 13(3): 398–410.

Dixon, W. E., de Queiroz, M. S., Zhang, F. and Dawson,D. M. (1999). Tracking control of robot manipulators withbounded torque inputs. Robotica, 17: 121–129.

Esposito, J. (2009). Decentralized cooperative manipula-tion with a swarm of mobile robots. Proceedings 2009IEEE/RSJ International Conference on Intelligent Robotsand Systems, StLouis, MO, pp. 5333–5338.

Farinelli, A., Iocchi, L. and Nardi, D. (2004). Multirobot sys-tems: a classification focused on coordination. IEEE Trans-actions on Systems, Man, and Cybernetics—Part B, 34(5):2015–2028.

Fink, J., Hsieh, M. and Kumar, V. (2008). Multi-robot manipu-lation via caging in environments with obstacles. Proceed-ings 2008 IEEE International Conference on Robotics andAutomation, Pasadena, CA, pp. 1471–1476.

Fossen, T. (2002). Marine Control Systems: Guidance, Navi-gation and Control of Ships, Rigs and Underwater Vehicles.Trondheim, Norway, Marine Cybernetics.

Hurdus, J. G. and Hong, D. W. (2008). Behavioral program-ming with hierarchy and parallelism in the DARPA UrbanChallenge and RoboCup. Proceedings 2008 IEEE Interna-tional Conference on Multisensor Fusion and Integrationfor Intelligent Systems, Seoul, Korea, pp. 503–509.

Indiveri, G. (2009). Swedish wheeled omnidirectional mobilerobots: kinematics analysis and control. IEEE Transactionson Robotics, 25: 164–171.

Kelly, R. and Moreno, J. (2005). Manipulator motion controlin operational space using joint velocity inner loops. Auto-matica, 41: 1423–1432.


Kim, S. and Park, F. C. (2008). Fast robot motion generationusing principal components: framework and algorithms.IEEE Transactions on Industrial Electronics, 55(6): 2506–2516.

Kumar, R. and Stover, J. A. (2000). A behavior-based intel-ligent control architecture with application to coordinationof multiple underwater vehicles. IEEE Transactions on Sys-tems, Man, and Cybernetics—Part A, 30: 767–784.

Maciejewski, A. (1988). Numerical filtering for the opera-tion of robotic manipulators through kinematically singu-lar configurations. Journal of Robotic Systems, 5(6): 527–552.

Mataric, M. (1997). Behavior-based control: examples fromnavigation, learning, and group behavior. Journal of Ex-perimental and Theoretical Artificial Intelligence, 9(2–3):323–336.

Matrox (2001). Matrox Imaging Library User Guide. Dorval,Quebec, Canada, Matrox Electronic Systems Ltd.

Murphey, T. and Horowitz, M. (2008). Adaptive cooperativemanipulation with intermittent contact. Proceedings 2008IEEE International Conference on Robotics and Automa-tion, Pasadena, CA, pp. 1483–1488.

Nakamura, Y., Hanafusa, H. and Yoshikawa, T. (1987). Task-priority based redundancy control of robot manipulators.The International Journal of Robotics Research, 6(2): 3–15.

Omr�cen, D., Žlajpah, L. and Nemec, B. (2007). Compensationof velocity and/or acceleration joint saturation applied toredundant manipulator. Robotics and Autonomous Systems,55: 337–344.

Parker, L. (1996). On the design of behavior-based multi-robotteams. Advanced Robotics, 10(6): 547–578.

Samson, C., Borgne, M. L. and Espiau, B. (1991). Robot Con-trol: The Task Function Approach. (Engineering ScienceSeries, No. 22). Oxford, Clarendon Press.

Sciavicco, L. and Siciliano, B. (2000). Modeling and Controlof Robot Manipulators. London, Springer.

Wu, F., Lin, Z. and Zheng, Q. (2007). Output feedback sta-bilization of linear systems with actuator saturation. IEEETransactions on Automatic Control, 52(1): 122–128.

Zavala-Río, A. and Santibáñez, V. (2007). A natural saturat-ing extension of the PD-with-desired-gravity-compensationcontrol law for robot manipulators with bounded inputs.IEEE Transactions on Robotics, 23(2): 386–391.

the null-space-based behavioral control for mobile robots...

Documents