A Hybrid Modelling Approach for Aerial Manipulators

Aerial manipulators (AM) exhibit particularly challenging, non-linear dynamics; the UAV and the manipulator it is carrying form a tightly coupled dynamic system, mutually impacting each other. The mathematical model describing these dynamics forms the core of many solutions in non-linear control and deep reinforcement learning. Traditionally, the formulation of the dynamics involves Euler angle parametrization in the Lagrangian framework or quaternion parametrization in the Newton-Euler framework. The former has the disadvantage of giving birth to singularities and the latter of being algorithmically complex. This work presents a hybrid solution, combining the benefits of both, namely a quaternion approach leveraging the Lagrangian framework, connecting the singularity-free parameterization with the algorithmic simplicity of the Lagrangian approach. We do so by offering detailed insights into the kinematic modeling process and the formulation of the dynamics of a general aerial manipulator. The obtained dynamics model is validated experimentally against a real-time physics engine. A practical application of the obtained dynamics model is shown in the context of a computed torque feedback controller (feedback linearization), where we analyze its real-time capability with increasingly complex models.


Introduction
The field of aerial manipulation has seen a lot of interest in recent years [1,2] with solutions being developed ranging from bridge inspection [3,4], to maintenance [5], to assembly [6], various pick and place tasks [7,8,9], slung load transportation [10], cooperative transportation [11], and even drilling and screwing [12]. UAVs are thus becoming increasingly aware of their surroundings, allowing them to autonomously navigate [13] and interact with the environment.
Some aerial manipulation tasks can be performed with relatively simple static claws [14] with only minimal impact on the carrying platform. However, more complex tasks require the additional degrees of freedom offered by serial link manipulators [15,5,16,7]. A typical AM is shown in figure 1. Those manipulators are often relatively heavy and have a substantial dynamic impact on their carrying, generally, under-actuated flying platform. This impact stems from method and Euler angles. In [26], the authors presented an approach within the Lagrangian framework (employing Euler angles) that handles constraints applied to the end-effector by introducing an additional constraint term to the dynamics equation. In this work, the coupled dynamics, using singularity-free quaternion representation, is derived in detail using the simpler Euler-Lagrange equations, thus offering an appealing alternative (resp. middle ground) to the state of the art.
This work primarily focuses on the dynamics model; therefore, only a very simple PD controller with a bias correction term gets introduced because it does not hide model imperfections. The state of the art with regard to model-based control techniques can be found in [27]. Some highlights include the robust, adaptive sliding mode controller introduced in [18], the variable parameter integral backstepping controller in [6,8], the passivity based controller in [28], the disturbance observer-based control in [25], the hybrid position and force controller respecting constraint conditions presented in [26], and the model reference adaptive control in [29]. The authors of [30] specifically addressed the problem of model uncertainty and unknown disturbances by employing a nonlinear disturbance observer in conjunction with a robust prescribed performance controller. All of those control schemes have the dynamics model of their controlling system at their core whilst being robust towards modeling imperfections.

Contributions
The main contribution of this work is a general-purpose, user-friendly, yet powerful dynamics framework for aerial manipulation that yields a singularity-free, closed-form dynamics model of an aerial manipulator. Although we do not target tilting propeller resp. fully actuated UAV configurations in this work, the results presented herein are general enough to be applicable to those configurations as well (assuming that the reaction torque of the tiling mechanism and its impact on the center of gravity are negligible).
The contributions of this work are thus stated as follows: 1. Providing a hybrid approach with regard to the dynamics modeling of aerial manipulators, by combining quaternion parametrization and Lagrangian mechanics. This is contrary to what most authors do in this field, namely using either Euler angle parameterization in the Lagrangian framework, or quaternion parameterization in the Newton-Euler framework. The approach presented here leads to a relatively generic, singularity-free, coupled dynamics model. Implementing our approach requires significantly less coding than Newton-based methods, thanks to the algorithmic simplicity of the Lagrangian framework. In fact, the bulk of the work consists in implementing the simple kinematic equations, contrary to the Newton-Euler algorithm, which also involves expressing the respective force and torque equations in a recursive manner [17]. 2. The derivation of the kinematics resp. the dynamic model often comes up short in many publications, where the focus is often on control rather than the modeling. This perceived lack of modeling methodology is addressed herein, showing all the required steps and thus providing insights into the normally hidden process, lowing the barrier to entry in the field of aerial manipulation. 3. The introduction of a general-purpose dynamics framework for aerial manipulators based on a URDF [31] description of the system. 4. By making use of the Lagrangian mechanics framework and by refraining from using dual quaternions (dual numbers), in combination with the mapping of all quaternion operations to matrix-vector products, the results presented here are particularly well suited to be implemented with common computer algebra systems (CAS). 5. Two applications of the model are shown. First, in a simulation resp. validation context where the obtained model is validated against an identical reference model simulated in a realtime physics engine. And second, as part of an adaptation of a computed torque controller adopted from classical robotics to work in an aerial manipulation context. 6. Performance figures with regard to the algebraic complexity and the real-time capability are provided for two typical scenarios, namely onboard resp. off-board control using increasingly complex AM. Furthermore, we compare the complexity of the resulting dynamics equations obtained from different methods.

Structure
This work is organized as follows. We start with the kinematics of a single body in the inertial frame, followed by the kinematic equations of an open kinematic chain manipulator with regard to its own base frame. Then, by combining those results, the base of the robot is turned into a floating platform, providing an additional 6-DOF, which results in the complete kinematic equations of the manipulating system in the world frame. The kinematic equations are then fed into the Lagrangian framework to obtain the dynamics equations. It follows the non-trivial mapping between body forces and generalized forces resp. propulsion forces and body forces. Finally, a general-purpose holonomic constraint solver is applied to the unconstrained system to impose the quaternion unit constraint via a constraint force. The obtained dynamics model is then validated in simulation against a real-time physics engine. As the last application, a stabilizing controller is introduced that uses the developed dynamics model to compensate for the nonlinearities present in the AM. This work is concluded by presenting performance figures concerning the algebraic complexity and the real-time capability of the model within the control context. A short introduction to (unit) quaternions is provided in A.

Kinematics
The following section derives the forward kinematics of the combined (coupled) system formed by the manipulator and its carrying, flying platform. Commonly, this is done by introducing a 6-DOF joint between the world frame and the first body. In this work, however, it is more convenient first to describe the kinematics of the two subsystems (drone and manipulator) and then, as the last step, merge both systems together. The drone then acts as the 6-DOF joint.

UAV Body Kinematics
Given a UAV, represented by base link L 0 and the body B 0 in the kinematic chain as depicted in figure 2. Its orientation is described by the quaternion q = (q w ,q v ) ∈ S 3 , and its position in the world frame W is given by the vector The base link's angular velocity WωW L0 ∈ R 3 , expressed in W, relative to W, is tied to the quaternion q via Which can be written in matrix form using (105) Similarly, the base link's angular velocity expressed in L 0 , relative to W can be written as respectively with the help of (104) Figure 2: Example of a URDF-compliant kinematic tree of a branched kinematic chain. The link and joint frames are annotated as L i resp. J i . The body frame B i has an associated mass and moment of inertia. It marks the center of mass of its parent link J i . L 0 marks the base link of the robot (namely the UAV). The edges between the nodes (see detail) represent coordinate transformations between two frames denoted by X A Y .
Since W ω W L0 and L0 ω W L0 are pure quaternions, their vector part can directly be obtained by dropping the first row from (3) resp. (5) yielding the two orthogonal mapping matrices E and G defined by Solving (7) forq and replacing it into (6) reveals the relation between W ω W L0 and L0 ω W L0 : where W R L0 ∈ SO (3) is the orthogonal rotation matrix associated with q.

Kinematics of the Manipulator
In the following section, the forward kinematics of an open-chain manipulator is developed. Although the base link of the manipulator is represented by the UAV, for the development in this section, the base link L 0 is regarded as static and all quantities of motion will be expressed in and with regard to L 0 (figure 2). The development largely follows the procedures found in classical robotics literature [32], [33], [17] and [34] with some changes to the kinematic tree that are required to interface with the Universal Robot Description Format specifications seamlessly.
A generic high-level description of a robotic arm can be stated in form of a URDF, providing all required kinematic and physical properties of a robotic manipulator. Its conventions concerning reference frames, links, joints and parents have been adopted here to facilitate the translation from an AM given as a URDF file to its system dynamics.
The URDF structure is standardized and describes an (open) kinematic chain formed by various types of nodes.
Nodes are defined as either joints J i , links L i or bodies B i ( fig. 2), and are related to each other by homogeneous transformations such as The key aspects of the URDF are summarized as follows (refer to [31] for further details): The transformation of each node i in the tree with regard to L 0 is given by the product of each local transformation A between the node i and L 0 Generally, Li A Bi , Li A Ji are constants defined by the physical configuration of the robot. On the other hand, Ji A Li is a function of the joint position. For revolute joints, Ji A Li (θ i ) designates a rotation around the local joint axis Jin Ji ∈ R 3 , n = 1 (e.g. the local z-axis), with an angle θ i , which corresponds to (108) and can be encapsulated in the homogeneous transformation Notice that although herein the focus is on revolute joints, a completely analogous procedure can be followed for prismatic joints.
The position of each node with respect to L 0 is obtained by the relation The angular velocity of B i relative to its parent joint J i and expressed in J i is denoted as JiωJi Bi ( fig. 3), which can be expressed in terms of its rotation axis and angular velocity JiωJi Bi = Jin JiθJi .
It is clear that the body frame B i , and the link L i must have the same angular velocity since Bi A Li is constant, therefore JiωJi Li = JiωJi Bi . By multiplying with the rotational part L0 R Ji of L0 A Ji , the angular velocity is obtained with respect to L 0 : Using (13), it follows With all angular velocities expressed in the same frame of reference, the total angular velocity (relative to the base link) of each body is obtained by the sum of the angular velocities (15) over all of its ancestors: Rewriting (16) as matrix-vector product then yields the Jacobian for rotation L0 J ω,i for the i-th body in the chain. The k-th element of that matrix is given by Noncontributing joints are thus accounted by a0 contribution.
The linear, sectional velocity of the body L0ṗJi Bi due to the rotational movement Jj A Bi of one of its ancestral joints J j ∈ p(B i ) is given by the tangential velocity where L0p Jj Bi defines the lever formed between the frames J j and B i (see figure 3). By using identity (14) and defining This sum can also be written as L0ṗL0 Bi = L0 J t,iθ , where L0 J t,i is the Jacobian matrix for translation. The k-th element of that matrix is given by Joints that are not an ancestor of B i do not add to its velocity, and their contribution is thus0.
Eq. (19) and (16) can thus be expressed in terms of the Jacobian matrices (17), (21). The total linear velocity is given by with the total angular velocity L0ω Bi of body i being whereθ = [θ J1 , . . . , θ JN ] ∈ R N J is the vector of all joint positions.

Kinematics of the Manipulator attached to the flying Base
In the previous section, the kinematics of the manipulator was derived with regard to L 0 . Attaching the manipulator to a floating base (the UAV here) endows the system with an additional 6 DOF. To account for those 6 DOF, it suffices to describe the forward kinematics of the manipulator with regard to the inertial frame W, knowing that the floating base is located at a positionp from the origin and that its orientation with regard to W is given by the unit quaternion q, essentially inserting a virtual 6-DOF joint in between W and L 0 that is parametrized byp and q.
Looking at the system from W, it is clear that the position WpL0 Bi of each body of the manipulator in W with regard to L 0 is superimposed by the position WpW L0 of the floating base, resulting in the absolute link position Each link's position in W is obtained by rotating it by q, such as: The link velocity is then obtained from the time derivative of its position vector (25) Inserting (25) in (26) yields By multiplying the left resp. right side of the last two terms in (27) by 1 = q ⊗ q * the following expression is obtained: Applying (4) yields Evaluating the two pure quaternion products using (92) then results in which further simplifies to Using (6), (7), (8) and (106), it can further be transformed into Introducing (7) and (22) showing the contributions of the moving base and the rotating links. In particular, the last term shows the component of the translational velocity due to the lever L0pL0 B,i and the angular velocity of the base. The angular velocity of the bodies is simply the superposition of their own angular velocity and that of the base link in the inertial frame:

System Dynamics
Using the kinematic equations of the system from the previous section, the system dynamics can now be obtained by leveraging the Lagrangian framework. In the following section, the direct dynamics are derived from the kinematic equations (33) and (35). Furthermore, the relation between the body-and generalized forces is established from the principle of virtual work. Lastly, those body forces are defined in the context of an AM.

State Space and System Jacobian
The state spacez of the combined system is defined by the choice of the (generalized) coordinates, namely the orientation given by the quaternion q, the positionp and the N J joint angles collected inθ: The rotational and translational velocity of each particle of the system can be written in form ofv = W J (x)ẋ, wherev is the vector of angular and linear velocities of each body of the system: W J is the system's Jacobian matrix which is composed by the individual components of (33) and (35): with the individual Jacobian matrices for translation being and for rotation being

Equations of Motion
Solving the Lagrangian equations is an algorithmically simple way of retrieving the dynamics equations. It does, however, come with some requirements concerning the choice of the coordinates. More precisely, the coordinates need to be independent and complete. It is clear that the chosen coordinates (37) are complete and can represent any configuration of the system. The requirement of independent coordinates is however not met e.g. fixing the coordinates q x , q y , q z of q will also fully constrain q w by the unity constraint q = 1. For the time being, it will be assumed that all the coordinates are independent. The holonomic constraint will be dealt with at a later stage.
In the Lagrangian framework, the system dynamics are obtained by solving the Euler-Lagrange equations where L = E kin − E pot is the Lagrangian,x the state space of the system,f x ∈ R Nx are the generalized forces resp. torques along the generalized coordinates, andf c ∈ R Nx mark the constraint forces, pulling the system towards the manifold defined by the holonomic constraints. Herein, the constraint forces arise by the virtue of the unit quaternion coordinates not being independent.
In the case of mechanical systems, the solution of (42) takes the following particular form [17]: where M ∈ R Nx×Nx is the mass matrix, C ∈ R Nx×Nx the Coriolis matrix andḡ ∈ R Nx the vector of gravitational terms. The total kinetic energy of the system is given by with M L being the generalized inertia matrix of the system represented by the block diagonal formed by the individual body masses m i and the moments of inertia W Θ i in the world frame [35,36]. It is defined as with W Θ i being linked to the classical inertia tensor W Φ i by Therein, ν is associated with the scaling DOF of a body, such would be the case without enforcing q = 1. However, since q is assumed to be of unit length, the bodies of the system are rigid and ν does not come into play. As such, ν can be defined as any positive number.
Solving (42) directly via calculating L becomes impractical resp. computationally expensive and thus time-consuming for larger systems. A much faster approach is the factorization into the individual system matrices (43), which has the additional benefit of not having to separate the individual components into their respective matrix. Once the expression for the system's mass matrix is obtained, the Coriolis matrix can be obtained from its Christoffel symbols. The gravitational terms are obtained from the potential energy, which is usually a relatively small expression and thus inexpensive to compute.
By analyzing (43), (44), it can be seen that the only resulting term inẍ, corresponding to the system's mass matrix M (symmetric, positive semi-definite [37]), is obtained from The system's Coriolis matrix C ∈ R Nx×Nx containing the centrifugal and centripetal terms is obtained by calculating the Christoffel symbols first kind from the mass matrix (47) [17]. Its individual components are given by The definition of the Coriolis matrix is not unique, and different formulations are possible (e.g. [38]) having different properties. The choice herein has the property ofṀ − 2C being skew-symmetric, a useful property for controller design [17].
Lastly, the system's gravity vectorḡ is obtained from the gradient of the potential energy withḡ 0 being the gravitational acceleration. The vectorḡ contains the gravitational terms and thus accounts for the shifting center of gravity due to the motion of the manipulator. It is obtained from the gradient of the potential energy by calculatingḡ An alternative to those equations is presented in [26], where the system's mass matrix M and lumped Coriolis vectorh are obtained from the system's total kinetic energy by calculating The vectorh is equal to Cẋ.

Force Mapping
The termf x in (43) is given in terms of forces along the generalized coordinates. In many situations, it is very convenient to have a mapping from local body forces to generalized forces. This mapping will greatly facilitate the application of the forces and torques originating from the propulsion system of the AM.
By the choice of the generalized coordinates, the torques applied to the AM have to be specified along the four coordinates of the quaternion. The principle of virtual work is used to establish a relationship between the quaternion torques and the Cartesian torques along the body-fixed axis.
The principle of virtual work states that the work δW is equal to the forcef acting on a body along its virtual displacement δs: δW =f · δs.
In a completely analogous manner, the work performed by the torque L0f q acting on the rotation δφ =nδϕ is defined as Following the approach in [39] stating that for small changes in rotations (i.e., q δ = 1), the following approximation holds: where δq is a small variation and q δ is a small change of rotation from q. Left multiplying both sides of that relation with q * yields: Using the axis angle relation (97), q δ , under consideration of small angles, can be approximated as Thus, (57) in (56) results in 2 (q * ⊗ δq) ≈ (0,nδϕ) .
Writing (54) as the dot product of two pure quaternions δW q ≈ 0, L0f q · (0,nδϕ) , and by inserting (58) the relation δW q ≈ 0, L0f q · 2 (q * ⊗ δq) is obtained. Using (7) yields With the help of the dot product propertyx · Aȳ = A x ·ȳ, (61) becomes wherein 2G can be identified as the mapping between Cartesian torques in body frame and the generalized quaternion torques. The forces along the body's fixed axis can be mapped to the generalized forces with The mapping of joint torques to generalized joint forces is trivial: The results (62), (63) and (64) are summarized in the following block diagonal force mapping matrix M F , mapping from body forces to generalized forcesf x : such that the generalized forces in (43) can be written as with L0f B being the body forces composed by the forces L0f xyz and torques L0f q in resp. about the local x, y and z body axis and the joint torques L0f J . Those forces typically originate from the propulsion system but can also include aerodynamic effects if so desired. The body force vector is thus defined by

Propulsion Forces
The body forces (67) are defined as a function of the thrust of the AM's propulsion system. Given a UAV with N M propulsion devices, then the forces from the propulsion system acting on the body of the AM are obtained from the following relations Thereinf thrust,i designates the thrust force vector andf drag,i the drag vector (reaction torque). The vector L0rL0 M,i designates the lever formed between the UAV's body and the motor frame (see figure 4). The mapping matrix M mot , also commonly referred to as efficiency resp. allocation matrix (used by the mixer in many controllers) contains the contribution of each motor to each force resp. torque along each body axis. It is constant (unless the motors themselves can tilt) and defined by M mot = blockdiag L0f thrust,1 , . . . , L0f thrust,N M , L0f rot,1 , . . . , L0f rot,N M , 1 N (69) and can be used to map body forces to motor forces.
Both quantities are a function of the angular velocity ω Mi of the propeller [40] as in (see figure 1) M,i is the motor's normalized axis of rotation. The constants k t and k p for thrust and drag can be identified experimentally. Furthermore, since ω Mi is rarely known and only very few ESCs provide telemetry resp. accept inputs as RPM references, (70a), (70b), can also be written as a function of the flight controller's output PWM signal u M,i = [0, 1], which yields the roughly linear relationship [40], [41]: L0f where s M,i = {1, −1}, for CW resp. CCW, designates the spin direction of the motor. Notice that for tilting propeller configurations, the normal vector L0n Mi would be a function of the tilt angles commanded to the tilting mechanism. A real motor cannot instantaneously change its velocity; therefore, it is common practice to model the motor dynamics as a first-order system [41]: where K is the maximum RPM and T is the time constant of the motor-propeller system.
Robotic joints are generally more complex and have a non-negligible amount of friction and damping due to their internal mechanics. Their modeling highly depends on their internals and thus has to be seen on a case-by-case basis.
For the sake of simplicity, the model in (72) is also used for the robotic joints throughout this work.

Simulation & Constraints
The numeric simulation of the AM is a very helpful or even critical step before deploying an AM in practice. Furthermore, there are applications in deep reinforcement learning where the model needs to be simulated such that the controller can be learned from the model dynamics [42]. The required equations for numerical simulation, respecting the holonomic constraints, are derived herein.
The system dynamics equation (43) can be written in form of the differential algebraic equation (DAE) [23] Mẍ whereφ are the holonomic constraints imposed on the system, Jφ designates the Jacobian matrix ofφ and λ is a Lagrange multiplier. The constraint vector contains (at least) the unity constraint originating from the unit quaternion: The system equation can be used for numerical simulation by solving (43) forẍ and implementing it as first-order ODEs:ż whereinā =ẍ marks the unconstrained acceleration: Naturally, such a system would not be constrained to the manifold defined by the constraintφ = 0, violating the unit length assumption of the quaternion, thus degrading the physical fidelity of the system up to a point where it is completely disjoint from reality. It is, therefore, mandatory to constrain the system such that the unit length assumption holds. Generally, this can be seen as a holonomic constraint problem, which is typically addressed by Lagrangian multipliers within the Lagrangian framework.
Herein, we opted for a general-purpose solution to this holonomic constraints problem as discussed in [43], [44] and [45], in the spirit that it provides additional value, e.g., in a reinforcement learning context, where it might be useful to temporarily stiffen the joints to speed up the training process. The formulation is, however, fairly heavy and the authors of [23,46] specifically addressed the problem of the quaternion unit constraint and provided a numerically faster method. The general idea behind those methods is, however, the same and consists of performing small corrective actions orthogonal to the manifolds defined by the constraints for both the system's velocities and positions. The authors of [35] showed how to calculate the lagrangian multipliers directly. However, this requires splitting the system into translational and rotational parts, which is unfeasible here due to the coupled nature of the system.
According to [45], the holonomic constraint is enforced onto the system by constraining the accelerations and velocities, which are obtained by the sum of the unconstrained accelerationā resp. velocityv and a corrective term. The system respecting the holonomic constraints is thus given bẏ where (.) + designates the Moore-Penrose pseudo inverse. The remaining terms are computed as where N x is the number of states of the system and N φ the number of constraints imposed onto the system.
Integrating this system using common numerical integration methods (e.g. RK4) results in a small error that occurs after each integration step, caused by the fact that S 3 is not closed for addition q + q δ / ∈ S 3 . The authors of [47] have shown how this error accumulates and alters the dynamics of the system. In practice, however, the error is relatively small under the condition that the time steps are sufficiently small, in which case (55) holds. Furthermore, it is not required to re-normalize the quaternion after each iteration since the constraint drives the unity error to zero.

Lagrange Bullet Propulsion Forces & Joint Trajectories
Simulate Compare

Validation
In the following section, the physical fidelity of the obtained constrained system model (79), is validated.

Methodology
Correctness of the model is shown by comparing the evolution of a dynamics simulation of an AM with a 1-DOF manipulator (see figure 9 using the constrained first order system (79) and comparing them with the simulation as performed by the Bullet physics engine [48] using the same URDF model in both cases. The Bullet simulation will thus be the reference model, called the Bullet model. For clarity, the model corresponding to (79) will be referred to as the Lagrange model.
The simulation in both cases is performed at a fixed time step of 240 Hz. The simulation of the Lagrange model is using the Euler forward (first order) integration scheme, Bullet physics uses the semi-implicit Euler method. As far as possible, equal conditions have been established for both simulations (e.g., the default linear and angular damping in Bullet has been turned off for this test).
A short dynamics simulation of the modeled AM is carried out, during which positions, velocities, and propulsion forces are recorded. Given the inherently unstable nature of the AM, it is stabilized via a stabilizing (sliding mode) controller similar to [18] around the equilibrium point. During the simulation, the first (and only) joint θ 1 is driven towards the commanded reference position, following the pattern shown in figure 7 (top). The movement of that joint is thus acting as a disturbance onto the closed-loop system and stresses the various terms in (43). Caused by the accumulation of small errors at each simulation step, the two systems will deviate from each other eventually. Furthermore, the same control input will no longer stabilize both systems. This limits the time interval over which the dynamics of both systems are comparable. Here, this interval was identified to be around 4 s.
The recorded forces applied to the Lagrangian-model via the propulsion devices are then copied to the Bullet-model, which is thus pseudo-open loop. If both models are sufficiently close, the exact same forces should stabilize both models (within a reasonable time span). The joint torques on the other hand are not transferred due to numerical stability concerns, but since the joint is following the same trajectory, the generated disturbance is the same in both cases. Therefore, the Bullet-model tracks the joint velocity and position of the Lagrange-model via Bullet's built-in joint position control algorithm. The process is depicted in figure 5.
The hypothesis is thus, if both systems have the same intrinsic dynamics, the time response of both systems will be the same, resp. very close. Two systems will diverge eventually due to the accumulation of small errors over time e.g. caused by numerical inaccuracies, fundamental architectural differences, and different integration schemes.
Lastly, by performing a 'backflip' maneuver during which the simulated AM, momentarily points straight up resp. down, we can verify that our proposed model remains consistent where the model using Euler angles [18] fails due to gimbal lock [22]. To that end, the aforementioned 1-link AM is commanded to flip about its local Y-axis, thus pitching the platform by more than 90°.

Results
The actuation of the first joint following the commanded pattern as shown in figure 7 (top), is creating a disturbance on the system. The manipulator moves in XZ plane (figure 9), with the reaction torque acting on the Y axis (pitch) and the Coriolis force pushing the vehicle in X resp. Z direction. The resulting motion is shown in figures 6 and 7.
The resulting velocities along the main axis of the disturbance are shown in figure 6a. The reaction torque creates a net torque around the Y axis, causing the system to accelerate, and turn about the Y axis. The same applies to the velocities in X and Z caused by the Coriolis forces initiated by the circular motion of the moving mass (the link here).
The corresponding positions are shown in figure 6b. Noticeably, is that the evolution of both models is extremely close, however, as can be seen in figure 6a (center), small errors do sum up and thus both systems deviate from each other eventually. As the states of both models deviate eventually, the control input will no longer stabilize both systems. A comparison thus only makes sense over a short interval (4 s).
The yaw and roll position discrepancies are shown in figure 7. Since the manipulator passes, during its movement, straight through the center of mass (in XZ plane), there should be no rotation about roll, which is the case here as it is measuring close to zero. The same reasoning applies to the yaw axis, except that also the propeller drag comes into play here. In Y direction, the AM also exhibits a net-zero movement as the manipulator moves in XZ plane.
The absolute quaternion unity error is shown in figure 7 (bottom). Since this is the fundamental assumption made at the start, it must deviate from unity as little as possible. The maximum error measured here was only 2.9 × 10 −6 indicating that the holonomic constraint solver pushes the quaternion norm to stay close to unity. It can also be seen that re-normalizing the quaternion after each iteration is unnecessary.
It can thus be concluded that our model is true to the reference model as simulated by the Bullet realtime physics engine.
Lastly, a backflip maneuver pitching the AM by more than 90°is shown in figure 8. The results show that the Euler model fails close to the south pole (90°mark) as it enters a gimbal lock condition, where the movement becomes unpredictable as the transformation matrix tying Euler rates to angular velocities becomes rank deficient. Consequently, the system states become 'NaN' -terminating the simulation. Our quaternion model has no such limitations and remains consistent.

Control
This section briefly demonstrates an application of the developed model in a control context applied to an AM consisting of quadcopter carrying a 2-DOF serial link manipulator. Furthermore, investigations are performed to analyze the realtime applicability of the control scheme with increasingly complex AM systems.

Computed Torque Control
The controller is largely based on the computed torque controller presented in [17] and depicted in figure 10. The controller makes use of the model for feedback linearization and is, at its core, a PD controller with bias terms (known disturbances). This very simple structure was deliberately chosen for the subsequent tests as it does not hide model imperfections, contrary to the various state-of-the-art robust control schemes, which are largely preferable for real-world applications. Nonetheless, they also make heavy use of a dynamics model such as presented in this work.
Given the non-linear AM system Mẍ + Cẋ +ḡ =τ , (81) by deliberately choosing the control inputτ = Mν + Cẋ +ḡ (82) that cancels the nonlinearities via the bias term Cẋ +ḡ, the following system is obtained by inserting (82) in (81): x =ν. (83) Using a PD-feedback law, the virtual control inputν can be defined bȳ which, by inserting (84) in (83) yields the linear error dynamics which is stable and guaranteed to converge by the right choice of the gain matrices K v and K p according to linear control theory. The mapping matrix M F (65) maps the pseudo-forces of the controller into generalized coordinates.  The positional error vectorē is defined asē whereq v,err is the vector part of the error quaternion defined as q err = q * ⊗ q ref .
The velocity error is given byė The acceleration error termë is not used asẍ is difficult to obtain in practice (in good quality), thusë =0.
Lastly, the body forces to be applied by the controller are obtained by calculatinḡ which can easily be mapped to motor forces using the mapping matrix (69): The motor forces can then further be mapped to RPM via relation (70).
It is clear that, on a real AM, the system matrices will often be based on estimations resp. approximations and will thus not exactly cancel the system's nonlinearities, which will degrade the performance of the controller. A common approach is thus to rely on the robustness of the controller (e.g. [8]) or to make it adapt to miscalculated mechanical properties as shown in [18].

Tests and Methodology
A quadcopter equipped with a planar 2-DOF manipulator featuring two revolute joints is modeled in URDF and loaded into the realtime physics simulation Bullet. The motor and joint forces are modeled as a first-order system (72) and applied locally as external forces resp. torques to the simulated body calculated with equation (67). The system matrices (M, C,ḡ) are generated from the same URDF description and exported to C code for performance reasons resp. to achieve close to bare metal performance for the subsequent performance benchmarks. The simulation and the controller are implemented in Python, making use of PyBullet for the realtime physics simulation, numpy for the linear algebra and cython to wrap and utilize the generated code. The overall structure is as depicted in figure 10.
The properties and the configuration of the AM is shown in figure 11, the gains of the controller were hand-tuned and read K v = diag (0, 0, 10,5,5,4,24,24) , 30,40,40,30,60,120) . The first two coefficients are zero for the velocity resp. position in x and y due to the lack of direct control authority over those quantities.
Two different tests are carried out. The first one analyzes the tracking performance of the computed torque controller in simulation over an interval of 20 s, performing several maneuvers whilst actuating the joints. The movement of the manipulator is a significant disturbance to the controlled system. Also, since the controller is in fact just a PD controller with a bias term, inconsistencies between the simulated model and the generated dynamics model would inevitably lead to an unstable system.
The second test is a series of benchmarks dealing with the realtime capability of the controller. The benchmarks are carried out on two different platforms that are representative for on-board resp. off-board control scenario. Herein, an AMD Ryzen 1600X is used as a typical ground station, whilst a Raspberry Pi 4 was picked as a low-cost, low-power platform commonly used as a companion computer on drones to perform more substantial onboard calculations. Several AM systems from zero to 3 links were generated and analyzed. Prior to counting the number of operations on the left-hand side of (43) via SymPy's 'count_ops' function, the expression is simplified with the 'expand' function. This operation is very costly but necessary to be able to get comparable results. For that reason, a maximum of 3 links was chosen for this test (no limitations are imposed by the framework itself). The generated C code is compiled with the LLVM Clang compiler using the flags '-O1 -ffast-math -fno-math-errno'. The code is using the double-precision floating point format. The choice of using '-O1' (minimal optimizations) on the generated C code is motivated by the huge size of the generated code files (up to several megabytes). Performing only minimal optimizations helps with compile times and memory consumption during compilation. Tests with '-O2' only showed marginal improvements in terms of performance. All of the C++ code involving expensive linear algebra is compiled with standard release flags (i.e., '-O3'). For this series of tests, we got rid of all Python code and replaced the numpy functionality with Eigen, thus performing our benchmarks on a 'pure' C/C++ codebase.
The average time for one iteration of forward dynamics (78) and inverse dynamics (81) was measured and averaged over several runs. The forward dynamics is important for simulation, the inverse dynamics is what is fed into the controller in terms of known disturbances. The main difference between the two is the additional computational cost associated with the calculation of the inverse mass matrix and the constraint solver which comes into play for the forward dynamics.
Lastly, we compare the number of operations of the dynamics equation of a two-link AM using Euler and quaternion parameterization. Furthermore, we test different methods to obtain the dynamics equation from the general Lagrange equations. More precisely: 1. The energy formulation from [26], (51), (52), which results in a lumped Coriolis force vector. 2. The factorization method used herein, involving the Christoffel symbols, (47), (48). 3. A mix of the two methods, which also yields a lumped expression for the Coriolis term, (47), (52).

Results
The results of the simulation over a duration of 20 s have been plotted in figure 12. Reference trajectories (position and velocity) have been generated for the joint angles, the altitude, roll, pitch, and yaw angles, and fed to the controller. Notice that the controller tracks the reference quaternion obtained from the roll, pitch, and yaw angles. For convenience, those angles have also been plotted by converting the attitude quaternion back to roll, pitch and yaw.
The results in figure 12 show stable and satisfactory performance, indicating that the nonlinearities of the system are well compensated by the bias term. The steady-state error in z direction between 8.5 s and 10 s is introduced by the pitch angle of the UAV, which leads to a loss in thrust in z-direction. This behavior is expected from a PD-controller (as is the care here). A robust control scheme can be used to fix this problem, or, in this case here, a simple addition of an integral term to the virtual control input would be sufficient. Furthermore, as can be seen from figure 11 resp. 9, the time constants of the system have been tuned down from 0.2 s to 0.1 s for the propulsion resp to 0.05 s for the joints. The time constants greatly impact the performance of the controller as they severely degrade its ability to cancel the nonlinearities via the bias term. The point could be made that AM systems with more but smaller propellers (thus with a smaller time constant) are preferable over systems with fewer but larger propellers.
The benchmark results concerning the computational time and realtime performance on the two model platforms (AMD Ryzen 1600X and Raspberry Pi 4) are shown in table 2. The results indicate that for the desktop CPU, the number of calculations hardly poses any problem with an average of just 66 µs per forward dynamics iteration for a very typical UAV equipped with a 2-DOF manipulator, which then grows exponentially, reaching an average of 731 µs for a UAV with a 3-DOF manipulator with a total of almost 6.6 million arithmetic operations. As expected, the inverse dynamics are slightly faster, by the lack of matrix-inverse calculations, peaking at 706 µs per iteration.
The Raspberry performs noticeably worse, reaching up to 4.93 ms per forward dynamics iteration in the 3-DOF scenario and only slightly better at 4.89 ms per inverse dynamics iteration. Whether or not this is acceptable for realtime control heavily depends on the application. Typically, the inner loop of commercial autopilots runs at 500 Hz, which would limit the Raspberry Pi to a 2-link configuration.
From table 3 it becomes clear that the computational cost increase from choosing quaternion parameterization over Euler angles is substantial. Across the board, the number of operations roughly doubled, passing from Euler angles to quaternion parameterization. This is at least partially due to the larger state space and, thus all system matrices being enlarged by one over the Euler model. The numbers also show that the method itself has a big impact on the complexity of the resulting dynamics expression. We achieved the best results with the "mixed"-method, (47), (52) for the quaternion model. The "energy"-method, (51), (52), delivered the best results for the Euler model. In all cases, a lumped expression for the Coriolis forces is preferable. Calculating the Christoffel symbols, (47), (48), always resulted in the largest expression. The presented methods are also vastly different in terms of the time it takes for the model to generate. Here, the mixed method was the fastest in all cases, while the energy method was the slowest. This can be attributed to the number of derivatives calculations involved in that method.

Conclusion
The kinematic equations for the multi-body system formed by an AM (i.e., UAV and a serial link manipulator attached thereto), have been derived starting with a general URDF description of the system. The complete closed-form,   singularity-free dynamics of an AM has been derived in detail via the Euler-Lagrange equations using quaternion parametrization for the rotational degrees of freedom of the floating base. The dynamics equations of the system were then factorized into the canonical form for mechanical systems.
The obtained dynamics model was numerically validated against its counterpart (simulated by the Bullet physics engine), showing adequate performance over a reasonable time span. The results also show that the unity constraint of the quaternion stays satisfied over the course of the simulation by enforcing it via a general-purpose holonomic constraint solver.
The relation between the generalized forces and the body forces has been established whilst providing detailed insights on how those forces are calculated resp. modeled within an AM context.
And lastly, as an application, the stabilization of an AM using a computed torque controller making use of the developed dynamics model was shown, indicating satisfactory tracking performance in most cases. A robust control approach is, however, necessary to compensate for the loss of vertical thrust due to the inclination of the flying platform. Our data shows that, although the complexity of the closed-form solution grows exponentially, the proposed solution is generally fast enough for realtime applications of typical 2-link AM systems on low-power embedded platforms. Our results also show that the computational cost increase of our quaternion model compared to the common Euler model is quite substantial. The selected method to extract the dynamics equations from the general Lagrange equations shows to have a significant impact on the complexity of the resulting expression.
The current implementation makes use of a general-purpose holonomic constraint solver to keep the quaternion at unit length, which currently prevents it from being used in a model predictive control framework. Future work will deal with this issue by eliminating the unity constraint and, thus, the need for the constraint solver.

A Quaternion Fundamentals
This appendix briefly introduces quaternions with a focus on unit quaternions, which play a fundamental role in representing singularity-free orientations in three-dimensional space. Its most relevant aspects are summarized here. For further details the reader is referred to [39], [35], [49] and [50].
A quaternion is an expression defined by a set of four coefficients q w , q x , q y , q z ∈ R, and three symbols i, j, k, which can be put in the more convenient form of where q w ∈ R is the scalar part andq v = (q i , q j , q k ) ∈ R 3 is the vector part containing the three imaginary coefficients.
The quaternion group H is endowed with the non-commutative quaternion product defined as The norm of a quaternion is defined as In the context of mechanics, quaternions describe both a (uniform) scaling and a rotation [35]. A quaternion q with q = 1, element of S 3 , is called a unit quaternion, parameterizing a 4-dimensional unit-sphere, and, contrary to a general quaternion in H, solemnly describe the orientation of a rigid body. S 3 is a double cover of SO(3) meaning that q and −q characterize the same orientation.
The inverse of a quaternion is defined as The conjugate of a quaternion is obtained by negating its vector part and is equal to the inverse in case q = 1.
The quaternion identity rotation is defined as Unit quaternions can directly be obtained from axis-angle notation (the equivalent of the Euler notation in quaternion space) s.t.: whereū represents the (normalized) axis of rotation and θ the angle of rotation.
A quaternion is called pure if its scalar part is zero: A vectorū ∈ R 3 defined in the body frame can be rotated into the world frame by q ∈ S 3 using the double quaternion product: (0,ū ) = q ⊗ (0,ū) ⊗ q * ∈ H p . (99) The vectorsū andū have the same magnitude if and only if q = 1. Otherwise, the length ofū is scaled by the norm of q.
Composition of two quaternions q 1 , q 2 ∈ S 3 is similar to the composition of rotation matrices: The quaternion product is linear and as such it can be expressed as a matrix-vector product: with the operators and wherein [.] × : R 3 → R 3×3 is the cross product operator Applying (104) and (105) to (99) yields the rotation of a vector in matrix notation: The matrix equivalent of (97) is given by the Rodrigues rotation formula R (θ,ū) : R × R 3 → SO(3 ), which represents a rotation of an angle θ around an arbitrary axisū and is define as Within this work, the quaternion is always assumed to be unit length.