Formation Control of Marine Surface Vessels using the Null-Space-Based Behavioral Control

Transcription

1 1 Formation Control of Marine Surface Vessels using the Null-Space-Based Behavioral Control Filippo Arrichiello 1, Stefano Chiaverini 1, and Thor I. Fossen 2 1 Dipartimento di Automazione, Elettromagnetismo, Ingegneria dell Informazione e Matematica Industriale. Università degli Studi di Cassino, via G. Di Biasio 43, 343 Cassino FR), Italy. {f.arrichiello,chiaverini}@unicas.it 2 Department of Engineering Cybernetics, Norwegian University of Science and Technology, NO-7491 Trondheim, Norway. fossen@ieee.org 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 applications point of view, the NSB can be considered as a guidance system that dynamically selects the motion reference commands for each vessel of the fleet. These motion commands are aimed at guiding the fleet in complex environments simultaneously performing multiple tasks, i.e., obstacle avoidance or keeping a formation. In order to apply the guidance system to a fleet of surface vessels through the entire speed envelope fully-actuated at low velocities, under-actuated at high velocities), the NSB works in combination with a low-level maneuvering control that, taking care of the dynamic models of the vessels, elaborates the motion commands to obtain the generalized forces at the actuators. The guidance system has been simulated while successfully 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 been object of considerable research efforts in the last years. The basic idea is that multi-robot systems can perform tasks more efficiently than a single robot or can accomplish tasks not executable by a single one. Moreover, multi-robot systems have advantages like increasing tolerance to possible vehicle fault, providing flexibility to the task execution or taking advantage of distributed sensing and actuation. In the context of this paper, the term multi-robot system means a generic system of autonomous vehicles like Unmanned Grounded Vehicles UGVs), Unmanned Aerial Vehicles UAVs), Unmanned Underwater

2 2 Arrichiello et al. Vehicles UUVs) or autonomous surface vessels. Thus, applications of multirobot systems can be widely different such as, e.g., exploration of an unknown environment with a team of mobile robots [1], navigation and formation control with multiple UAVs [29], UUVs [12] and surface vessels [18, 19] and object transportation with multiple UGVs [3]. When controlling a multi-robot system, a common requirement is the motion coordination among the vehicles; that is, the vehicles have to move in the environment keeping a suitable relative configuration. Among other possible methods, in this paper behavior-based approaches to coordinate a multi-robot system are considered. Behavior-based approaches, widely studied for robotic applications [5], are useful to guide a multi-robot system in an unknown or dynamically changing environment. These approaches give the system the autonomy to navigate in complex environments avoiding low-level path planning, using sensors to obtain instantaneous information of the environment and increasing flexibility of the system. Among the behavioral approaches, seminal works are reported in the papers [9] and [4], while the textbook [5] offers a comprehensive state of the art. A behavioral approach designed for exploration of planetary surfaces has been investigated in [17], while in [2] the experimental case of an autonomous navigation in outdoor natural terrain is presented. Lately, behavioral approaches have been applied to the formation control of multi-robot systems as in, e.g. [23], [22] and [6]. The use of techniques inherited from inverse kinematics for industrial manipulators is described in [7, 3]. In [26] an architecture for dynamic changes of the behavior selection strategies is presented. In the behavior-based approach, the mission of the system is usually decomposed in elementary sub-problems, eventually solvable in parallel, whose solutions need to be composed in one single motion command for each robot. The sub-problems are commonly termed behaviors or tasks. In presence of multiple behaviors, each task output is designed so as to achieve its specific goal but it is generally impossible that a single motion command to the robot can accomplish all the assigned behaviors at the same time. In particular, when a motion command cannot reduce simultaneously the value of all the task functions there is a conflict among the tasks that must be solved by a suitable policy. The behavior-based approach proposed in this paper, namely the Null- Space-Based behavioral control NSB), differs from the other existing methods in the behavioral coordination method, i.e., in the way the outputs of the single elementary behaviors are assembled to compose a complex behavior. The NSB can be seen as a centralized system aimed at guiding a platoon of autonomous vehicles in different scenarios and to achieve different missions. Following the main behavioral approaches, the mission of the platoon is decomposed in elementary tasks, i.e., move the barycenter of the platoon, avoid obstacles, keep a formation. For each task, a function that measures its degree of fulfilment e.g., a cost or a potential function) can be defined; thus, in a

3 1 Formation control of Marine Surface Vessels using the NSB 3 static environment, the task is achieved when its output is constant at a value that minimizes the task function. Using a suitable policy to manage multiple tasks, the NSB is aimed at elaborating the instantaneous motion references for each vehicle. How to follow these motion references, instead, is the aim of a low-level maneuvering control system. In this way, the NSB does not take care of the typology of the vehicles and let the maneuvering control system take into consideration the kinematics and the dynamics of the vehicles. This characteristic lets the NSB be applicable to different kinds of vehicles like autonomous robots both holonomic and nonholonomic), underwater robots or surface vessels fully actuated and underactuated). In this paper the application of the NSB to a fleet of marine surface vessels is presented. The proposed case considers vessels that are underactuated at high velocities and fully-actuated at low velocities. That is, the propulsive action 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 simulations and a case study of a fleet of vessels performing complex missions in realistic scenarios is presented in order to validate the effectiveness of the approach. 1.2 Guidance system for marine surface vessels The guidance system proposed in this paper has to solve simultaneously different problems: it is responsible for ships safety, that is, it has to make the ships able to avoid static and dynamic obstacles doing a dynamic path generation path generation on-the-fly); it has to achieve different kinds of missions, e.g., navigation keeping a fixed relative formation or changing the route or the formation considering weather and environmental conditions; it has to maneuver fully/under-actuated ships ships equipped with 3/less than 3 actuators). To this purpose, the guidance system see Figure 1.1) is decomposed in two main blocks: the Null-Space-Based behavioral control and the maneuvering control. The NSB takes into consideration the parameters of the mission, the environmental condition and the status of the fleet to elaborate the desired velocities for each vehicle. These velocities represent the reference input for the maneuvering controls that, taking into consideration kinematics and dynamics of the ships, have to define the generalized forces applied by the actuators. In the following both the blocks will be extensively explained Null-Space-Based Behavioral Control for Autonomous vehicles The Null-Space-Based behavioral approach is a centralized system aimed at guiding a platoon of generic autonomous vehicles in different scenarios and to

4 4 Arrichiello et al. v NSB,1 Maneuvering c.1 τ 1 Vessel 1 η 1, ν 1 σ d, σ d env NSB vnsb w 1 η 1, ν 1... v NSB,n Maneuvering c.n τn Vessel n η n, ν n η, ν η η n, ν n w n Fig Sketch of the guidance system control schema achieve different missions. As explained in [1, 2], the NSB can be considered as a behavioral approach that, differently from others, uses a hierarchy based logic to combine multiple conflicting tasks. In particular, the NSB is able to fulfill or partially fulfill each task according to their position in the hierarchy and according to the eventual conflicts with the highest priority tasks. The basic concepts of the NSB, with reference to a generic platoon of autonomous vehicles, will be recalled in the following. By defining as σ IR m the task variable to be controlled and as p IR n the system configuration, the task function f : IR n IR m is: with the corresponding differential relationship: σ = fp) 1.1) σ = fp) v = Jp)v, 1.2) p where J IR m n is the configuration-dependent task Jacobian matrix and v IR n is the system velocity. Notice that n depends on the specific autonomous system considered and the term system configuration simply refers to the vessel position/orientation in case of a material point n = 2, in the case of a platoon of z surface vessels n=3z). An effective way to generate smooth motion references p d t) for the vehicles, starting from smooth desired values σ d t) of the task function, is to act at the differential level by inverting the locally linear) mapping 1.2); in fact, this problem has been widely studied in robotics see, e.g., [27] for a tutorial). A typical requirement is to pursue minimum-norm velocity, leading to the least-squares solution: v NSB = J σ d, 1.3) where the pseudoinverse is defined as J = J T JJ T) 1.

5 1 Formation control of Marine Surface Vessels using the NSB 5 At this point, the vehicle motion controller needs a reference position trajectory besides the velocity reference; this can be obtained by time integration of v d. However, discrete-time integration of the vehicle s reference velocity 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, v NSB = J ) σ d + Λ σ, 1.4) where Λ is a suitable constant positive-definite matrix of gains and σ is the task error defined as σ=σ d σ. The Null-Space-Based behavioral control intrinsically requires a differentiable analytic expression of the tasks defined so that it is possible to compute the required Jacobians. Considering the case of multiple tasks, on the analogy of 1.4) the single task velocity is computed as ) v i = J i σ i,d + Λ i σ i, 1.5) where the subscript i denotes i-th task quantities. If the subscript i also denotes the degree of priority of the task e.g. Task 1 being the highest-priority one), in a case of 3 tasks, according to [11] the CLIK solution 1.4) is modified into v NSB = v 1 + ) [ I J 1 J 1 v 2 + ] I J 2 J 2 )v ) Remarkably, equation 1.6) has a nice geometrical interpretation. Each task velocity is computed as if it were acting alone; then, before adding its contribution 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 so as to remove those velocity components that would conflict with it. The Null-Space-Based behavioral control always fulfils the highest-priority task at nonsingular configurations. The lower-priority tasks, on the other hand, are fulfilled only in a subspace where they do not conflict with the ones having higher priority, that is, each task reaches a sub-optimal condition that optimizes the task respecting the constraints imposed by the highest-priority tasks. A functional scheme of this architecture is illustrated in Figure 1.2. The presence of a supervisor might be considered in order to dynamically change the relative task priorities. For instance, in absence of close obstacles, an obstacle-avoidance task could be locally ignored. It must be remarked that, with this approach, a full-dimensional highestpriority task would subsume the lower-priority tasks. In fact, with a fulldimensional full-rank J 1 matrix, its null space would be empty and the whole vector v 2 would be filtered out. In case of non-conflicting tasks, all the tasks are totally fulfilled.

6 6 Arrichiello et al. Task #A supervisor v A v 1 v d sensors Task #B v B v 2 I J 1 J1 Task #C v C v 3 I J 2 J2 Fig 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 = v NSB that is, the vessels behave as material points), the stability analysis of the NSB is reduced to only prove convergence of the task functions to the desired value. Let us consider the case of 2 tasks acting simultaneously. The velocity of the ships are: v = v NSB = J 1 σ 1,d + Λ 1 σ 1 ) + ) I J 1 J 1 J 2 σ 2,d + Λ 2 σ 2 ). 1.7) Considering the Lyapunov function V 1 = 1 2 σ2 1 and writing the equation of V 1, then, by 1.2): V 1 = σ T σ 1 1 = σ T 1 σ 1,d J 1 v) V 1 = σ T 1 σ 1,d J 1 J 1 = σ T 1 σ 1,d J 1 J 1 σ 1,d + Λ 1 σ 1 ) J 1 I J 1 J 1 σ 1,d + Λ 1 σ 1 )) = σ T 1 Λ 1 σ 1. ) )) J 2 σ 2,d + Λ 2 σ 2 Recalling that Λ 1 is positive definite, then the convergence to of σ 1 is proved; that is, the first task is always achieved. Considering the Lyapunov function for the secondary task V 2 = 1 2 σ2 2 V 2 = σ T σ 2 2 = σ T 2 σ 2,d J 2 v) ) ) = σ T 2 σ 2,d J 2 J 1 σ 1,d + Λ 1 σ 1 J 2 I J 1 J 1 ) = σ T 2 Λ 2 σ 2 J 2 J 1 σ 1,d + Λ 1 σ 1 + J 2 J 1 J 1J 2 J 2 σ 2,d + Λ 2 σ 2 )) σ 2,d + Λ 2 σ 2 )) Like widely explained in Section 1.2.1, the fulfillment of the secondary task is verified only when it is not conflicting with the primary one, that is, the product J 2 J 1 is null. In this case V 2 = σ T 2 Λ 2 σ 2 and the convergence to of σ 2 is proved.

7 1 Formation control of Marine Surface Vessels using the NSB Maneuvering control of marine surface vessels A maneuvering control is a on board controller aimed at steering the vessel along a desired path and moving it with a desired velocity [13, 14]. Thus, receiving motion reference commands, the maneuvering control has to dynamically elaborate the generalized forces applied by the actuators. For fully actuated ships, the maneuvering control problem is extensively explained in [28]. For underactuated ships, equipped with less than 3 actuators only 1-2 actuators are used to control the surge, sway and yaw modes), the maneuvering control is a challenging problem. In works [24, 21, 25] the main control problems regarding underactuated ships are recalled, while in [15] a nonlinear path-following for underactuated marine craft is presented. Fig Tugboat with two main azimuth thrusters aft and one tunnel thruster in the bow. Courtesy of Roll-Royce Marine In this paper the case of surface vessels underactuated at high velocities and fully-actuated at low velocities is considered. The only relevant types of vessels to consider are the ones which become underactuated in the sway direction lateral direction) at high speed. These vessels are typically equipped with 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 their outlets; therefore, at high speeds, the only mean of actuation are the main thrusters. 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], is considered: η = Rψ) ν 1.8) M ν + Nν)ν = τ + R T ψ)w, 1.9)

8 8 Arrichiello et al. where η = [n e ψ] T is the position and attitude vector in the North-East-Down NED) reference frame; ν = [uv r] T is the linear and angular velocity vector in the Body-fixed BODY) reference frame; Rψ) SO3) is the rotation matrix from NED to BODY; M is the vessel inertia matrix; Nν) is the sum of the centrifugal and Coriolis matrix and of the hydrodynamic damping matrix; τ is the vessel propulsion force and torque; w is the vector of the environmental forces wind, currents, etc.) acting on the vessel in the NED reference system. In the proposed guidance system, the maneuvering control is a velocity and heading controller aimed at making each vehicle follow its velocity reference command elaborated by the NSB, that, following the schema of Figure 1.4, can be converted from v NSB elaborated considering the vessels as material points) to U NSB and χ NSB. n ψ χ u {B} β v U e Fig Motion reference model of the surface vessel Following the approach proposed in [8], a nonlinear, model-based velocity and heading controller is designed referring to an adaptive backstepping technique. Start by defining the error variables z 1 IR and z 2 IR 3 : z 1 = χ χ NSB = h T η + β χ NSB = h T η ψ NSB z 2 = ν α, where h T =[ 1] T, ψ NSB =χ NSB β and α=[α 1 α 2 α 3 ] T IR 3 is a vector of stabilizing functions to be specified later. Step 1: Define the Control Lyapunov Function as: where k 1 >. V 1 = 1 2 k 1z 2 1, 1.1)

9 1 Formation control of Marine Surface Vessels using the NSB 9 Differentiating V 1 with respect to time V 1 = k 1 z 1 ż 1 = k 1 z 1 h T η ψ NSB ) V 1 = k 1 z 1 h T ν ψ NSB ), since η = Rν and h T Rν = h T ν. By definition of z 2, then: V 1 = k 1 z 1 h T z 2 + α) ψ NSB ) Choosing then, = k 1 z 1 h T z 2 + k 1 z 1 α 3 ψ NSB ). Step 2: Define the Control Lyapunov Function as: α 3 = ψ NSB z 1, 1.11) V 1 = k 1 z k 1 z 1 h T z ) V 2 = 1 2 k 1z zt 2 Mz wt Γ 1 w where w IR 3 is parameter error due to environmental disturbances defined as w = ŵ w with ŵ being estimate of w, and Γ = Γ T >. Differentiating V 2 along trajectories of z 1,z 2 and w and assuming ẇ =. then: since M = M T and w = ŵ. Since then: V 2 = k 1 z z T 2 V 2 = k 1 z k 1 z 1 h T z 2 + z T 2 Mż 2 + w T Γ 1 ŵ, Mż 2 = M ν α) = τ Nν)ν + R T ψ)w M α Being ν = z 2 + α and w = ŵ w, then: ) hk 1 z 1 + τ Nν + R T w M α + w T Γ 1 ŵ.

10 1 Arrichiello et al. ) V 2 = k 1 z1 2 z T 2 Nz 2 + z T 2 hk 1 z 1 + τ Nα + R T ŵ M α ) + w T Γ 1 ŵ ΓRz2. Assigning where K 2 >, and choosing: then: τ = M α + Nα R T ŵ hk 1 z 1 K 2 z ) ŵ = ΓRz ) V 2 = k 1 z 2 1 z T 2 N + K 2 )z ) As stated in [16], since the system is persistently excited, choosing smooth α 1, α 1,α 2, α 2 L then the proposed control and disturbance adaption law makes the origin of the error system [z 1,z 2,ŵ] UGAS/ULES. The choice of α 1,α 2 depends on the actuation system of the vessels, i.e., when the vessel is fully-actuated α 1 = U NSB cos β NSB α 2 = U NSB sin β NSB ; while when the vessel is under-actuated α 1 = U NSB cos β NSB and α 2 is defined such as τ 2 =. The proposed controller guarantees that z 1, z 2 go to zero, and that ŵ converges to w. Thus, the vessel velocity converges to the desired values {U NSB,χ NSB } only when the vessel is fully-actuated; for an underactuated vessel, instead, u converges to U NSB cos β NSB while v is uncontrolled. It is worth noting that, while an underactuated ship executes a straight motion at cruise speed it is U = u 2 + v 2 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, U NSB cos β NSB ) 2 +v 2. Nevertheless, e.g., in turning maneuvers) it is U = in real applications, ships move using way-point tracking and turning maneuvers are used for short time periods to adjust changes of course; thus, the convergence properties of the proposed control law are acceptable in practice. When implementing the control law 1.13) it is important to avoid expressions involving the time derivatives of the states. To this purpose, the time derivative of the stabilizing function α is conveniently computed by differentiating along the trajectories of the states [13]. Moreover, to obtain high-order derivatives 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 NSB and β 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].

11 1.3 Tasks 1 Formation control of Marine Surface Vessels using the NSB 11 According to the behavioral control approach, the mission of the fleet is decomposed in three elementary tasks: move the barycenter of the fleet, keep a formation relative to the barycenter, and avoid collisions with obstacles and among vehicles. In this section, the corresponding task functions are presented 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 expressed by: σ b = f b p 1,...,p n ) = 1 n n p i. i=1 where p i = [η i,1 η i,2 ] T is the position of the vehicle i. Deriving the previous relation: σ b = n i=1 f b p) v i = J b p)v, p i where the Jacobian matrix J b IR 2 2n is J b = 1 n [ Following equation 1.4), the output of the barycenter task function is v b = J b σ b,d + Λ b σ b ), 1.16) where the desired value of the task function represents the desired trajectory of the barycenter. ] Rigid Formation The rigid formation task moves the vehicles to a predefined formation respect to the barycenter. The task function is defined as: p 1 p b σ f =., p n p b where p n are the coordinates of the vehicle n and p b are the coordinates of the barycenter. Writing for simplicity the vector p as [η 1,1... η n,1 η 1,2... η n,2 ] T, then the Jacobian matrix J f IR 2n 2n is:

12 12 Arrichiello et al. J f = [ ] A O, 1.17) O A where A IR n n is: 1 1 n 1 n... 1 n A = 1 n 1 1 n... 1 n ) 1 n 1 n n Because the Jacobian matrix is singular, the pseudoinverse can not be calculated as J T JJ T) 1 but as a matrix that verifies the following properties: JJ J = J ; J JJ = J and with JJ and J J symmetric. Since J f is symmetric and idempotent, J f = J f. The desired value σ f,d of the task function describes the shape of the desired 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 = ), is: v f = J f Λ f σ f 1.19) 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 environment may be close to some vehicle but far from some other; moreover, each vehicle is 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 obstacle in the advancing direction, the task function has to elaborate a driving velocity, aligned to the vehicle-obstacle direction, that keeps the vehicle at a safe distance d from the obstacle. Therefore, it is: where p o is the obstacle position and σ o = p p o σ o,d = d J o = ˆr T, ˆr = p p o p p o is the unit vector aligned with the obstacle-to-vehicle direction. According to the above choice,equation 1.5) simplifies to

13 1 Formation control of Marine Surface Vessels using the NSB 13 v o = J oλ o σ o = λ o d p p o ) ˆr, 1.2) where λ o is a suitable constant positive scalar. It is worth noting that, being NJ o ) = I ˆrˆr T, the tasks of lower priority than the obstacle avoidance are only allowed to produce motion components tangent to the circle of radius d and centered in p o, so as to not interfere with the enforcement of the safe distance d. While the implementation of the proposed obstacle-avoidance task function is the same for both punctual environmental obstacles and other vehicles, in the case of continuous obstacles it changes a bit. In particular, for convexor straight-line obstacles, p o represents the coordinates of the closest point of the obstacle to the ship at the current time instant. In the frequent case of multiple obstacles acting simultaneously e.g., both an obstacle in the environment and the other vehicles of the team) a priority among their avoidance should be defined; a reasonable choice is to assign the currently closest obstacle the highest priority. In critical situations the obstacle avoidance function may give a null-velocity output; this causes delay to the mission or loss of vehicles to the formation but increases safety of the approach. 1.4 Simulations In this section results of two simulations in complex realistic scenarios will be presented. In the first proposed scenario see Figure 1.5) a fleet of 7 vessels has to overtake a bottleneck that can represent the costal profile of a fjord) and a small obstacle, in presence of environmental disturbances, keeping a V-formation a configuration widely used for Navy and military applications). The mission is decomposed in three elementary tasks: move the barycenter of the fleet, keep a formation relative to the barycenter, and avoid collisions with obstacles and among vehicles. The obstacle-avoidance is the highest priority task because its achievement is of crucial importance to preserve the integrity of the vessels, while the barycenter and the rigid formation are respectively the secondary and tertiary tasks. Following the stability analysis consideration of Section 1.2.1, it is easy to prove that the barycenter and the rigid formation task function are not conflicting J f J b = ). The obstacle avoidance task function has to ensure each vessel a safe distance of 6 m from the environmental obstacles and from other vessels. For each vessel, the task function is activated only when the distance from environmental obstacles or other vessels becomes lower than 6 m. When the vessel is simultaneously close to i.e., under 6 m from) multiple obstacles, then the closest obstacle has the highest priority. Since a moving obstacle is assumed to be more dangerous than a fixed one, if the vessel is simultaneously

14 14 Arrichiello et al Fig Navigation in complex environment. Obstacle Avoidance-Barycenter-Rigid Formation

15 1 Formation control of Marine Surface Vessels using the NSB 15 5 a 25 b 2 σo [m] t [s] 5 c 15 d 4 1 σb [m] σf [m] t [s] t [s] Fig a) Paths followed by the ships; b) error of the obstacle avoidance task function; c) error of the barycenter task function; d) error of the rigid formation task function close to an environmental obstacle and another vessel, the avoidance of the other vessel takes higher priority if the distance from it is greater than the distance from the environmental obstacle multiplied by a gain chosen by trial and error) equal to.3. The desired trajectory of the barycenter is a rectilinear segment that connects the initial position [ ] T m to the final position [8 ] T m according to a fifth-order polynomial time law of 7 s duration with null initial and final velocity. The vessels have to attain and keep a V-formation around the barycenter and, once reached the final configuration, the vessels has to keep the formation and orient themselves in the opposite direction to the current. The gain matrices of the barycenter task function and of the rigid formation task function are respectively a 2-dimensional diagonal matrix with coefficient.1 and a 14-dimensional diagonal matrix with coefficient.1. The vessels have the dynamic model described in Section 1.2.2, where the matrices M and N are defined as:

16 16 Arrichiello et al Fig Navigation in complex environment. Obstacle Avoidance-Barycenter-Rigid Formation M = N =.35 Ns2 m.46 Ns2 m.9 Ns2 m.7 Ns m.9ns Ns2.71 Ns m.1812ns Ns m 1.849Ns To simulate the under-actuation at the high velocities, the force in sway direction τ 2 ) is saturated by a value depending on the velocity in the surge direction τ 1,τ 3 are saturated by fixed values representing the realistic limits of the actuators). The environmental disturbances are represented by a force constant in the NED reference frame) of w = [ 1 3 ]N. Figure 1.5 shows that the vessels do overtake the bottleneck avoiding collisions and reach the final configuration both barycenter and formation). In particular, the vessels, starting from the initial V-formation, loose the configuration to avoid the obstacles and, once overtaken, reach again the desired for-

17 1 Formation control of Marine Surface Vessels using the NSB 17 5 a 3 b 25 σo [m] t [s] 2 c 8 d σb [m] 5 σf [m] t [s] t [s] Fig a) Paths followed by the ships; b) error of the obstacle avoidance task function; c) error of the barycenter task function; d) error of the rigid formation task function mation. To avoid oscillations around the final configuration, when the vessels are close to the desired configuration then the desired velocities are imposed equal to and the desired orientation equal to the estimated current opposite direction. In this way, the vessels orient themselves in the opposite direction to the current and keep their positions compensating the current with the only main propellers. It is worth noticing that the proposed approach permits also switching of the formations, thus, in a similar environment, it can be useful changing the formation to overtake the obstacles i.e., reducing the angle of the V or reaching a line configuration) and come back to the desired configuration 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 middle of the mission). The barycenter function error starts from a low value the barycenter of the initial configuration is close to the desired value), increases during the avoidance of the bottleneck obstacle and decreases once overtaken. The rigid formation function error starts from a null value, increases during the obstacle avoidance and converges to zero once overtaken the obstacles.

18 18 Arrichiello et al. In the second scenariosee Figure 1.7) a fleet of 7 vessels has to navigate through an environment full of punctual obstacles, in presence of environmental disturbances, keeping a V-formation. The mission parameters are the same of the previous example but the great number of obstacles and the presence of current permits to test the navigation system while performing missions in extreme conditions. Figures 1.7, 1.8 show that the mission is successfully performed also in this scenario. It is worth noticing that the error of the obstacle avoidance task function reaches a peak of 25m, thus each vessel keeps a safe distance from obstacles and other vessels always greater than 3m the peak is verified when a vessel is simultaneously close to an obstacle and another vessel). Figure 1.9.a) shows that the estimation of the current ŵ does converge to the real value w = [ 1 3 ]N while Figure 1.9.b) shows that in the final configuration all the ships orient themselves in the opposite direction to the current. Videos of the performed simulations can be found at http : //webuser.unicas.it/arrichiello/video/ ŵ [N] 3 x a) θ [rad] b) t [s] t [s] Fig a) Current estimation; b) Orientation of the ships 1.5 Conclusion In this paper the null-space-based behavioral control has been presented to guide a fleet of autonomous marine surface vessels in complex environments. The NSB works in combination with the low-level maneuvering controls of each ship to take into consideration the dynamics of the fleet. The guidance system has been simulated in realistic missions involving the attainment of a formation while moving through obstacles in presence of sea current; the obtained results show the effectiveness of the proposed method.

19 1 Formation control of Marine Surface Vessels using the NSB Acknowledgment This research has been partially supported by a Marie Curie Fellowship of the European Community programme CyberMar Marie Curie Training Site in Trondheim. The authors appreciate the comments from Gianluca Antonelli, Ivar-André F. Ihle and Fabio Celani. References 1. G. Antonelli, F. Arrichiello, and S. Chiaverini. Experimental kinematic comparison of behavioral approaches for mobile robots. In Proceedings of the 16th IFAC World Congress, Prague, CZ, July G. Antonelli, F. Arrichiello, and S. Chiaverini. The null-space-based behavioral control for mobile robots. In Proceedings of the 25 IEEE International Symposium on Computational Intelligence in Robotics and Automation, Espoo, Finland, June G. Antonelli and S. Chiaverini. Kinematic control of a platoon of autonomous vehicles. In Proceedings of the 23 IEEE International Conference on Robotics and Automation, pages , Taipei, TW, Sept R.C. Arkin. Motor schema based mobile robot navigation. The International Journal of Robotics Research, 84):92 112, R.C. Arkin. Behavior-Based Robotics. The MIT Press, Cambridge, MA, T. Balch and R.C. Arkin. Behavior-based formation control for multirobot teams. IEEE Transactions on Robotics and Automation, 146): , 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, 335):68 615, Sept M. Breivik and T.I. Fossen. A unified concept for controlling a marine surface vessel through the entire speed envelope. In Proceedings of the 25 IEEE International Symposium on Mediterrean Conference on Control and Automation, pages , Limassol, Cyprus, June R.A. Brooks. A robust layered control system for a mobile robot. IEEE Journal of Robotics and Automation, 2:14 23, W. Burgard, M. Moors, C. Stachniss, and F.E. Schneider. Coordinated multirobot exploration. IEEE Journal of Robotics, 213): , June S. Chiaverini. Singularity-robust task-priority redundancy resolution for realtime kinematic control of robot manipulators. IEEE Transactions on Robotics and Automation, 133):398 41, E. Fiorelli, N.E. Leonard, P. Bhatta, D. Paley, R. Bachmayer, and D.M. Fratantoni. Multi-auv control and adaptive sampling in monterey bay. In Proceedings IEEE Autonomous Underwater Vehicles 24: Workshop on Multiple AUV Operations, pages , Sebasco, ME, June T.I. Fossen. Marine Control Systems: Guidance, Navigation and Control of Ships, Rigs and Underwater Vehicles. Marine Cybernetics, Trondheim, Norway, T.I. Fossen. A nonlinear unified state-space model for ship maneuvering and control in a seaway. Journal of Bifurcation and Chaos, 25.

20 2 Arrichiello et al. 15. T.I. Fossen, M. Breivik, and R. Skjetne. Line-of-sight path following of underactuated marine craft. In Proceedings of the 23 IFAC Conference on Maneuvering and Control of Marine Craft, Girona, Spain, September 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. International Journal of Robust and Nonlinear Control, JRNC-11:95 18, 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, 14):49 53, 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, 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 Coordination and Cooperative Control, Tromso, Norway, springer-verlag s lecture notes in control and information systems series edition, May D. Langer, J.K. Rosenblatt, and M. Hebert. A behavior-based system for offroad navigation. IEEE Transactions on Robotics and Automation, 16): , E.L. Lefeber, K.Y. Pettersen, and H. Nijmeijer. Tracking control of an underactuated ship. IEEE Transactions on Control Systems Technology, 111):52 61, M.J. Mataric. Behavior-based control: Examples from navigation, learning, and group behavior. J. Experimental and Theoretical Artificial Intelligence, 92-3): , L.E. Parker. On the design of behavior-based multi-robot teams. Advanced Robotics, 16): , K.Y. Pettersen and T.I. Fossen. Underactuated dynamic positioning of a ship- experimental results. IEEE Transactions on Control Systems Technology, 85): , K.Y. Pettersen, F. Mazencs, and H. Nijmeijer. Global uniform asymptotic stabilization of an underactuated surface vessels: Experimental results. IEEE Transactions on Control System Technology, 126):891 93, 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, 346): , Dec B. Siciliano. Kinematic control of redundant robot manipulators: A tutorial. Journal of Intelligent Robotic Systems, 3:21 212, R. Skjetne. The Maneuvering Problem. PhD.Thesis, Norwegian University of Science and Technology, Trondheim, Norway, D.M. Stipanovic, G. Inalhan, R. Teo, and C.J. Tomlin. Decentralized overlapping control of a formation of unmanned aerial vehicles. Automatica, 4: , Z. Wang, E. Nakano, and T. Takahashi. Solving function distribution and behavior design problem for cooperative object handling by multiple mobile robots. IEEE Transactions on Systems, Man, and Cybernetics, Part A, 335): , 23.