Modeling and control of a hexacopter with a passive manipulator for aerial manipulation

In this paper, a multi-propeller aerial robot with a passive manipulator for aerial manipulation is presented. In order to deal with the collision, external disturbance, changing inertia, and underactuated characteristic during the aerial manipulation, an adaptive trajectory linearization control (ATLC) scheme is presented to stabilize the multi-propeller aerial robot during the whole process. The ATLC controller is developed based on trajectory linearization control (TLC) method and model reference adaptive control (MRAC) method. The stability of the proposed system is analyzed by common Lyapunov function. Numerical simulations are carried out to compare the ATLC with TLC controller facing collision, external disturbance and changing inertia during an aerial manipulation. Experimental results prove that the developed robot can achieve aerial manipulation in the outdoor environment.


Introduction
In the recent years, aerial manipulation on multi-propeller aerial robot (MAR) has received extensive attention and rapid development worldwide. Due to the unique characteristics and low-cost advantages, MARs have been used for different applications, such as aerial manipulation, construction, and transportation in the early days of their appearance.

Background
The early aerial manipulation experiments are usually carried out in indoor environment under motion capture system. In   1 School of Mechanical Engineering and Automation, Robotics Institute, Beihang University, Beijing, China recent years, outdoor aerial manipulation gets further attention from researchers. The competition named MBZIRC 2020 requires a team of MARs and unmanned ground vehicle (UGV) to collaborate to fight the fire and build a wall in indoor and outdoor environment [1].
The aerial manipulation process usually includes manipulating object, transportation, and placing object part. An MAR for manipulation consists of a multi-propeller rotorcraft and manipulating devices [2]. Usually, multi-degree of freedom (DOF) aerial manipulators, grippers, cables/tethers, and other similar equipment [3] are selected as manipulating devices [4]. For an aerial transportation, cables and tethers [5,6] are popular transporting device because they are cheap and easy to implement for researchers. Usually researchers tie the object to the cable or tether before experiment. Also some companies launch the MAR express project by utilizing the landing gear like Amazon [7] and SF Technology [8] in a relatively simplified way. However, this kind of transporting device is not suitable for the scenarios that urge the MAR to pick the object during aerial manipulation. The grippers and manipulators are the most widely used manipulating devices when picking up or placing the objects. The MARs with grippers are limited by the workspace while manipulators' weight is a negative factor for maneuvering. Also some researchers utilize the the gripper [9,10] as the manipulating device, but the workspace is limited while too small distance between MAR and ground will lead to severe ground effect. In the robot competition named MBZIRC 2017, rigid rods are adopted for the picking device by some teams [11,12] while the workspace is still not big enough. A parallel mechanism and an electromagnetic end-effector are used to realize the building in MBZIRC 2020 [13].
During an aerial manipulation, dynamic stability is challenging because the MAR should deal with the collision, external disturbance, changing inertia and underactuated characteristic [14][15][16]. The MAR can be regarded as a rigid body during the transportation when the grippers or landing gears are used as the manipulating devices, because the payload remains relatively static to the MAR. The dynamics changes when the cables/tethers or manipulators are applied during the manipulation.
For an aerial manipulation, most systems, including multi-propeller aircraft and manipulating devices, use the proportional-integral-derivative (PID) control at an earlier time [10,17]. With the development of technologies, advanced systems make use of hierarchical [18] and hybrid controls [19] that are more suitable for complex conditions. These control methods separate the control of an MAR and the control of a manipulating device, and then fuse them to control the entire system. The control of multi-propeller aircraft adopts PID [9,10,17], adaptive [19,20], trajectory linearization control (TLC) [21], backstepping [22], slidingmode [23], and other methods, whereas impedance control [24] and visual servoing control [25,26] are the most common methods for the manipulating devices. TLC is an effective nonlinear control methodology based on strict theories in differential algebra and differential geometry [27]. In TLC, the tracking error is linearized dynamically along the nominal trajectory to obtain the linearized system, which is different from the feedback linearization control. TLC method combines open-loop inverse control and closed-loop feedback stability regulation, which has good dynamic characteristics and robustness [28]. So it's very suitable for MAR's aerial manipulation scenarios.

Contribution
The contributions of this paper can be summarized as follows: On the one hand, we analyze the dynamic modeling of the hybrid system for an aerial manipulation. Based on the dynamic model, the MAR is stabilized by a cascade controller of TLC and model reference adaptive control (MRAC) method called adaptive trajectory linearization controller (ATLC). The MRAC is applied in actuator dynamics to deal with the external forces from environment and inaccurate modeling caused by the swinging passive manipulator, which is designed by Lyapunov method. The stability of the proposed switched system is analyzed by Lyapunov method. The adaptive controller in ATLC can deal with the disturbance and the change of stages during the aerial manipulation. The TLC is applied because of its effectiveness and simple structure. In [21,29], an MAR with dual active arms is stabilized by a controller designed by TLC method. But it does not consider variable inertia during aerial manipulation. Also in this paper, the passive manipulator puts forward higher requirements on the controller.
On the other hand, the simulations and experiments are carried out based on the prototype with the passive manipulator as shown in Fig. 1. Comparing with active manipulator, it is lighter because of the underactuatation. In [28], an MAR with one active arm is stabilized by TLC method but it does not realize real-world experiments. In this paper, the simulations' results show that ATLC is more stable and with more accurate tracking than TLC dealing with the collision, external disturbance, changing inertia and underactuated characteristic during the aerial manipulation.The ATLC is better than TLC because the adaptive controller can deal with the disturbance and the switching system can deal with the stages' change during the aerial manipulation. Field experiments are carried out to prove the feasibility of the ATLC controller for an aerial manipulation.And comparing with the TLC controller for aerial arm waving experiment in [21], ATLC performs better than TLC in control accuracy.
The remainder of this paper is composed of five sections: Structure and mechanism design of the proposed MAR is presented in Sect. 2. In Sect. 3, the dynamic modeling and controller design of the MAR is presented. Section 4 describes the simulation and experiments. Finally, Section 5 provides some concluding remarks and areas of future work.

Structure of MAR
The configuration of MAR is illustrated in Fig. 2. It is consist of a hexacopter and a passive manipulator. The manipulator Fig. 2 Configuration of MAR. E stands for the earth-fixed frame while B denotes the hexacopter's body frame.The origin of the frame II is in the joint. The Y -axis of II is parallel to the Y -axis of B . The X -axis of II is along the second rod. The Z -axis is perpendicular to the X I I O I I Z I I plane and follows the right-hand rule. l 1 and l 2 are the length of two arms respectively. m 3 is the mass of the end-effector. T i and Q i are the thrust and torque produced by the ith propeller. The angle between the first rod and the second rod in the X B O B Z B plane is written as ϕ l2 , while the angle around the Z B axis is θ l2 includes two joints, two rods, and an electromagnetic endeffector. The joints are both Hooke joints and the first arm is attached to the bottom of the frame with a damper element. In order to avoid the ground effect induced by walls, bricks, and the ground, the second rod can't be too short. The length of the two arms are designed as L = (l 1 , l 2 ) T = (0.4103, 0.5114) T m according to the optimized method considering picking, load capacity, and landing of the MAR [30].

Hybrid system
The overview of the aerial manipulation is shown in Fig. 3. It can be regarded as a hybrid system [31] because the important term mass matrix in the dynamics equation changes during the whole process of an aerial manipulation. The dynamics of system can be written as follows: where M K ∈ R 6×6 , C K ∈ R 6×6 , G K ∈ R 6 are the generalized inertia matrix, Coriolis matrix, and gravity matrix in the different K stages, respectively. ζ = (v, ω) ∈ R 6 represents the linear velocity and the angular velocity expressed in the body-fixed frame. R is the attitude of the MAR's frame. u K = (T , ø MAR ) K ∈ R 4 is the control input of the system, T is the total thrust and ø MAR is the torque produced by the propellers. H ∈ R 6×4 stands for the control allocation matrix of the MAR. When K = 1, the system is in take off stage. The mass and inertia do not change significantly because of the ground support; then we consider M 1 = M MAR , G 1 = G MAR .The subscript M AR stands for the variable in a standard initial configuration.
When K = 2, the system is in free flight stage and is the inertia tensor of the first rod and buffer device because the first rod has no relative rotation with the frame. The mean inertia tensor of the second rod and the end-effector M B l 2 (t) can be expressed to describe the change of the inertia tensor of this process as , and ε freefight is the motion epoch. The second arm of the model can be regarded as a pendulum in 3-dimensional space. Its motion can be divided into two sections: one is a swing along the XOZ plane, the other is a uniform circular motion along XOY plane. Then M B l 2 (t) can be expressed as where m l 2 and I II l 2 are the mass and inertia expressed in the II frame, respectively. R B II (t) is the rotation matrix between II and B , r l 2 (t) is the origin of the II expressed in B . And E 3 is the identity matrix.
When K = 3, the system is in pick and raise stage. The inertia is changing during this process and it will be stabilized by the ATLC controller, and inertia can be expressed where ε pick is the picking time. M B l 2 and M B brick are generalized inertia matrix of the second rod and the target brick expressed in B , respectively. When K = 4, the system is in fly with payload stage. The inertia is similar to the free flight stage except for payload. It can also be expressed by the mean generalized inertia tensor When K = 5, the system is in a construct stage. The inertia is similar with the pick and raise stage. It can be expressed Similarly, ε construct is the construct time. The entire aerial manipulation process of MAR can be regarded as a hybrid system because the dynamics of the entire system changes with the mode switching as shown in Fig. 3. The dynamic model has a significant jump when the brick is picked or constructed, it can be confirmed in Sect. 4. The system consists of two parts, namely, a continuous variable system and a discrete event system [31]. In each sub-stage, the equation of state is continuous. Thus, each sub-stage is a continuous variable system. In the switching state of the work mode, discrete events drive the change in mode. A continuous dynamic time evolution and discrete time variation are combined to form the entire hybrid system.
Define the switching coefficient matrix S(K ) ∈ R 6 as where and ε ∈ (0, 1) is the completeness of the manipulation or construction phase.
Then the M K can be further expressed as where The upper and lower elements of S(K ), respectively, represent the influence of the manipulator and the target on the overall inertial parameter. The expression of C K (ζ )ζ can be expressed in detail by Lagrange method as where r C (t) is the center of mass (COM) expressed in B , I K is inertia matrix of the generalized inertia matrix M K , and m k is the total mass of the system at all stages. And the gravity matrix G K (t) is

Passive decomposition
The dynamics of system (1) can be decoupled utilizing passive decomposition method [32] aṡ where p is position of COM expressed in E . Then the above equation can be transformed from the body frame to the earth-fixed frame as follows [28]: where g is the gravity acceleration, and e 3 = (0, 0, 1) T is reversed along the Z -axis of the body system. S E is part of Δ expressed in (2). Through this part of the decoupling, we can design the cascade TLC controller structure corresponding to the rotation subsystem and the transition subsystem.

TLC controller
The rotation subsystem and transition subsystem are designed by TLC method [28]. Both of them consist of two cascade TLC sub-controllers. As shown in Fig. 4, a TLC controller is made up of two parts: A dynamic pseudo-inverse of an openloop controlled module, where a nominal control input is generated based on the desired system output. And the other part is a closed-loop tracking error stabilization controller for stabilizing the system so that the system has certain response characteristics.
Assuming that the tracking error of the system is small, the nonlinear tracking error can be linearized along the nominal trajectory. The partial derivative coefficient matrix can be found in [33]. And the TLC structure can be stabilized by linear time-varying control law u TLC = K TLC (t)x TLC . The control coefficient matrix K TLC (t) can be obtained by PD spectrum theory of linear time-varying system [27].

MRAC controller
During the process of an aerial manipulation, a change in the dynamics is involved, which is caused by the impact of disturbance or manipulation target. We add an MRAC subcontroller to overcome these series of effects. The process can essentially be considered as a gain [19], namely an MRAC system with adjustable gain. It is designed utilizing Lyapunov method as shown in Fig. 5.
We consider the simplified actuator dynamics as a firstorder system, whose parameters are identified by a 6-axes force/torque sensor. In addition, we consider extracting generalized force F d = f d τ d T as six separate onedimensional inputs u mrac (t). They are independent of each other so they are not coupled. The dynamic equation of an open-loop system can be written as follows: where e mrac is the error between the referenced model output y m and plant output y p , k a is input-output curve parameter of the measured motor, and k a > 0. In addition, β mrac is the adaptive gain and k p is the influence parameter from the external disturbance. Let Consider the following Lyapunov candidate function: where λ mrac > 0 is a positive constant. Then, By observing the dynamic equation of the open-loop system, a deformation can be obtained. e mrac = −k a e mrac + k a k mrac u mrac (t) . Evidently, is always valid. In addition, −2k a e 2 mrac is negative definite when e mrac = 0. To ensure Lyapunov stability, 2e mrac k a k mrac u mrac (t) + 2λ mrac k mrackmrac = 0, (15) and when simplified, the following is obtained: Consider the impact and disturbance in the process of manipulation, we need to design the parameter k p in the adaptive controller. And the momentum-based disturbance estimator is built as follows: [14] where est = f boldsymbol ,est τ ,est T ∈ R 6 is the estimation of generalized disturbance caused by collision and modeling error, K est ∈ R 6×6 is positive-definite diagonal matrix, and Ξ = M K ζ is generalized momentum. We design the k p as where F = f τ T ∈ R 6 is generalized force and k p0 is a constant to be designed. Differentiate the (10); theṅ Combining (16), (18) and (19), Remark To ensure the stability of the robotics in the realworld, usually the produced generalized force F is bigger than the generalized disturbance est , which means that the exponential− est / F ∈ [−1, 0].

Stability of the switched system
The entire controller is presented in Fig. 6. The transition system controller and rotation system controller are both cascade TLC structure. Similar to the rotation system controller, two cascade TLC sub-controllers formed the transition system controller. Each controller includes the dynamic inverse of the outer loop and the PI stabilizer of the inner loop. They output velocity, force, angular velocity, and torque, respectively. And the attitude is output by transition system allocation. An adaptive sub-controller is introduced to overcome the disturbance in dynamics. In order to deal with the switching and jumps between different stages, switching controller is designed. Define a following state equation: where q 1 ∈ R 6 is the generalized error of position and attitude, ζ mrac = (v mrac , ω mrac ) ∈ R 6 represents the linear velocity and the angular velocity expressed in the body-fixed frame caused by F mrac = f mrac τ mrac T , a 1 > 0 is a scalar parameter. Transform and differentiate it, we can geṫ Consider the following Lyapunov candidate function: Differentiate it along time and substitute (20) intȯ Fig. 6 Entire controller of the MAR system. The subscript d represents desired/command value while the value without subscript is the sensed value.
The transition system and rotation system controller are designed by TLC method, the adaptive system controller is desired by MRAC method and chosen with Lyapunov method. The switching and jumps are handled by switching controller As discussed before, M K is nonsingular and the ζ can be derived from the dynamics equation (1) aṡ Define another scalar parameter a 2 > 0 and construcṫ where q T 1 q 2 = q T 2 q 1 is a scalar. Design a following control law: Substitute (27) into (26) and we geṫ The equation (28) does not include the parameter K . It means that in every stage the system has the positive definite common Lyapunov function V 2 (q 1 , q 2 ) with the negative definite derivativeV 2 (q 1 , q 2 ). And the equilibrium point is q 1 = q 2 = 0 3×1 .Then the proposed system is stable [34].
The main feature of the new ATLC controller is a new adaptive subsystem used in dealing with inertia changes in the system and external disturbances. The development from TLC to ATLC is shown in simulation and the feasible of ATLC is presented in real-world field experiments; they are introduced in Sect. 4.

Simulation of ATLC vs TLC
The mentioned dynamic model and controller have been implemented in MATLAB/SIMULINK 2019b. Consider a following desired trajectory of the given MAR system: The results under TLC controller is shown in Fig. 7; it is carried out without disturbance or manipulation task. Taking the characteristics of every motor k a = 24.39. The setting parameter is a 1 = 0.35, a 2 = 0.65. And the coefficient of MRAC for this numerical case is Assuming that there are complex external conditions during this manipulation process, we will compare the response results under TLC controller with ATLC controller. First, assuming the MAR collides with the environment for a short time at 4 s. The value is 50 N and the direction is along the positive Y -axis in B . It can be represented by a Dirac delta function. Then from 6 s to 10 s, the MAR continues to be affected by the F-5 wind, and the direction is along the positive X -axis, which can be settled to about 10 N according to aerodynamics. Finally, the MAR picks the object at 11 s, and places the object at 17 s. The weight of the object is 0.3 kg, Fig. 7 Simulation results of TLC controller without disturbance or manipulation task. The red line is the desired trajectory and the blue one is the sensed trajectory and the time of both picking and placing is 0.5 s. Assuming that the relative speed before picking is 10 m/s, the simulated force on Z -axis can be calculated according to conservation of momentum. The above process can be expressed as The disturbance f dis (t) acts on the end-effector and derived disturbance wrench is calculated by the MAR model.And ε is a Heaviside function. Also, the quality change of MAR during this process can be expressed as follows: The controller about roll angle achieved obvious stable improvement around the 4 s dealing with collision problem. The roll angle is chosen to prove the ability of controller because the direction of the external force has the biggest influence on this parameter. The result is shown in Fig. 8. The controller about pitch angle realize stabilization during 6-10 s facing the simulated wind resistance. Similar to the roll angle stabilization, pitch angle is mainly affected during this period and the result is shown in Fig. 9. A big fluctuation appears at the beginning of the response because the command is a sine curve. It will be much smoother if the command is linear. Comparing with the TLC controller, the ATLC controller makes 42% improvement of degree overshoot error and 32% improvement of peak time error around the beginning of the wind, 255% improvement of degree Fig. 11 Simulation results of ATLC controller with collision, wind resistance, and manipulation task. The red line is the desired trajectory and blue one is the sensed trajectory overshoot error, and 876% improvement of peak time error around the end of the wind. The manipulation task including picking and placing a brick mainly affects the MAR in the Z -axis of E . And in order to keep stable, the MAR usually hovers at picking and  . 12 Hardware of the experiment platform. a Camera is mounted in the hardware system for the future autonomous manipulation work. But the visual information is not involved in this paper. b The area of the sector represents the mass proportion of each part in the total system. And the weight of different intervals is distinguished by different colors Fig. 13 Aerial manipulation experiment. The process of on ground, take off, freeflight, pick, transport, and construct are shown in the same scene. More details about experimental video can be found in https://youtu.be/ 1aRrelrtfoU placing time. So the response can be compared in B . The command and response are enlarged in Fig. 10. The steadystate error with TLC controller is 0.344 m while the steadystate error with ATLC controller is 0.281 m. ATLC controller performs 22.4% better than TLC controller in dealing with the manipulating a brick task.
The entire tracking result is shown in Fig. 11. In conclusion, the ATLC performs significant improvement compared with TLC during an aerial manipulation with three typical cases: collision, wind resistance, and picking or placing. The comparison of simulation results are summarized in Table 1.

Hardware of the experiment platform
To further prove the feasibility of the controller and the proposed MAR system, an MAR is built to carry out the corresponding experiments. Experiments are conducted to verify the adaptability of the controller to the dynamic inertial parameters.
The manipulator consists of two joints, two rods, and an electromagnetic gripper. A minor IMU is attached to the second rod to get the attitude. The total weight of the passive manipulator system is 225 g while the suction power is 300 N. It is very important to evaluate the weight of MAR especially in a manipulation task because the thrust of the motors are limited. A DJI F550 frame is adopted to accelerate the construction of the platform, while DJI E310 is selected as A custom-made flight controller based on an STM32F427 ARM chip is applied while another MCU based on an STM32F103 ARM chip is used to process the visual data and the RTK data. The electromagnetic gripper is mounted with a hall to detect the electromagnetic surface. All information is transmitted to the ground control station (GCS) through 5G-wifi module of the onboard computer NVDIA Jeston TX2. The whole system is powered by a 3S Ace 5200 mAh battery. The MAR weighs 2,755 g including the battery. The hardware frame can be found in Fig. 12 and the overall of the MAR can be found in Fig. 1.

Experiments for aerial manipulation
Field experiments are carried out to verify the feasibility of the ATLC controller which are more complex than the controlled laboratory experiments. An aerial manipulation experiment is shown in Fig. 13. And the picking experiment, transportation experiment, and construction experiment can be found in attached video. The object weighs 243 g with a ferromagnetic surface.
The attitude responses during the aerial manipulation with ATLC controller recorded by the GCS are presented in Fig. 14. The fluctuation of the roll angle and pitch angle are kept within ±5 • during the manipulation process, and the yaw angle is kept within ±1 • . The picking and placing happen around 60 s and 110 s.
As illustrated in Fig. 15, the picking happens around 60 s and placing happens around 110 s affecting the MAR in the Z -axis. An obvious position change occurs while the total thrust jumps at picking and placing time as shown in Fig.  15 and Fig. 16. The total force jumps from 28 N to 34 N at the picking time, and it returns to 28 N after releasing the brick. The brick is 243 g but the increased thrust is 6 N because the MAR needs more force to complete the lifting; also collision occurs in the Z -axis even the MAR achieves picking and placing as smoothly as possible. We can also find the changes at 60 s and 110 s in PWM commands of the motors in Fig. 17.  Attitude error and position error during the aerial manipulation with ATLC controller. a The red line is roll angle error, the blue line is pitch angle error, and the green line is yaw angle error. b The red line is X -axis error, the blue line is Y -axis error, and the green line is Z -axis error As shown in Fig. 18, the disturbance force and wrench from the external disturbance estimation block are presented. From the picture, we can find that the disturbance of the external force is within the range of ±2 N except for 60 s and 110 s. The disturbance is caused by the random wind disturbance and the inaccurate modeling of the passive manipulator; however, it is bounded as a whole. Around 60 s , there is a peak value lasting for 6 s. The peak occurs due to the contact between MAR and the ground through the target object. And an upward force occurs (the positive direction of the Z -axis is downward). Also, there is a peak value lasting for 2 s around 110 s, which is caused by the construction of MAR. MAR is also subject to upward force at this time. In the whole process, there is no movement along the yaw direction. We can find that the disturbance wrench along the Z -axis is smaller than X -axis and Y -axis.
The attitude errors and position errors are shown in Fig.  19. In the Fig. 19a, we can find that the attitude errors are almost within ±3 • except for some peak time in roll angle (still within ±5 • ). It happens because the onboard sensors are not absolutely arranged symmetrically in the X -axis (roll angle). In the Fig. 19b, obvious peaks occur around 60 s and 110 s in Z -axis error for picking and placing happen. Otherwise, the position errors are within ±0.5 m. The accurate attitude and position tracking are very important for the precise manipulation work in outdoor environment without motion capture system.
Experimental results prove that the ATLC controller can stabilize the MAR proposed in this paper during an aerial manipulation task. The MAR can achieve accurate manipulation with collision, disturbance, and swinging rigid payload.

Conclusion and future work
This paper presents an MAR consisting of a hexacopter and a passive manipulator for aerial manipulation. The passive manipulator is utilized because it has larger workspace and is more adaptive to inclined surface than grippers while being lighter than active manipulator. Considering inertia changes during aerial manipulation, the whole process is modeled by hybrid system method and dynamics of the MAR is decoupled. The MAR is suffered from the collision, disturbances and changing inertia caused by passive manipulator, the ATLC is designed to stabilize the system. Basing on TLC method and MRAC method, the ATLC controller is designed by Lyapunov method in cascade structure. The stability of the proposed switching system is analyzed utilizing common Lyapunov method. In the simulation, the ATLC achieves 4 times better than TLC averagely in the attitude control and approximate 1.2-times better in the position control. The experimental results prove that the ATLC controller can effectively deal with changes in the inertial parameters and counteract disturbances. The proposed MAR works well with the ATLC controller during an aerial manipulation.
In the future, more work will be carried out on the proposed MAR system, and the fully autonomous flight experiments of aerial manipulation under visual servoing will be carried out. At the same time, experiments using an active manipulator will be further developed to compare with the passive manipulator.