Aerial Tele-Manipulation with Passive Tool via Parallel Position/Force Control ^†

Abstract

This paper addresses the problem of unilateral contact interaction by an under-actuated quadrotor UAV equipped with a passive tool in a bilateral teleoperation scheme. To solve the challenging control problem of force regulation in contact interaction while maintaining flight stability and keeping the contact, we use a parallel position/force control method, commensurate to the system dynamics and constraints in which using the compliant structure of the end-effector the rotational degrees of freedom are also utilized to attain a broader range of feasible forces. In a bilateral teleoperation framework, the proposed control method regulates the aerial manipulator position in free flight and the applied force in contact interaction. On the master side, the human operator is provided with force haptic feedback to enhance his/her situational awareness. The validity of the theory and efficacy of the solution are shown by experimental results. This control architecture, integrated with a suitable perception/localization pipeline, could be used to perform outdoor aerial teleoperation tasks in hazardous and/or remote sites of interest.

1. Introduction

Aerial robotics has become increasingly popular in research, industry, and for commercial applications. Beyond the traditional visual inspection functionality that made them widely used and appreciated, aerial robots have recently received profound interest for applications which require to seek, establish, and maintain some sort of physical interaction with the environment in order to fulfill a certain task. Relevant examples are epitomized by maintenance operations in the energy sector, for example, oil, gas, refinery, and power plants, in particular, to perform non-destructive tests that require keeping some sensors in touch with objects not easily accessible by a human, due to their installation altitude, and also in hazardous environments [1,2]. Apart from their growing use in industrial and civil sites, these systems are starting to also be employed for the in-contact documentation of historical buildings [3]. Other applications of aerial interaction involve the transportation of cable-suspended payloads [4] and packages for search and rescue missions [5].

Aerial manipulation is the deliberately controlled physical interaction of an aerial manipulator with objects in its environment. For an extensive overview of the works on this topic, the interested reader is referred to [6,7]. By aerial manipulator we mean a small size Vertical Take-Off and Landing (VTOL) Unmanned Aerial Vehicle (UAV) equipped with a manipulation tool. This manipulation tool is either an active robotic arm manipulator or a passive tool.

When dexterous manipulation by an aerial manipulator is not required, for example, when the robot is intended to apply desired force vectors to an object in order to push, inspect, or probe its surface, or when the normal grippers are not effective, for example, for an object with a wide flat surface, the use of a lightweight passive tool is preferable to a heavier active arm manipulator, as the smaller payload imposed to the aerial robot results in a more energy-efficient system and longer operational flight time. Moreover, simplicity and low weight of a passive tool allow the usage of a broader range of UAVs.

Despite significant achievements in the fully autonomous control of drones, limited problem-solving capabilities, inadequacy in unexpected environmental conditions, legal restrictions, and imperfect position control [8] often require the presence of human operator(s) in aerial manipulation tasks. In bilateral teleoperation schemes, the human capabilities are enhanced by providing them with tangible interaction information of the remote side in the form of force and motion feedbacks (haptic feedback), besides the traditional visual feedback.

In this paper, we propose an aerial manipulation solution using passive tools with a compliant end-effector. Applying a desired force profile in a uni-lateral contact, and at the same time maintaining the flight stability, that is, position and orientation control, and keeping the contact stable, that is, to avoid losing contact and sliding over the contact surface, represents a challenging control problem, especially when it is performed with an under-actuated aerial manipulator in a bilateral teleoperation loop. Fully-actuated aerial manipulators have been demonstrated to be more effective for this kind of application [8,9,10,11] but consume more energy due to internal forces. That is why the use of under-actuated UAVs is investigated in this work. Our aerial manipulator is a quadrotor UAV equipped with a lightweight passive tool rigidly attached to the top of it (Figure 1 and Figure 2). The end-effector has a mechanical damper on its surface to smooth free flight to contact transition, and a passive compliant spherical joint to keep the contact while changing the orientation. This compliant mechanism, along with appropriate control policy conforming with the system constraints, allows involving all the robot’s degrees of freedom to generate the desired force vector. The desired motion and force of the aerial manipulator are attained using a parallel position/force control scheme within a bilateral teleoperation control framework.

The proposed control scheme regulates the aerial manipulator pose in free flight and the applied force in contact conditions. A human operator using a haptic device (with a limited workspace) commands the aerial manipulator pose (with virtually unlimited workspace) in free flight. When the aerial manipulator’s end-effector comes in contact with the environment, the haptic device movement is interpreted as desired force. Position control in free flight and force control in contact are achieved by utilizing a cascaded parallel position/force controller. The reference pose is composed by the operator’s pose command (free flight pose command) and the output of a force controller in an outer loop. To avoid losing the contact, the desired force is always kept in a feasible range that satisfies the friction and compliance constraints. On the master side, the human operator is provided with force feedback proportional to the robot’s velocity in free flight and the applied force in contact.

The aerial tele-manipulation system presented in this paper may contribute to addressing and solving a broad class of relevant use-case applications where a UAV is remotely operated in hardly accessible and life-threatening sites, for example, at high altitudes, by a human safely located in a protected place, thus relieving him/her from potentially dangerous tasks. A conceptual example is depicted in Figure 1. Apart from the aforementioned applications of remote sensor placement, contact holding and remote button pushing, another idea that could be envisioned is to employ such a system to push boxes located on a shelf onto a conveyor belt, in an industrial warehouse scenario. Thanks to the enhanced situational awareness guaranteed by the haptic feedback, the operator could easily regulate the force applied to the load. Furthermore, the designed control law would ensure that the contact is maintained throughout the manipulation. As should be appreciated, many other relevant applications involving the aforementioned conditions can be easily conceived.

The rest of the paper is organized as follows. The next subsection reviews the related works and the contribution of the paper. Section 2 explains the platform and its dynamic model in the teleoperation system. Section 3 presents the proposed control approach, the stability analysis of which is presented in Section 4. Experimental results are presented in Section 5, while concluding remarks and hints about intended future works are outlined in Section 6.

Related Works and Contribution

There has been a growing interest in aerial robots with physical interaction in the past few years, and many research projects such as [12] have focused on this context. In the following, we try to concisely provide a general overview of the state of the art of this broad topic, focusing then on the works more closely related to the one presented in this paper. Aerial physical interaction with the environment can be macro-categorized as:

(i)

using active manipulators; and

(ii)

using passive tools;

and naturally, different mechanical solutions demand different controllers.

To mention some examples belonging to the first group, in [13] the authors designed and installed a small parallel manipulator on one side of a VTOL, while the use of one and two serial manipulators to grasp objects was proposed in [14] and in [15], respectively. In these works, the authors implemented and validated different instances of hybrid force control. A visual servoing approach to control a quadrotor equipped with a serial manipulator is suggested by [16], while a passivity-based adaptive controller, which can be applied to both position and velocity control to guide a quadrotor aerial manipulator, is utilized by [17]. Furthermore, the behavioral control of an aerial manipulator is presented in [18]. In the multi-robot scenario, a team of quadrotors, equipped with serial manipulators, controlled by a visual servoing technique, is demonstrated in [19]. Differently from these works, we tackle the scenario of aerial tele-manipulation with haptic feedback of an object of interest via parallel/force control, using a passive tool.

In the second group, which encompasses the research presented in this paper, different works have focused on solutions to applications requiring less dexterous manipulation capabilities, but with the benefits of being more cost-effective, versatile, and lightweight, thus allowing for longer operational flight time. In [20], a quadrotor UAV equipped with a rigid tool, controlled based on a mapping between the desired vehicle attitude and the commanded force, is used to establish contacts with the environment. The control strategy therein is designed as a variation of near-hovering control, and does not take into account the friction cone constraints that allow maintenance of the contact between the robot tool-tip and the environment, while instead our strategy does. The authors of [21] propose instead an interesting combination of both mechanical design and control strategy to handle collisions and interaction in a more compliant way, without focusing on direct force control. A hybrid position/force control framework for a quadrotor is presented in [22], which allows the exertion of forces with the quadrotor airframe, without any tool, on the environment. A similar control approach is also adopted in [23] for the very relevant task of tool operation with quadrotors. Planning and control for an aerial robot in contact with its environment, based again on a hybrid position/force switching controller, is presented in [24], where obstacle avoidance is also performed. The control schemes of [22,23,24] are based on the decoupling of axes of the applied force and motion; that is, force is applied in the motion constrained axes while on the other axes the motion is controlled. Differently from [22,23,24] and other similar approaches, in this work, we do consider friction constraints in a compliant uni-lateral contact to avoid slipping of the tool in the non-constrained axes of motion and to maintain the contact, which allows us to generate 3D force vectors. For this reason, we use a parallel (and not hybrid) position/force control approach, which also deploys rotational degrees of freedom to attain a broader feasible range of forces. It is worth noting that the traditional parallel position/force controller applied to generic six-DoF grounded manipulators is not directly applicable to our system, as it is an under-actuated floating robot. All the wrench components of the contact interaction are transmitted to the robot CoM and affect its orientation, by which the position is controlled, which makes the problem more challenging, especially in the transition from free flight to contact interaction and vice versa.

Furthermore, in all the aforementioned papers, the use of haptic feedback is not envisioned. Haptic teleoperation of UAVs is mainly used for obstacle avoidance in free flight, aiming to improve the situational awareness of the human operator using haptic feedback, such as the generic hierarchical passive teleoperation control architecture presented in [25]. We use haptic feedback not only to improve the performance of position tracking in free flight, but also to reflect the applied force, in order to let the user feel the force in the contact interaction, which eventually leads to a more accurate tele-manipulation. The stability analysis and experimental results validate the proposed bilateral teleoperation scheme for aerial manipulation using passive tools.

To the best of our knowledge, the problem of teleoperating VTOL UAVs with haptic feedback to establish contact and apply forces on objects of interest while ensuring the compliance with friction constraints and avoiding slipping has not yet been deeply investigated by the community researching aerial physical interaction. We introduced the aerial haptic tele-manipulation idea in [26]. The present paper completes, improves, and extends this concept in the following ways:

(1)

the force control in [26] is based on a mapping that calculates the appropriate robot’s desired orientation to generate the desired force; in this work, we use a more efficient sensor-based closed-loop force control;

(2)

the force controller considers the limited friction of the end-effector and object surfaces, and utilizes the independently controlled yaw motion, which allows a wider range of feasible force commands by using the passive compliant spherical joint mechanism of the end-effector, enforcing the contact maintenance;

(3)

the stability analysis of the system is presented;

(4)

this work presents experimental results of tracking 3D force vectors applied to both stationary and moving objects.

2. Dynamic Model

The bilateral teleoperation system consists of: a human operator, a haptic device (master), an aerial manipulator (slave), and the remote environment (cf. Section 3). This section presents the dynamic model of the aerial manipulator (a quadrotor UAV equipped with a passive lightweight tool) in contact with the environment. The tool is rigidly connected to the top of the quadrotor, and a lightweight rigid link ensures enough room between the propellers and the end-effector to allow safe contact with the environment. A compliant spherical joint connects the lightweight rigid link to the end-effector, and an elastic shock absorber damper along with the compliant joint help to establish smoother contacts (Figure 2). In the following we assume that the spherical joint rotation is small, so the springs remain in their linear region, the weight of the tool is negligible, and the tool link is rigid.

Let us define the world frame $\mathcal{W}:\{O_{\mathcal{W}},{\mathit{x}}_w,{\mathit{y}}_w,{\mathit{z}}_w\}$, the body frame $\mathcal{B}:\{O_{\mathcal{B}},{\mathit{x}}_b,{\mathit{y}}_b,{\mathit{z}}_b\}$ for the robot, and the contact frame $\mathcal{C}:\{O_{\mathcal{C}},\mathit{o},\mathit{t},\mathit{n}\}$ placed at the contact point (Figure 3). The position of $O_{\mathcal{B}}$ in $\mathcal{W}$ is indicated with $\mathit{p}={\left[xyz\right]}^{\top }\in {\mathbb{R}}^3$, $R\in SO\left(3\right)$ is the rotation matrix representing the orientation of $\mathcal{B}$ in $\mathcal{W}$. We consider the RPY parameterization of R, that is, $\mathit{\eta }:={\left[\varphi \theta \psi \right]}^{\top }\in {\mathbb{R}}^3$, where $\varphi ,\theta ,\psi$ are the roll, pitch and yaw angles, respectively, and are bounded as $\frac{-\pi }{2}<\varphi <\frac{\pi }{2}$, $\frac{-\pi }{2}<\theta <\frac{\pi }{2}$, and $-\pi <\psi \le \pi$. The end-effector position expressed in $\mathcal{W}$ is ${\mathit{p}}_{\mathit{e}}=\mathit{p}+R\mathit{d}$, where $\mathit{d}$ is the end-effector position vector in $\mathcal{B}$. The angular velocity of $\mathcal{B}$, denoted by $\mathit{\omega }\in {\mathbb{R}}^3$, is related to the derivative of Euler angles $\dot{\mathit{\eta }}$ by $\mathit{\omega }=E\left(\mathit{\eta }\right)\dot{\mathit{\eta }}$ where $E\left(\mathit{\eta }\right)\in {\mathbb{R}}^{3\times 3}$ is defined according to R. The robot dynamics can be expressed in terms of robot pose $\mathit{x}={\left[{\mathit{p}}^{\top }{\mathit{\eta }}^{\top }\right]}^{\top }\in {\mathbb{R}}^6$, in $\mathcal{W}$, as follows.

$$M_s\left(\mathit{x}\right)\ddot{\mathit{x}}+C_s(\mathit{x},\dot{\mathit{x}})\dot{\mathit{x}}+\mathit{g}\left(\mathit{x}\right)=\mathit{w}+{\mathit{w}}_{\mathit{e}},$$

(1)

where $M_s=diag\{m{I}_{3\times 3},M\}$, with $M=E^{\top }JE$, is the inertia matrix in which $m\in {\mathbb{R}}^{+}$, $J\in {\mathbb{R}}^{3\times 3}$ are the robot mass and moment of inertia matrix, $C_s=diag\{{\mathbf{0}}_{3\times 3},C\}$, with $C=E^{\top }(J\dot{E}+S\left(E\dot{\mathit{\eta }}\right)JE)$ includes the Coriolis/centripetal dynamics in which $S\left(\mathit{a}\right)$ is the skew-symmetric matrix of a generic vector $\mathit{a}$; $\mathit{g}={[mg{\mathit{z}}_w^{\top },{\mathbf{0}}_3]}^{\top }$ is the gravity vector with g being the gravity acceleration constant. $\mathit{w}={[{\left(u_{z}R{\mathit{z}}_b\right)}^{\top },{\mathit{u}}_{\eta }^{\top }]}^{\top }$ is the robot control wrench in which the magnitude of the total thrust acting along the ${\mathit{z}}_b$ direction is denoted with $u_z\in {\mathbb{R}}^{+}$, and ${\mathit{u}}_{\eta }\in {\mathbb{R}}^3$ is the rotational control moment. ${\mathit{w}}_{\mathit{e}}={\left[{{\mathit{f}}_{\mathit{t}}}^{\top }{(S\left(\mathit{d}\right){\mathit{f}}_{\mathit{t}}+{\mathit{\tau }}_{\mathit{r}})}^{\top }\right]}^{\top }$ is the external wrench applied to the system. The wrench applied to the end-effector is modeled as spring wrench (${\mathit{f}}_{\mathit{t}},{\mathit{\tau }}_{\mathit{r}}$), expressed in $\mathcal{W}$ as ${\mathit{f}}_{\mathit{t}}=-R_c^w{K}_t\mathit{\delta }{\mathit{p}}^{\mathit{c}}$ and ${\mathit{\tau }}_{\mathit{r}}=-R_c^w{K}_r\mathit{\delta }{\mathit{\eta }}^{\mathit{c}}$, where $R_c^w$ is the rotation matrix representing the orientation of $\mathcal{C}$ w.r.t. $\mathcal{W}$, and $K_t=diag\{k_{to},k_{tt},k_{tn}\},K_r=diag\{k_{ro},k_{rt},k_{rn}\}$ are the diagonal stiffness matrices, $\mathit{\delta }{\mathit{p}}^{\mathit{c}}={\left[\delta o\delta t\delta n\right]}^{\top }$ is the compression of the linear spring, and $\mathit{\delta }{\mathit{\eta }}^{\mathit{c}}={\left[\delta {\eta }_o\delta {\eta }_t\delta {\eta }_n\right]}^{\top }$ is the compression of the angular spring, expressed in $\mathcal{C}$, respectively.

We consider the soft finger model for the contact in which all three components of force and the normal component of the torque are transmitted in the contact independently [27]. Considering this model and the force torque balance for the end-effector, the wrench transmitted by the end-effector, that is, applied force $\mathit{f}={\left[f_o{f}_t{f}_n\right]}^{\top }$ and torque $\mathit{\tau }={\left[00{\tau }_n\right]}^{\top }$, in $\mathcal{C}$, can be obtained as:

$$\left\{\begin{array}{c}\mathit{f}=-{{\mathit{f}}_{\mathit{t}}}^c-\frac{1}{{\parallel \mathit{d}\parallel }^2}S\left({\mathit{d}}^c\right){\mathit{\tau }}_r^c=K_t\mathit{\delta }{\mathit{p}}^{\mathit{c}}+\frac{1}{{\parallel \mathit{d}\parallel }^2}S\left({\mathit{d}}^c\right)K_r\mathit{\delta }{\mathit{\eta }}^{\mathit{c}}\hfill \\ {\tau }_n=k_{rn}\delta {\eta }_n.\hfill \end{array}\right.$$

(2)

Equation (2) is used in the next section to define the control inputs of the force controller. The constraints of the force and torque to keep the contact are as follows:

$$\left\{\begin{array}{c}0\le f_n\hfill \\ \sqrt{f_o^2+f_t^2}/{\mu }_s\le f_n\hfill \\ |{\tau }_n|/{\mu }_t\le f_n\hfill \\ \sqrt{{\tau }_o^2+{\tau }_t^2}/r_d\le f_n,\hfill \end{array}\right.$$

(3)

where ${\mu }_s$ and ${\mu }_t$ are the linear and angular friction coefficients of the end-effector surface with the object surface, and $r_d$ is the end-effector surface disk radius. The first constraint is the unilateral condition, the second one is to avoid translation slippage, the third one is to avoid rotational slippage, and the last one is to prevent the disk lifting up. The constraint (3) is used in the next section to modify the desired force vector in order to keep the contact.

The dynamic model of the haptic interface, that is, the master robot, considering an inverse dynamic controller with gravity and nonlinear compensation [28], in the operational workspace, can be described as

$$M_m{\ddot{\mathit{x}}}_m+K_{mD}{\dot{\mathit{x}}}_m=K_{mP}{\tilde{\mathit{x}}}_m+{\mathit{f}}_{\mathit{h}}-{\mathit{f}}_{\mathit{c}},$$

(4)

where ${\mathit{x}}_m\in {\mathbb{R}}^{n_m}$ ($n_m$ is the number of master robot’s actuated DOFs is the master device pose, ${\tilde{\mathit{x}}}_m$ is the pose error, $M_m$ is the diagonalized inertia matrix, $K_{mP},K_{mD}$ are the PD-controller gains regulating the desired master robot pose, ${\mathit{f}}_{\mathit{h}}$ is the force applied by the human operator and ${\mathit{f}}_{\mathit{c}}$ is the reflected teleoperation force.

3. Control System

The proposed bilateral teleoperation scheme controls the robot’s position in the free flight, in an unlimited workspace using a limited workspace haptic device, and regulates the force tracking during physical interaction, keeping the contact based on the human operator’s commanded force. The human operator is provided with force feedback proportional to the velocity in free flight and the applied force in contact. The overall teleoperation scheme is depicted in Figure 4, and the parallel position/force controller is shown in Figure 5.

3.1. Position Control

The position control of the mechanically under-actuated quadrotor is implemented using a two layer cascade controller. The orientation is controlled using PID in the inner loop, and the outer loop provides the inner loop with reference roll and pitch (${\varphi }_d,{\theta }_d$) to control the robot planar motion using a gravity-compensated-PD. The yaw motion is controlled independently.

$$\left\{\begin{array}{c}{u}_z=\frac{m}{cos\left(\varphi \right)cos\left(\theta \right)}(g-k_{pz}\tilde{z}-k_{dz}\dot{z})\hfill \\ u_x=-k_{px}\tilde{x}-k_{dx}\dot{x}\hfill \\ u_y=-k_{py}\tilde{y}-k_{dy}\dot{y}\hfill \\ \left[\begin{array}{c}sin\left({\varphi }_d\right)\\ sin\left({\theta }_d\right)\end{array}\right]=\frac{m}{u_z}\left[\begin{array}{cc}sin\left(\psi \right)& -cos\left(\psi \right)\\ \frac{cos\left(\psi \right)}{cos\left(\varphi \right)}& \frac{sin\left(\psi \right)}{cos\left(\varphi \right)}\end{array}\right]\left[\begin{array}{c}{u}_y\\ u_x\end{array}\right]\hfill \\ {\mathit{u}}_{\eta }=-K_D\dot{\eta }-K_P\tilde{\mathit{\eta }}-K_I{\int }_0^t\tilde{\mathit{\eta }}\left(s\right)ds,\hfill \end{array}\right.$$

(5)

where $\mathit{p}-{\mathit{p}}_{\mathit{d}}={\left[\tilde{x}\tilde{y}\tilde{z}\right]}^{\top }$ is the position error with ${\mathit{p}}_{\mathit{d}}\in {\mathbb{R}}^3$ being the desired position, $k_{px},k_{dx},k_{py},k_{dy},k_{pz},k_{dz},\in {\mathbb{R}}^{+}$ are proportional and derivative gains, $\tilde{\mathit{\eta }}=\mathit{\eta }-{\mathit{\eta }}_{\mathit{d}}$ is the orientation error with ${\mathit{\eta }}_{\mathit{d}}={[{\varphi }_d,{\theta }_d,{\psi }_d]}^{\top }$ being the desired orientation, and $K_P,K_D,K_I$ are the proportional, derivative, and integral diagonal gain matrices to regulate the attitude.

3.2. Force Control

The force is regulated by providing the internal position control loop with an appropriate reference. The robot’s desired position ${\mathit{p}}_{\mathit{d}}$ and desired yaw ${\psi }_d$ are four commanded states of the system, noting that the system is mechanically under-actuated. As depicted in Figure 5, the outer force regulating the feedback loop generates additional terms (${\mathit{p}}_f,{\psi }_f$), depending on the force error, that are added to the previously commanded pose (${\mathit{p}}_p,{\psi }_p$) in the free flight. As we aimed at contact interaction with the environment using the end-effector, we command the end-effector pose; therefore, we convert the end-effector pose to the COM pose by including the term $-R\mathit{d}$ in the desired pose. Thus, ${\mathit{p}}_{\mathit{d}}$ and ${\psi }_d$ are expressed as:

$$\left\{\begin{array}{c}{\mathit{p}}_{\mathit{d}}={\mathit{p}}_{\mathit{p}}+{\mathit{p}}_{\mathit{f}}-R\mathit{d}\hfill \\ {\psi }_d={\psi }_p+{\psi }_f.\hfill \end{array}\right.$$

(6)

In the sequel, we see how ${\mathit{p}}_{\mathit{p}}$, ${\psi }_p$, ${\mathit{p}}_{\mathit{f}}$, and ${\psi }_f$ are generated from the user command, given by ${\mathit{x}}_m$, based on the aerial manipulator contact condition.

When the robot is in free flight condition, the master robot position (${\mathit{x}}_m$) is interpreted as a position command; while when the end-effector comes in contact ${\mathit{x}}_m$ is interpreted as the desired force. In order to distinguish the two conditions, let us introduce the contact function $u(\mathit{f},{\mathit{f}}_{\mathit{d}})$ as:

$$u\left(t_k\right)=\left\{\begin{array}{cc}1\hfill & \mathrm{if}{f}_n\left(t_k\right)>0\&f_n\left(t_{k-1}\right)>ϵ\hfill \\ 0\hfill & \mathrm{if}{f}_n\left(t_k\right)=0\&f_{d,n}\left(t_k\right)<-ϵ\hfill \\ u\left(t_{k-1}\right)\hfill & \mathrm{otherwise}.\hfill \end{array}\right.$$

(7)

This hysteresis-like function is intended to prevent the chattering phenomenon in the attachment and detachment phases. In Equation (7), $f_n\left(t_k\right)$ is the normal component of the measured contact force at $t_k$ instance and $f_n\left(t_{k-1}\right)$ is the previous sample of the same measurement; $f_{d,n}\left(t_k\right)$ is the normal component of the commanded force, $u\left(t_{k-1}\right)$ is the previous output of the function ($u\left(0\right)=0$), and $ϵ\in {\mathbb{R}}^{+}$ is a small positive value. In the contact condition, if the commanded normal component of the force is negative ($f_{d,n}\left(t_k\right)<-ϵ$) and the normal component of the applied force is zero, the detachment takes place. During free flight $u\left(t\right)=0$ and the component of the desired position that is intended to control the aerial manipulator position in free flight is generated by integrating the master position as:

$${\mathit{p}}_{\mathit{p}}={\mathit{p}}_{\mathit{d}}\left(t_0\right)+{\int }_{t_0}^t(1-u\left(t\right))K_p^f{\mathit{x}}_{\mathit{m}}dt,$$

(8)

where $K_p^f$ is a $3\times 3$ matrix that rotates and scales the master robot motion appropriately. The aerial manipulator heading ${\psi }_p$ in free flight is directly commanded by the user, through mapping one of the master robot motions, similar to ${\mathit{p}}_p$.

When the end-effector establishes a contact $u\left(t\right)=1$, and thus $(1-u(t\left)\right)=0$, in this case the integral stores the end-effector position at the contact moment, and the motion of the master robot during the contact interaction is used to command the desired force. The force commanded by the human operator, ${\mathit{f}}_{\mathit{d}}^{*}\in {\mathbb{R}}^3$, expressed in $\mathcal{C}$, is generated as:

$${\mathit{f}}_{\mathit{d}}^{*}=u\left(t\right)K_f^f{\mathit{x}}_{\mathit{m}},$$

(9)

where $K_f^f$ is a $3\times 3$ matrix that rotates and scales the master robot motion appropriately.

To prevent the desired force violating the contact constraint (3), the commanded force is modified as follows:

$${\mathit{f}}_{\mathit{d}}=\left\{\begin{array}{cc}{\mathit{f}}_{\mathit{d}}^{*}\hfill & \mathrm{if}\frac{1}{{\mu }_s}\sqrt{{\left(f_o^{*}\right)}^2+{\left(f_t^{*}\right)}^2}+\frac{1}{\sqrt{r_d^2+{\mu }_t^2}}\left|\tau \right|\le f_n^{*}\hfill \\ \sigma \left({\mathit{f}}_{\mathit{d}}^{*}\right)\hfill & \mathrm{otherwise},\hfill \end{array}\right.$$

(10)

where ${\mathit{f}}_{\mathit{d}}^{*}={[f_o^{*},f_t^{*},f_n^{*}]}^{\top }$, and $\sigma \left({\mathit{f}}_{\mathit{d}}^{*}\right)$ is a function projecting the desired commanded force ${\mathit{f}}_{\mathit{d}}^{*}$ to the surface of the contact constraints (3) as follows:

$$\sigma \left({\mathit{f}}_{\mathit{d}}^{*}\right)=cos\left(\alpha \right){\mathit{f}}_{\mathit{d}}^{*}+sin\left(\alpha \right)\left(S\left(\mathit{a}\right){\mathit{f}}_{\mathit{d}}^{*}\right)+(1-cos\left(\alpha \right))\left({\mathit{a}}^{\top }{\mathit{f}}_{\mathit{d}}^{*}\right)\mathit{a}+\frac{1}{\sqrt{r_d^2+{\mu }_t^2}}\left|\tau \right|\mathit{n},$$

(11)

where $\mathit{a}=\left(S\left({\mathit{f}}_{\mathit{d}}^{*}\right)\mathit{n}\right)/\left|\right|\left(S\left({\mathit{f}}_{\mathit{d}}^{*}\right)\mathit{n}\right)\left|\right|$ is the unit axis of rotation, $\alpha =\beta -\gamma$ is the required angle to rotate ${\mathit{f}}_{\mathit{d}}^{*}$ around $\mathit{a}$ to project it on the surface of the friction cone, $\beta$ is the angle between ${\mathit{f}}_{\mathit{d}}^{*}$ and $\mathit{n}$, and $\gamma ={tan}^{-1}\left({\mu }_s\right)$ is the translational friction cone angle. This function minimally increases the normal component (by adding ${(r_d^2+{\mu }_t^2)}^{-0.5}\left|\tau \right|\mathit{n}$) to avoid torsional slippage and lifting up the end-effector disk, and applies the minimum rotation to its direction to keep it within the feasible force range.

The next step to generate ${\mathit{p}}_{\mathit{f}}$ and ${\psi }_f$ is to feed the force error $\tilde{\mathit{f}}=\mathit{f}-{\mathit{f}}_{\mathit{d}}={\left[\widetilde{f_o}\widetilde{f_t}\widetilde{f_n}\right]}^{\top }$ to a PI-controller as:

$${\mathit{u}}_{\mathit{f}}=-K_{Pf}\tilde{\mathit{f}}-K_{If}{\int }_0^t\tilde{\mathit{f}}dt,$$

(12)

where $K_{Pf},K_{If}\in {\mathbb{R}}^{3\times 3}$ are proportional and integral diagonal gain matrices, respectively. The PI-output ${\mathit{u}}_{\mathit{f}}={\left[u_o{u}_t{u}_n\right]}^{\top }$—after appropriate transformations—generates ${\mathit{p}}_{\mathit{f}}$ and ${\psi }_f$.

At the contact moment, in which the springs are in rest position, we define the contact frame $\mathcal{C}$ which is its orientation w.r.t. $\mathcal{B}$ represented by rotation matrix $R_c^b$. Let $\mathit{d}={[d_x,0,d_z]}^{\top }$; we can always define $\mathcal{C}$ with a constant $R_c^b$ such that ${\mathit{d}}^c={[d_o,0,d_n]}^{\top }$. Expanding (2) we get:

$$\left\{\begin{array}{c}{f}_o(\delta o,\delta {\eta }_t)=k_{to}\delta o-\frac{d_n{k}_{rt}}{{\parallel d\parallel }^2}\delta {\eta }_t\hfill \\ f_t(\delta t,\delta {\eta }_o,\delta {\eta }_n)=\frac{d_n{k}_{ro}}{{\parallel d\parallel }^2}\delta {\eta }_o+k_{tt}\delta t-\frac{d_o{k}_{rn}}{{\parallel d\parallel }^2}\delta {\eta }_n\hfill \\ f_n(\delta n,\delta {\eta }_t)=k_{tn}\delta n+\frac{d_o{k}_{rt}}{{\parallel d\parallel }^2}\delta {\eta }_t.\hfill \end{array}\right.$$

(13)

Therefore, $f_n$ and $f_o$ can be regulated by commanding the motion along $\mathit{n}$ and $\mathit{t}$, respectively. To control $f_t$, we choose $\delta {\eta }_o$, and the reason is: due to the under-actuation of the quadrotor, changing $\delta t$ requires changing the roll and pitch angles of the aerial manipulator and this results in applying an undesirable moment around the normal axis of the contact frame. The usage of rotation to generate the desired force also leads to a wider range of feasible forces, as it not only relies on the limited linear friction of the end-effector and object surfaces. $\delta {\eta }_o$ can be considered as changing the yaw angle $\psi$ (see Figure 3). Therefore, ${\mathit{p}}_{\mathit{f}}$ and ${\psi }_f$ can be obtained as follows:

$$\left\{\begin{array}{c}{\mathit{p}}_{\mathit{f}}=R^{\top }{R}_c^b{\left[u_{0}0u_n\right]}^{\top }\hfill \\ {\psi }_f=u_t.\hfill \end{array}\right.$$

(14)

3.3. Master Control and Haptic Feedback

The input of the master robot (haptic device), as expressed by (4), receives two elements from the teleoperation scheme: the human force ${\mathit{f}}_{\mathit{h}}$ and the haptic feedback force ${\mathit{f}}_{\mathit{c}}$, which is itself constituted by two parts ${\mathit{f}}_{\mathit{PD}}$, and ${\mathit{f}}_{\mathit{pf}}$. The term ${\mathit{f}}_{\mathit{PD}}$ is a negative proportional derivative term, based on master position ${\mathit{x}}_m$, that is intended to bring back the device to its zero position gently when the device is not moved by the operator, so that the quadrotor will not move or apply force when the haptic device handle is released. On the other hand, ${\mathit{f}}_{\mathit{pf}}$ is the haptic feedback given to the user depending on the velocity in free flight or force error in contact. The haptic feedack ${\mathit{f}}_c$ is synthesized as:

$${\mathit{f}}_{\mathit{c}}={\mathit{f}}_{\mathit{PD}}+{\mathit{f}}_{\mathit{pf}}=(K_{Ph}{\mathit{x}}_{\mathit{m}}+K_{Dh}{\dot{\mathit{x}}}_{\mathit{m}})+(K_f^b\mathit{f}+(1-u)K_p^b\dot{\mathit{p}}),$$

(15)

where $K_f^b,K_p^b,K_{Ph},K_{Dh}\in {\mathbb{R}}^{3\times 3}$ are positive diagonal gains.

4. Stability Analysis

We first show the stability of the rotational dynamics in contact, then the stability of the force controller, and finally the stability of the teleoperation scheme. In order to facilitate the tractability, let $\mathcal{C}$ and $\mathcal{W}$ coincide, $R_c^w=I_3$, and at the contact instance $R_c^b=I$ (see Figure 3).

4.1. Rotational Stability

The angular dynamics $M\left(\mathit{\eta }\right)\ddot{\mathit{\eta }}+C(\mathit{\eta },\dot{\mathit{\eta }})\dot{\mathit{\eta }}+K_r\mathit{\eta }+{\mathit{\tau }}_f={\mathit{u}}_{\eta }$, where ${\mathit{\tau }}_f=-S\left(\mathit{d}\right){\mathit{f}}_{\mathit{t}}$ has the following properties:

• M is symmetric positive definite and bounded as ${\mu }_M\le \parallel M\parallel \le {\gamma }_M$; where ${\mu }_M,{\gamma }_M$ are its minimum and maximum eigenvalues. Moreover, M is strictly diagonally dominant (SDD), that is $M_{ii}>{\Sigma }_{i\ne j}\left|M_{ij}\right|$;
• $\dot{M}-2C$ is skew-symmetric, or $\dot{M}=C+C^{\top }$;
• $\parallel C(\mathit{\eta },\dot{\mathit{\eta }})\parallel \le {\delta }_c\parallel \dot{\mathit{\eta }}\parallel$, where ${\delta }_c\le 2{\sigma }_{max}\left(J\right)$;
• the rotational stiffness matrix is bounded as ${\mu }_k\le \parallel K_r\parallel \le {\gamma }_k$.

Theorem 1.

Applying the rotational part of the control law (5) to the aerial manipulator described by (1), its rotational dynamic is locally asymptotically stable such that $\tilde{\mathit{\eta }},\dot{\mathit{\eta }}\to \mathbf{0}$ and ${\int }_0^t\tilde{\mathit{\eta }}\left(s\right)ds\to -K_I^{-1}(K_r\mathit{\eta }+{\mathit{\tau }}_f)$ as $t\to \infty$.

Proof. 

Let $\mathit{\zeta }={\left[{\Delta \mathit{\eta }}^{\top }{\tilde{\mathit{\eta }}}^{\top }{\dot{\mathit{\eta }}}^{\top }\right]}^{\top }$, where $\Delta \mathit{\eta }={\int }_0^t\tilde{\mathit{\eta }}\left(s\right)ds+K_I^{-1}(K_r\mathit{\eta }+{\mathit{\tau }}_f)$, and consider the following scalar function:

$$V\left(\mathit{\zeta }\right)=\frac{1}{2}{\mathit{\zeta }}^{\top }P\mathit{\zeta },$$

(16)

where the symmetric matrix P is defined as follows:

$$P=\left[\begin{array}{ccc}{K}_I& K_{12}& \epsilon M\\ K_{12}& K_{22}& \epsilon M\\ \epsilon M& \epsilon M& M\end{array}\right],$$

(17)

with $\epsilon =k_{\epsilon }\frac{{\mu }_M}{{\gamma }_M}$ and $0<k_{\epsilon }<0.5$, $K_{12}=K_I+\epsilon K_D-K_r$, and $K_{22}=K_P+\epsilon K_D-K_{12}{K}_I^{-1}{K}_r$. Let $K_s=k_{s}I$ for $s=\{P,I,D\}$ with $k_s>0$; in a range satisfying inequities,

$$\begin{array}{c}0<k_D<\frac{{\mu }_K}{\epsilon }-{\gamma }_M,\epsilon <\frac{{\mu }_K}{{\gamma }_M}\\ k_p>k_I+\frac{{\gamma }_K}{k_I}(\epsilon k_D-{\mu }_K)+\epsilon {\gamma }_M.\end{array}$$

(18)

P becomes SDD with positive diagonal elements, and is therefore positive definite. Thus, $V\left(\mathit{\zeta }\right)$ is a positive definite function and hence a Lyapunov function candidate, which is radially unbounded and satisfies the Rayleigh–Ritz inequality [29] as:

$${\mu }_P{\parallel \zeta \parallel }^2\le V\left(\zeta \right)\le {\gamma }_P{\parallel \zeta \parallel }^2,$$

(19)

where ${\mu }_P$ and ${\gamma }_P$ are the minimum and maximum eigenvalues of P. The time derivative of the Lyapunov candidate (16) is:

$$\dot{V}=-{\mathit{\zeta }}^{\top }Q\mathit{\zeta }+{\mathit{\zeta }}^{\top }B{[{\dot{\mathit{\tau }}}_f^{\top }{\dot{\mathit{\eta }}}_d^{\top }]}^{\top },$$

(20)

with

$$\begin{array}{cc}\hfill B& =\left[\begin{array}{cc}I& -K_{12}\\ K_{12}& -K_{22}\\ \epsilon K_I^{-1}M& -\epsilon M\end{array}\right]\hfill \end{array}$$

(21)

$$\begin{array}{cc}\hfill Q& =\left[\begin{array}{ccc}\epsilon K_I& \frac{1}{2}((\epsilon -1)K_I+\epsilon K_P)& -\frac{1}{2}\epsilon C^{\top }\\ \frac{1}{2}((\epsilon -1)K_I+\epsilon K_P)& \epsilon K_P-(K_I+\epsilon K_D-K_r)& -\frac{1}{2}\epsilon (C^{\top }+M)\\ -\frac{1}{2}\epsilon C^{\top }& -\frac{1}{2}\epsilon (C^{\top }+M)& K_D-\epsilon M(I+K_I^{-1}{K}_r),\end{array}\right]\hfill \end{array}$$

(22)

In the following we shall show that:

$$\dot{V}\left(\mathit{\zeta }\right)\le -{\mathit{\zeta }}_1^{\top }{Q}_1{\mathit{\zeta }}_1+{\mathit{\zeta }}^{\top }B{\left[{\dot{\mathit{\tau }}}_f^{\top }{\dot{\mathit{\eta }}}_d^{\top }\right]}^{\top },$$

(23)

where ${\mathit{\zeta }}_1=[\parallel \Delta \mathit{\eta }\parallel \parallel \tilde{\mathit{\eta }}\parallel \parallel \dot{\mathit{\eta }}{\parallel ]}^{\top }$ and

$$\begin{array}{c}\hfill Q_1=\left[\begin{array}{ccc}\epsilon k_I& \frac{1}{2}((\epsilon -1)k_I+\epsilon k_P)& \frac{1}{2}\epsilon {\delta }_C\parallel \dot{\eta }\parallel \\ \frac{1}{2}((\epsilon -1)k_I+\epsilon k_P)& \epsilon (k_P-k_D)+{\mu }_K-k_I& \frac{1}{2}\epsilon ({\gamma }_M+{\delta }_C\parallel \dot{\eta }\parallel )\\ \frac{1}{2}\epsilon {\delta }_C\parallel \dot{\eta }\parallel & \frac{1}{2}\epsilon ({\gamma }_M+{\delta }_C\parallel \dot{\eta }\parallel )& k_D-\epsilon {\gamma }_M(1+\frac{{\gamma }_K}{k_I})\end{array}\right].\end{array}$$

(24)

To obtain (23), we choose $k_P,k_I,k_D$ such that:

$$\begin{array}{c}\epsilon (2{\mu }_K+{\gamma }_M)<k_I<{\mu }_K\\ (\frac{1}{\epsilon }-\epsilon )k_I<k_P<2{\mu }_K+{\gamma }_M\\ \epsilon {\gamma }_M(\frac{3}{2}+\frac{{\gamma }_K}{k_I})<k_D<{\mu }_K(1-\epsilon )\end{array}.$$

(25)

Consequently, $\epsilon$ must be chosen such that

$$\epsilon <min\left\{\frac{{\mu }_K-k_I}{{\mu }_K-\frac{k_P}{2}+\frac{{\gamma }_M}{2}},\frac{2}{3}\frac{{\mu }_K(2{\mu }_K+{\gamma }_M)-{\gamma }_K{\gamma }_M}{{\gamma }_M(2{\mu }_K+{\gamma }_M)},\frac{{\mu }_M}{2{\gamma }_M},\frac{{\mu }_K}{{\gamma }_M}\right\}.$$

(26)

Then, it is also assumed that the spherical joint mechanical stiffness is chosen properly in accordance with the system moment of inertia such that ${\mu }_K>\frac{{\gamma }_M}{2}(1+\frac{{\gamma }_M}{{\mu }_K})$, which can be simplified as ${\sigma }_{min}\left(K_r\right)>{\gamma }_M$, which simply means that the higher the inertia, the stiffer the spring that must be chosen. Let ${\mu }_v=min\{k_I-ϵk_P,ϵk_P+2({\mu }_K-\epsilon k_D)-(\epsilon +1)k_I-\epsilon {\gamma }_M,k_D-\frac{3}{2}\epsilon {\gamma }_M-\frac{\epsilon {\gamma }_M{\gamma }_K}{k_I}\}$, for trajectories bounded by:

$$\parallel \dot{\mathit{\eta }}\parallel \le \frac{1}{{\delta }_C}{\mu }_v.$$

(27)

$Q_1$ will be SDD with positive diagonal elements, and therefore positive definite. Thus, we can conclude:

$$\dot{V}(t,\mathit{\zeta })\le -{\mu \parallel \mathit{\zeta }\parallel }^2+\kappa \parallel \mathit{\zeta }\parallel ,$$

(28)

where $\mu ={\sigma }_{min}\left(Q_1\right)$ and $\kappa ={\delta }_{B1}{\delta }_f+{\delta }_{B2}{\delta }_v$ with ${\delta }_{B1}=max\{1,k_I+\epsilon k_D-{\mu }_K,k_P+\epsilon {\gamma }_M/k_I\}$, ${\delta }_{B2}=max\{k_I+\epsilon k_D-{\mu }_K,k_P+\epsilon k_D-{\mu }_K(k_I+\epsilon k_D-{\mu }_K)/k_I,\epsilon {\gamma }_M\}$, and $\parallel {\dot{\mathit{\eta }}}_d\parallel \le {\delta }_v$ is the upper bound on the norm of ${\dot{\mathit{\eta }}}_d$. $\parallel {\dot{\mathit{\tau }}}_f\parallel \le {\delta }_f$ is the upper bound on the norm of the ${\dot{\mathit{\tau }}}_f$, which is a reasonable assumption considering that the rotational dynamics is faster than translational [30]. Let W be the positive root of $V=W^2$, as in [31], considering (19) we can state $\dot{W}\le -\frac{\mu }{2{\gamma }_P}W+\frac{\kappa }{2\sqrt{{\mu }_P}}$, which means:

$$W\left(t\right)\le W\left(0\right)e^{-\frac{\mu }{2{\gamma }_P}t}-\frac{\kappa }{2\sqrt{{\mu }_P}}{\int }_0^t{e}^{-\frac{\mu }{2{\gamma }_P}(t-s)}ds\le W\left(0\right)e^{-\frac{\mu }{2{\gamma }_P}t}+\frac{\kappa {\gamma }_P}{\sqrt{{\mu }_P}\mu }.$$

(29)

Considering (19), we can ensure that (27) is satisfied if $W\left(0\right)+\frac{\kappa {\gamma }_P}{\sqrt{{\mu }_P}\mu }\le \frac{1}{{\delta }_C}{\mu }_v$. Therefore, choosing sufficiently large $k_P$, and sufficiently small $k_I$ and $k_D$ such that (18) and (25) are satisfied, the solution of the error system converges to zero asymptotically. □

4.2. Force Control Stability

The stability of the force dynamic in the closed-loop force controlled system is investigated in a decentralized manner, that is, each component of the force is analyzed independently by considering the effect of other state variables as disturbances.

4.2.1. Force along n-Axis

The forces along n-axis is controlled by translational pose command along $\mathit{n}$, from (13) we have

$$f_n=k_{tn}n+k_j{\eta }_t,$$

(30)

where $k_j=\frac{d_o{k}_{rt}}{{\parallel d\parallel }^2}$. If we substitute the pose controller in (30), assuming the gravity term compensated by feedback linearizing term, the dynamics will be:

$$m_n\ddot{n}+k_{tn}n=-k_p\tilde{n}-k_d\dot{n},$$

(31)

where $m_n=m$ is derived from (1). Substituting n and its derivatives, in the left hand side of (31), with $f_n$ from (30), and control terms, in the right hand side, with (12) we obtain:

$$m_f{\ddot{f}}_n+b_f{\dot{f}}_n+k_f{f}_n=-k_1{\int }_0^t{\tilde{f}}_n-k_2{\tilde{f}}_n+h_n({\ddot{\eta }}_t,{\dot{\eta }}_t,{\eta }_t),$$

(32)

where $m_f=m_n{k}_{tn}^{-1}$, $b_f=k_d{k}_{tn}^{-1}$, $k_f=(1+k_p{k}_{tn}^{-1})$, $h_n=(m_n{k}_{tn}^{-1}{k}_j{\ddot{\eta }}_t+k_d{k}_{tn}^{-1}{k}_j{\dot{\eta }}_t+(1+k_p)k_{tn}^{-1}{k}_j{\eta }_j)$, and $k_1=k_p{k}_{If}$, $k_2=k_p{k}_{Pf}$.

One can express (32) in the frequency domain by introducing the controller transfer function $C\left(s\right)=k_2+\frac{k_1}{s}$ and plant transfer function $G\left(s\right)=1/(m_f{s}^2+b_{f}s+k_f)$, where ($s=\sigma +j\omega$). The system output $f\left(s\right)$ is then obtained as:

$$f\left(s\right)=\frac{G\left(s\right)C\left(s\right)}{1+G\left(s\right)C\left(s\right)}{f}_d\left(s\right)+\frac{1}{1+G\left(s\right)C\left(s\right)}h\left(s\right).$$

(33)

For the stability of the system, the characteristic polynomial of the system, that is, $s(m_f{s}^2+b_{f}s+k_f)+(k_{2}s+k_1)$, must have all roots with a real negative part and, to achieve this, according to the Routh–Hurwitz stability criterion it is required that:

$$k_{Pf}+\frac{1}{k_P}+\frac{1}{k_{tn}}>\frac{m}{k_D}{k}_{If}.$$

(34)

Choosing sufficiently high PD gains and an appropriately low I-gain for slowly-varying force commands, that is, $s\to 0$, we obtain $\frac{G\left(s\right)C\left(s\right)}{1+G\left(s\right)C\left(s\right)}\to 1$, $\frac{1}{1+G\left(s\right)C\left(s\right)}\to 0$, thus $f\to f_d$.

4.2.2. Force along o-Axis

The forces along o-axis are controlled by the translational pose command along $\mathit{o}$ and the stability analysis is the same as the force along the $n-axis$.

4.2.3. Force along t-Axis

The forces along the t-axis are controlled by the rotational pose command around $\mathit{o}$, that is, ${\eta }_o=\psi$ considering the frame convention. From (14) one can write:

$$f_t=k_o\psi +\left[k_{tt}{k}_{rn}\right]{\left[t{\eta }_n\right]}^{\top },$$

(35)

where $k_o=\frac{d_n{k}_{ro}}{{\parallel d\parallel }^2}$ and $k_{rn}=\frac{d_o{k}_{r,n}}{{\parallel d\parallel }^2}$. If we substitute the orientation controller in the dynamic model, considering the results of Theorem 1, we obtain:

$$m_{33}\ddot{\psi }+(c_{33}+k_D)\dot{\psi }=-k_p\tilde{\psi }-(c_{31}\dot{\varphi }+c_{32}\dot{\theta }),$$

(36)

where $m_{ij},c_{ij}$ are extracted from M and C. It is worth noting that $m_{33}$ and $c_{33}$ are constant with respect to $\psi$ and its derivatives. Substituting $\psi$ and from (35) in (36) we get:

$$\frac{m_{33}}{k_o}{\ddot{f}}_t+\frac{k_D+c_{33}}{k_o}{\dot{f}}_t+\frac{k_P}{k_o}{f}_t=-k_1{\int }_0^t{\tilde{f}}_t-k_2{\tilde{f}}_t+h_t,$$

(37)

with

$$\begin{array}{cc}{h}_t=& -(c_{31}\dot{\varphi }+c_{32}\dot{\theta })+(\frac{m_{33}{k}_t}{k_o}\ddot{t}+\frac{k_t(k_D+c_{33})}{k_o}\dot{t}+\frac{k_t{k}_P}{k_o}t)+\\ & +(\frac{m_{33}{k}_n}{k_o}{\ddot{\eta }}_n+\frac{k_n(k_D+c_{33})}{k_o}{\dot{\eta }}_n+\frac{k_n{k}_P}{k_o}{\eta }_n).\end{array}$$

(38)

Following the same procedure of force along the n-axis, the Routh–Hurwitz criterion enforces the controller coefficients to be chosen as:

$$k_{Pf}+\frac{1}{k_o}>\frac{m_{33}}{k_D+c_{33}}{k}_{If},$$

(39)

which is fulfilled by setting appropriately high proportional and derivative gains and sufficiently low integral gain, and for slowly-varying force commands $f_t\to f_d$.

Choosing coefficients according to (34) and (39) makes the system over damped, that is, without overshoot, which is that $\mathit{f}$ is not getting higher than ${\mathit{f}}_d$. Thus, if $f_d$ is the output of (10), contact maintenance is ensured.

4.3. Stability of Teleoperator in Contact Interaction

During the contact interaction, from (4) we can define the dynamics of the master robot and its controller in the frequency domain as: $G_r\left(s\right)=M_m{s}^2+K_{mD}s,C_m\left(s\right)=K_{mP}$. The haptic feedback in the master side (PD term) could also be expressed as: $C_h\left(s\right)=K_{hP}+K_{hD}s$. We have shown that the force interaction dynamics and force controller in the remote side could be expressed as: $G_s\left(s\right)=diag{(M_f{s}^2+B_{f}s+K_f)}^{-1},C_m\left(s\right)=K_2+K_1\frac{1}{s}$. We define the transfer function of the master robot and its controller as: $G_m\left(s\right)=C_m{G}_r{(I+C_m{G}_r)}^{-1}$. The internal stability of the system requires that the roots of the denominator of $G_m$ all have real negative parts, for which $K_{mP},K_{mD}>0$ suffice. The overall transfer function of the master side, with force input and position output, is defined as:

$$G_1\left(s\right)=G_m{(I+G_m{C}_h)}^{-1}.$$

(40)

The characteristic polynomial of $G_1$ is $\left(M_m{K}_{mP}{K}_{hD}\right)s^3+(M_m{K}_{mP}(1+K_{hP})+K_{mP}{K}_{mD}{K}_{hD})s^2$ + $(K_{mD}{K}_{mP}{K}_{hP}+K_{mP}{K}_{mD})s+1$. For internal stability, the controller coefficients must conform with the following constraint:

$$\begin{array}{c}\hfill k_{mD}{k}_{mP}{k}_{hP}+k_{mP}{k}_{mD}>\frac{{\sigma }_{max}\left(M_m\right)k_{hP}}{{\sigma }_{min}\left(M_m\right)(1+k_{hP})+k_{mD}{k}_{hD}}.\end{array}$$

(41)

The transfer function of the slave side, with force input and force output, is obtained as: $G_2\left(s\right)=G_s{C}_s{(I+G_s{C}_s)}^{-1}$. Its internal stability constraints are expressed by (34) and (39). The teleoperator output f (see Figure 6), can be obtained as:

$$\begin{array}{c}\hfill f\left(s\right)=G_2{K}_f^f{G}_1{(I+G_2{K}_f^f{G}_1{K}_f^b)}^{-1}{f}_h\left(s\right)\\ \hfill +{(I+G_s{C}_s)}^{-1}h\left(s\right).\end{array}$$

(42)

The constraint (34), (39) and (41) gives all the poles of $G_1$ and $G_2$ a negative real part; therefore, $G_1$ and $G_2$ are strictly positive real and thus passive systems, and the negative feedback interconnection of a passive system is a passive system [32]; thus, (42), that is, the teleoperator in the contact interaction, is stable.

4.4. Stability of Teleoperator in Free Flight

During the free flight, assuming fast rotational dynamics compared to translational dynamics, and gravity compensation, we may express the slave robot and its controller as $G_s\left(s\right)=M{s}^2+K_{D}s$ and $C_s\left(s\right)=K_p$, the input to $G_s$ is the integrated value of scaled ${\mathit{x}}_m$, and the force feedback to the master side is the robot velocity. Therefore, $G_2$ can be expressed as (Figure 6):

$$G_2=(\frac{1}{s}I)(G_s{C}_s{(I+G_s{C}_s)}^{-1})\left(Is\right)=G_s{C}_s{(I+G_s{C}_s)}^{-1},$$

(43)

which is internally stable by choosing pose controller gains $k_P,k_D>0$. The master side of the teleoperator is the same as in the contact interaction; therefore, $G_1$ does not change. Considering velocity $\dot{\mathit{p}}$ as the output of the system, it can be obtained as:

$$s\mathit{p}\left(s\right)=G_2{K}_p^f{G}_1{(I+G_2{K}_p^f{G}_1{K}_p^b)}^{-1}{f}_h.$$

(44)

$s\mathit{p}\left(s\right)$, that is, velocity in frequency domain, is the system output constituted by the negative feedback interconnection of passive systems $G_1$ and $G_2$. Therefore, the teleoperator in free flight is passive and stable.

5. Experimental Results

In order to evaluate the proposed aerial tele-manipulation solution, and to assess the functionalities of the proposed controller in the bilateral teleoperation scheme, two experiments with fixed and movable objects were performed. In the first experiment, a human operator drives the aerial manipulator to establish a contact with a stationary target and applies force to it, receiving force feedback. In the second experiment, the human operator drives the quadrotor to establish a contact with a wheeled cart and pushes it to generate motion, while receiving force feedback.

We encourage the interested reader to watch the video of the experiments in the multimedia attachment to this paper, cf. Supplementary Materials, to better appreciate the presented validation.

5.1. Experimental Setup

Our aerial manipulator was equipped with a lightweight tool (Figure 2) with a total weight of $0.05$ $\mathrm{k}\mathrm{g}$. It was rigidly connected to the top of a quadrotor UAV with $1.0$ $\mathrm{k}\mathrm{g}$ weight. The quadrotor platform used for the experiments was a Mikrokopter^© x4 platform (HiSystems GmbH). The distance vector from the quadrotor COM to the end-effector surface was $\mathit{d}=$ [0 m, 0.5 m and 0.2 $\mathrm{m}$]. The link lengths were $d_1=$ $0.51$ $\mathrm{m}$, and $d_2=$ $0.02$ $\mathrm{m}$ long. A compliant spherical joint, that connects the lightweight rigid link to the end-effector, and an elastic shock absorber damper on the end-effector helped to establish smooth contacts. The end-effector surface was covered by a high friction material to expand the feasible force vector range.

The robot positioning was performed by a Vicon tracking system (Vicon Capture Systems, London, UK). The quadrotor thrust and rotational controller was implemented on its onboard microcontroller (Atmel AVR 8-bit, ATmega-1284, running at 20 ), based on the inertial sensors of the robot, the rest of the control law was implemented on an external PC (Core i7, 16GB RAM, running Ubuntu 14), communicating with the robot using a pair of Zig-Bee transceiver chips. An Omega.3 haptic device (Force Dimension, Nyon, Switzerland) was used as the master device. We used the force/torque ATI sensor (ATI Industrial Automation, Apex, NC, USA) embedded inside the object to measure the applied force by the aerial manipulator. The software was implemented in the ROS, and all control loops ran at a frequency of 100 $\mathrm{Hz}$.

5.2. Results

5.2.1. Stationary Object Experiment

Initially the quadrotor was located at the origin of the global coordinate, while the object, which was a $0.15$ $\mathrm{m}$× $0.15$ $\mathrm{m}$ plate, was located at [1.0 m, 0.0 m and −0.85 $\mathrm{m}$] with downward pointing ${\mathit{z}}_w$. The human operator drove the quadrotor towards the object, and once the robot reached the object, the driver commanded a variable continuous force vector, by means of the haptics device. Finally, the human operator commanded the robot to leave the object, and brought it back to free flight. Figure 7-top shows the snapshots of different moments of the experiment, while Figure 8-left, Figure 9-left, and Figure 10-left show the results of the experiment.

Figure 8a shows the position tracking during the experiment, while the position error is explicitly reported in Figure 9-left, for the reader’s convenience. As can be seen, before the contact event, and after the quadrotor leaves the object, the position error is low, that is, bounded below $7 \mathrm{c}\mathrm{m}$, while during the contact there is significant position error, that is, $\approx 35 \mathrm{c}\mathrm{m}$, specifically in x-direction which corresponds to the normal component of force. It is worth underlining the fact that, during the interaction, the position error does not provide relevant information for evaluating the controller’s performance, as during that phase the platform is force-controlled, and the position error represents a necessary condition to guarantee the force tracking. As a matter of fact, a higher position error during the contact with stationary object means a higher thrust and a higher pitch angle is demanded, which results in larger applied forces.

Figure 8b shows the contact maintenance function output, which keeps the desired force vector inside the friction cone. It can bee seen that, when the commanded force violates the friction constraints of (10), the normal component of the desired commanded force is increased while the other two tangential components are decreased, and the desired norms of the input (${\mathit{f}}_{\mathit{d}}^{*}$) and output (${\mathit{f}}_{\mathit{d}}$) are equal.

Figure 8c shows the force tracking control result during the experiment. As is evident, the force tracking performance is very good (average absolute error of $0.11$ $\mathrm{N}$, which represents less than 10% relative error, and a maximum absolute error of $0.51$ $\mathrm{N}$). The contact transition is shown in a separate window inside Figure 8c, which introduces a very smooth contact transition with only a small single bouncing event. Note that in the transition phase of the robot force control with a rigid environment, having a small amount of bouncing and inadvertent losses of contact are common, even for grounded robotic arm manipulators with non-zero reaching velocity [33].

The haptic feedback components are shown in Figure 10-left. The master position (the position of the haptic device’s handle with respect to the center of its workspace) is shown in creating the spring-damper force ${\mathit{f}}_{PD}$ that brings back the master device’s handle to the center of its workspace (Figure 10a). The position tracking error of the aerial manipulator in the free flight condition along with the force applied to the object in contact create the second constitutive component of the haptic feedback ${\mathit{f}}_{pf}$, which is shown in Figure 10b. The haptic feedback in the contact condition is equal to the measured force from the force sensor with the opposite direction.

5.2.2. Movable Object Experiment

In the second experiment, the robot was initially located at the global frame origin. The movable object, which was a cart with plate of $0.15$ $\mathrm{m}$× $0.15$ $\mathrm{m}$ attached to it, was located at [0.5 m, 0.0 m, and −0.90 $\mathrm{m}$], with downward pointing ${\mathit{z}}_w$. The human operator was driving the quadrotor towards the cart, and once the robot reached the cart, the driver pushed it until it passed $1.0$ $\mathrm{m}$, and eventually the driver commanded the robot to leave the object, and brought the robot back to free flight. Figure 7-bottom shows the snapshots of this experiment, while Figure 8-right, Figure 9-right, and Figure 10-right depict the results of the experiment.

Figure 8a’ shows the position tracking during the experiment. The difference between the new situation (a movable object) compared to the previous experiment (a stationary object) can be seen through this plot, where after establishing the contact, while the quadrotor is applying the force to the object (cart), its position is not changing until the moment (in this experiment at $14.5$ $\mathrm{s}$) the applied force overcomes the static friction of the cart wheels with the ground. Then, the cart (and the aerial manipulator too) experiences an accelerated motion. The position error is explicitly reported in Figure 9-right), for the reader’s convenience. As in the previous scenario, before the interaction the error is relatively close to zero, that is, bounded below $7 \mathrm{c}\mathrm{m}$, while during the contact there is significant position error, that is, $\approx 50 \mathrm{c}\mathrm{m}$, specifically in x-direction which corresponds to the normal component of force. As already mentioned, this is in accordance with the fact that, in this phase, the robot is tracking the desired force provided by the user, and not the desired position. During the undocking phase, namely when the human operator sees the accelerating cart passing the goal line and pushes back the handle of the haptic device in order to bring the robot back, a peak of $\approx 75 \mathrm{c}\mathrm{m}$ is reached due to a small false contact detection, after which the position error converges back to its typical free-flight values. The large error value in the detachment phase can be improved by limiting the integral term of the PI-force controller, and increasing the proportional gain of PI-controller, or also using a more agile controller in the inner position loop. It is worth noting that, in this experiment, the goal was to evaluate the efficiency of the proposed controller in applying force to objects in order to move them in a way the human operator wants, rather than performing precise force tracking.

In relation to this last point, another meaningful note is that the two experiments here discussed were explicitly designed and performed with the main goal of validating the presented aerial robotic solution in indoor laboratory conditions, which imply the use of a motion capture system for state estimation, a cabled connection between the robot and the ground workstation, and the absence of non-negligible external disturbances. Under these conditions, we basically experienced a rate of success of $100\%$. A more fair and comprehensive validation and testing of the this system in outdoor, mocap-denied, and realistic scenarios in the presence of external disturbances and using only onboard computation, which is not in the scope of this paper, will be the subject of future work.

Figure 8b’ shows the contact maintenance function input and output forces. Since the main purpose is to push the cart, the major component of the commanded force is along the normal direction; therefore, most of the time the feasible commanded force is the same as the desired commanded force by the human operator and, as is evident from Figure 8c’, the contact is kept during the force interaction.

The force tracking control results, shown in Figure 8c’, represent a more difficult task compared to the previous task (force tracking control in contact with a stationary object). As can be seen, while the cart is stationary, that is, when the force applied by the quadrotor end-effector is compensated for by the friction of the cart wheels with the surface, the force tracking has a similar performance to the previous experiment. On the other hand, once the cart starts to move the error increases. However, considering the particular application of this experiment, which aims at pushing a cart forward, as far as the contact is kept and the operator is able to accomplish the task, the experiment is considered successful, although the average of the absolute force error norm is $0.5$ $\mathrm{N}$, that is a 20% relative error.

The haptic feedback components are shown in Figure 10-right. ${\mathit{f}}_{PD}$ is depicted in Figure 10a’, and the haptic feedback component related to the slave side, ${\mathit{f}}_{pf}$, is shown in Figure 10b’. As is evident, the magnitude of the latter is generally bigger than the magnitude of the former, allowing the human operator to be aware of the aerial manipulator condition in the master side (this is also true for the experiment with the stationary object). Haptic feedback in this experiment is even more important for the human operator to not lose the contact, as ${\mathit{f}}_{pf}$ allows the human operator to feel the applied force during the contact interaction. When the perceived force is small, it means that the contact is weak, and the human operator can command a higher force to ensure the contact maintenance.

5.3. Discussion

The experimental results validated the capability and efficacy of the aerial manipulator with a passive tool in free flight pose tracking and in contact force tracking while keeping the contact and maintaining the flight stability. After performing several experiments to investigate the properties of the system and the controller, the following results from interpreting observations were obtained. In the free flight phase, the autonomous control of pose is not very precise, and there are position/velocity errors that prevent the autonomous controller from doing very fine tasks, even in the case of knowing the environment perfectly. This happens due to the existence of aerodynamics disturbances and unmodeled dynamics that are not incorporated in the controller design. However, a human teleoperated aerial system is capable of doing such tasks, thanks to superior human learning and sensory motor capabilities. A similar observation has been made in [8].

The contact establishing phase (docking) involves bouncing; this is a natural behavior because the reaction force of the contact is directly applied to the robot CoM that is a floating (hovering) object, and this results in jumping back; unlike grounded manipulators this force is not transmitted to the ground. To mitigate this effect, velocity slowing-down policy and physical shock absorber on the end-effector surface is used, that results in a smooth transition and reduces the bouncing effect significantly. Moreover, deploying $f_{PD}$ in the haptic feedback prevents the operator from loosely grasping the master robot, which could increase the effect. Note that in establishing a contact with non-zero reaching velocity, having a small amount of bouncing is commonly acceptable, even for grounded robotic arm manipulators [33].

Contact maintenance function, in conjunction with appropriate gain tunings (to avoid overshoots as mentioned in the stability analysis section) plays an important role; without respecting these constraints the end-effector could slide on the surface. Therefore, using this function is necessary if the task involves keeping the contact, unlike the hybrid position-force controllers such as in [2,24,34] in which it is desirable to slide over the surface.

The force error constituted by a non-zero-mean part (DC) and a high frequency signal (AC). The DC part is the effect of the not-force-controlled axes of motion (due to the under-actuation and uni-lateral contact), and the AC part is because of the propeller rotational (aero)dynamics. The DC part can be decreased by choosing appropriately large gains, while increasing these gains increases the AC part. Therefore, care is to be taken in tuning the controller to achieve an acceptable trade-off depending on the task.

The detachment phase also showed some bouncing effects, which is due to the time it takes to deplete the integral term, and imprecise free flight position control in the vicinity of the object. This effect is mitigated by deploying the switch function (7), and by bounding the integral term and choosing a small coefficient for it while choosing appropriately big proportional and derivative terms. A similar phenomenon is also observed in the case of grounded manipulators controlled by the parallel position/force control method in [35] and referred to as the sticky-effect.

The proposed controller can also be utilized in redundant omni-directional or partially omni-directional aerial manipulators, such as [1,8], by keeping their position controllers and providing it with a reference based on the presented force controller that considers contact maintenance and deploying orientation to generate the desired force.

6. Conclusions and Future Works

The topic of aerial physical interaction using a typical under-actuated VTOL UAV equipped with a passive tool was considered in this paper. We use a simple passive light weight tool, instrumented with a compliant end-effector, rigidly attached to the top of an aerial robot. We propose a control method in a bilateral teleoperation scheme to let a human operator drive the aerial manipulator in a remote environment, controlling the position of the robot in free flight and regulating the force applied to the object in contact interaction while maintaining the flight stability and keeping the contact. For this purpose, a parallel position/force controller is utilized, which also uses the rotational dynamic axes, that is, yaw motion, to generate the desired force that allows maintenance of the contact with a wider range of forces. On the one hand, theoretical proof of the stability of controlled system is derived and presented. On the other, experiments on driving the aerial robot toward the desired point, docking, and applying the desired forces to stationary and movable objects represented the feasibility and efficacy of the proposed solution. The human operator is provided with force haptic feedback proportional to the velocity in free flight and the applied force in contact, allowing for an improved situational awareness.

Future work will involve vision based outdoor implementation of the proposed approach, and investigating bandwidth and delay effects in haptic teleoperation based on passivity theories such as [36]. Performing more complex tasks cooperatively with a team of aerial manipulators will also be considered.