Control of an Omnidirectional UAV for Transportation and Manipulation Tasks

Abstract

This paper presents a motion control scheme for a new concept of omnidirectional aerial vehicle for transportation and manipulation tasks. The considered aerial platform is a novel quadrotor with the capability of providing multi-directional thrust by adding an actuated gimbal mechanism in charge of modifying the orientation of the frame on which the four rotors are mounted. The above mechanical design, differently from other omnidirectional unmanned aerial vehicles (UAVs) with tilted propellers, avoids internal forces and energy dissipation due to non-parallel propellers’ axes. The proposed motion controller is based on a hierarchical two-loop scheme. The external loop computes the force to be applied to the vehicle and the reference values for the additional joints, while the inner loop computes the joint torques and the moment to be applied to the multirotor. In order to make the system robust with respect to the external loads, a compensation of contact forces is introduced by exploiting the estimate provided by a momentum based observer. The stability of the motion control scheme is proven via Lyapunov arguments. Finally, two simulation case studies prove the capability of the omnidirectional UAV platform to track a 6-DoFs trajectory both in free motion and during a task involving grasping and transportation of an unknown object.

1. Introduction

In recent years, the use of aerial robotic platforms, due to their nearly unlimited motion space, has become popular in many applications scenarios, e.g., inspection and maintenance services [1], data collection and exploration operations [2], and precision agriculture [3]. More and more frequently, aerial robots are involved in tasks requiring both autonomous flight and physical interaction capabilities, such as assembly of structures in remote environments, object grasping [4], and payload transportation [5,6].

The most popular configuration of vertical take off and landing (VTOL) unmanned aerial vehicle (UAV) is that of a multi-rotor vehicle, i.e., an aerial vehicle equipped with four or more coplanar propellers, which offers the best trade-off between mobility, simplicity, and cost. The main drawback of classical multi-rotor vehicles is that they are underactuated systems, since thrusts generated by propellers are directed along parallel axes, orthogonal to the plane of the propellers [7]. Therefore, in order to change its position and/or counteract external disturbances, rotation of vehicle’s body is required: thus, only four degrees of freedom (DOFs) are left to execute the task assigned to the UAV. This limitation may become severe in some applications that might benefit from availability of all six DOFs, e.g., manipulation tasks involving contact with the environment [8], and/or limit the disturbance counteraction capabilities of the system. To overcome this problem, in recent years, platforms with multi-directional thrust have been designed. One of the most common approaches is based on the use of tilted propellers, e.g., as in [9,10]. The overactuated platforms require additional motors for tilting the propellers, and this has the negative effect of increasing the system weight and the complexity of the control for handling both vertical and lateral air flows. Such a problem can be overcame by adopting hexarotors with tilted propellers, e.g., as in [11], where each rotor is mounted in a fixed configuration rotated about two axes. More efficient solutions are proposed in [12,13]. More in detail, in [12], an additional motor is added in order to tilt all propellers at the same time, in such a way as to reduce the energy consumption and the total mass, allowing to drive the platform from a configuration energetically efficient, but underactuated to a fully actuated but less efficient one. In [13], an hexarotor with independently tiltable rotors is designed; the tilting angle of the rotors is computed online via a suitable control allocation scheme. This platform by exploiting its bi-directional thrusters can apply forces in any direction, while for the other platforms using uni-directional thrusters, the forces are usually limited in the upper hemisphere. An interesting analysis conducted in [14] proved that, for uni-directional thrusters, the minimum number of rotors in order to achieve omnidirectional forces is seven. Such analysis leads to the platform in [15], which is equipped with seven propellers, whose tilting angles are computed according to the optimization procedure in [14] and guarantee the omnidirectional property of the platform, while enforcing equal sharing of the desired forces between the propellers.

The previous solutions, do not avoid internal wrenches caused by counteracting actuators and require redundant control inputs. In order to overcome this problem, in [16], a quadrotor vehicle is designed, with the tilting angle of the propellers simultaneously controlled by two additional actuators via a parallelogram mechanism. In this way, only six inputs are adopted to control six degrees of freedom (DOFs), and the thrust generated by all propellers is always aligned along the same direction, thus avoiding energy dissipation due to internal wrench. A similar configuration has been proposed in [17], where a new concept of omnidirectional aerial vehicle, named ODQuad, is designed in particular for transportation and manipulation tasks. The ODQuad is a quadrotor with the capability of providing multi-directional thrusts by rotating the frame on which the four rotors are mounted through two actuated rotational joints realizing a two-axis (roll and pitch) gimbal mechanism.

For classical underactuated multi-rotors, several controllers have been proposed, mostly based on linear approaches [18,19], often requiring model linearization about a set of equilibrium conditions. Such controllers allow to achieve satisfactory performance near the design conditions and in hovering conditions, but performance degradation is usually observed when the aircraft moves away from these conditions. To overcome these drawbacks, nonlinear controllers have been designed, including model predictive control [20], backstepping and sliding mode techniques [21,22], adaptive control [23], impedance control [24,25], and geometric control [26]. More recently, data-based control approaches have been designed for achieving high-performance trajectory tracking with UAVs: in detail, in [27], an innovative iterative learning control algorithm is proposed based on an estimation of repetitive disturbances to improve the learning performance.

Regarding the omnidirectional aerial platforms, the most common control approach is feedback linearization, aimed at decoupling the six DOFs that can be controlled separately. Such approaches are model-based; the nonlinear dynamical effects are compensated, and then the thrust is derived from the inversion of the control allocation matrix. Some examples of feedback linearization approaches can be found in [11,16,28]. One of the main issues occurring in feedback linearization is that the actuator saturation can destabilize the system. Solutions to this drawback have been proposed. In [29], the six inputs of a coplanar hexarotor are mapped in a four dimensional task output (i.e., the total thrust and the three moment components) via a weighted pseudoinverse. In [30], for constant position and attitude references, input saturation is avoided by resorting to an approach based on the explicit reference governor, which provides constraint handling capabilities by suitably manipulating the derivative of the applied reference [31]. In [32], a general geometric tracking controller for time-varying references is proposed, which also takes into account the bounds on the lateral control force.

The aim of this paper is to present a novel motion control scheme for the ODQuad vehicle [17] and to test it both in free-space motion and in the presence of external disturbances, such as the contact with an object during a task of manipulation and transportation. More in detail, a model-based feedback linearization controller is proposed that computes the reference signals using a hierarchical two-loop scheme: the outer loop is a position controller aimed at computing the global thrust force and the reference values for the pitch and roll angles of the gimbal mechanism; then, in the inner loop, an attitude and gimbal controller, on the basis of these references, computes the moments acting on the quadrotor and the joint torques. A preliminary version of such a scheme has been introduced in [17]; here, two contribution are added: (i) the stability analysis of the system, based on Lyapunov arguments; and (ii) the extension to the case of contact with an object with the compensation of the interaction wrench performed via an online estimator. Finally, in order to prove the effectiveness of the approach, simulation case studies have been developed, in which the controller is tested in a non-ideal context by considering model uncertainties and sensor noise.

It is worth pointing out that, although the ODQuad is conceived to perform tasks both in free space and involving interaction with the environment, the aim of this paper is to design and validate the first building block of the whole control architecture, namely the motion control. Notice that the sole motion controller presented in this paper cannot tackle interaction with the environment and needs to be integrated in an interaction control scheme (e.g., impedance control, hybrid force/position control).

The paper is organized as follows. In Section 2, the mechanical design of the ODQuad is reported and detailed; Section 3 reports both the kinematic and dynamic models, which will be exploited by the control law. The two-loop control scheme is presented in Section 4, while its stability properties are analyzed in Section 5. Section 6 reports the formulation of the control laws in the presence of contact with an object during transportation or manipulation tasks. Section 7 reports the simulation results and finally some conclusions are drawn.

2. ODQuad: A Novel Omnidirectional Quadrotor

The ODQuad vehicle, firstly introduced in [17], is an omnidirectional quadrotor composed of the following three components:

• A platform, hosting computing hardware, electronics, batteries, and sensors;
• A rotor frame that supports the propellers;
• A mobile frame that connects the platform and the rotor frames with the roll and pitch rotational joints.

The first rotational joint connects the platform to the mobile frame (roll joint, see Figure 1a), while the other joint (pitch joint, see Figure 1b) connects the mobile frame to the rotor frame. Both the joints are actuated by servo motors equipped with angular position sensors.

Differently from the common multi-rotor design, the ODQuad is able to change the orientation of the total thrust with respect to the horizontal plane without modifying the platform orientation by moving the roll and pitch joints. This allows one to have all the six DOFs available for the platform motion. In the current design, due to mechanical constraints, the roll and pitch joints can be moved in a range from −25 deg to 25 deg.

The mechanical limits imposed to the roll and pitch joints limit the solid angle that the total trust could span. Thus, according to the taxonomy introduced in [33], it could be more appropriate to refer to the ODQuad as a platform with multi-directional thrust instead of an omnidirectional platform, but in practical applications and in the absence of acrobatic maneuvers, the spanned wrench space is enough to guarantee the fulfillment of the most common tasks.

In order to derive the ODQuad’s kinematics, let us define the following coordinate frames:

• The inertial coordinate frame, $\mathsf{\Sigma }\{O,x,y,z\}$
• The coordinate frame ${\mathsf{\Sigma }}_0\{O_0,x_0,y_0,z_0\}$, attached to the platform in such a way that the axis $x_0$ coincides with the axis of the roll joint
• The coordinate frame ${\mathsf{\Sigma }}_1\{O_1,x_1,y_1,z_1\}$, attached to the mobile frame in such a way that the axis $x_1$ coincides with the axis of the pitch joint
• The coordinate frame ${\mathsf{\Sigma }}_2\{O_2,x_2,y_2,z_2\}$, attached to the rotor frame in such a way that the axis $x_2$ coincides with $x_1$.

The reference frames ${\mathsf{\Sigma }}_0$, ${\mathsf{\Sigma }}_1$, and ${\mathsf{\Sigma }}_2$ have the same origin, i.e., $O_0\equiv O_1\equiv O_2$, while the relative orientation is expressed by the following rotation matrices:

$${\mathit{R}}_1^0\left(\alpha \right)=\left[\begin{array}{ccc}0& -1& 0\\ c_{\alpha }& 0& -s_{\alpha }\\ s_{\alpha }& 0& c_{\alpha }\end{array}\right],{\mathit{R}}_2^1\left(\beta \right)=\left[\begin{array}{ccc}1& \phantom{-}0& 0\\ 0& \phantom{-}{c}_{\beta }& -s_{\beta }\\ 0& \phantom{-}{s}_{\beta }& c_{\beta }\end{array}\right],$$

(1)

where $\alpha$ is the pitch angle, $\beta$ is the roll angle, $c_{\alpha }=cos\alpha$, $s_{\alpha }=sin\alpha$, $c_{\beta }=cos\beta$, $s_{\beta }=sin\beta$, and ${\mathit{R}}_i^j$ denote the rotation matrix expressing the orientation of ${\mathsf{\Sigma }}_i$ with respect to ${\mathsf{\Sigma }}_j$.

3. Modeling

The dynamic model of the system has been derived in [17] according the Euler–Lagrange formulation, using the following generalized coordinates:

$$\mathit{q}={\left[{\mathit{p}}_0^{\mathrm{T}}{\mathit{\varphi }}_0^{\mathrm{T}}{\mathit{\delta }}^{\mathrm{T}}\right]}^{\mathrm{T}}\mathbf{\in }\mathrm{I}{\mathrm{R}}^8,$$

(2)

where ${\mathit{p}}_0\mathbf{\in }\mathrm{I}{\mathrm{R}}^3$ is the position of $O_0$ with respect to the inertial frame, ${\mathit{\varphi }}_0={\left[\varphi \theta \psi \right]}^{\mathrm{T}}\mathbf{\in }\mathrm{I}{\mathrm{R}}^3$ is the vector of roll-pitch-yaw angles, which express the platform orientation with respect to the inertial frame, and $\mathit{\delta }={\left[\alpha \beta \right]}^{\mathrm{T}}\mathbf{\in }\mathrm{I}{\mathrm{R}}^2$ collects the roll-pitch joint positions.

3.1. Kinematics

To properly derive the dynamic model of the system, the position and velocities of the center of mass (CoM) of the ODQuad components, i.e., platform, mobile frame, and propeller’s frame, have to be computed. To this aim, let us define three new reference frames: ${\mathsf{\Sigma }}_{C_0}$, ${\mathsf{\Sigma }}_{C_1}$ and ${\mathsf{\Sigma }}_{C_2}$, attached, respectively, to the CoM of the platform ($C_0$), mobile frame ($C_1$) and propeller’s frame ($C_2$). The linear and angular velocities of ${\mathsf{\Sigma }}_{C_{*}}$, $(*=0,1,2)$, can be expressed in terms of the generalized coordinates as

$$\left\{\begin{array}{ccc}{\dot{\mathit{p}}}_{C_0}& =& {\mathit{J}}_{P_0}\left(\mathit{q}\right)\dot{\mathit{q}},\hfill \\ {\mathit{\omega }}_{C_0}& =& {\mathit{J}}_{O_0}\left(\mathit{q}\right)\dot{\mathit{q}},\hfill \\ {\dot{\mathit{p}}}_{C_1}& =& {\mathit{J}}_{P_1}\left(\mathit{q}\right)\dot{\mathit{q}},\hfill \\ {\mathit{\omega }}_{C_1}& =& {\mathit{J}}_{O_1}\left(\mathit{q}\right)\dot{\mathit{q}},\hfill \\ {\dot{\mathit{p}}}_{C_2}& =& {\mathit{J}}_{P_2}\left(\mathit{q}\right)\dot{\mathit{q}},\hfill \\ {\mathit{\omega }}_{C_2}& =& {\mathit{J}}_{O_2}\left(\mathit{q}\right)\dot{\mathit{q}},\hfill \end{array}\right.$$

(3)

where the Jacobian matrices ${\mathit{J}}_{P_{*}}\in \mathrm{I}{\mathrm{R}}^{3\times 8}$ and ${\mathit{J}}_{O_{*}}\in \mathrm{I}{\mathrm{R}}^{3\times 8}$, $(*=0,1,2)$ are given by [17]

$$\begin{array}{ccc}\hfill {\mathit{J}}_{P_0}& =& \left[{\mathit{I}}_3-\mathit{S}\left({\mathit{r}}_{0,C_0}\right)\mathit{T}\left({\mathit{\varphi }}_0\right){\mathbf{0}}_3{\mathbf{0}}_3\right],\hfill \\ \hfill {\mathit{J}}_{O_0}& =& \left[{\mathit{O}}_3\mathit{T}\left({\mathit{\varphi }}_0\right){\mathbf{0}}_3{\mathbf{0}}_3\right],\hfill \\ \hfill {\mathit{J}}_{P_1}& =& \left[{\mathit{I}}_3-\mathit{S}\left({\mathit{r}}_{1,C_1}\right)\mathit{T}\left({\mathit{\varphi }}_0\right)-\mathit{S}\left({\mathit{r}}_{1,C_1}\right){\mathit{x}}_0{\mathbf{0}}_3\right],\hfill \\ \hfill {\mathit{J}}_{O_1}& =& \left[{\mathit{O}}_3\mathit{T}\left({\mathit{\varphi }}_0\right){\mathit{x}}_0{\mathbf{0}}_3\right],\hfill \\ \hfill {\mathit{J}}_{P_2}& =& \left[{\mathit{I}}_3-\mathit{S}\left({\mathit{r}}_{2,C_2}\right)\mathit{T}\left({\mathit{\varphi }}_0\right)-\mathit{S}\left({\mathit{r}}_{2,C_2}\right){\mathit{x}}_0-\mathit{S}\left({\mathit{r}}_{2,C_2}\right){\mathit{x}}_1\right],\hfill \\ \hfill {\mathit{J}}_{{\mathit{O}}_2}& =& \left[O_3\mathit{T}\left({\mathit{\varphi }}_0\right){\mathit{x}}_0{\mathit{x}}_1\right],\hfill \end{array}$$

(4)

the vector ${\mathit{r}}_{*,C_{*}}$ denotes the position of $C_{*}$ with respect to $O_{*}$, $(*=0,1,2)$, expressed in the inertial frame; $\mathit{T}\left({\mathit{\varphi }}_0\right)$ is the matrix relating the derivative of the roll-pitch-yaw angles, ${\dot{\mathit{\varphi }}}_0$, to the angular velocity, ${\mathit{\omega }}_0$ [34]; ${\mathit{I}}_3$ and ${\mathit{O}}_3$ are the $(3\times 3)$ identity and null matrices, respectively; ${\mathbf{0}}_3$ is the $(3\times 1)$ null vector; while ${\mathit{x}}_{*}$ is the first column of ${\mathit{R}}_{*}$$(*=0,1)$, which is the rotation matrix expressing the orientation of the frame ${\mathsf{\Sigma }}_{*}$ with respect to the inertial frame.

3.2. Dynamics

The dynamic model can be written in compact form as

$$\mathit{M}\left(\mathit{q}\right)\ddot{\mathit{q}}+\mathit{C}(\mathit{q},\dot{\mathit{q}})\dot{\mathit{q}}+\mathit{g}\left(\mathit{q}\right)=\mathit{\eta },$$

(5)

where $\mathit{M}\left(\mathit{q}\right)\in \mathrm{I}{\mathrm{R}}^{8\times 8}$ is the symmetric and positive definite inertia matrix

$${\displaystyle \mathit{M}\left(\mathit{q}\right)=\sum _{\mathit{I}=0}^2\left(m_i{\mathit{J}}_{P_i}^{\mathrm{T}}\left(\mathit{q}\right){\mathit{J}}_{P_i}\left(\mathit{q}\right)+{\mathit{J}}_{O_i}^{\mathrm{T}}\left(\mathit{q}\right){\mathit{M}}_i{\mathit{J}}_{O_i}\left(\mathit{q}\right)\right),}$$

(6)

$m_i$$(i=0,1,2)$ is the mass of the ith component (platform, rotor frame, and mobile frame) of the ODQuad, ${\mathit{M}}_i$ is its inertia tensor expressed in the inertial frame. The matrix $\mathit{C}(\mathit{q},\dot{\mathit{q}})\in \mathrm{I}{\mathrm{R}}^{8\times 8}$ collects the Coriolis and centrifugal terms

$$\mathit{C}(\mathit{q},\dot{\mathit{q}})\dot{\mathit{q}}=\dot{\mathit{M}}\left(\mathit{q}\right)\dot{\mathit{q}}-{\left[\frac{\partial }{\partial \mathit{q}}\left(\frac{1}{2}{\dot{\mathit{q}}}^{\mathrm{T}}\mathit{M}\left(\mathit{q}\right)\dot{\mathit{q}}\right)\right]}^{\mathrm{T}}.$$

(7)

The vector $\mathit{g}\left(\mathit{q}\right)\in \mathrm{I}{\mathrm{R}}^8$ contains the generalized gravitational forces

$$\mathit{g}\left(\mathit{q}\right)=-\sum _{i=0}^2{m}_i{\overline{\mathit{g}}}^{\mathrm{T}}{\mathit{J}}_{P_i}\left(\mathit{q}\right),$$

(8)

with ${\overline{\mathit{g}}}^{\mathrm{T}}={[00-9.81]}^{\mathrm{T}}$.

The vector $\mathit{\eta }$ collects the external generalized forces acting as inputs, i.e.,

$$\mathit{\eta }=\left[\begin{array}{c}{\mathit{f}}_0\\ {\mathit{\mu }}_0\\ {\mathit{\tau }}_{\delta }\end{array}\right],$$

(9)

where ${\mathit{f}}_0$ and ${\mathit{\mu }}_0$ are the force and moment, respectively, acting on the platform, and ${\mathit{\tau }}_{\delta }\in \mathrm{I}{\mathrm{R}}^2$ are the torques acting on the roll-pitch joint. The force ${\mathit{f}}_0$ depends on the four propellers’ thrusts, ${\sigma }_i$ ($i=1,\dots ,4$), that are directed along the ${\mathit{z}}_2$ axis. Therefore, the total force acting on the platform, expressed in ${\mathsf{\Sigma }}_2$, has the following expression, ${\mathit{f}}^2={\left[00f_z\right]}^{\mathrm{T}}$. The input ${\mathit{f}}_0$ can be derived from ${\mathit{f}}^2$, expressed in the inertial frame

$${\mathit{f}}_0={\mathit{R}}_2{\mathit{f}}^2,$$

(10)

where ${\mathit{R}}_2$ represents the orientation of the frame ${\mathsf{\Sigma }}_2$ with respect to the inertial one. Analogously, the moment ${\mathit{\mu }}_0$ can be written as

$${\mathit{\mu }}_0={\mathit{R}}_2\left({\mathit{\mu }}^2+\mathit{S}\left({\mathit{r}}_{2,C_2}^2\right){\mathit{f}}^2\right),$$

(11)

where ${\mathit{\mu }}^2$ is the moment generated by the propellers expressed in frame ${\mathsf{\Sigma }}_2$. In turn, ${\mathit{f}}^2$ and ${\mathit{\mu }}^2$ can be expressed as [7]

$$\left[\begin{array}{c}{f}_z\\ {\mathit{\mu }}^2\end{array}\right]=\left[\begin{array}{cccc}1& 1& 1& 1\\ l& 0& -l& 0\\ 0& -l& 0& l\\ c& -c& c& -c\end{array}\right]\left[\begin{array}{c}{\sigma }_1\\ {\sigma }_2\\ {\sigma }_3\\ {\sigma }_4\end{array}\right]=\mathit{N}\mathit{\sigma },$$

(12)

where $l>0$ is the distance of each rotor from $O_2$, $c={\gamma }_d/{\gamma }_t$, while ${\gamma }_d$ and ${\gamma }_t$ are the drag and thrust coefficients, respectively.

The torque vector ${\mathit{\tau }}_{\delta }$ is given by the sum of the torques generated by the joint actuators, ${\mathit{\tau }}_c$, and the effect of the thrusts on the joints

$${\mathit{\tau }}_{\delta }={\mathit{\tau }}_c+{\mathit{J}}_2^{\mathrm{T}}{\mathit{h}}_2,$$

(13)

where

$${\mathit{J}}_2=\left[\begin{array}{c}{\mathit{J}}_{P_2}\\ {\mathit{J}}_{O_2}\end{array}\right],{\mathit{h}}_2=\left[\begin{array}{c}{\mathit{R}}_2{\mathit{f}}^2\\ {\mathit{R}}_2{\mathit{\mu }}^2\end{array}\right].$$

For control design purposes, the matrices in (5) can be written as block matrices

$$\mathit{M}=\left[\begin{array}{ccc}{\mathit{M}}_{pp}& {\mathit{M}}_{p\varphi }& {\mathit{M}}_{p\delta }\\ {\mathit{M}}_{p\varphi }{}^{\mathrm{T}}& {\mathit{M}}_{\varphi \varphi }& {\mathit{M}}_{\varphi \delta }\\ {\mathit{M}}_{p\delta }{}^{\mathrm{T}}& {\mathit{M}}_{\varphi \delta }{}^{\mathrm{T}}& {\mathit{M}}_{\delta \delta }\end{array}\right],$$

where ${\mathit{M}}_{pp}\in \mathrm{I}{\mathrm{R}}^{3\times 3}$, ${\mathit{M}}_{p\varphi }\in \mathrm{I}{\mathrm{R}}^{3\times 3}$, ${\mathit{M}}_{p\delta }\in \mathrm{I}{\mathrm{R}}^{3\times 2}$, ${\mathit{M}}_{\varphi \varphi }\in \mathrm{I}{\mathrm{R}}^{3\times 3}$, ${\mathit{M}}_{\varphi \delta }\in \mathrm{I}{\mathrm{R}}^{3\times 2}$ and ${\mathit{M}}_{\delta \delta }\in \mathrm{I}{\mathrm{R}}^{2\times 2}$, and

$$\mathit{C}=\left[\begin{array}{c}{\mathit{C}}_p\\ {\mathit{C}}_{\varphi }\\ {\mathit{C}}_{\delta }\end{array}\right],\mathit{g}=\left[\begin{array}{c}{\mathit{g}}_p\\ {\mathit{g}}_{\varphi }\\ {\mathit{g}}_{\delta }\end{array}\right],$$

where ${\mathit{C}}_p\in \mathrm{I}{\mathrm{R}}^{3\times 8}$, ${\mathit{C}}_{\varphi }\in \mathrm{I}{\mathrm{R}}^{3\times 8}$, ${\mathit{C}}_{\delta }\in \mathrm{I}{\mathrm{R}}^{2\times 8}$, ${\mathit{g}}_p\in \mathrm{I}{\mathrm{R}}^3$, ${\mathit{g}}_{\varphi }\in \mathrm{I}{\mathrm{R}}^3$, ${\mathit{g}}_{\delta }\in \mathrm{I}{\mathrm{R}}^2$.

The block decomposition allows us to rearrange the dynamic model (5) as

$$\begin{array}{ccc}\hfill & & {\mathit{M}}_{pp}{\ddot{\mathit{p}}}_0+{\mathit{M}}_{p\varphi }{\ddot{\mathit{\varphi }}}_0+{\mathit{M}}_{p\delta }\ddot{\mathbf{\delta }}+{\mathit{C}}_p{\dot{\mathit{p}}}_0+{\mathit{g}}_p={\mathit{f}}_0\hfill \end{array}$$

(14)

$$\begin{array}{ccc}\hfill & & {\mathit{M}}_{p\varphi }^{\mathrm{T}}{\ddot{\mathit{p}}}_0+{\mathit{M}}_{\varphi \varphi }{\ddot{\mathit{\varphi }}}_0+{\mathit{M}}_{\varphi \delta }\ddot{\mathbf{\delta }}+{\mathit{C}}_{\varphi }{\dot{\mathit{\varphi }}}_0+{\mathit{g}}_{\varphi }={\mathit{\mu }}_0\hfill \end{array}$$

(15)

$$\begin{array}{ccc}\hfill & & {\mathit{M}}_{p\delta }^{\mathrm{T}}{\ddot{\mathit{p}}}_0+{\mathit{M}}_{\varphi \delta }^{\mathrm{T}}{\ddot{\mathit{\varphi }}}_0+{\mathit{M}}_{\delta \delta }\ddot{\mathbf{\delta }}+{\mathit{C}}_{\delta }\dot{\mathbf{\delta }}+{\mathit{g}}_{\delta }={\mathit{\tau }}_{\delta },\hfill \end{array}$$

(16)

where the arguments of the matrices have been dropped for compactness.

4. Motion Control

The proposed motion control is a two-loop model-based scheme inherited from [35]: the outer loop performs the position controller, while the inner loop controls the platform orientation and the roll-pitch joint position. A sketch of the control block scheme is reported in Figure 2. More in detail, the position controller is in charge of computing the force,${\mathit{f}}_0$, to be exerted to the vehicle’s platform in order to achieve the desired vehicle’s position trajectory and the reference angles, ${\alpha }_{des}$ and ${\beta }_{des}$, for the roll-pitch joints. The reference angles are fed to the inner loop, which computes, on the basis of the platform’s desired orientation, both the torques ${\mathit{\tau }}_c$ for the roll-pitch joint actuators and the moment ${\mathit{\mu }}_0$.

The control purpose is the tracking of an off-line planned desired trajectory for the platform (i.e., for the coordinate frame ${\mathsf{\Sigma }}_0$) defined by the vectors ${\mathit{p}}_{0,d}\left(t\right)$ and ${\mathit{\varphi }}_{0,d}\left(t\right)$, their corresponding velocities, ${\dot{\mathit{p}}}_{0,d}\left(t\right)$, ${\dot{\mathit{\varphi }}}_{0,d}\left(t\right)$, and accelerations, ${\ddot{\mathit{p}}}_{0,d}\left(t\right)$, ${\ddot{\mathit{\varphi }}}_{0,d}\left(t\right)$.

Remark 1.

It is worth noticing that the ODQuad, differently from usual underactuated quadrotors, allows us to control both position and orientation, provided that the assigned orientation, ${\mathit{\varphi }}_{0,d}$, can be reached without violation of the limit of roll and pitch angles of the gimbal mechanism (i.e., $\alpha ,\beta \in [-25,+25]$ deg for the current design).

4.1. Outer Loop: Position Controller

The outer control loop is designed to track the assigned position by introducing a virtual control vector aimed at linearizing the position dynamics. In order to design the control law, it is worth rewriting Equation (14) as

$${\mathit{M}}_{pp}{\ddot{\mathit{p}}}_0+{\mathit{M}}_{p\varphi }{\ddot{\mathit{\varphi }}}_0+{\mathit{C}}_p{\dot{\mathit{p}}}_0+{\mathit{g}}_p={\mathit{f}}_0,$$

(17)

where it has been assumed ${\mathit{M}}_{p\delta }\simeq {\mathit{O}}_{3\times 2}$, with ${\mathit{O}}_{3\times 2}$ the ($3\times 2$) null matrix. This assumption allows to decouple the inner and the outer loop. Hence, on the basis of (17), the force to be exerted to the platform is computed as

$${\mathit{f}}_0={\mathit{M}}_{pp}{\mathbf{\gamma }}_p+{\mathit{M}}_{p\varphi }{\mathbf{\gamma }}_{\varphi }+{\mathit{C}}_p{\dot{\mathit{p}}}_0+{\mathit{g}}_p,$$

(18)

where the auxiliary inputs ${\mathbf{\gamma }}_{*}$ ($*=p,\varphi$) are defined as

$$\begin{array}{ccc}\hfill {\mathbf{\gamma }}_p& =& {\ddot{\mathit{p}}}_{0,d}+{\mathit{K}}_{D,p}\left({\dot{\mathit{p}}}_{0,d}-{\dot{\mathit{p}}}_0\right)+{\mathit{K}}_{P,p}\left({\mathit{p}}_{0,d}-{\mathit{p}}_0\right)+{\mathit{K}}_{I,p}{\int }_0^t\left({\mathit{p}}_{0,d}-{\mathit{p}}_0\right)d\rho ,\hfill \end{array}$$

(19)

$$\begin{array}{ccc}\hfill {\mathbf{\gamma }}_{\varphi }& =& {\ddot{\mathit{\varphi }}}_{0,d}+{\mathit{K}}_{D,\varphi }\left({\dot{\mathit{\varphi }}}_{0,d}-{\dot{\mathit{\varphi }}}_0\right)+{\mathit{K}}_{P,\varphi }\left({\mathit{\varphi }}_{0,d}-{\mathit{\varphi }}_0\right)+{\mathit{K}}_{I,\varphi }{\int }_0^t\left({\mathit{\varphi }}_{0,d}-{\mathit{\varphi }}_0\right)d\rho ,\hfill \end{array}$$

(20)

and ${\mathit{K}}_{D,*}$, ${\mathit{K}}_{P,*}$, ${\mathit{K}}_{I,*}$ ($*=p,\varphi$) are symmetric and positive definite gain matrices.

Remark 2.

The position control law is designed on the basis of (17) in lieu of (14), because (14) depends on $\mathbf{\delta }$, and thus computing ${\mathit{f}}_0$ would have required the reference values for $\mathbf{\delta }$ not available at this stage.

Once the force ${\mathit{f}}_0$ is determined, it is possible to compute the reference values for the roll-pitch angles by exploiting (10) and the expression of ${\mathit{R}}_1^0$ and ${\mathit{R}}_2^1$. More in detail, let us express ${\mathit{f}}_0$ in the frame ${\mathsf{\Sigma }}_0$, i.e.,

$${\mathit{f}}_0^0={\mathit{R}}_0^{\mathrm{T}}{\mathit{f}}_0.$$

(21)

Then, in view of (10), it is

$${\mathit{f}}_0^0={\mathit{R}}_2^0{\mathit{f}}^2={\mathit{R}}_1^0{\mathit{R}}_2^1{\mathit{f}}^2.$$

(22)

By considering (1), the following relation can be derived

$$\left[\begin{array}{c}{f}_{0,x}^0\\ f_{0,y}^0\\ f_{0,z}^0\end{array}\right]=\left[\begin{array}{ccc}0& -c_{\beta }& s_{\beta }\\ c_{\alpha }& -s_{\alpha }{s}_{\beta }& -c_{\beta }{s}_{\alpha }\\ s_{\alpha }& c_{\alpha }{s}_{\beta }& c_{\alpha }{c}_{\beta }\end{array}\right]\left[\begin{array}{c}0\\ 0\\ f_z\end{array}\right].$$

(23)

Therefore, the desired values of the joints angles and the total thrust to be provided by the propellers are given by

$$\begin{array}{ccc}\hfill f_z& =& ∥{\mathit{f}}_0^0∥=∥{\mathit{f}}_0∥,\hfill \end{array}$$

(24)

$$\begin{array}{ccc}\hfill {\alpha }_{des}& =& arctan\left(-\frac{f_{0,y}^0}{f_{0,z}^0}\right),\hfill \end{array}$$

(25)

$$\begin{array}{ccc}\hfill {\beta }_{des}& =& arcsin\left(\frac{f_{0,x}^0}{∥{\mathit{f}}_0^0∥}\right).\hfill \end{array}$$

(26)

The desired velocity and acceleration of the roll-pitch joints can be derived by numerical differentiation; in practice, since ${\alpha }_{des}$ and ${\beta }_{des}$ are affected by noise, it is recommended to use suitable robust-to-noise filters. An effective approach is detailed in [35], where the time-varying filter proposed in [36] is adopted in order to compute both the first and second time-derivatives of the reference values.

Remark 3.

In view of (23), it could be noted that (25) and (26) are not well defined if

• $∥{\mathit{f}}_0∥$vanishes: it can happen only if$f_z=0$, namely in the presence of a null total thrust;
• $\alpha =\pm 90$deg or$\beta =\pm 90$deg. It cannot happen since both α and β are mechanically limited to [$-25+25$] deg.

The assumption of total thrust always being non-null is a reasonable assumption for vehicles not involved in acrobatic maneuvers.

4.2. Inner Loop: Attitude and Roll-Pitch Controller

Once the reference values for roll-pitch angles, ${\mathbf{\delta }}_d={[{\alpha }_{des},{\beta }_{des}]}^{\mathrm{T}}$, and their derivatives, ${\dot{\mathbf{\delta }}}_d$ and ${\ddot{\mathbf{\delta }}}_d$, have been determined, the auxiliary input ${\mathbf{\gamma }}_{\delta }$ is computed as

$$\begin{array}{ccc}\hfill {\gamma }_{\delta }& =& {\ddot{\mathbf{\delta }}}_d+{\mathit{K}}_{D,\delta }\left({\dot{\mathbf{\delta }}}_d-\dot{\mathbf{\delta }}\right)+{\mathit{K}}_{P,\delta }\left({\mathbf{\delta }}_d-\mathbf{\delta }\right)+{\mathit{K}}_{I,\delta }{\int }_0^t\left({\mathbf{\delta }}_d-\mathbf{\delta }\right)d\rho ,\hfill \end{array}$$

(27)

where ${\mathit{K}}_{D,\delta }$, ${\mathit{K}}_{P,\delta }$ and ${\mathit{K}}_{I,\delta }$ are symmetric and positive definite gain matrices.

Therefore, the moment to be applied to the platform and the joint torques can be computed similarly to (18)

$$\begin{array}{ccc}\hfill {\mathit{\mu }}_0& =& {\mathit{M}}_{p\varphi }^{\mathrm{T}}{\mathbf{\gamma }}_p+{\mathit{M}}_{\varphi \varphi }{\mathbf{\gamma }}_{\varphi }+{\mathit{M}}_{\varphi \delta }{\mathbf{\gamma }}_{\delta }+{\mathit{C}}_{\varphi }{\dot{\mathit{\varphi }}}_0+{\mathit{g}}_{\varphi }\hfill \end{array}$$

(28)

$$\begin{array}{ccc}\hfill {\mathit{\tau }}_c& =& {\mathit{M}}_{p\delta }^{\mathrm{T}}{\mathbf{\gamma }}_p+{\mathit{M}}_{\varphi \delta }^{\mathrm{T}}{\mathbf{\gamma }}_{\varphi }+{\mathit{M}}_{\delta \delta }{\mathbf{\gamma }}_{\delta }+{\mathit{C}}_{\delta }\dot{\mathbf{\delta }}+{\mathit{g}}_{\delta }-{\mathit{J}}_2^{\mathrm{T}}{\mathit{h}}_2.\hfill \end{array}$$

(29)

In view of (11), the commanded total moment to be provided by the four propellers, ${\mathit{\mu }}^2$, can be computed as

$${\mathit{\mu }}^2={\mathit{R}}_2^{\mathrm{T}}{\mathit{\mu }}_{0,c}-\mathit{S}\left({\mathit{r}}_{2,C_2}^2\right){\mathit{f}}^2.$$

(30)

Finally, the rotor thrusts can be computed via (12) as

$$\mathit{\sigma }={\mathit{N}}^{-1}\left[\begin{array}{c}{f}_z\\ {\mathit{\mu }}^2\end{array}\right].$$

(31)

5. Stability Analysis

By folding the control inputs (18), (28), and (29) into the ODQuad dynamics (14)–(16), the following set of equations can be written

$$\begin{array}{ccc}\hfill & & {\mathit{M}}_{pp}({\ddot{\mathit{p}}}_0-{\mathbf{\gamma }}_p)+{\mathit{M}}_{p\varphi }({\ddot{\mathit{\varphi }}}_0-{\mathbf{\gamma }}_{\varphi })+{\mathit{M}}_{p\delta }\ddot{\mathbf{\delta }}={\mathbf{0}}_3\hfill \end{array}$$

(32)

$$\begin{array}{ccc}\hfill & & {\mathit{M}}_{p\varphi }^{\mathrm{T}}({\ddot{\mathit{p}}}_0-{\mathbf{\gamma }}_p)+{\mathit{M}}_{\varphi \varphi }({\ddot{\mathit{\varphi }}}_0-{\mathbf{\gamma }}_{\varphi })+{\mathit{M}}_{\varphi \delta }(\ddot{\mathbf{\delta }}-{\mathbf{\gamma }}_{\delta })={\mathbf{0}}_3\hfill \end{array}$$

(33)

$$\begin{array}{ccc}\hfill & & {\mathit{M}}_{p\delta }^{\mathrm{T}}({\ddot{\mathit{p}}}_0-{\mathbf{\gamma }}_p)+{\mathit{M}}_{\varphi \delta }^{\mathrm{T}}({\ddot{\mathit{\varphi }}}_0-{\mathbf{\gamma }}_{\varphi })+{\mathit{M}}_{\delta \delta }(\ddot{\mathbf{\delta }}-{\mathbf{\gamma }}_{\delta })={\mathbf{0}}_2,\hfill \end{array}$$

(34)

where ${\mathbf{0}}_2$ is the ($2\times 1$) vector of zeros. By adding and subtracting ${\mathit{M}}_{p\delta }{\mathbf{\gamma }}_{\delta }$, (32) can be rearranged in the form

$${\mathit{M}}_{pp}({\ddot{\mathit{p}}}_0-{\mathbf{\gamma }}_p)+{\mathit{M}}_{p\varphi }({\ddot{\mathit{\varphi }}}_0-{\mathbf{\gamma }}_{\varphi })+{\mathit{M}}_{p\delta }(\ddot{\mathbf{\delta }}-{\mathbf{\gamma }}_{\delta })+{\mathit{M}}_{p\delta }{\mathbf{\gamma }}_{\delta }={\mathbf{0}}_3.$$

(35)

By defining the vector $\gamma$ and the block matrix $\tilde{\mathit{M}}$ as

$$\gamma =\left[\begin{array}{c}{\gamma }_p\\ {\gamma }_{\varphi }\\ {\gamma }_{\delta }\end{array}\right]\in \mathrm{I}{\mathrm{R}}^8,\tilde{\mathit{M}}=\left[\begin{array}{ccc}{\mathit{O}}_3& O_3& {\mathit{M}}_{p\delta }\\ {\mathit{O}}_3& {\mathit{O}}_3& {\mathit{O}}_{3\times 2}\\ {\mathit{O}}_{2\times 3}& {\mathit{O}}_{2\times 3}& {\mathit{O}}_2\end{array}\right]\in \mathrm{I}{\mathrm{R}}^{8\times 8},$$

where ${\mathit{O}}_2$ and ${\mathit{O}}_{2\times 3}$ are, respectively, the $(2\times 2)$ and $(2\times 3)$ null matrices, Equations (33)–(35) can be rewritten in the following compact form:

$$\begin{array}{c}\hfill \mathit{M}(\ddot{\mathit{q}}-\mathbf{\gamma })+\tilde{\mathit{M}}\mathbf{\gamma }={\mathbf{0}}_8,\end{array}$$

(36)

where ${\mathbf{0}}_8$ is the (8 × 1) vector of zeros. By exploiting Equations (19), (20) and (27), the following error dynamics is obtained:

$$\begin{array}{c}\hfill \ddot{\mathit{e}}+{\mathit{K}}_D\dot{\mathit{e}}+{\mathit{K}}_p\mathit{e}+{\mathit{K}}_I\int \mathit{e}+{\mathit{M}}^{-1}\tilde{\mathit{M}}\mathbf{\gamma }={\mathbf{0}}_8,\end{array}$$

(37)

where $\mathit{e}={\mathit{q}}_d-\mathit{q}$, with ${\mathit{q}}_d={[{\mathit{p}}_{0,d}^{\mathrm{T}},{\mathit{\varphi }}_{0,d}^{\mathrm{T}},{\mathbf{\delta }}_d^{\mathrm{T}}]}^{\mathrm{T}}$, and the matrices ${\mathit{K}}_{🟉}$ ($🟉=\{I,P,D\}$) are

$${\mathit{K}}_{🟉}=\mathrm{blkdiag}\left\{{\mathit{K}}_{🟉,p},{\mathit{K}}_{🟉,\varphi },{\mathit{K}}_{🟉,\delta }\right\}.$$

Define the state vector $\mathit{z}={\left[{\mathit{z}}_1^{\mathrm{T}},{\mathit{z}}_2^{\mathrm{T}},{\mathit{z}}_3^{\mathrm{T}}\right]}^{\mathrm{T}}={\left[\int {\mathit{e}}^{\mathrm{T}},{\mathit{e}}^{\mathrm{T}},{\dot{\mathit{e}}}^{\mathrm{T}}\right]}^{\mathrm{T}}$ and rewrite (37) in the state space form as

$$\begin{array}{ccc}\hfill {\dot{\mathit{z}}}_1& =& {\mathit{z}}_2\hfill \\ \hfill {\dot{\mathit{z}}}_2& =& {\mathit{z}}_3\hfill \\ \hfill {\dot{\mathit{z}}}_3& =& -({\mathit{K}}_D{\mathit{z}}_3+{\mathit{K}}_p{\mathit{z}}_2+{\mathit{K}}_I{\mathit{z}}_1)-{\mathit{M}}^{-1}\tilde{\mathit{M}}\mathbf{\gamma },\hfill \end{array}$$

(38)

which can be rewritten as

$$\dot{\mathit{z}}=\left[\begin{array}{ccc}{\mathit{O}}_8& {\mathit{I}}_8& {\mathit{O}}_8\\ {\mathit{O}}_8& {\mathit{O}}_8& {\mathit{I}}_8\\ -{\mathit{K}}_I& -{\mathit{K}}_P& -{\mathit{K}}_D\end{array}\right]z+\left[\begin{array}{c}{\mathbf{0}}_8\\ {\mathbf{0}}_8\\ -{\mathit{M}}^{-1}\tilde{\mathit{M}}\mathbf{\gamma }\end{array}\right]=\mathit{K}\mathit{z}+\tilde{\mathbf{\gamma }},$$

(39)

where ${\mathit{I}}_8$ and ${\mathit{O}}_8$ are the $(8\times 8)$ identity and null matrices, respectively. In other words, the system (39) can be seen as a nominal linear system ($\dot{\mathit{z}}=\mathit{K}\mathit{z}$) with a perturbation term $\tilde{\gamma }$. Since matrix K is in block companion form and can be proven to be Hurwitz [37], the nominal system is exponentially stable.

As for the perturbation term, the following property of the inertia matrix can be exploited [34,38]:

Property 1.

M is symmetric and positive definite; therefore, if ${\lambda }_{min}\left(·\right)$ (${\lambda }_{max}\left(·\right)$) denotes the minimum (maximum) eigenvalue, it is

$$0<{\lambda }_{min}\left(\mathit{M}\right){\mathit{I}}_8\le \mathit{M}\left(\mathit{q}\right)\le {\lambda }_{max}\left(\mathit{M}\right){\mathit{I}}_8.$$

(40)

Thus, based on Property 1, the following holds:

$${\lambda }_{max}^{-1}\left(\mathit{M}\right)<∥{\mathit{M}}^{-1}∥<{\lambda }_{min}^{-1}\left(\mathit{M}\right).$$

(41)

Regarding matrix $\tilde{\mathit{M}}$, it is worth considering the explicit expression of its sole non-null block (6), i.e.,

$${\mathit{M}}_{p\delta }=\left[\begin{array}{cc}-\left(m_1\mathit{S}\left({\mathit{r}}_{1,C_1}\right)+m_2\mathit{S}\left({\mathit{r}}_{2,C_2}\right)\right){\mathit{x}}_0,& -m_2\mathit{S}\left({\mathit{r}}_{2,C_2}\right){\mathit{x}}_1\end{array}\right],$$

(42)

where ${\mathit{x}}_0$ and ${\mathit{x}}_1$ are unit vectors and all the other elements are constant and depend on the system’s geometrical and inertial properties. Thus, its norm is constant

$$∥\tilde{\mathit{M}}∥=∥{\mathit{M}}_{p\delta }∥=\Lambda .$$

(43)

Finally, the term $\mathbf{\gamma }$ can be written in the explicit form as

$$\gamma ={\ddot{\mathit{q}}}_d+{\mathit{K}}_D\dot{\mathit{e}}+{\mathit{K}}_P\mathit{e}+{\mathit{K}}_I\int \mathit{e}.$$

The following assumption on the planned trajectory is made:

Assumption 1.

The planned acceleration ${\ddot{\mathit{q}}}_d$ is bounded $\forall t\in \mathcal{T}$, i.e., $∥{\ddot{\mathit{q}}}_d\left(t\right)∥\le \ddot{\overline{q}}$.

Therefore, the non-null block of the perturbation term can be rewritten as sum of two terms

$${\tilde{\gamma }}_3=-\left({\mathit{M}}^{-1}\tilde{\mathit{M}}{\ddot{\mathit{q}}}_d+{\mathit{M}}^{-1}\tilde{\mathit{M}}\left({\mathit{K}}_D\dot{\mathit{e}}+{\mathit{K}}_P\mathit{e}+{\mathit{K}}_I\int \mathit{e}\right)\right).$$

(44)

The first term, in view of (41), (43) and of Assumption 1, is norm-bounded $\forall t$

$${\displaystyle ∥{\mathit{M}}^{-1}\tilde{\mathit{M}}{\ddot{\mathit{q}}}_d∥\le \frac{1}{{\lambda }_{min}\left(\mathit{M}\right)}\Lambda \ddot{\overline{\mathit{q}}}.}$$

(45)

The second term in (44) is a vanishing perturbation (in the sense of [39]), since the following chain of inequalities holds:

$$\begin{array}{ccc}\hfill ∥{\mathit{M}}^{-1}\tilde{\mathit{M}}\left({\mathit{K}}_D\dot{\mathit{e}}+{\mathit{K}}_P\mathit{e}+{\mathit{K}}_I\int \mathit{e}\right)∥& \le & {\displaystyle \frac{1}{{\lambda }_{min}\left(\mathit{M}\right)}\Lambda ∥{\mathit{K}}_D\dot{\mathit{e}}+{\mathit{K}}_p\mathit{e}+{\mathit{K}}_{\mathit{I}}\int \mathit{e}∥}\hfill \\ & \le & {\displaystyle \frac{1}{{\lambda }_{min}\left(\mathit{M}\right)}\Lambda k_M∥\mathit{z}∥,}\hfill \end{array}$$

(46)

where $k_M=max\{∥{\mathit{K}}_P∥,∥{\mathit{K}}_D∥,∥{\mathit{K}}_I∥\}$.

Thus, by recurring to the theory of perturbed systems [39], the effects of the two perturbation terms can be summarized as follows:

• by virtue of Lemma 9.1 in [39], the origin of the system (39), under the vanishing perturbation is an exponentially stable equilibrium;
• by virtue of Lemma 9.2 in [39], the origin of the system (39), under the non-vanishing bounded perturbation is uniformly bounded.

By exploiting the linearity of the nominal system (39), it can be stated that, under Assumption 1, the error z is uniformly bounded. Moreover, if the planned acceleration vanishes after a time instant $t_f\in \mathcal{T}$, the error converges to zero exponentially.

6. Interaction Wrench Compensation

In tasks involving contact with the environment, e.g., manipulation or cooperative transportation, the capability of exerting both forces and moments without modifying the orientation of the load make the omnidirectional platforms preferable with respect to standard multirotors [8,40].

Moreover, even in simple transportation tasks, it is of the utmost importance to react to external disturbances due to contact with the object by compensating the wrench exerted by the object. Namely, an estimation of the wrench exerted by the object on the vehicle is fed back to the controller as an additional term in (18), (28), and (29). In fact, in the presence of contact with an object, the dynamics (5) of the ODQuad becomes

$$\mathit{M}\left(\mathit{q}\right)\ddot{\mathit{q}}+\mathit{C}(\mathit{q},\dot{\mathit{q}})\dot{\mathit{q}}+\mathit{g}\left(\mathit{q}\right)=\mathit{\eta }+{\mathit{J}}_C^{\mathrm{T}}{\mathit{h}}_C,$$

(47)

where ${\mathit{J}}_C$ is the Jacobian matrix that relates the velocity of the contact point to the velocity of the generalized coordinates q, and ${\mathit{h}}_C$ is the wrench exerted by the object on the contact point of the ODQuad. Given an estimate, ${\widehat{\mathit{h}}}_C$, of ${\mathit{h}}_C$, the control outputs (18), (28), and (29) become

$$\begin{array}{ccc}\hfill {\mathit{f}}_0& =& {\mathit{M}}_{pp}{\mathbf{\gamma }}_p+{\mathit{M}}_{p\varphi }{\mathbf{\gamma }}_{\varphi }+{\mathit{C}}_p{\dot{\mathit{p}}}_0+{\mathit{g}}_p-{\mathit{J}}_{C,\mathit{f}}^{\mathrm{T}}{\widehat{\mathit{h}}}_C,\hfill \\ \hfill {\mathit{\mu }}_0& =& {\mathit{M}}_{p\varphi }^{\mathrm{T}}{\mathbf{\gamma }}_p+{\mathit{M}}_{\varphi \varphi }{\mathbf{\gamma }}_{\varphi }+{\mathit{M}}_{\varphi \delta }{\mathbf{\gamma }}_{\delta }+{\mathit{C}}_{\varphi }{\dot{\mathit{\varphi }}}_0+{\mathit{g}}_{\varphi }-{\mathit{J}}_{C,\mu }^{\mathrm{T}}{\widehat{\mathit{h}}}_C\hfill \\ \hfill {\mathit{\tau }}_c& =& {\mathit{M}}_{p\delta }^{\mathrm{T}}{\mathbf{\gamma }}_p+{\mathit{M}}_{\varphi \delta }^{\mathrm{T}}{\mathbf{\gamma }}_{\varphi }+{\mathit{M}}_{\delta \delta }{\mathbf{\gamma }}_{\delta }+{\mathit{C}}_{\delta }\dot{\mathbf{\delta }}+{\mathit{g}}_{\delta }-{\mathit{J}}_2^{\mathrm{T}}{\mathit{h}}_2-{\mathit{J}}_{C,\tau }^{\mathrm{T}}{\widehat{\mathit{h}}}_C,\hfill \end{array}$$

(48)

where ${\mathit{J}}_{C,f}$, ${\mathit{J}}_{C,\mu }$, and ${\mathit{J}}_{C,\tau }$ are obtained from ${\mathit{J}}_C$ by selecting, respectively, the first three columns, the forth to sixth columns, and the last two columns.

Wrench Estimation

In order to measure the wrench exerted by the object on the ODQuad, a force/torque sensor could be mounted on the contact point robot-object [6]. This is usually capable to provide a reliable measure, but increases both the cost and the weight of the robot. In the aerial robotics field, a more viable solution is the adoption of a wrench estimator, which can provide a sufficiently accurate estimation [8].

In this work, the interaction wrench, exerted by the manipulated object on the vehicle, has been estimated via a momentum-based observer [41]. With reference to the system dynamics (47), the angular momentum $\mathit{\nu }$ can be computed as

$$\mathit{\nu }=\mathit{M}\left(\mathit{q}\right)\dot{\mathit{q}}.$$

(49)

Let us recall the following property of dynamic model (5) [34,38]:

Property 2.

There always exists a choice of C, such that

$$\dot{\mathit{M}}\left(\mathit{q}\right)=\mathit{C}(\mathit{q},\dot{\mathit{q}})+{\mathit{C}}^{\mathrm{T}}(\mathit{q},\dot{\mathit{q}}).$$

(50)

By exploiting Property 2 and model (5), the time-derivative of (49) can be expressed as

$$\dot{\mathbf{\nu }}={\mathit{C}}^{\mathrm{T}}(\mathit{q},\dot{\mathit{q}})\dot{\mathit{q}}-\mathit{g}\left(\mathit{q}\right)+\mathit{\eta }+{\mathit{J}}_C^{\mathrm{T}}{\mathit{h}}_C.$$

(51)

An estimate of ${\mathit{h}}_C$ is, then, computed as

$${\widehat{\mathit{h}}}_C={\mathit{J}}_C^{†}{}^{\mathrm{T}}{\mathit{K}}_o\left[(\mathbf{\nu }\left(t\right)-\mathbf{\nu }\left(t_0\right))-{\int }_{t_0}^t({\mathit{C}}^{\mathrm{T}}(\mathit{q},\dot{\mathit{q}})\dot{\mathit{q}}-\mathit{g}\left(\mathit{q}\right)+\mathit{\eta }+{\mathit{J}}_C^{\mathrm{T}}{\widehat{\mathit{h}}}_C)d\varsigma \right],$$

(52)

where t and $t_0$ are the current and initial time instant, respectively, ${\mathit{K}}_o$ is a positive definite gain matrix, and ${\mathit{J}}_C^{†}$ is the right pseudo-inverse of the Jacobian matrix.

The estimation dynamics is

$${\dot{\widehat{\mathit{h}}}}_C+{\mathit{K}}_o{\widehat{\mathit{h}}}_C={\mathit{K}}_o{\mathit{h}}_C,$$

(53)

which is a first-order low-pass dynamic system. Under the assumption of constant or slowly varying interaction wrench, ${\widehat{\mathit{h}}}_C$ converges to ${\mathit{h}}_C$ when $t\to \infty$ for any positive definite gain matrix ${\mathit{K}}_o$. The matrix ${\mathit{K}}_o$ is chosen as a trade-off between the convergence rate and the filtering properties of the observer: larger values of the gains allow faster convergence, while smaller values allow us to filter the high-frequency noise.

7. Simulation Results

The proposed controller has been tested via numerical simulation in the MATLAB^© SimMechanics environment. The geometric and dynamic parameters of the ODQuad as well as details on the multi-body SimMechanics model can be found in [17]. In order to verify the effectiveness and the robustness of the controller, simulations in free-space motion and in the presence of contact with an object have been conducted. In the first one, in order to show the capability of controlling both position and orientation at same time, a trajectory in free-space has been commanded to the vehicle platform; in the latter simulation, the behavior in the presence of a task requiring grasp and transportation of an object has been tested.

The following realistic assumptions have been considered:

• reliable, but noisy, measurements of the platform position and orientation are available, e.g., provided by a motion capture system and an IMU;
• white noise has been added to measures of the platform position (with standard deviation ${10}^{-2}$ m) and orientation (with standard deviation ${10}^{-2}$ rad);
• the angular positions of the roll and pitch joints are available via sensors usually integrated into the servomotors, while the angular velocities are obtained via numerical filtering. Thus, the orientation of the rotor frame relative to the platform frame can be computed via (1);
• centrifugal and Coriolis terms have been neglected in control laws (18), (28), and (29);
• viscous friction has been included in the simulation model but not considered in the control design.

The control gains that are reported in

Table 1. Gain matrices for the auxiliary inputs (19), (20) and (27).

Gain	Value	Gain	Value	Gain	Value
${\mathit{K}}_{P,p}$	$\left[\begin{array}{ccc}25& 0& 0\\ 0& 25& 0\\ 0& 0& 25\end{array}\right]$	${\mathit{K}}_{D,p}$	$\left[\begin{array}{ccc}6.3& 0& 0\\ 0& 6.3& 0\\ 0& 0& 6.3\end{array}\right]$	${\mathit{K}}_{I,p}$	$\left[\begin{array}{ccc}0.025& 0& 0\\ 0& 0.025& 0\\ 0& 0& 0.025\end{array}\right]$
${\mathit{K}}_{P,\varphi }$	$\left[\begin{array}{ccc}375& 0& 0\\ 0& 375& 0\\ 0& 0& 125\end{array}\right]$	${\mathit{K}}_{D,\varphi }$	$\left[\begin{array}{ccc}25.5& 0& 0\\ 0& 25.5& 0\\ 0& 0& 8.5\end{array}\right]$	${\mathit{K}}_{I,\varphi }$	$\left[\begin{array}{ccc}150& 0& 0\\ 0& 150& 0\\ 0& 0& 50\end{array}\right]$
${\mathit{K}}_{P,\delta }$	$\left[\begin{array}{cc}375& 0\\ 0& 375\end{array}\right]$	${\mathit{K}}_{D,\delta }$	$\left[\begin{array}{cc}25.5& 0\\ 0& 25.5\end{array}\right]$	${\mathit{K}}_{I,\delta }$	$\left[\begin{array}{cc}150& 0\\ 0& 150\end{array}\right]$

have been tuned via a trial-and-error procedure by taking into account that the internal loop must be faster than the outer one and with the goal of minimizing the tracking error. The observer gain is reported in

Table 2. Momentum observer gain.

Gain	Value
${\mathit{K}}_o$	$\mathrm{diag}\left[\begin{array}{cccccccc}1000& 1000& 1000& 400& 400& 35& 200& 200\end{array}\right]$

, and it has been set via a trial-and-error procedure by looking for the best trade-off between the convergence rate and the robustness to measurement noise.

In order verify the trajectory tracking capabilities, in the first simulation case study, time-varying desired position (see Figure 3a) and orientation (see Figure 3b) are commanded to the ODQuad platform. As previously remarked, it is worth noticing that, differently from common quadrotors, motion involving all six degrees of freedom can been assigned.

The obtained results are reported in Figure 4a,b in terms of position (${\mathit{p}}_{0,d}-{\mathit{p}}_0$) and orientation (${\mathit{\varphi }}_{0,d}-{\mathit{\varphi }}_0$) errors. In both cases, satisfactory performance is achieved, since the maximum position error is about 5 mm along the axes x and y and 15 mm along the z axis, while the orientation error oscillates around zero, with some peaks below 5 deg.

The performance of the internal control loop is measured by the roll-pitch joint tracking errors ((${\alpha }_{des}-\alpha$) and (${\beta }_{des}-\beta$)), reported in Figure 5. Furthermore, in this case, the peak error is below 5 deg, and the oscillation magnitude is about 2 degrees.

The second simulation case study is aimed to prove the controller robustness in the presence of an external disturbance. In this case, the control outputs (18), (28), and (29) are replaced by (48). A task involving grasping and transportation of an object has been considered: the ODQuad moves to the grasping point, grasps the object, and then rigidly holds it during its motion. Figure 6 shows some snapshots of the simulation.

The position tracking error, reported in Figure 7a, increases along the z axis during the grasping phase (at about 10 s), as expected, and comes back to the initial value after 3 s. In other words, after an initial transition due to the first contact with the object, the wrench estimator allows to correctly compensate the effect of the additional load attached to the vehicle.

Then, during the transportation, an error is experienced along the y axis and in the platform orientation (with a peak of about 6 deg), probably due to the dynamics of the gimbal mechanism.

Figure 8a reports the roll-pitch joint tracking errors: the roll and pitch errors during the transportation oscillate between −5 and 5 deg. Figure 8b reports the actuation torques of the roll-pitch joints: it can be noticed that, even in the presence of an external load, the required effort can be easily obtained via off-the-shelf motors.

Finally, the estimated interaction forces and torques, computed via the observer (52), are reported in Figure 9a,b. In detail, the force along z axis correctly converges to the load weight (Figure 9a), while small forces along x and y axes are experienced during the transportation, due to the object inertia and the unmodeled ODQuad’s dynamics (Coriolis and friction terms are neglected both in controller and in observer). Regarding the estimated torques (Figure 9b), small torques are experienced around the x and y axes due to the misalignment during the motion between the object and the platform CoM.

8. Discussion

This paper has reported the design and verification of a model-based motion control scheme for an omnidirectional UAV, the ODQuad, enriched with a wrench estimation approach to manage situations in which contact with the external environment is involved. Verification has been carried out via simulations, where the simulation model includes several real-world effects (e.g., measurement noise, unmodeled friction). The results confirm that the adopted control scheme achieves satisfactory performance in terms of tracking capabilities of free-space motion as well as of correct estimation and compensation of the contact wrench. This work is part of a long-term project aimed at developing a complete prototype of the ODQuad. Ongoing work is indeed focused on the mechanical, hardware, and software development of the ODQuad. Current research work is focused on the experimental verification of the designed control scheme on the developed prototype, as well as on the design and verification of interaction control schemes for tasks involving contact with the external environment.