Design and Implementation of a Fully-Actuated Integrated Aerial Platform Based on Geometric Model Predictive Control

Unlike individual unmanned aerial vehicles (UAVs), integrated aerial platforms (IAPs) containing multiple UAVs do not suffer from underactuation and can move omnidirectionally in six dimensions, providing a basis for constructing aerial manipulation platforms. Compared to single UAVs, multi-UAV IAPs are also advantageous in terms of payload and fault-tolerance capacity, making them promising candidates as platforms with integrated-response, observation, and strike capabilities. Herein, an IAP structure design containing three sub-UAVs connected in a star-like configuration is presented. This form of integration enables the IAP, as a whole, to simultaneously adjust its position and attitude in six dimensions. The dynamics of the overall system of the IAP are modeled. On this basis, an overall system controller is designed. To simplify control, based on stability of cascaded system, the rotational motion of the sub-UAVs is treated as a inner-loop subsystem, whereas the overall motion of the IAP is seen as a outer-loop subsystem. Because the configuration space of the sub-UAVs is non-Euclidean, a controller is designed for the outer-loop subsystem based on model predictive control on the manifold. Subsequently, the stability of the closed-loop system is demonstrated. Fieldbus technology is employed to design a real-time, scalable communication architecture for multiple sub-UAVs, followed by the development of a principle prototype of the multi-UAV IAP that consists of hardware and software systems. The effectiveness of the IAP design and control method is validated through simulation and real-world prototype-based tests. In the simulation and real-world tests, the proposed methodology can make the IAP system converge to the desired configuration at the presence of large initial configuration error. The same test scenario cannot be finished by a baseline PID controller. The advantage of the proposed control scheme in dealing with state and input constraints is shown via such tests.


Introduction Motivation and Background
Unmanned aerial vehicles (UAVs) are expected to play a significant role in future applications [1][2][3][4]. Most conventional UAVs are only equipped with observation capabilities. In recent years, aerial manipulators with manipulation and response capabilities have garnered increasing attention [5][6][7][8][9]. These robots can expand the functions of aerial vehicles (AVs) and enhance the mobility and response range of conventional task-oriented robotic systems, showing promise as an important tool for future combat operations.
Most aerial manipulators rely on micro AV (MAV) platforms. To improve energy efficiency during flight, the available MAVs are generally underactuated, that is, they are actuated by a three-dimensional (3D) torque and a one-dimensional thrust. This underactuated design allows MAVs to be controllable and energy-efficient during flight. However, underactuated MAVs face challenges in tracking any six-dimensional (6D) trajectory in • A control scheme is carefully designed such that it is singularity-free and can deal with the non-Euclidean configuration space, state and input constraints, hence is suitable for the IAP system. The control scheme is designed based on model predictive control and the dynamics of the IAP. • The stability of the IAP under the control of the proposed MPC-based controller along with the existing flight controller of each individual sub-UAV is proved. This provides theoretical basis for the development of the IAP. • By developing the hardware and the software system of the IAP, the proposed control scheme is successfully implemented in a prototype of IAP. To the best of the authors' knowledge, this is the first time that the geometric model predictive control-based control scheme was successfully implemented in the real IAP prototype. The advantage of the proposed control scheme is shown through the comparison.
The remaining of the paper consists of four sections. The configuration and the dynamic modeling of the IAP is proposed in Section 2. The controller design and the convergence analysis are presented in Section 3. The simulation and real-world tests of the proposed control scheme are shown in Section 4.

Configuration and Dynamic Modeling of IAP
A dynamic model of the IAP is established using the following procedure. First, a world coordinate system {E} fixed to the ground is constructed, followed by the establishment of a coordinate system {F 1 } fixed to the IAP at its overall center of mass. Then, a coordinate system {F 2 } fixed to the MP and with its origin located at the center of mass of the MP is constructed. Finally, for each sub-UAV, a body coordinate system {O i } (where i = 1, 2, . . . , n) fixed to it at its center of mass is established. Figure 1 is the schematic of an IAP with three sub-UAVs. In Figure 1, the three sub-UAVs connect the mission-platform via spherical joints. As have stated, the COM of each sub-UAV coincides with the center of spherical joints. Therefore, each sub-UAV can rotate around the spherical joints freely, and each sub-UAV is an under-actuated UAV, which means that it can generate a thrust and three torques. Because of this configuration, the thrust vector provided by each sub-UAV can be adjusted by adjusting the thrust magnitude and the attitude of each sub-UAV. By coordinately adjusting the thrust vector of each sub-UAV, the motion of the entire IAP can therefore be adjusted. Thus, a dynamic model of the system can be derived from the Newton-Euler equation: where V 0 := (v 0 , ω 0 ) ∈ R 6 (where v 0 ∈ R 3 is the linear velocity in {E} and ω 0 ∈ R 3 is the angular velocity in {F 1 }) is the velocity of the overall system at its center of mass; M ∈ R 6×6 , C ∈ R 6×6 , and G ∈ R 6 are the mass, Coriolis, and gravitational acceleration matrices, respectively; R 0 ∈ SO(3) is a rotation matrix of the MP; I is the identity matrix; and u 0 ∈ R 6 is the resultant force and torque generated by the sub-UAVs described in {F 1 }. The mass, Coriolis, and gravitational acceleration matrices are expressed as follows: where m i denotes the mass of sub-UAV i, m 0 represents the MP's mass, M 0 is the rotational inertia of the MP with respect to {F 2 }, g is the gravitational acceleration, w i ∈ R 3 is the position of sub-UAV i in {F 2 }, w c ∈ R 3 represents the coordinates of the center of mass of the whole system in {F 2 }, and e 3 = (0, 0, 1) T . Here, vector a is defined as follows: a = (a 1 , a 2 , a 3 ) T ∈ R 3 . Letâ be the corresponding antisymmetric matrix of a, as shown below:â   A dynamic model of the system can be obtained using the equations below: where p 0 ∈ R 3 is the position of the center of mass of the overall system in {E}.
Because the connecting spherical hinge is located at the center of mass of each sub-UAV in the IAP, the rotational motion of each sub-UAV follows the equations below: where R i ∈ SO(3) and ω i ∈ R 3 are the attitude rotation matrix and angular velocity of sub-UAV i, respectively, and τ i ∈ R 3 is the input torque of sub-UAV i (where i = 1, 2,. . . , n).
Here, T i ∈ R is used to denote the thrust generated by sub-UAV i. Based on the quadrotor structure, the thrust acting on the body coordinate system and points in the negative direction of R i e 3 . Therefore, the thrust vector generated by each sub-UAV can be expressed as: Based on the results presented above, the input u 0 in Equation (1) can be expressed as follows: where l i is a distance defined by w i − w c , ( i = 1, 2, . . . , n). Equations (1)-(4) collectively constitute the dynamic model of the IAP system. For the IAP system, as the input of each sub-UAV is bounded, the 6D wrench acting on the entire IAP system is also bounded. We express such boundedness as,

Overall Architecture
Based on the dynamic model, the attitude motion of each sub-UAV in the IAP is unaffected by the overall motion of the IAP. Therefore, the stability theorem of cascaded system is adopted to design a controller [19]. Specifically, the motion of the overall system is viewed as a inner-loop subsystem, whereas the attitude motion of each sub-UAV is treated as a outer-loop subsystem. As a result of this separation, the motion control of the overall system constitutes the outer loop of the attitude control of the sub-UAVs. Mature attitude motion control methods for UAVs are available and can be used directly. Here, focus is placed primarily on the motion control design of the outer loop of the IAP. The overall closed-loop system of the IAP can be proven to be stable under the action of a controller designed using this approach.
The overall controller of the IAP is composed of model predictive control (MPC) on the manifold. MPC outputs a 6D force/torque u 0 . The first three components of u 0 are force vectors, while its last three components are 3D torque vectors. Figure 2 shows the overall architecture of the controller. In the overall control architecture, the outerloop is a Model Predictive Control which outputs the 6D desired wrench u 0,d ∈ R 6 . The 6D wrench is then transformed into the commanded attitude R i ∈ SO(3) and thrust T i ∈ R of each sub-UAV via the control allocation. By adopting such an architecture, the attitude controller of each sub-UAV is kept, while the outerloop MPC of the entire IAP can be used to deal with the state and input constraints of the IAP. The architecture supports the faster design and the implementation of the new controller while preserving the existing controllers. Figure 2. Architecture of the overall IAP controller based on geometric model predictive control.
In outer-loop control, the 6D wrench generated by position and attitude control needs to be converted to the thrust vectors of the sub-UAVs. The input and output of this control, distribution, and mapping process are the control quality u 0 ∈ R 6 and the thrust vector of each sub-UAV, respectively. The following equation can be used to determine the thrust vector of each sub-UAV:

Outer Loop of the MPC Controller
The discrete time-series method is often employed to solve MPC problems. Given a sampling interval δ t , a sampling time series t k (where k ∈ N) can be defined. At sampling instant t k , MPC is established via the solution of the optimal control problem as: are all positive-definite functions (their definitions are described in detail later in Propositions 1 and 2), X × V are the admissible state set, and Ω r and Ω t are the sets of terminal constraints for the position and attitude channels, respectively, which are used to ensure the stability of the overall IAP system under MPC and the solvability of MPC (they are similarly described in detail later in Propositions 1 and 2).

Terminal Set Constraints and Terminal Control of Attitude Motion
For the attitude control of the slow-time varying system, the angular velocity command is defined as follows: where k 1 is a positive constant, and e R,0 = 1 2 (R 0,e − R T 0,e ) ∨ . Based on Equation (9), a torque control law τ 0 is designed as follows: where k 2 is a positive constant, and ω 0,e = (ω 0 − ω 0,d ). Substituting Equation (9) into the equation of motion gives the following: The attitude state of the MP can be expressed using ζ r = (e T R,0 , ω T 0 ) T and ξ r = (e T R,0 , ω T 0,e ) T , and can be reorganized into the following equation: Then, the following proposition can be defined to analyze the feasibility and convergence of attitude control.

Proposition 1. The equation of attitude motion of the MP is analyzed. A set of terminal constraints in MPC is defined as follows
can be derived from (10). Selecting suitable control parameters allows the following conclusions to be reached for all s ∈ (t k + Γ, t k+1 where q 11 , q 12 , and r are all positive constants.
The proof can be obtained by observing V r and the evolution ofV r . This process is omitted here.

Terminal Set Constraints and Terminal Control of Position Motion
For the position control of the MP, the velocity command is first defined as follows: where k 3 is a positive constant. Based on Equation (13), a feedback control law for force vectors is further designed, as shown below: where k 4 is a positive constant, and v 0, The position state of the MP can be expressed using ζ t = (p T 0,e , v T 0 ) T , and ξ t = (p T 0,e v T 0,e ) T , which can be reorganized into the following equation: The following proposition is defined to analyze the convergence of position control.

Proposition 2.
The equation of position motion of the MP is analyzed. The attitude of the MP is subject to e T 3 R 0 e 3 ≥ L l , where L l is a positive constant that is less than 1. A set of terminal constraints in MPC is defined as follows: where H t is a positive-definite matrix, where h 21 and h 22 are positive constants, and When ζ t (t k + Γ) ∈ Ω t t, Equations (14) can be used to generate a control law for force vectors, F 0 (s), s ∈ (t k + Γ, t k+1 + Γ)]. Selecting suitable controller parameters allows the following propositions to hold for all s ∈ (t k + Γ, t k+1 + Γ)], where q 21 , q 22 , and r 21 are all positive constants.
The proof process is omitted here.

Solvability and Stability of the Closed Loop IAP
Theorem 1. Consider the IAP outer-loop sub-system (1). The controller is the MPC solved from the finite time optimal problem (8). Suppose (8) is solvable at initial time. Then the closed loop outer-loop system is asymptotically stable.
Proof. The solvability of the MPC can be proved recursively. Assume the optimal control problem is solvability at instant t k , and the sulution is expressed as u * 0 (s), s ∈ [t k , t k + Γ]. Then According to definition of the constraints, at next sampling instant t k , the state ζ is always in the terminal region under u * 0 (t k ). In the time interval [t k+1 , t k+1 + Γ], the following solution for (8) can be constructed, where the control u 0,ter = (F 0,ter , τ 0,ter ) is given by terminal control law (10) and (14). From Propositions 1 and 2, it is seen that under the control of u 0,ter defined by (10) and (14), u 0,ter (s) ∈ U for all s ∈ (t k + Γ, t k+1 + Γ], and state will be kept in terminal region at time t k+1 + Γ. Therefore, under the control u 0,ter we have, In summary, we say that u 0, f (s), s ∈ [t k+1 , t k+1 + Γ] is a feasible solution of (8). The solvability of (8) is therefore provided recursively.
The second part is the convergence proof of the system under MPC. For this purpose, define the following Lyapunov candidate, We can derive the difference of V from t k to t k+1 as, We integrateV r +V t + N t + N r in the time interval [t k + Γ, t k+1 + Γ]. Considering Proposition 1 and 2 we have, Substituting (20) into (19) yields, Then we can conclude that the closed outer-loop subsystem is asymptotically stable.

Remark 1.
Theorem 1 only shows the stability of the closed outer loop subsystem. It shows that the geometric model predictive control for the outer loop subsystem is always solvable if it is solvable at initial time. Actually, Theorem 1 reflects the asymptotical stability of the system without considering the attitude tracking error of each sub-UAV, i.e., assuming the tracking error of each sub-UAV is zero. One can imagine that in this case, the equivalent input to the outer loop subsystem is exactly the same with the resultant force and torque generated by the sub-UAVs. This is an ideal condition as the attitude tracking error of each sub-UAV always exists in actual systems. However, for the real system we can conclude that the entire system is stable if the attitude tracking controller of each sub-UAVs is stable.
Then we consider the attitude tracking controller of each sub-UAV. The control vector u 0 is obtained by solving the finite-time optimal control problem at each time, as shown in Equation (8). Here, the attitude control of each sub-UAV is assumed to be exponentially stable. This is easy to realize as there has been mature control technologies for the attitude control of sub-UAVs [20,21]. Then, based on Theorem 1, in conjunction with stability of the system in cascade [19,20], the overall closed-loop system is asymptotically stable, and MPC is recursively solvable at each time.

Remark 2.
It is noted that the cost function in (8) does not reply on any local coordinate of the attitude. Therefore, the controller of the outer-loop system is singularity-free. Moreover, if the innerloop subsystem which reflects the rotational motion of the system is adequate, then the closed-loop entire system is also singularity free.

Simulation System Construction
Prior to real-world flight tests, the closed-loop control of the IAP was first simulated using the Robot Operating System (ROS) in an Ubuntu 18.04 environment. An ROS node was written based on (1) and (3) to simulate the body of the IAP. Perturbations were added to the simulated model of the body of the IAP to mimic uncertain disturbances in real-world flight. The input of the body simulation node consisted of the thrust vectors generated by the three sub-UAVs, and its output comprised the position, linear and angular velocities, and attitude of the MP of the IAP. In the simulation test, the remaining ROS nodes were used as interface nodes to receive commands from a remote control (RC) and transform them into the expected position and attitude of the multi-UAV IAP.

Simulation Results
An integrated platform with six sub-UAVs was simulated under the MPC described earlier in the simulation test. The initial and expected positions of the simulated IAP were set to (2, 0.05, 10) and (0, 0, 0) m, respectively, and its initial and expected attitudes were set to R 0 = exp(0.6( 1 , 0)), respectively. An obstacle (radius 1 m ) was considered at (1, 0.05, 5) m in the simulation test. Figures 3-6 show the tracked position and attitude of the IAP. It is evident that under the geometric MPC described earlier, the IAP reached the expected position and attitude from its initial position and attitude in a stable manner by simultaneously adjusting the attitude and thrust commands for its sub-UAVs. Because the state and input constraints could be satisfied in MPC, the IAP was able to maintain a distance from the obstacle throughout the process, as shown in Figure 3. The position evolution of the IAP is shown in Figure 4. It also shows the position evolves from the initial position to the designed position while keeping distance from the obstacles. The attitude evolution is shown in Figure 5 which indicates the convergence of the attitude from initial value to desired value. As an example, the attitude evolution of sub-UAV 1 is shown in Figure 6. The attitude of the sub-UAV is adjusted according to the output of the outer-loop controller. Due to the varying of the attitude of sub-UAV 1, the thrust vector provided by each sub-UAV is also varying. In summary, the feasibility, correctness, and completeness of the written controller node and overall software architecture were validated using the simulation model, providing a basis for conducting real-world flight tests.    The feasibility, correctness, and completeness of the written controller node and overall software architecture were validated using the simulation model, providing a basis for conducting real-world flight tests.

Development of an IAP Prototype Consisting of Software and Hardware Systems
Based on the results presented above, a principle prototype of the IAP, consisting of software and hardware systems, was developed. The MP of the IAP was integrated with appropriate avionic devices, including a flight control board (FCB) , an on-board computer (OBC) , a RC receiver (RCR) , a Global Positioning System (GPS) receiver , an inertial measurement unit (IMU) sensor , and a data transmission radio (DTR) . The FCB needed to be installed as close to the center of mass of the MP as possible; it was used to receive the commands from the RC and the position and attitude data produced by the sensor fusion algorithm and to transmit the commands, along with position and attitude data, to the OBC through a Universal Serial Bus (USB) serial port. The DTR was primarily used to monitor and record flight status and communicate with the ground station. The main structural components of the IAP prototype were made of a carbon-fiber material. The range of motion of the spherical hinges in the prototype of IAP was 55 • .
Commercially available small quadrotors were used as sub-UAVs in the prototype. Each sub-UAV was equipped with a complete set of avionic devices (e.g., an FCB, a GPS device, an RCR, and an IMU sensor). Therefore, the sub-UAVs could fly in an integrated manner when combined together and individually when separated from each other. The commercially available PX4 open-source autopilot was used to control the flight of the sub-UAVs.
To ensure the required reliable and real-time communication between the sub-UAVs during stable movement of the IAP, a system was developed based on the controller area network (CAN) bus architecture to enable mutual communication between the MP and the sub-UAVs. The OBC on the MP and the flight controller of each sub-UAV are considered nodes mounted on the CAN bus. This mode of communication can ensure reliable and real-time communication and facilitate the expansion of the number of sub-UAVs in the IAP. Figure 7 shows the overall hardware architecture of the IAP. The ROS was used to design a software architecture for the IAP in an Ubuntu 18.04 environment. As a ROS node, the controller ran completely on the OBC installed on the MP. A one-to-many data communication protocol, termed Swarmlink, was also developed for the communication of the IAP. Corresponding encoding and decoding ROS nodes were set on both the MP and sub-UAV ends. Table 1 summarizes the relevant physical parameters of the developed overall IAP prototype and its sub-UAVs.

Real-World Test Results
The position and attitude of the IAP prototype with three sub-UAVs were tracked during tests. The initial and expected position in the real-world test were set to (0, 0, 0) and (−10, −8, −2) m, respectively The initial and expected attitudes in the real-world test were set to R 0 = I and R 0,d = exp(−0.5( 1 , 0)ˆ), respectively. It is noted that there is noticeable initial error in the test. A typical PID controller cannot force the system stable at the presence of such initial error. The admissible input set in the real-world IAP is set as, While the velocity constraint is also configured by the MPC-based scheme as v 0 ≤ 2 m/s. Figure 8 shows images captured during a flight test (the IAP maintains a stable hover at roll and pitch angles of approximately 0 • in the left image and at an inclined attitude in the right image). It is visually evident in Figure 8 that the IAP was able to maintain a hover while changing its roll and pitch angles. Such a maneuver cannot be achieved by conventional underactuated UAVs.  Figure 9 show the tracking test results for the IAP prototype under the control of the proposed control scheme. The commanded attitude trajectory of the IAP was independent of its commanded position trajectory. Figure 10 shows the position trajectory of the IAP under the proposed control. Figure 11 shows the attitude trajectory of the IAP under the proposed control. For ease of display, Euler angles are used in the figure to depict the attitude. From Figures 9-11 it is seen that the IAP converges to the desired configuration under the proposed control. It is evident that the roll, pitch, and yaw errors were all finally within reasonable range. The prototype was tested in an outdoor real-time kinematic GPS environment. As shown in Figure 10, the maximum horizontal and vertical position tracking errors were around 0.6 m, indicating that ideal tracking results were achieved for both attitude and position. It should be noted that there is large initial error in this test. Because the proposed control scheme can deal with input boundedness, the IAP remains stable during the entire flight. A typical PID is difficult to stabilize the IAP in such condition as the controller may output large action which violates the input boundedness. In contrast, our proposed control scheme successfully stabilizes the system as it can deal with the state and input constraints.  The test results show that the IAP prototype was capable of moving omnidirectionally in six dimensions while tracking the position and attitude trajectories in a stable manner, without the need for the position and attitude trajectories to satisfy dynamic constraints. These capabilities are lacking in conventional underactuated UAVs. The real-world test results for the prototype also validate the stability of the designed controller.

Remark 3.
We also test the IAP with the same scenario under a PID controller. However, as the PID controller could not deal with the state and input boundedness, the IAP prototype could not remain stable. Given the initial error, a PID controller may generate saturated action which may make the real-world IAP prototype unstable soon. The advantage of the proposed controller for the IAP in dealing with the input boundedness is thus verified through this experiment.

Conclusions
This study presents an IAP configuration design containing three sub-UAVs, investigates the geometric control of the integrated multi-UAV system, and develops an IAP prototype that consists of software and hardware systems, providing a design and control basis for developing new multi-UAV IAPs. The IAP containing three UAVs connected through spherical hinges can move omnidirectionally in six dimensions and is an ideal choice for an aerial manipulation platform. This configuration can be altered to construct other IAPs. The designed geometric MPC controller can be effectively used to control the motion of the IAP. Its stability is validated both theoretically and experimentally. Our proposed control approach is easy for implementation and is globally effective. Compared with other control scheme, the proposed control scheme is capable of dealing with input and state constraints. Fieldbus technology is employed to construct a low-latency, highreliability mode of communication for the IAP prototype. Subsequent real-world flight tests demonstrate the correctness and completeness of the developed software and hardware systems of the IAP prototype. This is the first time that the proposed control scheme was implemented in a real-world IAP. In future works, the motion planning of the proposed IAP for specific tasks will be considered. Development of IAP with different configurations is also with our interests.