Publish and Subscribe-Based Formation and Containment Control of Heterogeneous Robotic System with Actuator Time Delay

: This paper addresses the problem of controlling a heterogeneous system composed of multiple Unmanned Aerial Vehicles (UAVs) and Autonomous Underwater Vehicles (AUVs) for formation and containment maintenance. The proposed approach considers actuator time delay and, in addition to formation and containment, considers obstacle avoidance, and offers a robust navigation algorithm and uses a reliable middleware for data transmission and exchange. The methodology followed uses both ﬂocking technique and modiﬁed L1 adaptive control to ensure the proper navigation and coordination while avoiding obstacles. The data exchange between all the agents is provided through the data distribution services (DDS) middleware, which solves the interoperability issue when dealing with heterogeneous multiagent systems. The modiﬁed L1 controller is a local controller for stabilizing the dynamic model of each UAV and AUV, and the ﬂocking approach is used to coordinate the followers around the leader or within the space delimited by their leaders. Potential Field (PF) allows obstacle avoidance during the agents’ movement. The performance of the proposed approach under the considerations mentioned above are veriﬁed and demonstrated using simulations.


Motivation
tasks that impose specific execution requirements [2]. An optimal sequence of task-agent allocation to minimize the task allocation time or the consumption of the robots' energy for a team of heterogeneous mobile robotic agents is introduced in [3]. The formation containment control designs of multi-leader follower systems have been proposed and reported in [4,5]; however, the influence of the information transmission between the agents was not considered in the design. Classical formation control designs of multi-agent systems with limited agents have been proposed by implanting controllers such as a state feedback controller, a Fuzzy Logic Control, an LQR controller, and a feedback linearization control technique [6,7]. However, the presence of parameters' uncertainties and external disturbances may critically influence the performances of the previously mentioned controllers. DDS middleware has been proposed and designed to overcome the issue of the data loss between multiple UAVs, as well as the interoperability issue associated with heterogeneous systems. Therefore, application of the DDS middleware technique augmented with the flocking approach resulted in significant improvements in flexibility, reliability, and portability [8,9].

Related Research
Generally, multi-vehicle formation control or containment control design can be categorized based on the following three structures: virtual-based design (see [10,11]), leader-following (see [12,13]), and behavior-based [14,15]. Multi-mobile robots formation control design has been proposed in [16] where the model predictive control (MPC) for the robot's trajectory tracking is implemented together with the adaptive terminal sliding mode control (ATSMC) for tracking the robot's velocity. The Pythagorean theorem has been applied for cooperation between mobile robots. The proposed controller performance has been evaluated in the presence of external disturbances that may influence the mobile robot model. Relevant to this work, the formation control of multiple ground vehicle systems has been addressed in [17] and in more detail in [18]. In these studies, the authors build on the literature results to address the formation maintenance and control of several robots where the leader or virtual leader is always at the center of the formation. The control design that is used locally is a feedback linearization, and the team navigation is stabilized based on a potential field force. Multi-mecanum-wheeled mobile robots' (MWMRs') leader-follower formation control design has also been presented in [19], where the fuzzy sliding mode controller for trajectory tracking has been implemented considering the parameters' uncertainties. The graph theorem has been applied for consensus between the heterogeneous systems of MWMRs. The consensus control design of a heterogeneous system of multi unmanned ground vehicles (UGVs) and multi unmanned aerial vehicles (UAVs) has been reported in [20], where the particle swarm optimization (PSO) approach was implemented for consensus between UGVs and UAVs. Furthermore, the issue of the path planning problem of multi UGVs and UAVs has been considered, while the consensus of the UGVs and the UAVs system has been investigated considering some assumptions and constraints. The leader-follower control design of multi-robot manipulator heterogeneous system has been proposed and reported in [21], where the leader-follower approach of multi-robot manipulator heterogeneous system has been studied in the presence of the parameters' uncertainties, while the leaders' information has been estimated by implementing the cooperative estimator technique. In addition, a graph theory approach has been utilized for consensus between the multi-robot manipulator's heterogeneous system. The framework to integrate multi-UAV applications has been developed and reported in [22]; the new proposed framework GzUavChannel is basically a combination of the ns-3, Gazebo, and ArduPilot network simulators. The GzUavChannel middleware can guarantee the integration and communication between multiple interconnected computers with different simulator tools such as ns-3, Gazebo, and ArduPilot. The framework for the integration between multi-ground robots in real-time applications has been introduced and proposed in [23], where a Domain Specific Language (DSL) and constraints analysis of the software applications' safety is used to provide a path between the design of architectures. The authors applied the MAUVE DSL deployment to define the model of execution; however, it is necessary to define some information about Worst-Case Execution Time (WCET) at the onset so that the Extreme Value Theory (EVT) is used to estimate the WCET execution time. The Orocos middleware is used to integrate and exchange information between the multi-ground robots. The framework of multip;e AUVs based on Mission-Oriented Operating Suite-Interval Programming (MOOSIvP) middleware has been developed in [24]. The publish-subscribe framework has been applied to exchange information between the AUVs. The authors identified the object as a sonar image for each AUV by utilizing the Sidescan Sonar Sensor (SSS). A classical PID controller has been implemented on each AUV to track the elevator, rudder, and thrust references. From the literature coverage, one can deduce that: • the influence of the information transmission between the agents has been identified as a crucial factor in performance and reliability of the control design; • the presence of parameters' uncertainties and external disturbances may critically influence the performances of the prior.; • application of the DDS middleware technique augmented with the flocking approach has not been applied for heterogeneous systems, which is closer to the real-word robotic applications.
This paper proposes an approach that can possibly address all of the above points. Indeed, navigation and control for formation and containment mainteannce of a fleet of multiagent heterogeneous systems composed of AUVs and UAVs is considered. A distributed method has been developed using an L1 adaptive control technique taking into account actuator time delay to stabilize the agents and make them more robust towards parameters change. The flocking approach has been used for the navigation, and the DDS middleware has been utilized for the data transmission between the agents. The navigation ensures formation as well as containment control in the leader-follower context. Potential field-like methodology has been adopted to avoid obstacle while maintaining the formation and containment.
The main contributions of this research are highlighted as follows: • A robust architecture for the formation control design of heterogeneous systems has been developed. • A robust navigation system for the containment control design of heterogeneous systems has been proposed and designed. • The navigation algorithm avoids obstacles while still maintaining the required formation or containment. • Reliable middleware for data transmission has been developed.
The remainder of this manuscript is organized as follows. In Section 2, the preliminaries of DDS middleware, the dynamics of the quadrotor, and the dynamics of the underwater system are presented. In Section 3, the multi UAVs formation stability has been proved using the L1 technique with an actuator time delay and Lyapunov analysis. The simulation results are presented and discussed in Section 4. Finally, the findings and the concluding remarks have been highlighted at the end.

Preliminaries
As mentioned above, the system that this work is considering is composed of a heterogeneous fleet of UAVs and AUVs connected using DDS middleware. This section will cover the necessary background of DDS middleware and then present the model for each type of agent (AUV and UAV). The modeling background is necessary for the development of the local agent controller, which will be presented in Section 3. In this section, the following symbols in Table 1will be used. For the UAV η 1 ∈ R 3×1 the vector representing z, y, and x coordinates.
the vector of Euler angles yaw (ψ), pitch (θ), and roll (φ). g represents the gravity acceleration. m represents the mass. k t is the drag coefficient. u is the total thrust force input created by the 4 propellers.
represents the quadrotor's inertia. ν 2 is the angular velocity. z e is equal to [0, 0, 1] T . I R represents the propeller inertia. k r represent the rotational drag. × is the cross product.
is the coriolis matrix.
is the damping matrix. g(η) ∈ R 6 is the gravitational vector. τ ∈ R 6×1 is the input control signals.

M RB
is the rigid body's inertia matrix. M A represents the additional momentum and forces.
is the gravity vector. I 3×3 is the 3-by-3 identity matrix.
is the inertia tensor.

Data Distribution Service (DDS)
DDS middleware has emerged as a solution for many novel distributed frameworks. It boosts the publish-subscribe technique for transmitting data, commands, and events between the agents. Agents that are producing data publish the data, and they can subscribe to the data from other agents. The middleware is responsible for providing the data to subscribers who subscribed to that topic. It has been shown that DDS middleware is a very effective approach for many-to-many communication networks as it is very successful in transmitting time-critical information. DDS offers a group of Quality-of-Service (QoS) regulations to adapt to communication behaviors. These QoS regulations might be used separately or as a set in order to influence different aspects of communications, such as reliability, durability, and data persistence. These QoS regulations are a feature of the DDS which are not available in all other middleware approaches to communication. This feature guarantees high reliability, performance, low latency integration of DDS data transmission, and DDS interoperability with the capability of DDS frameworks to communicate from various vendors. The standardized DDS Real-Time Publish-Subscribe (RTPS) protocol allows for connection interoperability between various DDS frameworks. The DDS middleware was considered for our study for the formation and containment control of heterogeneous systems. Each agent must publish its velocity and subscribe to other agents' velocities. A selection of Quality of Service regulations must be fulfilled for interaction between the agents (publisher/subscribers), such as: • Durability: Determines whether or not the previous data sent by the publisher has been received by a new subscriber. • Reliability: Specifies whether or not the publisher will resend information lost over the network. Reliability involves two setups: BEST EFFORT (not re-send data lost) and RELIABLE (resend data lost). • History: Determines a publisher's preservation of the data received or sent to a subscriber. History involves two setups: KEEP LAST and KEEP ALL.
• Ownership: Determines whether a subscriber will simultaneously accept new samples from multiple publishers. Ownership involves two setups: SHARED and EXCLU-SIVE.

Dynamics of Quadrotor
A detailed dynamic model of the quadrotor can be found in [25]. The quadrotor's translational motion is given bÿ The transformation matrix J 1 (η 2 ) relates to the Euler angles according to The property of J 1 (η 2 ) is where s(.) = sin(.), c(.) = cos(.). The input u is defined as follows: where an upward-lifting force is f i = k i Ω 2 i , i = 1, 2, 3, and 4, with positive constants k i and Ω i represents the angular speed motors. The quadrotor's rotational motion is given bẏ and The force u and the torques τ = [τ p , τ q , τ r ] on the frame's body. The quadrotor's rotational and translational motion is given by where d represents the drag factor and l represents the distance between motors and the center of mass. The number actuation from the above equations is less than the degree of freedom. Therefore, the following description of underactuated structures is taken into account in our case. Consider the next system: where F is a dynamic vector , Q is coordinates vector , U is control inputs , and G is input matrix . If G's rank is less than Q's dimensions, then Equation (8) is under-actuated. A detailed quadrotor dynamic is available in [8,9,25].

Dynamics of Underwater Vehicles
A detailed dynamic model of the underwater vehicle can be found in [26].
The underwater motion equation in the body frame is given by: The inertia matrix has two parts: and The skew symmetric matrix S(r g ) is given by In addition, where, the torques and the linear forces applied on the underwater model are x . = ∂x ∂(.) . X, Y, Z, K, M, N. For example, the hydrodynamic force in x-direction due to accelerationu is represented by Xu, so the inertia matrix can be used to determine the Coriolis matrix by constructing it in terms of sub-matrices: The Coriolis and damping matrices are thus given respectively by: The underwater equation of motion in the Earth frame is given by: where and the transformation matrix R(η) is as follows with A detailed description of the underwater dynamic can be found in [26].

L1 Adaptive Control with Actuator Time Delay
In this section, the L1 adaptive control utilized in [26] is used for a single AUV extended with an actuator time delay to stabilize multiple AUVs.
The main feature of utilizing the L1 adaptive controller with an actuator time delay is its ability to adapt rapidly and robustly, which augments the performance of the system. This capacity has been achieved by separating adaption from robustness. Utilizing a modified L1 controller, the uncertainties are predestined by using a fast method and the recompensed uncertainties proceed through a filter. Then, the overall output is included within the input signal. There are two key functions to this filter. First, it ensures that the control signal stays within an acceptable frequency range. Second, it separates the robustness from the adaptation. With this filter, we can improve robustness as well as performance. For more details, see [27][28][29].
In order to implement the modified L1 controller to the quadrotor/underwater vehicle, a representation of the state space for (5) and (17) is required.
where, the system's states are denoted by x(m) = [x 1 (m), x 2 (m)] ∈ R 2n , the known matrix is represented by A 2 ∈ R n×n , the constant matrix is represented by B 2 ∈ R m×n , u(m − τ) ∈ R m represents the control vector with time delay τ, w ∈ R denote the input's uncertainty, the known constant matrix is represented by C ∈ R m×n , the system's output is represented by y(m) ∈ R m , and an unknown nonlinear function is denoted by f 2 (m). We can rewrite (19) as follows [30]: with From (17), one can havë Let Therefore, the state space representation can be obtained from (21) and (22) as follows: Equation (23) can be rewritten aṡ Therefore, the dynamic of the state predictorx(m) ∈ R n can be defined as:x We build on same analysis of the L1 adaptive controller applied to a single AUV in [26] by adding some modifications based on the actuator time delay to make the proposed approach more comprehensive.
Consider the following Lyapunov function The system stability is guaranteed for any u such thatV(m) ≤ 0. Consequently, Therefore, By utilizing the projection operators, Proj, the parameter derivatives can be rewritten as follows:˙θ where the rate's adaptation laws are denoted by (γ 1 , γ 2 and γ 3 ) > 0, P = P > 0 to satisfy the Lyapunov function: and Q = Q > 0. Substituting Equation (29) into Equation (28), one obtains Equation (31): Finally, Equation (31) becomes the following: Therefore, the system is asymptotically stable. The Laplace transform of the control signal is calculated as follows: where the Laplace ofμ(m) =θ(m) x(m) ∞ +σ(m) and the reference signals r(m) arê µ(s) and r(s), respectively, and the feed-forward gain is represented by So, the H(s) filter has been chosen as follows: By selecting C(s) = I s with H(0) = I, Equation (35) can be written as follows: The control signal can be obtained by substituting Equation (35) into Equation (34) as follows: u ad (s)e −τs = −kC(s)(ωu ad (s)e −τs +μ(s) − k g r(s)) Last, the following condition must be satisfied for the L1 controller: and More details can be found in [26,27].

Flocking and Boids Mode
One of the most interesting sights in nature is the mutual movement of animals in sets, where each animal has its own speed and direction even though they move along with each others to form a single entity. Following social practices, ants, fish, and bird communities are examples of biological entities in their movements. The first method to simulate these movements was established in 1986 by Craig Reynolds and referred to as a bird-like object called the Boids model [31]. The Boids model explains birds' movement and organization activities in a flock. These cooperative movements typically follow two basic rules: the need for each agent to remain in the group while avoiding collision with the adjacent agent. In order to explain these rules specifically, Reynolds split them into three groups to simplify them for computer simulation [32]. Based on the rules of Reynolds [31], the group N, which consists of n agents, is supposed to represent the whole flock.
Each b i boid in the group N has p position, u unit vector, and v velocity vector in (x,y,z) plane. The visibility sphere of the b i boid is described by: The flocking model of Reynolds is based on the following three principles: Separation : avoiding collision with the adjacent agent. The general equation of the separation rule is as follows [33]: By considering all boids inside one boid's visibility sphere, b i , then link each boid vector to b i . The function for separation is the sum of the corresponding vectors.
Alignment: aligning velocity and position with adjacent boids. The general equation of the alignment rule is as follows [33]: where the boid's motion is denoted by m i . Cohesion: staying close to the flock's center created by its adjacent boids. Suppose that the flock centering is expressed by K i for the boid b i . The important step is to obtain the mass center of all visible bodies in the sphere's visibility, v i as follows: where c i denotes the mass center of the boids . Therefore, the vector of cohesion displacement is given as follows [33]: In the above equations, we clarified the rules of Reynolds through simple formulas. Therefore, we utilized the formulas in [32,34,35] as follows: • Separation: The separation force is utilized for avoiding collision between the UAVs and AUVs, which is defined as follows: where the position of agent s and agent j is represented by p s and p j respectively, n represents the agent's number, k represents a positive constant, p s (m) − p j (m) represents the Euclidean distance between the agent s from the agent j, the separation gain is represented by ξ, and d 0 represents the constant distance between the agents. • Alignment: The matching average velocity of the agents is defined as follows: where the velocity of agent j with respect to agent s is represented by v s,j (m) and the velocity of agent j and agent s is represented by v j and v s , respectively. • Cohesion: The cohesion force is utilized to calculate the AUVs attraction to the desired position based on the UAVs leader's position, which is defined as follows: where the desired agent position is represented by p d and a constant gain is represented by k coh . • Obstacle Avoidance: The obstacle avoidance force is utilized for avoiding collision between the obstacles and the agents (UAVs and AUVs), which is defined as follows [34]: with where the position of agent j and obstacle o are represented by p j and p o , respectively, n represents the agent's number, k o represents a positive constant, p j (m) − p o (m) represents the Euclidean distance between agent j and obstacle o, the separation gain is represented by ξ o , and r 0 represents the constant distance between agent j and the obstacle o.
The total control law is a combination formula of all the forces, which will be given as follows: where the control law U r is affected by positive constants S, M, C, and S o .

Formation Control Based on L1 Adaptive Control with Actuator Time Delay
The L1 controller with actuator time delay, DDS middleware, and flocking technique built above have been introduced. The underwater and quadrotor model parameters are selected as in [25,26]. Table 2 list the parameters for the quadrotor. Tables 3-5 list the MARES parameters, inertia and added masses respectively.
For formation control, the DDS middleware communication between the agents is shown in Figure 1 by considering some policies of QoS in Table 6 that must be fulfilled before the interaction between the subscribers and the publishers. The QoS illustrated in Figure 1 will be adopted during all the tests shown in Figures 2-7.
Two cases have been included for the formation control results based on the modified L1 controller.

•
AUVs-UAVs formation control without disturbance and uncertain parameters. Figure 2 illustrates a group of AUVs in a triangular formation tracking their UAVs leader and coordinating themselves around it in 2D space. At the same time, Figure 3 depicts the 3D space of the set of three AUVs without disturbance and uncertainty in the inertia matrix. • Formation of AUVs-UAVs with disturbance and uncertainty in parameters. In Figures 2 and 3, the disturbance and parameter uncertainties (water density and the inertia matrix) were not taken into account. Figures 4 and 5 show the 2D space and the 3D space of the group of three AUVs with disturbance and uncertainty in water density and inertia matrix. Figures 6 and 7 illustrate the formation control of three AUVs in triangular form and one UAV avoiding some obstacles during their movement in 2D and 3D space. One can observe that during this maneuver, the controller still maintains the formation.

Pub&Sub 2
Follower 2 Figure 1. The formation structure based on DDS middleware.

Containment Control Based on L1 Adaptive Control with Actuator Time Delay
For containment control, the communication between the agents through the DDS middleware is shown in Figure 8 by considering the same policies of QoS in Table 6. This communication structure is maintained during all the tests shown in Figures 9-14.
Two cases have been included for containment control results based on the modified L1 controller.
• AUVs-UAVs containment without disturbance and uncertain parameters: The set of five AUVs can be constituted in a certain topology inside their UAVs leaders in 2D space and 3D space without disturbance and uncertainty in the inertia matrix , which is indicated in Figures 9 and 10. • AUVs-UAVs containment with disturbance and uncertain parameters: The disturbance and the parameter uncertainty were not considered in Figures 9 and 10, but now it is considered. Figures 11 and 12 show the 2D space and 3D space of the set of five AUVs with disturbance and uncertainty in water density and the inertia matrix. Figures 13 and 14 illustrate the containment control of five AUVs and four UAVs avoiding some obstacles during their movement in 2D and 3D space. One can see that during this maneuver the controller maintain the containment and the formation of the followers within their leaders.

Results Discussion and Comparative Analysis
The formation of heterogeneous AUVs-UAVs without disturbance and uncertain parameters (water density and the inertia matrix) based on the modified L 1 controller in 2D space and 3D space is shown in Figures 2 and 3, respectively. All the AUVs are following their UAV leader and maintaining their triangle formation during tracking. Information is exchanged between all the AUVs and their UAVs leader through the DDS middleware as in Figure 1.
The formation of heterogeneous AUVs-UAVs with disturbance and uncertain parameters (water density and the inertia matrix) based on the modified L1 controller in 2D space and 3D space as shown in Figures 4 and 5, respectively. In the presence of these disturbances, the fleet of AUVs maintain their triangle formation with the leader in the center. The performance of the system is not influenced by these disturbances and the large variation in the water density. The communication between all the AUVs and their leader is achieved through the DDS middleware as in Figure 1. Figures 6 and 7 show how the three AUVs and one UAV avoided obstacles during their movement. The formation of the AUVs in triangle form with the leader at the center has been maintained during obstacle avoidance in both 2D and 3D space.
The containment of heterogeneous AUVs-UAVs without disturbance and uncertain parameters (water density and the inertia matrix) based on the modified L1 controller in 2D space and 3D space is illustrated in Figures 9 and 10, respectively. All the AUVs have been localized in formation inside the space delimited by the four UAVs leaders. The exchange of information between all of the AUVs and their UAVs leaders has been conducted through the DDS middleware.
The containment of heterogeneous AUVs-UAVs with disturbance and uncertain parameters (water density and the inertia matrix) based on the modified L1 controller in 2D space and 3D space is shown in Figures 11 and 12, respectively. The followers are tracking the leaders and are remaining within the space delimited by these leaders. The performance of the system has not been influenced by these disturbances and the large variation in the water density. The star formation of the followers and their containment within the four leaders has been maintained. The communication between all of the AUVs themselves and their UAVs leaders has been done through the DDS middleware with QoS adapted to the leaders and followers as in Figure 8. Figures 13 and 14 show how the five AUVs and four UAVs avoided the obstacles during their movement while maintaining their containment in 2D and 3D space. The obstacle has been avoided by the fleet following their leaders.
The navigation and control approach provided an excellent robustness to disturbances and also avoided obstacles while maintaining the formation or the containment. The DDS middleware provided the right environment for communication and data exchange with the appropriate QoS.

Conclusions
A novel design for heterogeneous formation and containment control has been addressed throughout this paper. The heterogeneous formation and containment control is proposed based on an L1 controller with an actuator time delay, the DDS middleware, and a flocking approach. The L1 controller with actuator time delay is utilized to stabilize the AUVs' and UAVs' dynamic models. Furthermore, the flocking technique has been utilized to manage the AUVs' positions and keep them in their desired paths in accordance with their UAVs leaders as well as to avoid the obstacles during the agents' movement. Moreover, the DDS middleware is used for exchanging information between all of the AUVs themselves and their UAVs leaders and for solving the interoperability issue between the UAVs and the AUVs. The modified L1 adaptive controller shows high performance. The simulation results indicate that the modified L1 adaptive controller and the DDS middleware enhanced the overall performance.