Attitude coordination of multi-HUG formation based on multibody system theory

Application of multiple hybrid underwater gliders (HUGs) is a promising method for large scale, long-term ocean survey. Attitude coordination has become a requisite for task execution of multi-HUG formation. In this paper, a multibody model is presented for attitude coordination among agents in the HUG formation. The HUG formation is regarded as a multi-rigid body system. The interaction between agents in the formation is described by artificial potential field (APF) approach. Attitude control torque is composed of a conservative torque generated by orientation potential field and a dissipative term related with angular velocity. Dynamic modeling of the multibody system is presented to analyze the dynamic process of the HUG formation. Numerical calculation is carried out to simulate attitude synchronization with two kinds of formation topologies. Results show that attitude synchronization can be fulfilled based on the multibody method described in this paper. It is also indicated that different topologies affect attitude control quality with respect to energy consumption and adjusting time. Low level topology should be adopted during formation control scheme design to achieve a better control effect.


Introduction
Development and application of underwater gliders (Eriksen et al., 2001;Webb et al., 2001;Sherman et al., 2001) have promoted adaptive ocean observation and monitoring. Since most ocean processes are spatial-varying and time-varying, coordination and cooperation of underwater gliders are necessary to perform spatial coverage and obtain high resolution data. Several experiments (Alvarez et al., 2013;Fiorelli et al., 2006;Leonard et al., 2007Leonard et al., , 2010Liang et al., 2012;Merckelbach et al., 2008;Paley et al., 2008;Zhao et al., 2013) were executed by a fleet of underwater gliders. Recently, hybrid underwater gliders (HUGs) have become one of the best choices for long time observation of micro-scale and meso-scale dynamic processes due to their advantages in endurance, controllability, maneuverability and through water ability. These gliders include Slocum (Claus et al., 2010), Folaga (Alvarez et al., 2009), Petrel-II (Liu et al., 2014, typically. Related studies include, Wang et al. (2011) and Liu et al. (2014) developed Petrel-II glider successfully, and the dynamic properties, and the trajectory control of Petrel are also researched . Chen et al. (2013Chen et al. ( , 2016) designed a folding propulsion for HUG to reduce the drag force. Formation of multiple HUGs can extend capabilities of single agent and enable accomplishment of more complicated missions. Two Slocum G2 gliders were deployed in the observation in the Northwestern Gulf of Mexico (Perry et al., 2013). Yang et al. (2011) provided a path planning method of multi-HUG formation, and Xue et al. (2015) introduced a statistical method for uncertainty analysis in multi-HUG formation control.
Coordination among underwater vehicles has been focused recently. Coordination of formation contains attitude coordination and position coordination. Position coordination of multiple underwater vehicles can be used in simultaneous monitoring to obtain data at different sampling positions at the same time. Position coordination can also reduce surveying time for large-scale monitoring. Attitude coordination is necessary in multi-HUG formation task for the following reasons. First, differing from traditional glider, HUG is more powerful in the attitude adjustment by the addition of screw propeller, which makes it more competitive in ocean observation. Therefore, the research of the attitude coordination is of more practical significance in the HUG formation. Second, owing to the band width limits of the acoustic modems, attitude coordination provides better quality for communication among agents in deep sea. Third, in the case that a fleet of sensor-equipped HUGs are used as an acoustic array, synchronization of vehicle orientation also becomes critical (Nair and Leonard, 2007). Moreover, Attitude coordination is also helpful for the cooperation of HUGs fleet with detective sensors to track object in area defense and detection system. Since the trajectory of HUG is more susceptible by the ocean current than AUV's, the method that can reply to the changeable topologies of HUG formation in the attitude coordination is necessary.
Among attitude coordinate control algorithms, an error orientation matrix (Wang and Xie, 2013) has always been discussed to investigate attitude deviation among agents. The orientation of the agents in an N-HUG formation with respect to inertial frame can be represented by rotation matrices and the error orientation matrix between the i-th and the j-th agents can be expressed by . Eliminating the matrix can fulfill the goal of attitude alignment. Orientation potential field Leonard, 2007, 2008;Sarlette et al., 2009;Smith et al., 2001), as one kind of artificial potentials was introduced into formation mechanical modeling to stabilize the agents' attitude at the global minimum of the potential where . Arbitrary relative attitude requirement among agents was also reached by ensuring , where K≠0. Another attitude coordination method (Ren, 2007;Wang and Xie, 2013) was proposed using a physically motivated PD-like attitude consensus scheme based on the error orientation matrix. Yang et al. (2011) provided a view of multibody system description in multi-HUG path planning. They adopted several types of artificial potential fields (APFs) related to the position of each HUG to fulfill the path planning, formation shaping and obstacle avoidance of multi-HUG. They provided a model of three HUGs and proved the effect of the method by simulation. Nevertheless, they considered HUGs in the formation as particles, which was incapable in representation of each HUG and the formation's attitude. As previously mentioned, the research of attitude coordination is necessary for multi-HUG formation, differing from what is presented in Yang et al. (2011), and a more universal system constructed by N HUGs will be provided. The HUGs in the formation are regarded as rigid bodies and the formation is treated as a multi-rigid body system to describe the attitude problem in this paper. Thus, the attitude coordination problem of the formation is to solve the orientation coordination of the multi-rigid body system. An orientation potential field (Sarlette et al., 2009) related to the error orientation matrix (which is determined by the formation topology) is adopted to generate the attitude control torque. The dynamic equations of the multi-rigid body system are based on Kane's equations (Kane and Levinson, 1985). Unlike the Newton-Euler formulation of equations, Kane's equations are advantageous because they do not require computation of workless constraint forces and moments, which makes this method of formulation of equations more computationally efficient compared with the other methods (for details see Angeles et al., 1989;Šalinić et al., 2014). Huston's method (Huston, 1989(Huston, , 1990Huston and Kamman, 1982;Huston and Liu, 1991) is adopted in the procedures of multibody model establishment, which is adept to select the appropriately generalized coordinates and form multibody formulations. Lower body array is used to describe the topology of the formation, control to generate the attitude torque and further determine the characters of the multibody system. In the formation, each HUG's attitude is represented by Euler parameters to avoid global singularity. This approach would be applicable and efficient in real time formation control.
The rest of the paper is organized as follows. Section 2 presents the construction of the virtual multibody system and dynamic modeling of multi-HUG formation. In Section 3, attitude synchronization of two formation topologies is conducted by numerical calculation. Section 4 summarizes the main contributions.

Kinematics
Hybrid underwater gliders combine advantages of autonomous underwater vehicles (AUVs) and conventional underwater gliders (UGs). They are more flexible than conventional UGs and more endurable than AUVs. A fleet of HUGs can complete more complicated operations in a longer time-scale by formation coordination. The multi-HUG formation can be regarded as a holonomic multi-rigid body system. The HUGs in the formation are modeled as rigid bodies with fully actuated dynamics and are virtually connected to each other by body-associated torques applied on each body. Fig. 1 shows the multibody model of a three-HUG formation as an example, where the inertial frame is regarded as a body represented by B 0 , and B 1 , B 2 and B 3 represent the HUGs in the formation with an order decided by formation topology. The multibody system of the three-HUG formation is numbered using a lower body array L 3 (K), as shown in Table 1, to derive the kinematics based on Huston's method (Huston, 1990;Huston and Liu, 1991). The lower body array is used to describe the relationship between the HUGs.
In case an N-body system with no constraints, the rotation freedom of the system is 3N. For formation attitude co-ordination, the generalized coordinate contains only rotation variables. In order to avoid singularity and to facilitate programming, the generalized coordinate is constructed by 4N elements and can be given by: where e ki (k=1, 2, …, N; i=1, 2, 3, 4) is the Euler parameters of the k-th body in its lower body array L n (K) frame. The generalized velocity is given by: is the relative angular velocity of the k-th body with respect to its lower bodies in L n (K) frame. The relationship between the Euler parameters and the relative angular velocities is: The transformation matrix which describes the relationship between the k-th body and its j-th lower body is given by: 2(e k1 e k2 − e k3 e k4 ) 2(e k1 e k3 + e k2 e k4 ) 2(e k1 e k2 + e k3 e k4 ) −e 2 k1 + e 2 k2 − e 2 k3 + e 2 k4 2(e k2 e k3 − e k1 e k4 ) 2(e k1 e k3 − e k2 e k4 ) 2(e k2 e k3 + e k1 e k4 ) −e 2 k1 − e 2 k2 + e 2 k3 + e 2 The angular velocity of the k-th body is expressed by the following equation (Huston, 1990;Huston and Liu, 1991): ω k = ω klm y l n 0m , sum on l, m; l = 1, 2, . . . , 3N; m = 1, 2, 3, (5) where n 0m represents mutually perpendicular unit vectors fixed in the inertial frame B 0 and ω klm represents components of the partial angular velocity. The angular acceleration can be given by differentiating Eq. (5) as: Then, the partial angular velocity with respect to the generalized velocity y l can be expressed as: 2.2 Kinetics Attitude coordination among HUGs in the formation is achieved by adjustment of torques on each HUG. The overall torque consists of two parts: conservative torque and dissipative torque. The conservative torque can be generated by the designed artificial potentials, while the dissipative torque is associated with angular velocities of agents in the formation.
Several potential fields (Hanβmann et al., 2006;Leonard, 2007, 2008;Sarlette et al., 2009;Smith et al., 2001) have been proposed for formation orientation control. The agents in the formation are supposed to be under the influence of potential fields, so that attitude synchronization or other attitude coordination requirements can be achieved. The principle to design the potential fields is to make the agents come to a stable equilibrium with desired attitudes. In this paper, the artificial potential presented in Sarlette et al. (2009) is adopted for the orientation control of HUG formation. The artificial potential is expressed as: (k=1, 2, …, N) is the rotation matrix which represents the orientations of the N rigid bodies with respect to the inertial frame, σ is a scalar control gain which is strictly negative to ensure that Q k equals Q j at the global minimum of V, which is the goal of attitude synchronization, and the matrix transpose is denoted by [·] T . According to Section 2.1, the transformation matrices SO(J) and SO(K), i.e., Q k and Q j , are orthogonal. The conservative torque generated by the orientation potential field can be given by: Table 1 Lower body array of the three-HUG formation shown in Fig. 1 L Modeling of a three-HUG formation. The black coordinate system represents the inertial frame which is regarded as a body represented by B 0 . The ellipsoids in red, purple and blue are B 1 , B 2 , and B 3 which denote the HUGs in the formation, respectively. The coordinates in red, purple and blue denote the body frames. G 1 , G 2 , and G 3 are the centroids of B 1 , B 2 , and B 3 ; and [α k , β k , γ k ] (k=1, 2, 3) is the relative orientation of the k-th body relative to its reference coordinate system. The black one-way dash arrows show the reference systems of the bodies: the reference system of B 1 is the inertial frame and the reference system of B k is B k-1 's body system in this case.
[·] ∨ [·] ∧ where is the inverse of . In order to ensure the system could converge to the desired equilibrium with arbitrary initial condition, the dissipative torque proposed by Sarlette et al. (2009) is adopted to asymptotically stabilize the attitude synchronization process as: where γ is the control dissipative gain and ω k (k=1, 2, …, N) represents the angular velocity of the k-th body associated with the k-th body frame. When ω k =0, the system converges to the desired state. The total control torque is the sum of the conservative torque and the dissipative torque. It can be given by: The contribution of the control torque M k on B k to the generalized active force associated with the generalized speed y l is F l = M km ω klm , sum on k, m; k = 1, 2, . . . , N; l = 1, 2, ..., 3N; m = 1, 2, 3, where M km is the component of M k related to n 0m and the contribution from all bodies of the system can be written as: M * k By focusing on attitude coordination problem, the inertial force on the k-th body can be denoted by a torque , which is expressed as: where I k is the central inertia dyadic of the k-th body. Similar to the generalized active force, the generalized inertia force of the system can be expressed by:

Dynamics
Kane's method is used for dynamical analysis in attitude coordination of multi-HUG formation. Kane's equation combines the advantages of Newton-Euler equation and Lagrange equation. Kane's equation is established without creating energy function. Scalar energy function can be avoided. Kane's equation for the HUG formation is expressed by: F * l where F l and (l=1, 2, …, 3N) are the generalized active force and the generalized inertia force in the HUG formation respectively. By substituting Eqs. (5)-(7), Eq. (13) and Eq. (15) into Eq. (16), Eq. (16) can be rewritten as: where a lp is the generalized inertia coefficient. a lp can be obtained by: a lp = I kmn ω klm ω kpn (sum on k, m, n; k = 1, 2, . . . , N; l, p = 1, 2, ..., 3N; m, n = 1, 2, 3) , (18) and f l can be obtained by: where h l is the generalized inertia force coefficient and can be obtained by: sum on k, p, q, m, n, r, s; k = 1, 2, ..., N; l, p, q = 1, 2, ..., 3N; m, n, r, s = 1, 2, 3, (20) where y p and y q are the derivatives of the generalized coordinates, I kmn and I ksn are components of the inertia dyadic I k on the inertia frame axis n 0m and n 0n respectively, and e rsm is the standard permutation joint.

Numerical simulations
In this section, attitude synchronization of a multi-HUG formation composed of three agents is conducted by numerical simulation based on the following assumptions: (1) The communication among HUGs is assumed to be perfect. There is no communication latency or communication loss. Each agent can obtain the right orientation information of the others in time during the attitude adjustment.
(2) The HUGs in the formation are assumed to be homogeneous ellipsoid bodies (Smith et al., 2001) with no difference among each other. The mass and volume of the ellipsoid are similar to the Petrel-II underwater glider developed by our team (Liu et al., 2014;Wang et al., 2011). At low angle of attack during the motion of glider, the hydrodynamic effect on the attachment can be neglected (Leonard and Grave, 2001). And the artificial potential field is robust in the control of attitude coordinate. Therefore, the antenna mast in the rear and the wings mounted on both sides of the hull are neglected in the model.
Numerical solution of the dynamic equations of the virtual multibody system is derived using the adaptive Runge-Kutta method. Orientation of the bodies can therefore be obtained by iterative calculation. During the numerical simulation, the dynamic equations for the i-th time interval change according to the state in the (i-1)-th time interval. The Euler parameters and angular parameters can be converted to each other. The numerical calculation flow chart is shown in Fig. 2.
Two formation topologies are discussed in the simulation. Formations with different topologies could supposedly achieve attitude synchronization. Orientation of the bodies in the multibody system would be coordinated to the desired state. In this section, two formation topologies with three agents in the formation are discussed, as shown in Fig. 3.
The formation topology shown in Fig. 3a can be understood as a queue structure with four levels. The inertial frame is at the first level. The three bodies in the multibody system are set at different levels respectively, with Body 1 at the second level, Body 2 at the third level, and Body 3 at the fourth level. In the queue formation, the bodies are virtually connected to their nearest lower body, which is also the unique reference for orientation control. During orientation control process, the attitude of Body 1 is controlled to align with the inertial frame, Body 2 is controlled to align with Body 1, and Body 3 is controlled to align with Body 2. The formation topology in Fig. 3b shows another topology which can be regarded as a branch structure with three levels. The inertial frame is at the first level, Body 1 is at the second level, and Body 2 and Body 3 are at the same level, the third level, with no connection in between. Similar to the queue structure, the nearest lower body is also the unique reference for orientation control. During the orientation control process, Body 1 is controlled to align with the inertial frame, while Body 2 and Body 3 are both affected by Body 1 simultaneously. The lower body arrays of the two topologies are in Table 2. Table 3 shows the key pseudocodes of two topologies where line 5 of two topologies are different. The main algorithm is to obtain the current rotation matrix to generate control torque by time step t', which can be finished when the control torque on each body is equal to zero. In the simulation, the objective of the orientation control is to make the three bodies reach the same attitude of [0°, 0°, 0°] with respect to the inertial frame. By taking the attitude adjusting ability of Petrel-II into consideration, the initial orientation angles of the three HUGs, i.e. roll angle, pitch angle and yaw angle are [5°, 30°, 10°], [10°, -20°, 5°], and [15°, 40°, -10°], respectively. The initial angular velocities are all defined to be 0 rad/s. Simulation results of attitude synchronization are presented in Fig. 4 and Fig. 5 for the two formation topologies. Fig. 4 shows the attitude changing process and variations of angular velocities with regard to the inertial frame when the HUGs are controlled using the queue formation structure indicated in Fig. 3a. Attitude synchronizing process of HUGs with the branch structure indicated in Fig. 3b is shown in Fig. 5. It can be observed in Figs. 4a-4c and Figs. 5a-5c that the attitudes of the three HUGs all get synchronized and arrive at 0° from different initial conditions. It is also shown in Figs. 4d-4f and Figs. 5d-5f that the orientation of the HUG formation becomes stable when the angular velocities converge to 0 rad/s under the influence of the dissipative torque.
As it is shown in Fig. 4 and Fig. 5, variations of attitude and angular velocities are identical for both HUG 1 and HUG 2 in the two formation topologies. This is because the control torques on the two bodies are the same for the two formation topologies as shown in Table 2, where the first three elements of L 1 (K) are [0 0 1] for both cases. On the contrast, the attitude adjusting processes of HUG 3 in the two topologies are different. Comparison between Fig. 4 Table 2 Lower body arrays of queue formation and branch formation topologies

Topologies
Queue structure Branch structure L 0 (K) 0 Table 3 Pseudocodes of two topologies shown in Fig. 3 Queue structure Branch structure Input: initial attitude of bodies Input: initial attitude of bodies 1 calculate Q 1 , Q 2 , Q 3 2 for i←1 to n 3 T 1 i ←f(Q 1 ); 4 T 2 i ←f(Q 1 , Q 2 ); 5 T 3 i ←f(Q 2 , Q 3 ); 6 if T 1 i ≠0 and T 2 i ≠0 and T 3 i ≠0 7 solve Dy/dt,  and Fig. 5 shows that the amplitudes of attitudes and angular velocities of HUG 3 are higher in the queue formation than that in the branch formation. This difference is caused by the different control structures. In the queue structure, HUG 2 is the lower body of HUG 3, while in the branch structure HUG 2 and HUG 3 are at the same level and their lower body is HUG 1. In the formation orientation control, the control torque is decided by the attitude difference between HUGs. The effect of the queue structure on the control torque on HUG 3 can be considered as an "accumulation effect" which is bigger than the effect of the branch structure. Note that the bigger the effect is, the more energy would be cost during the attitude adjustment. This result may suggest that the branch structure with lower formation levels is better than the queuestructure for it can reduce the energy consumption in HUG formation control.
Comparison between Fig. 4 and Fig. 5 also shows that the attitude synchronization of the HUG formation takes more time for the queue structure than for the branch structure with the same control conditions. In Figs. 4a-4c, the roll, pitch, and yaw angles come to stable states at about 260 s, 400 s, and 380 s, respectively, while in Figs. 5a-5c, the angles become stable at 160 s, 270 s, and 300 s, approximately. The difference is also caused by the formation topology. In the formation, orientations of the HUGs are affected by the conservative torques. The attitude synchronization process may be lagged behind since the adjusting effect on HUGs is constrained by their lower bodies, which are also decided by formation topology. As indicated in Fig.  3, the queue structure topology has four levels and the branch structure has three levels. The 'lag-behind effect' would be larger for the first formation structure since HUG 2 and HUG 3 belong to the same level in the second formation scheme. This result suggests that attitude synchronization could be accomplished more easily for HUG formation with a lower topology level. XUE Dong-yang et al. China Ocean Eng., 2017, Vol. 31, No. 2, P. 248-255 253 The numerical simulation of attitude synchronization is conducted in MATLAB. In the numerical simulation, the lower body array L n (K) is treated as a matrix, which can also be regarded as a switching element to achieve the calculation of the two formation topologies by the matrix reassignment. The Kane's equation can also be readily rewritten for different formation topologies. This suggests that the Huston's method is a suitable way in HUG formation control where formation topology changes frequently.

Conclusion
This paper presents an attitude synchronization approach for a fleet of hybrid underwater gliders (HUGs) using Kane-Huston method. The work is an extension of Yang et al. (2011). The multi-HUG formation is regarded as a virtual multi-rigid body system. The connection between bodies in the multibody system is represented by artificial potential fields and the formation topology is described by a lower body array. Orientation of the HUGs in the formation is controlled by the conservative torque generated by the orientation potential fields and the dissipative torque associated with vehicles' angular velocities. Multibody dynamic modelling has been adopted to analyze the dynamic process of the HUG formation. The dynamic equations are established based on Kane's equations. Numerical calculation has been carried out to simulate the attitude synchronizing process of multi-HUG formation with two different formation structures. The results indicate that the attitude coordination may be influenced by formation topology. Formation structures with a lower topology level should be used during the formation control scheme design to achieve a better control effect.