1 Formation Control of Marine Surface Vessels using the ...webuser.unicas.it/arrichiello/papers/ArrChiFos_06.pdfioral approaches have been applied to the formation control of multi-robot

1

Formation Control of Marine Surface Vessels

using the Null-Space-Based Behavioral Control

Filippo Arrichiello1, Stefano Chiaverini1, and Thor I. Fossen2

1 Dipartimento di Automazione, Elettromagnetismo, Ingegneria dell’Informazionee Matematica Industriale. Universita degli Studi di Cassino, via G. Di Biasio 43,03043 Cassino (FR), Italy. {f.arrichiello,chiaverini}@unicas.it

2 Department of Engineering Cybernetics, Norwegian University of Science andTechnology, NO-7491 Trondheim, Norway. [email protected]

Summary. In this paper the application of the Null-Space-Based behavioral control(NSB) to a fleet of marine surface vessels is presented. From a marine applicationspoint of view, the NSB can be considered as a guidance system that dynamicallyselects the motion reference commands for each vessel of the fleet. These motioncommands are aimed at guiding the fleet in complex environments simultaneouslyperforming multiple tasks, i.e., obstacle avoidance or keeping a formation. In orderto apply the guidance system to a fleet of surface vessels through the entire speedenvelope (fully-actuated at low velocities, under-actuated at high velocities), theNSB works in combination with a low-level maneuvering control that, taking care ofthe dynamic models of the vessels, elaborates the motion commands to obtain thegeneralized forces at the actuators. The guidance system has been simulated whilesuccessfully performing complex missions in realistic scenarios.

Key words: Behavioral control, formation control, fleet of marine vessels.

1.1 Introduction

The field of cooperation and coordination of multi-robot systems has beenobject of considerable research efforts in the last years. The basic idea is thatmulti-robot systems can perform tasks more efficiently than a single robot orcan accomplish tasks not executable by a single one. Moreover, multi-robotsystems have advantages like increasing tolerance to possible vehicle fault,providing flexibility to the task execution or taking advantage of distributedsensing and actuation. In the context of this paper, the term multi-robot sys-tem means a generic system of autonomous vehicles like Unmanned GroundedVehicles (UGVs), Unmanned Aerial Vehicles (UAVs), Unmanned Underwater

2 Arrichiello et al.

Vehicles (UUVs) or autonomous surface vessels. Thus, applications of multi-robot systems can be widely different such as, e.g., exploration of an unknownenvironment with a team of mobile robots [10], navigation and formation con-trol with multiple UAVs [29], UUVs [12] and surface vessels [18, 19] and objecttransportation with multiple UGVs [30].

When controlling a multi-robot system, a common requirement is the mo-tion coordination among the vehicles; that is, the vehicles have to move in theenvironment keeping a suitable relative configuration. Among other possiblemethods, in this paper behavior-based approaches to coordinate a multi-robotsystem are considered.

Behavior-based approaches, widely studied for robotic applications [5], areuseful to guide a multi-robot system in an unknown or dynamically changingenvironment. These approaches give the system the autonomy to navigate incomplex environments avoiding low-level path planning, using sensors to ob-tain instantaneous information of the environment and increasing flexibilityof the system. Among the behavioral approaches, seminal works are reportedin the papers [9] and [4], while the textbook [5] offers a comprehensive stateof the art. A behavioral approach designed for exploration of planetary sur-faces has been investigated in [17], while in [20] the experimental case of anautonomous navigation in outdoor natural terrain is presented. Lately, behav-ioral approaches have been applied to the formation control of multi-robotsystems as in, e.g. [23], [22] and [6]. The use of techniques inherited frominverse kinematics for industrial manipulators is described in [7, 3]. In [26]an architecture for dynamic changes of the behavior selection strategies ispresented.

In the behavior-based approach, the mission of the system is usually de-composed in elementary sub-problems, eventually solvable in parallel, whosesolutions need to be composed in one single motion command for each robot.The sub-problems are commonly termed behaviors or tasks. In presence ofmultiple behaviors, each task output is designed so as to achieve its specificgoal but it is generally impossible that a single motion command to the robotcan accomplish all the assigned behaviors at the same time. In particular,when a motion command cannot reduce simultaneously the value of all thetask functions there is a conflict among the tasks that must be solved by asuitable policy.

The behavior-based approach proposed in this paper, namely the Null-Space-Based behavioral control (NSB), differs from the other existing meth-ods in the behavioral coordination method, i.e., in the way the outputs of thesingle elementary behaviors are assembled to compose a complex behavior.The NSB can be seen as a centralized system aimed at guiding a platoon ofautonomous vehicles in different scenarios and to achieve different missions.Following the main behavioral approaches, the mission of the platoon is de-composed in elementary tasks, i.e., move the barycenter of the platoon, avoidobstacles, keep a formation. For each task, a function that measures its degreeof fulfilment (e.g., a cost or a potential function) can be defined; thus, in a

1 Formation control of Marine Surface Vessels using the NSB 3

static environment, the task is achieved when its output is constant at a valuethat minimizes the task function. Using a suitable policy to manage multipletasks, the NSB is aimed at elaborating the instantaneous motion referencesfor each vehicle. How to follow these motion references, instead, is the aim ofa low-level maneuvering control system. In this way, the NSB does not takecare of the typology of the vehicles and let the maneuvering control systemtake into consideration the kinematics and the dynamics of the vehicles. Thischaracteristic lets the NSB be applicable to different kinds of vehicles likeautonomous robots (both holonomic and nonholonomic), underwater robotsor surface vessels (fully actuated and underactuated).

In this paper the application of the NSB to a fleet of marine surface vesselsis presented. The proposed case considers vessels that are underactuated athigh velocities and fully-actuated at low velocities. That is, the propulsiveaction of the vessels can span all the generalized directions at low speeds (i.e.,both force in the surge and sway direction an torque in the yaw direction),and part of them at high speeds.

The proposed guidance system has been widely tested in numerical sim-ulations and a case study of a fleet of vessels performing complex missionsin realistic scenarios is presented in order to validate the effectiveness of theapproach.

1.2 Guidance system for marine surface vessels

The guidance system proposed in this paper has to solve simultaneously dif-ferent problems: it is responsible for ships’ safety, that is, it has to makethe ships able to avoid static and dynamic obstacles doing a dynamic pathgeneration (path generation on-the-fly); it has to achieve different kinds ofmissions, e.g., navigation keeping a fixed relative formation or changing theroute or the formation considering weather and environmental conditions; ithas to maneuver fully/under-actuated ships (ships equipped with 3/less than3 actuators).

To this purpose, the guidance system (see Figure 1.1) is decomposed in twomain blocks: the Null-Space-Based behavioral control and the maneuveringcontrol. The NSB takes into consideration the parameters of the mission, theenvironmental condition and the status of the fleet to elaborate the desiredvelocities for each vehicle. These velocities represent the reference input for themaneuvering controls that, taking into consideration kinematics and dynamicsof the ships, have to define the generalized forces applied by the actuators.

In the following both the blocks will be extensively explained.

1.2.1 Null-Space-Based Behavioral Control for Autonomous

vehicles

The Null-Space-Based behavioral approach is a centralized system aimed atguiding a platoon of generic autonomous vehicles in different scenarios and to


σd, σdNSB

vNSB

vNSB,1Maneuvering c.1

τ1Vessel 1

w1

η1, ν1

η1, ν1

......

vNSB,nManeuvering c.n

τnVessel n

wn

ηn, νn

ηn, νn

η, ν

η

env

Fig. 1.1. Sketch of the guidance system control schema

achieve different missions. As explained in [1, 2], the NSB can be consideredas a behavioral approach that, differently from others, uses a hierarchy basedlogic to combine multiple conflicting tasks. In particular, the NSB is able tofulfill or partially fulfill each task according to their position in the hierarchyand according to the eventual conflicts with the highest priority tasks. Thebasic concepts of the NSB, with reference to a generic platoon of autonomousvehicles, will be recalled in the following.

By defining as σ ∈ IRm the task variable to be controlled and as p∈ IRn

the system configuration, the task function f : IRn → IRm is:

σ = f(p) (1.1)

with the corresponding differential relationship:

σ =∂f(p)

∂pv = J(p)v , (1.2)

where J ∈ IRm×n is the configuration-dependent task Jacobian matrix and v∈IRn is the system velocity. Notice that n depends on the specific autonomoussystem considered and the term system configuration simply refers to thevessel position/orientation (in case of a material point n = 2, in the case of aplatoon of z surface vessels n=3z).

An effective way to generate smooth motion references pd(t) for the ve-hicles, starting from smooth desired values σd(t) of the task function, is toact at the differential level by inverting the (locally linear) mapping (1.2); infact, this problem has been widely studied in robotics (see, e.g., [27] for atutorial). A typical requirement is to pursue minimum-norm velocity, leadingto the least-squares solution:

vNSB = J†σd , (1.3)

where the pseudoinverse is defined as J† = JT

(JJT

)−1

.


At this point, the vehicle motion controller needs a reference position tra-jectory besides the velocity reference; this can be obtained by time integra-tion of vd. However, discrete-time integration of the vehicle’s reference veloc-ity would result in a numerical drift of the reconstructed vehicle’s position;the drift can be counteracted by a so-called Closed Loop Inverse Kinematics(CLIK) version of the algorithm, namely,

vNSB = J†(σd + Λσ

), (1.4)

where Λ is a suitable constant positive-definite matrix of gains and σ is thetask error defined as σ=σd−σ.

The Null-Space-Based behavioral control intrinsically requires a differen-tiable analytic expression of the tasks defined so that it is possible to computethe required Jacobians.

Considering the case of multiple tasks, on the analogy of (1.4) the singletask velocity is computed as

vi = J†i

(σi,d + Λiσi

), (1.5)

where the subscript i denotes i-th task quantities. If the subscript i also de-notes the degree of priority of the task (e.g. Task 1 being the highest-priorityone), in a case of 3 tasks, according to [11] the CLIK solution (1.4) is modifiedinto

vNSB = v1 +(I − J

†1J1

) [v2 +

(I − J

†2J2

)v3

]. (1.6)

Remarkably, equation (1.6) has a nice geometrical interpretation. Each taskvelocity is computed as if it were acting alone; then, before adding its con-tribution to the overall vehicle velocity, a lower-priority task is projected (by

I − J†i−1

J i−1) onto the null space of the immediately higher-priority task soas to remove those velocity components that would conflict with it.

The Null-Space-Based behavioral control always fulfils the highest-prioritytask at nonsingular configurations. The lower-priority tasks, on the otherhand, are fulfilled only in a subspace where they do not conflict with the oneshaving higher priority, that is, each task reaches a sub-optimal condition thatoptimizes the task respecting the constraints imposed by the highest-prioritytasks.

A functional scheme of this architecture is illustrated in Figure 1.2. Thepresence of a supervisor might be considered in order to dynamically changethe relative task priorities. For instance, in absence of close obstacles, anobstacle-avoidance task could be locally ignored.

It must be remarked that, with this approach, a full-dimensional highest-priority task would subsume the lower-priority tasks. In fact, with a full-dimensional full-rank J1 matrix, its null space would be empty and the wholevector v2 would be filtered out. In case of non-conflicting tasks, all the tasksare totally fulfilled.


sensors

Task #CvC v3

I − J†2J2

Task #BvB v2

∑I − J

†1J1

Task #AvA v1

∑ vdsupervisor

Fig. 1.2. Sketch of the Null-Space-Based behavioral control in a 3-task example.The supervisor is in charge of changing the relative priority among the tasks

Stability of the NSB for material points under two tasks

Assuming that the vessels perfectly follow the desired motion references v =vNSB (that is, the vessels behave as material points), the stability analysisof the NSB is reduced to only prove convergence of the task functions to thedesired value. Let us consider the case of 2 tasks acting simultaneously. Thevelocity of the ships are:

v = vNSB = J†1

(σ1,d + Λ1σ1

)+

(I − J

†1J1

)J

†2

(σ2,d + Λ2σ2

). (1.7)

Considering the Lyapunov function V1 = 1

2σ

2

1and writing the equation of V1,

then, by (1.2):

V1 = σT

1˙σ1 = σ

T

1(σ1,d − J1v)

V1 = σT

1

(σ1,d − J1J

†1

(σ1,d + Λ1σ1

)− J1

(I − J

†1J1

)J

†2

(σ2,d + Λ2σ2

))

= σT

1

(σ1,d − J1J

†1

(σ1,d + Λ1σ1

))= −σ

T

1Λ1σ1.

Recalling that Λ1 is positive definite, then the convergence to 0 of σ1 isproved; that is, the first task is always achieved.Considering the Lyapunov function for the secondary task V2 = 1

2σ

2

2

V2 = σT

2˙σ2 = σ

T

2(σ2,d − J2v)

= σT

2

(σ2,d − J2J

†1

(σ1,d + Λ1σ1

)− J2

(I − J

†1J1

)J

†2

(σ2,d + Λ2σ2

))

= σT

2

(−Λ2σ2 − J2J

†1

(σ1,d + Λ1σ1

)+ J2J

†1J1J

†2

(σ2,d + Λ2σ2

))

Like widely explained in Section 1.2.1, the fulfillment of the secondary taskis verified only when it is not conflicting with the primary one, that is, theproduct J2J

†1

is null. In this case V2 = −σT

2Λ2σ2 and the convergence to 0

of σ2 is proved.


1.2.2 Maneuvering control of marine surface vessels

A maneuvering control is a on board controller aimed at steering the ves-sel along a desired path and moving it with a desired velocity [13, 14]. Thus,receiving motion reference commands, the maneuvering control has to dynam-ically elaborate the generalized forces applied by the actuators. For fully actu-ated ships, the maneuvering control problem is extensively explained in [28].For underactuated ships, equipped with less than 3 actuators (only 1-2 actu-ators are used to control the surge, sway and yaw modes), the maneuveringcontrol is a challenging problem. In works [24, 21, 25] the main control prob-lems regarding underactuated ships are recalled, while in [15] a nonlinearpath-following for underactuated marine craft is presented.

Fig. 1.3. Tugboat with two main azimuth thrusters aft and one tunnel thruster inthe bow. Courtesy of Roll-Royce Marine

In this paper the case of surface vessels underactuated at high velocitiesand fully-actuated at low velocities is considered. The only relevant typesof vessels to consider are the ones which become underactuated in the swaydirection (lateral direction) at high speed. These vessels are typically equippedwith a number of tunnel thrusters, designed to assist at low-speed maneuvers,which are inoperable at high speeds due to the relative water speed past theiroutlets; therefore, at high speeds, the only mean of actuation are the mainthrusters. An example of such a vessel is a tugboat from Rolls-Royce Marine(Figure 1.3).

For the vessels, the following 3 DOF nonlinear maneuvering model [13], isconsidered:

η = R(ψ)ν (1.8)

Mν + N(ν)ν = τ + RT(ψ)w, (1.9)


where η = [n eψ]T

is the position and attitude vector in the North-East-Down

(NED) reference frame; ν = [u v r]T

is the linear and angular velocity vector inthe Body-fixed (BODY) reference frame; R(ψ) ∈ SO(3) is the rotation matrixfrom NED to BODY; M is the vessel inertia matrix; N(ν) is the sum of thecentrifugal and Coriolis matrix and of the hydrodynamic damping matrix; τ

is the vessel propulsion force and torque; w is the vector of the environmentalforces (wind, currents, etc.) acting on the vessel in the NED reference system.

In the proposed guidance system, the maneuvering control is a velocity andheading controller aimed at making each vehicle follow its velocity referencecommand elaborated by the NSB, that, following the schema of Figure 1.4,can be converted from vNSB (elaborated considering the vessels as materialpoints) to UNSB and χNSB.

n

e

U

ψ

u

v

χ

β{B}

Fig. 1.4. Motion reference model of the surface vessel

Following the approach proposed in [8], a nonlinear, model-based veloc-ity and heading controller is designed referring to an adaptive backsteppingtechnique.Start by defining the error variables z1∈ IR and z2∈ IR3:

z1 = χ− χNSB = hTη + β − χNSB = hTη − ψNSB

z2 = ν − α,

where hT =[ 0 0 1 ]T, ψNSB =χNSB−β and α=[α1 α2 α3 ]T∈ IR3 is a vectorof stabilizing functions to be specified later.

Step 1:

Define the Control Lyapunov Function as:

V1 =1

2k1z

2

1, (1.10)

where k1 > 0.


Differentiating V1 with respect to time

V1 = k1z1z1 = k1z1

(hTη − ψNSB

)

V1 = k1z1

(hTν − ψNSB

),

since η = Rν and hTRν = hTν.By definition of z2, then:

V1 = k1z1

(hT (z2 + α) − ψNSB

)

= k1z1hTz2 + k1z1

(α3 − ψNSB

).

Choosing

α3 = ψNSB − z1, (1.11)

then,

V1 = −k1z2

1+ k1z1h

Tz2 (1.12)

Step 2:

Define the Control Lyapunov Function as:

V2 =1

2k1z

2

1+

1

2zT

2Mz2 +

1

2wTΓ−1w

where w ∈ IR3 is parameter error due to environmental disturbances definedas w = w − w with w being estimate of w, and Γ = Γ T > 0.Differentiating V2 along trajectories of z1,z2 and w and assuming w = 0.then:

V2 = −k1z2

1+ k1z1h

Tz2 + zT

2Mz2 + wTΓ−1 ˙w,

since M = MT and ˙w = ˙w.Since

Mz2 = M (ν − α)

= τ − N(ν)ν + RT(ψ)w − Mα

then:

V2 = −k1z2

1+ zT

2

(hk1z1 + τ − Nν + RT w − Mα

)+ wTΓ−1 ˙w.

Being ν = z2 + α and w = w − w, then:


V2 = −k1z2

1− zT

2Nz2 + zT

2

(hk1z1 + τ − Nα + RT w − Mα

)

+wTΓ−1

(˙w − ΓRz2

).

Assigningτ = Mα + Nα − RT w − hk1z1 − K2z2 (1.13)

where K2 > 0, and choosing:

˙w = ΓRz2 (1.14)

then:V2 = −k1z

2

1− zT

2(N + K2)z2. (1.15)

As stated in [16], since the system is persistently excited, choosing smoothα1, α1, α2, α2 ∈ L∞ then the proposed control and disturbance adaption lawmakes the origin of the error system [z1,z2, w] UGAS/ULES.

The choice of α1, α2 depends on the actuation system of the vessels, i.e.,when the vessel is fully-actuated

α1 = UNSB cosβNSB

α2 = UNSB sinβNSB ;

while when the vessel is under-actuated

α1 = UNSB cosβNSB

and α2 is defined such as τ2 = 0.The proposed controller guarantees that z1, z2 go to zero, and that w

converges to w. Thus, the vessel velocity converges to the desired values{UNSB , χNSB} only when the vessel is fully-actuated; for an underactuatedvessel, instead, u converges to UNSB cosβNSB while v is uncontrolled. It isworth noting that, while an underactuated ship executes a straight motion atcruise speed it is U=

√u2 + v2 ≃ u since the sway velocity v is much smaller

than u; on the other hand, when v cannot be neglected with respect to u (as,

e.g., in turning maneuvers) it is U =

√(UNSB cosβNSB)

2+v2. Nevertheless,

in real applications, ships move using way-point tracking and turning maneu-vers are used for short time periods to adjust changes of course; thus, theconvergence properties of the proposed control law are acceptable in practice.

When implementing the control law (1.13) it is important to avoid expres-sions involving the time derivatives of the states. To this purpose, the timederivative of the stabilizing function α is conveniently computed by differenti-ating along the trajectories of the states [13]. Moreover, to obtain high-orderderivatives of ψNSB in (1.11), it is worth noticing that ψNSB is defined asψNSB = χNSB−β, where χNSB is the reference input obtained by the NSBand β is measured using GPS velocities. Thus, the high-order derivativesψNSB , ψNSB are computed through a low-pass filter.

More details of the described maneuvering control can be found in [8].


1.3 Tasks

According to the behavioral control approach, the mission of the fleet is de-composed in three elementary tasks: move the barycenter of the fleet, keep aformation relative to the barycenter, and avoid collisions with obstacles andamong vehicles. In this section, the corresponding task functions are presented.

1.3.1 Barycenter

The barycenter of a platoon expresses the mean value of the vehicles positions.In a 2-dimensional case (like for material points) the task function is expressedby:

σb = f b (p1, . . . , pn) =1

n

n∑

i=1

pi.

where pi = [ηi,1 ηi,2]T

is the position of the vehicle i.Deriving the previous relation:

σb =n∑

i=1

∂f b (p)

∂pi

vi = Jb (p)v,

where the Jacobian matrix Jb ∈ IR2×2n is

Jb =1

n

[1 00 1

. . .1 00 1

].

Following equation (1.4), the output of the barycenter task function is

vb = J†b

(σb,d + Λbσb

), (1.16)

where the desired value of the task function represents the desired trajectoryof the barycenter.

1.3.2 Rigid Formation

The rigid formation task moves the vehicles to a predefined formation respectto the barycenter. The task function is defined as:

σf =

p1 − pb

...pn − pb

,

where pn are the coordinates of the vehicle n and pb are the coordinates of thebarycenter. Writing for simplicity the vector p as [ η1,1 . . . ηn,1 η1,2 . . . ηn,2]

T,then the Jacobian matrix Jf ∈ IR2n×2n is:


Jf =

[A O

O A

], (1.17)

where A∈ IRn×n is:

A =

1− 1

n− 1

n. . . − 1

n

− 1

n1− 1

n. . . − 1

n

......

. . ....

− 1

n− 1

n. . . 1− 1

n

. (1.18)

Because the Jacobian matrix is singular, the pseudoinverse can not be cal-

culated as JT

(JJT

)−1

but as a matrix that verifies the following properties:

JJ†J = J†; J†JJ† = J

and with JJ† and J†J symmetric. Since Jf is symmetric and idempotent,

J†f = Jf .

The desired value σf,d of the task function describes the shape of thedesired formation; that is, once defined the formation, the elements of σf,d

represent the coordinates of each vehicle in the barycenter reference frame.The output of the formation task function, in the case of fixed desired

formation (σf,d = 0), is:

vf = J†fΛf σf (1.19)

1.3.3 Obstacle avoidance

The obstacle avoidance task function is built individually to each vehicle, i.e.,it is not an aggregate task function. In fact, an obstacle in the environmentmay be close to some vehicle but far from some other; moreover, each vehicleis an obstacle for the others in the team but not for itself.

With reference to the generic vehicle in the team, in presence of an ob-stacle in the advancing direction, the task function has to elaborate a drivingvelocity, aligned to the vehicle-obstacle direction, that keeps the vehicle at asafe distance d from the obstacle. Therefore, it is:

σo = ‖p − po‖ σo,d = d Jo = rT ,

where po is the obstacle position and

r =p − po

‖p − po‖

is the unit vector aligned with the obstacle-to-vehicle direction. According tothe above choice,equation (1.5) simplifies to


vo = J†oλoσo = λo (d− ‖p − po‖) r, (1.20)

where λo is a suitable constant positive scalar.It is worth noting that, being

N (Jo) = I − rrT ,

the tasks of lower priority than the obstacle avoidance are only allowed toproduce motion components tangent to the circle of radius d and centeredin po, so as to not interfere with the enforcement of the safe distance d.

While the implementation of the proposed obstacle-avoidance task func-tion is the same for both punctual environmental obstacles and other vehicles,in the case of continuous obstacles it changes a bit. In particular, for convex-or straight-line obstacles, po represents the coordinates of the closest point ofthe obstacle to the ship at the current time instant.

In the frequent case of multiple obstacles acting simultaneously (e.g., bothan obstacle in the environment and the other vehicles of the team) a priorityamong their avoidance should be defined; a reasonable choice is to assignthe currently closest obstacle the highest priority. In critical situations theobstacle avoidance function may give a null-velocity output; this causes delayto the mission or loss of vehicles to the formation but increases safety of theapproach.

1.4 Simulations

In this section results of two simulations in complex realistic scenarios will bepresented. In the first proposed scenario (see Figure 1.5) a fleet of 7 vesselshas to overtake a bottleneck (that can represent the costal profile of a fjord)and a small obstacle, in presence of environmental disturbances, keeping aV-formation (a configuration widely used for Navy and military applications).The mission is decomposed in three elementary tasks: move the barycenter ofthe fleet, keep a formation relative to the barycenter, and avoid collisions withobstacles and among vehicles. The obstacle-avoidance is the highest prioritytask because its achievement is of crucial importance to preserve the integrityof the vessels, while the barycenter and the rigid formation are respectively thesecondary and tertiary tasks. Following the stability analysis consideration ofSection 1.2.1, it is easy to prove that the barycenter and the rigid formationtask function are not conflicting (JfJ

†b = 0).

The obstacle avoidance task function has to ensure each vessel a safe dis-tance of 60 m from the environmental obstacles and from other vessels. Foreach vessel, the task function is activated only when the distance from en-vironmental obstacles or other vessels becomes lower than 60 m. When thevessel is simultaneously close to (i.e., under 60 m from) multiple obstacles,then the closest obstacle has the highest priority. Since a moving obstacle isassumed to be more dangerous than a fixed one, if the vessel is simultaneously


0 500 1000−500

0

500

0 500 1000−500

0

500

0 500 1000−500

0

500

0 500 1000−500

0

500

0 500 1000−500

0

500

0 500 1000−500

0

500

0 500 1000−500

0

500

0 500 1000−500

0

500

n[m

]

n[m

]

n[m

]

n[m

]

n[m

]

n[m

]

n[m

]

n[m

]

e [m]e [m]

e [m]e [m]

e [m]e [m]

e [m]e [m]

Fig. 1.5. Navigation in complex environment. Obstacle Avoidance-Barycenter-RigidFormation


0 200 400 600 800 1000−150

−100

−50

0

50

100

150d

0 500 1000 1500 2000−20

−10

0

10

20

30

40

50c

0 200 400 600 800 1000−5

0

5

10

15

20

25b

0 500 1000−500

0

500a

n[m

]

e [m]

σo

[m]

t [s]

t [s] t [s]

σb

[m]

σf

[m]

Fig. 1.6. a) Paths followed by the ships; b) error of the obstacle avoidance taskfunction; c) error of the barycenter task function; d) error of the rigid formationtask function

close to an environmental obstacle and another vessel, the avoidance of theother vessel takes higher priority if the distance from it is greater than thedistance from the environmental obstacle multiplied by a gain chosen (by trialand error) equal to 0.3 .

The desired trajectory of the barycenter is a rectilinear segment that con-nects the initial position [ 0 0 ]T m to the final position [ 800 0 ]T m accordingto a fifth-order polynomial time law of 700 s duration with null initial andfinal velocity. The vessels have to attain and keep a V-formation around thebarycenter and, once reached the final configuration, the vessels has to keepthe formation and orient themselves in the opposite direction to the current.The gain matrices of the barycenter task function and of the rigid formationtask function are respectively a 2-dimensional diagonal matrix with coefficient0.1 and a 14-dimensional diagonal matrix with coefficient 0.1 .

The vessels have the dynamic model described in Section 1.2.2, where thematrices M and N are defined as:


−200 0 200 400 600 800 1000−500

0

500

−200 0 200 400 600 800 1000−500

0

500

−200 0 200 400 600 800 1000−500

0

500

−200 0 200 400 600 800 1000−500

0

500

n[m

]

n[m

]

n[m

]

n[m

]

e [m]e [m]

e [m]e [m]

Fig. 1.7. Navigation in complex environment. Obstacle Avoidance-Barycenter-RigidFormation

M =

0.0035 Ns2

m0 0

0 0.0046 Ns2

m−0.0009Ns2

0 −0.0009 Ns2

m1.0740Ns2

∗ 109

N =

0.0007 Ns

m0 0

0 0.0071 Ns

m0.1812Ns

0 −0.1079 Ns

m1.8490Ns

∗ 108

To simulate the under-actuation at the high velocities, the force in swaydirection (τ2) is saturated by a value depending on the velocity in the surgedirection (τ1, τ3 are saturated by fixed values representing the realistic limitsof the actuators). The environmental disturbances are represented by a force(constant in the NED reference frame) of w = [−10000 − 30000 0]N.

Figure 1.5 shows that the vessels do overtake the bottleneck avoiding col-lisions and reach the final configuration (both barycenter and formation). Inparticular, the vessels, starting from the initial V-formation, loose the configu-ration to avoid the obstacles and, once overtaken, reach again the desired for-


0 200 400 600 800 1000−60

−40

−20

0

20

40

60

80d

0 500 1000 1500 2000−15

−10

−5

0

5

10

15

20c

0 200 400 600 800 10000

5

10

15

20

25

30b

0 500 1000−500

0

500a

n[m

]

e [m]

σo

[m]

t [s]

t [s] t [s]

σb

[m]

σf

[m]

Fig. 1.8. a) Paths followed by the ships; b) error of the obstacle avoidance taskfunction; c) error of the barycenter task function; d) error of the rigid formationtask function

mation. To avoid oscillations around the final configuration, when the vesselsare close to the desired configuration then the desired velocities are imposedequal to 0 and the desired orientation equal to the estimated current oppositedirection. In this way, the vessels orient themselves in the opposite directionto the current and keep their positions compensating the current with theonly main propellers. It is worth noticing that the proposed approach per-mits also switching of the formations, thus, in a similar environment, it canbe useful changing the formation to overtake the obstacles (i.e., reducing theangle of the V or reaching a line configuration) and come back to the desiredconfiguration once far from the obstacles.

Figure 1.6 shows the errors of the task functions during the all mission.The obstacle avoidance is activated only close to the obstacles (in middleof the mission). The barycenter function error starts from a low value (thebarycenter of the initial configuration is close to the desired value), increasesduring the avoidance of the bottleneck obstacle and decreases once overtaken.The rigid formation function error starts from a null value, increases duringthe obstacle avoidance and converges to zero once overtaken the obstacles.


In the second scenario(see Figure 1.7) a fleet of 7 vessels has to navigatethrough an environment full of punctual obstacles, in presence of environmen-tal disturbances, keeping a V-formation. The mission parameters are the sameof the previous example but the great number of obstacles and the presenceof current permits to test the navigation system while performing missions inextreme conditions. Figures 1.7, 1.8 show that the mission is successfully per-formed also in this scenario. It is worth noticing that the error of the obstacleavoidance task function reaches a peak of 25m, thus each vessel keeps a safedistance from obstacles and other vessels always greater than 30m (the peakis verified when a vessel is simultaneously close to an obstacle and anothervessel). Figure 1.9.a) shows that the estimation of the current w does con-verge to the real value w = [−10000 − 30000 0]N while Figure 1.9.b) showsthat in the final configuration all the ships orient themselves in the oppositedirection to the current.Videos of the performed simulations can be found athttp : //webuser.unicas.it/arrichiello/video/

0 1000 2000 3000−4

−3

−2

−1

0

1

2

3x 10

4 a)

0 1000 2000 3000−3

−2

−1

0

1

2

3b)

w[N

]

θ[r

ad]

t [s]t [s]

Fig. 1.9. a) Current estimation; b) Orientation of the ships

1.5 Conclusion

In this paper the null-space-based behavioral control has been presented toguide a fleet of autonomous marine surface vessels in complex environments.The NSB works in combination with the low-level maneuvering controls ofeach ship to take into consideration the dynamics of the fleet. The guidancesystem has been simulated in realistic missions involving the attainment ofa formation while moving through obstacles in presence of sea current; theobtained results show the effectiveness of the proposed method.


1.6 Acknowledgment

This research has been partially supported by a Marie Curie Fellowship ofthe European Community programme CyberMar Marie Curie Training Sitein Trondheim.The authors appreciate the comments from Gianluca Antonelli, Ivar-AndreF. Ihle and Fabio Celani.

References

1. G. Antonelli, F. Arrichiello, and S. Chiaverini. Experimental kinematic com-parison of behavioral approaches for mobile robots. In Proceedings of the 16th

IFAC World Congress, Prague, CZ, July 2005.2. G. Antonelli, F. Arrichiello, and S. Chiaverini. The null-space-based behav-

ioral control for mobile robots. In Proceedings of the 2005 IEEE International

Symposium on Computational Intelligence in Robotics and Automation, Espoo,Finland, June 2005.

3. G. Antonelli and S. Chiaverini. Kinematic control of a platoon of autonomousvehicles. In Proceedings of the 2003 IEEE International Conference on Robotics

and Automation, pages 1464–1469, Taipei, TW, Sept. 2003.4. R.C. Arkin. Motor schema based mobile robot navigation. The International

Journal of Robotics Research, 8(4):92–112, 1989.5. R.C. Arkin. Behavior-Based Robotics. The MIT Press, Cambridge, MA, 1998.6. T. Balch and R.C. Arkin. Behavior-based formation control for multirobot

teams. IEEE Transactions on Robotics and Automation, 14(6):926–939, 1998.7. B.E. Bishop. On the use of redundant manipulator techniques for control of

platoons of cooperating robotic vehicles. IEEE Transactions on Systems, Man

and Cybernetics, 33(5):608–615, Sept. 2003.8. M. Breivik and T.I. Fossen. A unified concept for controlling a marine surface

vessel through the entire speed envelope. In Proceedings of the 2005 IEEE In-

ternational Symposium on Mediterrean Conference on Control and Automation,pages 1518– 1523, Limassol, Cyprus, June 2005.

9. R.A. Brooks. A robust layered control system for a mobile robot. IEEE Journal

of Robotics and Automation, 2:14–23, 1986.10. W. Burgard, M. Moors, C. Stachniss, and F.E. Schneider. Coordinated multi-

robot exploration. IEEE Journal of Robotics, 21(3):376–386, June 2005.11. S. Chiaverini. Singularity-robust task-priority redundancy resolution for real-

time kinematic control of robot manipulators. IEEE Transactions on Robotics

and Automation, 13(3):398–410, 1997.12. E. Fiorelli, N.E. Leonard, P. Bhatta, D. Paley, R. Bachmayer, and D.M. Fratan-

toni. Multi-auv control and adaptive sampling in monterey bay. In Proceedings

IEEE Autonomous Underwater Vehicles 2004: Workshop on Multiple AUV Op-

erations, pages 134–147, Sebasco, ME, June 2004.13. T.I. Fossen. Marine Control Systems: Guidance, Navigation and Control of

Ships, Rigs and Underwater Vehicles. Marine Cybernetics, Trondheim, Norway,2002.

14. T.I. Fossen. A nonlinear unified state-space model for ship maneuvering andcontrol in a seaway. Journal of Bifurcation and Chaos, 2005.


15. T.I. Fossen, M. Breivik, and R. Skjetne. Line-of-sight path following of under-actuated marine craft. In Proceedings of the 2003 IFAC Conference on Maneu-

vering and Control of Marine Craft, Girona, Spain, September 2003.16. T.I. Fossen, A. Loria, and A. Teel. A theorem for ugas and ules of (passive)

nonautonomous systems: Robust control of mechanical systems and ships. In-

ternational Journal of Robust and Nonlinear Control, JRNC-11:95–108, 2001.17. E. Gat, R. Desai, R. Ivlev, J. Loch, and D.P. Miller. Behavior control for

robotic exploration of planetary surfaces. IEEE Transactions on Robotics and

Automation, 10(4):490–503, 1994.18. I.-A. F. Ihle, J. Jouffroy, and T.I. Fossen. Formation control of marine surface

craft using lagrange multipliers. In Proceedings of the 44th IEEE Conference on

Decision and Control and European Control Conference, Seville, Spain, 2005.19. I.-A. F. Ihle, J. Jouffroy, and T.I. Fossen. Robust Formation Control of Marine

Craft using Lagrange Multipliers. Submitted to the Workshop on Group Co-ordination and Cooperative Control, Tromso, Norway, springer-verlag’s lecturenotes in control and information systems series edition, May 2006.

20. D. Langer, J.K. Rosenblatt, and M. Hebert. A behavior-based system for off-road navigation. IEEE Transactions on Robotics and Automation, 10(6):776–783, 1994.

21. E.L. Lefeber, K.Y. Pettersen, and H. Nijmeijer. Tracking control of an under-actuated ship. IEEE Transactions on Control Systems Technology, 11(1):52–61,2003.

22. M.J. Mataric. Behavior-based control: Examples from navigation, learning, andgroup behavior. J. Experimental and Theoretical Artificial Intelligence, 9(2-3):323 336, 1997.

23. L.E. Parker. On the design of behavior-based multi-robot teams. Advanced

Robotics, 10(6):547–578, 1996.24. K.Y. Pettersen and T.I. Fossen. Underactuated dynamic positioning of a

ship- experimental results. IEEE Transactions on Control Systems Technology,8(5):856–863, 2000.

25. K.Y. Pettersen, F. Mazencs, and H. Nijmeijer. Global uniform asymptotic stabi-lization of an underactuated surface vessels: Experimental results. IEEE Trans-

actions on Control System Technology, 12(6):891–903, 2004.26. M. Scheutz and V. Andronache. Architectural mechanisms for dynamic changes

of behavior selection strategies in behavior-based systems. IEEE Transactions

on Systems, Man and Cybernetics, 34(6):2377–2395, Dec. 2004.27. B. Siciliano. Kinematic control of redundant robot manipulators: A tutorial.

Journal of Intelligent Robotic Systems, 3:201–212, 1990.28. R. Skjetne. The Maneuvering Problem. PhD.Thesis, Norwegian University of

Science and Technology, Trondheim, Norway, 2005.29. D.M. Stipanovic, G. Inalhan, R. Teo, and C.J. Tomlin. Decentralized overlap-

ping control of a formation of unmanned aerial vehicles. Automatica, 40:1285–1296, 2004.

30. Z. Wang, E. Nakano, and T. Takahashi. Solving function distribution and behav-ior design problem for cooperative object handling by multiple mobile robots.IEEE Transactions on Systems, Man, and Cybernetics, Part A, 33(5):537–549,2003.

Documents

1 Formation Control of Marine Surface Vessels using the ...webuser.unicas.it/arrichiello/papers/ArrChiFos_06.pdfioral approaches have been applied to the formation control of multi-robot