Aerial Physical Interaction in Grabbing Conditions with Lightweight and Compliant Dual Arms

Abstract

This paper considers the problem of performing bimanual aerial manipulation tasks in grabbing conditions, with one of the arms grabbed to a fixed point (grabbing arm) while the other conducts the task (operation arm). The goal was to evaluate the positioning accuracy of the aerial platform and the end effector when the grabbing arm is used as position sensor, as well as to analyze the behavior of the robot during the aerial physical interaction on flight. The paper proposed a control scheme that exploits the information provided by the joint sensors of the grabbing arm for estimating the relative position of the aerial platform w.r.t. (with respect to) the grabbing point. A deflection-based Cartesian impedance control was designed for the compliant arm, allowing the generation of forces that help the aerial platform to maintain the reference position when it is disturbed due to external forces. The proposed methods were validated in an indoor testbed with a lightweight and compliant dual arm aerial manipulation robot.

1. Introduction

The reliability in the realization of an aerial manipulation task on flight strongly depends on the positioning accuracy of the aerial robot, which mainly depends on the accuracy of the position sensors, the performance of the multirotor controller, and the effect of endogenous/exogenous forces raised during the execution of the operation. On the one hand, it is desirable that the accuracy in the position estimation of the aerial vehicle is below the 10% of the reach of the manipulator [1], being capable of compensating the undesired deviations while the multirotor hovers within the workspace. Different positioning systems have been employed in the literature. Motion capture systems such as Vicon or Opti-Track have been extensively used in indoor testbeds [2,3,4,5,6] due to their high accuracy (<1 cm) and high update rates (100–200 Hz), as well as because no additional devices have to be integrated in the aerial platform, but only the passive markers. Similarly, the laser tracking systems used in [7] only require the addition of a reflective marker or a prism to the multirotor, although this solution imposes that the marker is not occluded by any obstacle in the line of the laser. Several on-board perception systems have been developed for multirotor platforms, including optical flow [8], stereo vision [9], live 3D dense reconstruction [10], or laser scanners [11]. However, these solutions reduce the payload capacity of the aerial platform, as additional devices such as cameras and on-board computers have to be added and complicate the system integration. Not only that, but each of these technologies presents certain limitations relative to the operation range, accuracy, reliability, or the update rate. The docking system described in [12] is an alternative solution that exploits the proximity of the aerial platform to the workspace during the manipulation phase, using an articulated link for obtaining the relative position from the encoders of the joints.

An aerial manipulation robot operating on flight will be affected by three types of perturbations: the reaction wrenches induced over the multirotor platform due to the motion of the arms [13,14], the contact forces associated to the physical interactions on flight [4,6,15,16,17], as well as the aerodynamic effects [7]. As consequence, the realization of certain manipulation tasks requiring the correct positioning of the end effector, such as object grasping [2,5], valve turning [3], or inspection by contact [7,15,18], may be compromised or become unfeasible. In order to overcome these problems, several methods and strategies have been proposed, such as the estimation and control of the external wrenches acting over the aerial platform [19,20], the development of multilayer control architectures [13], or the design of lightweight and compliant robotic arms [21,22]. In this sense, it is desirable to improve the accommodation of the aerial platform to the position deviations when it is operating in contact with the environment, exploiting for this purpose the mechanical compliance of the arms [22].

The main contribution of this paper is the design, modelling, and validation of a lightweight and compliant dual arm system that allows for the estimation and control of the position of an aerial robotic manipulator relative to a fixed grabbing point, using one of the arms for grabbing and as position sensor (grabbing arm), while the other is intended to conduct the operation while flying (operation arm). Figure 1 illustrates the application of the dual arm aerial manipulator for the installation of clip-type bird diverters on a power line [23]. Two methods are proposed and evaluated. Firstly, a zero-torque controller was implemented in the grabbing arm, and thus the reaction wrenches induced over the multirotor were relatively low, using the position estimation obtained from the joint servos to control the deviations in the position of the platform with respect to the reference pose. Secondly, a force controller based on Cartesian deflection was developed to achieve the desired impedance behavior during the aerial physical interaction, using the grabbing arm to exert a force that helps the multirotor controller to reach the reference pose relative to the grabbing point. Experimental results carried out in test-bench and indoor flight tests (Figure 1) demonstrated the performance of both approaches.

The innovative aspects of this paper with respect to our previous published works [17,21,22] can be summarized in the following points:

• The development and testing of a new functionality for the dual arm aerial manipulator: estimating the position of the robot relative to the grabbing point with one of the arms while the other is intended to conduct the operation on flight.
• The evaluation of the positioning accuracy in the estimation provided by the grabbing arm compared to the ground truth given by an Opti-Track system.
• The combination of passive (mechanical) and active (control) compliance methods in the grabbing arm to facilitate the accommodation of the aerial robot to sustained grabbing forces.
• The experimental evaluation and qualitative analysis of the effects of the grabbing arm and the injected disturbances over the stability of the multirotor controller.

The rest of the paper is organized as follows. The prototype employed in the experiments is firstly described in Section 2. Section 3 covers the kinematics and position estimation, with the mentioned control methods described in Section 4. The experimental results are presented in Section 5, and the conclusions are summarized in Section 6.

2. System Description

2.1. Compliant Dual Arm

The manipulator used for validating the methods described in Section 4 is a lightweight and compliant dual arm system developed at the GRVC Robotics Labs. A picture of the arms can be seen in Figure 2, indicating in

Table 1. Main features of the lightweight and compliant dual arm.

Total Weight	1.0 (kg)
Maximum lift load (1 s playtime)	At elbow: 0.3 (kg)
	At shoulder: 0.12 (kg)
Joint stiffness	5 (Nm/rad)—all joints
Maximum joint speed	300 (°/s)

its main features. Each arm provides three degrees of freedom for end effector positioning in the following kinematic configuration [22]: shoulder yaw at the base ($q_1$), shoulder pitch ($q_2$), and elbow pitch ($q_3$). The arms are built with the Herkulex DRS-0201 smart servos, and a customized frame structure manufactured in carbon fiber and aluminum, providing full servo protection at the shoulder yaw joint with a pair of polymer bearings, and partial servo protection in the other two joints [21]. In order to estimate and control the torques and forces, the grabbing arm (right arm) integrates 14-bit resolution magnetic encoders that are interfaced through a STM32F303 microcontroller board, sending the deflection measurement to the main computer at 200 Hz with 1 ms latency. Each of the arms and the sensors are connected to the Raspberry Pi 3B+ through USB-to-USART interfaces, where control program of the arms is executed. The manipulator is fed with a 2S, 650 mAh LiPo battery, providing an operation time of around 20 min.

The tip of the forearm link includes an aluminum flange to facilitate the integration of the end effector in the manipulator. For safety reasons, the grabbing operation is conducted with a magnetic gripper (around 5 N force), using the linear metallic structure shown in Figure 1 for grabbing.

2.2. Aerial Manipulation Robot

The dual arm system is integrated in an S550 platform, a hexarotor similar to a DJI F550. This is equipped with six DJI 2312E brushless motors with 9 × 4.5 inch propellers. Figure 3 shows a picture of the aerial robot, identifying its components, including the Pixhawk 2.1 autopilot, the Raspberry Pi 3B+ computer board, and the 4S 4400 mAh LiPo battery (0.5 kg) used as counterweight of the arms. The setup is similar to the one described in [14], constraining the motion of the arms to prevent the collision with the landing gear. The hardware and software architecture of the system is represented in Figure 4. The control program of the arms, developed in C/C++, as well as the different software modules that control the aerial platform, based on ROS and the UAV Abstraction Layer [24], are executed in the Raspberry Pi and interfaced by the Ground Control Station (GCS) through the wireless link. Four USB-to-USART devices are connected to the computer board: (1) the Pixhawk autopilot, (2) the left arm, (3) the right arm, and (4) the microcontroller board that reads the sensors. The position and orientation of the multirotor are obtained from an Opti Track system, used as ground truth.

3. Modelling

3.1. Kinematics

As usual, three reference frames are defined for the aerial manipulation system, as illustrated in Figure 5: the Earth fixed frame $\left\{\mathit{E}\right\}$ (inertial), the multirotor body frame $\left\{\mathit{B}\right\}$, and the manipulator frame $\left\{\mathit{i}\right\}$, with $i=\left\{1,2\right\}$ for the left/right arms. In the following, ${}_{}{}^{\mathit{A}}\mathit{r}{}_{\mathit{B}}$ denotes the position of a certain point $\mathit{B}$ w.r.t. (with respect to) reference frame $\left\{\mathit{A}\right\}$. In this way, ${}_{}{}^{\mathit{E}}\mathit{r}{}_{\mathit{B}}={\left[x, y, z\right]}^T$ and ${}_{}{}^{\mathit{E}}\mathit{\eta }{}_{\mathit{B}}={\left[\varphi , \theta , \psi \right]}^T$ represent the multirotor position and orientation relative to $\left\{\mathit{E}\right\}$, and ${}_{}{}^{\mathit{i}}\mathit{r}{}_{TCP,i}$ is the position of the tool center point (TCP) of the $i$-th manipulator expressed in its own frame. The origin of $\left\{\mathit{i}\right\}$ is located at the intersection of the shoulder joints, with the x-axis pointing forwards, the y-axis parallel to the baseline of the two arms, and the z-axis pointing upwards.

The three reference frames are related through the corresponding transformation matrices:

$$\begin{array}{ccc}{}_{}{}^{\mathit{E}}\mathit{T}{}_{\mathit{B}}=\left[\begin{array}{cc}{}_{}{}^{\mathit{E}}\mathit{R}{}_{\mathit{B}}\left(\varphi , \theta , \psi \right)& {}_{}{}^{\mathit{E}}\mathit{r}{}_{\mathit{B}}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]& ;& \begin{array}{ccc}{}_{}{}^{\mathit{B}}\mathit{T}{}_{\mathit{i}}=\left[\begin{array}{cc}{\mathit{I}}_{3\times 3}& {}_{}{}^{\mathit{B}}\mathit{r}{}_{\mathit{i}}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]& ;& {}_{}{}^{\mathit{E}}\mathit{T}{}_{\mathit{i}}={}_{}{}^{\mathit{E}}\mathit{T}{}_{\mathit{B}}·{}_{}{}^{\mathit{B}}\mathit{T}{}_{\mathit{i}}\end{array}\end{array}$$

(1)

where ${}_{}{}^{\mathit{E}}\mathit{R}{}_{\mathit{B}}$ is the multirotor rotation matrix; ${}_{}{}^{\mathit{B}}\mathit{r}{}_{\mathit{i}}={\left[D_x, \pm D/2, D_z\right]}^T$ is the origin of $\left\{\mathit{i}\right\}$ relative to $\left\{\mathit{B}\right\}$, with $D_x$ and $D_z$ being the displacement of the arms with respect to $\left\{\mathit{B}\right\}$ in the x- and z-axes, respectively; and $D$ is the separation distance between the arms in the y-axis.

The arms implement the 3-DOF (degrees of freedom) configuration considered in our previous work [14,22], with three joints for TCP positioning: shoulder yaw (base), shoulder pitch, and elbow pitch. The wrist joints are not considered due to the convenience to simplify the mechanical construction and reduce the weight of the arms. The rotation angle of the $j$-th joint of the $i$-th arm is denoted as $q_j^i$, whereas ${\theta }_j^i$ is the corresponding servo shaft position. The difference between these two variables is the deflection angle, $\Delta {\theta }_j^i={\theta }_j^i-q_j^i$, measured by the encoders [17,21]. The forward and inverse kinematic models are computed as follows (superscript $i$ is omitted for clarity reasons):

$${}_{}{}^{\mathit{i}}\mathit{r}{}_{TCP,i}=\mathit{F}\mathit{K}\left({\mathit{q}}^i\right)=\left[\begin{array}{c}r\left(q_2, q_3\right)·\cos \left(q_1\right)\\ r\left(q_2, q_3\right)·\sin \left(q_1\right)\\ L_1\cos \left(q_2\right)+L_2\cos \left(q_2+q_3\right)\end{array}\right]$$

(2)

$${\mathit{q}}^i=\mathit{I}\mathit{K}\left({}_{}{}^{\mathit{i}}\mathit{r}{}_{TCP,i}\right)=\left[\begin{array}{c}atan2\left(y,x\right)\\ {\cos }^{-1}\left(\frac{x^2+y^2+z^2-L_1^2-L_2^2}{2L_1\sqrt{x^2+y^2}}\right)\\ {\cos }^{-1}\left(\frac{x^2+y^2+z^2-L_1^2-L_2^2}{2L_1{L}_2}\right)\end{array}\right]$$

(3)

where $L_1$ and $L_2$ are the upper arm/forearm link lengths, and $r\left(q_2,q_3\right)$ is given by

$$r\left(q_2, q_3\right)=L_1\sin \left(q_2\right)+L_2\sin \left(q_2+q_3\right)$$

(4)

3.2. Relative Position Estimation

If the end effector of the grabbing arm is firmly attached to a fixed point, then it is possible to estimate the position of the multirotor relative to this grabbing point just applying the homogeneous transformation from $\left\{\mathbf{2}\right\}$ to $\left\{\mathit{B}\right\}$, taking into account that the position of the TCP referred to $\left\{\mathit{2}\right\}$ is directly obtained from the forward kinematic model given by Equation $\left(2\right)$. The accuracy in the position estimation can be obtained multiplying the joint position error (including the deflection error) by the Jacobian of the arm. The wrist joints are not essential for this purpose, as the multirotor orientation can be obtained from the inertial measurement unit (IMU) of the aerial platform. However, a certain level of accommodation is required at the wrist so the robotic arm can follow the position and orientation deviations of the aerial platform while it is grabbed.

Two similar mechanisms have been proposed in previous works for estimating the pose of an aerial manipulator relative to a contact point. Reference [12] presents a docking tool consisting of an articulated arm with passive joints that is deployed over a pipe with a stiff-joint dual arm, whereas reference [25] relies on a passive spherical wrist joint and an IMU integrated at the end effector of a 3-DOF arm. In this paper, we combine the passive/active compliance methods for estimating the multirotor position while controlling the interaction force.

3.3. Dynamics

The dynamic model of the compliant joint dual arm aerial manipulation robot is derived from the Lagrangian and the generalized equations of the forces and torques:

$$L=K-V$$

(5)

$$\frac{d}{dt}\left\{\frac{\partial L}{\partial \dot{\mathit{\xi }}}\right\}-\left\{\frac{\partial L}{\partial \mathit{\xi }}\right\}=\mathit{\Gamma }+{\mathit{\Gamma }}_{ext}$$

(6)

where $L$ is the Lagrangian; $K$ and $V$ are the kinetic and potential energies, respectively; $\mathit{\xi }$ is the vector of generalized coordinates; and $\mathit{\Gamma }$ and ${\mathit{\Gamma }}_{ext}$ respectively represent the generated and external wrenches acting on the aerial robot. The vector of generalized coordinates includes the multirotor position and orientation, as well as the servo shaft and output link angular position vectors, ${\mathit{\theta }}^i=\left[\begin{array}{ccc}{\theta }_1^i& {\theta }_2^i& {\theta }_3^i\end{array}\right]$ and ${\mathit{q}}^i=\left[\begin{array}{ccc}{q}_1^i& q_2^i& q_3^i\end{array}\right]$, respectively, and thus $\mathit{\xi }$ is defined as follows.

$$\mathit{\xi }={\left[\begin{array}{ccc}{}_{}{}^{\mathit{E}}\mathit{r}{}_{\mathit{B}}& {}_{}{}^{\mathit{E}}\mathit{\eta }{}_{\mathit{B}}& \begin{array}{cc}{\mathit{\theta }}^1& \begin{array}{cc}{\mathit{q}}^1& \begin{array}{cc}{\mathit{\theta }}^2& {\mathit{q}}^2\end{array}\end{array}\end{array}\end{array}\right]}^T\in {\Re }^{18}$$

(7)

Analogously, the vector of generalized forces comprises the forces and torques acting over the multirotor and the joints of the manipulator.

$$\mathit{\Gamma }={\left[\begin{array}{ccc}{\mathit{F}}_{\mathit{B}}& {\mathit{\tau }}_{\mathit{B}}& \begin{array}{cc}{\mathit{\tau }}_m^1& \begin{array}{cc}{\mathit{\tau }}^1& \begin{array}{cc}{\mathit{\tau }}_m^2& {\mathit{\tau }}^2\end{array}\end{array}\end{array}\end{array}\right]}^T\in {\Re }^{18}$$

(8)

The vector of external forces ${\mathit{\Gamma }}_{ext}$ models the disturbance wrenches exerted on the multirotor base [18,19,20], the contact forces at the end effector [4,6,17], as well as the aerodynamic forces raised when the thrust of the rotors is affected by close surfaces in the environment [7,26].

The kinetic energy of the aerial manipulator can be expressed as the sum of the kinetic energy of the aerial platform and the kinetic energy of the robotic arms:

$$K=K_{UAV}+K_{arms}$$

(9)

Each of these components comprises two terms corresponding to the translation and rotation of the masses with respect to the inertial frame $\left\{\mathit{E}\right\}$:

$$K_{UAV}=\frac{1}{2}{m}_{UAV}{\Vert {}_{}{}^{\mathit{E}}\dot{\mathit{r}}^{\mathit{B}}\Vert }^2+\frac{1}{2}{}_{}{}^{\mathit{E}}\mathit{\omega }{}_{\mathit{B}}^T{I}_{\mathit{U}AV}{}_{}{}^{\mathit{E}}\mathit{\omega }{}_{\mathit{B}}$$

(10)

$$K_{arms}={\displaystyle \sum }_{i=1}^2{\displaystyle \sum }_{j=1}^4\left(\frac{1}{2}{m}_j^i{\Vert {}_{}{}^E\dot{\mathit{r}}_i^j\Vert }^2+\frac{1}{2}{}_{}{}^{\mathit{E}}\mathit{\omega }{}_j^{i,T}{\mathit{I}}_j^i{}_{}{}^{\mathit{E}}\mathit{\omega }{}_j^i\right)$$

(11)

where $m_{UAV}$ and ${\mathit{I}}_{UAV}$ are the mass and inertia tensor of the aerial platform, respectively, whereas $m_j^i$ and ${\mathit{I}}_j^i$ are mass and inertia of the $j$-th joint of the $i$-th arm, respectively. The potential energy of the aerial manipulator also includes two terms, the gravity and elastic potential of the compliant joints:

$$V=g\left(m_{UAV}{z}_{UAV}+{\displaystyle \sum }_{i=1}^2{\displaystyle \sum }_{j=1}^4{m}_j^i{z}_j^i\right)+{\displaystyle \sum }_{i=1}^2{\displaystyle \sum }_{j=1}^4{k}_j^i{\left({\theta }_j^i-q_j^i\right)}^2$$

(12)

Here, $g$ is the gravity constant, and $k_j^i$ is the corresponding joint stiffness. After some work, it is possible to express the dynamic model in the usual compact matrix form [14]:

$$\mathit{M}\ddot{\mathit{\xi }}+\mathit{C}\left(\mathit{\xi },\dot{\mathit{\xi }}\right)+\mathit{G}\left(\mathit{\xi }\right)+\mathit{K}\left(\mathit{\xi }\right)+\mathit{D}\left(\dot{\mathit{\xi }}\right)=\mathit{\Gamma }+{\mathit{\Gamma }}_{ext}$$

(13)

where $\mathit{M}\in {\Re }^{18\times 18}$ is the generalized inertia matrix; $\mathit{C}$ and $\mathit{G}\in {\Re }^{18}$ represent the centrifugal, Coriolis, and gravity terms; and $\mathit{K}$ and $\mathit{D}\in {\Re }^{18}$ correspond to the stiffness and damping terms of the compliant manipulator, respectively [17]. The dynamic coupling between the arms and the aerial platform is associated with the cross terms in the generalized inertia matrix, which can be decomposed in three groups of submatrices identified in [14]: multirotor translation (decoupled from rotation), multirotor rotation with coupling terms, and dual arm manipulator with coupling terms. Since the grabbing arm will be held to a fixed point, the corresponding inertia, Coriolis, and gravity terms will be negligible compared to the operation arm. Moreover, if the sensitivity of the zero-torque controller described in next section is good enough, then the magnitude of the wrenches associated to the stiffness and damping terms will be relatively low, and with it, the influence over the multirotor controller.

As stated in the introduction and illustrated in Figure 1, the operation arm is intended to perform the manipulation operation (the installation of a bird flight diverter) while the grabbing arm provides the position estimation. This can be assimilated to a close kinematic chain [27,28] with floating base [29], in which the pushing/pulling force exerted by the operation arm will cause a reaction torque on the aerial platform that should be cancelled by the multirotor controller with the help of the grabbing arm in order to prevent undesired position deviations. This motivates the implementation of an active impedance control scheme with the grabbing arm, as explained below.

4. Control

4.1. Definition of the Control Task

As stated in the introduction, a dual arm system allows the realization of aerial manipulation tasks in grabbing conditions, using one arm for grabbing and for estimating the position of the aerial platform relative to the grabbing point (Section 3.2), whereas the other takes care of conducting the task, for example, the installation of a sensor device [21]. Since the grabbing arm is actuated and mechanically compliant, it is possible to estimate and control the forces and torques acting over the manipulator from the deflection of the joints [17,21]. The idea is that the arm helps the multirotor to reach the desired position when it is disturbed by an external force, exerting a pushing/pulling force in the opposite direction of the position error, as Figure 6 illustrates.

Therefore, the grabbing arm can be used in two ways:

• As relative position sensor, with zero torque control, so the reaction wrenches induced over the multirotor are relatively small.
• As an active impedance link, exerting a controlled force over the aerial platform to compensate external forces and guide the multirotor towards the reference position.

In the first case, the joints of the grabbing arm implement a PI (proportional-integral) controller to maintain a zero deflection (torque) reference, acting over the servo position as follows:

$${\theta }_{j,ref}^2={\theta }_j^2+\left(K_p\left({\theta }_j^2-q_j^2\right)+K_i{\displaystyle \int }\left({\theta }_j^2-q_j^2\right)dt\right)$$

(14)

Here, ${\theta }_j^2$ is the position of the $j$-th servo of the right arm, whereas the term on the right side is the incremental position correction. The proportional and integral gains, $K_p$ and $K_i$, can be tuned experimentally, taking into account the nominal values of the deflection (≈5 degrees). Now, we impose that the nominal operation position of the aerial robot relative to the grabbing point is the L-shaped configuration of the arm, since this is far enough from the joint limits and the kinematic singularities (although any other could be considered):

$${}_{}{}^{\mathbf{2}}\mathit{r}{}_{TCP2}^{ref}=\left[\begin{array}{c}{L}_2\\ 0\\ -L_1\end{array}\right]=\mathit{F}\mathit{K}\left({\mathit{q}}_{ref}^2\right)=\mathit{F}\mathit{K}\left(\left[\begin{array}{c}0\\ 0\\ -\pi /2\end{array}\right]\right)$$

(15)

The position deviation of the aerial platform is then defined as the displacement of the TCP of the grabbing arm w.r.t. the reference position. That is,

$$\mathit{\epsilon }={\left[\begin{array}{ccc}{\epsilon }_X& {\epsilon }_Y& {\epsilon }_Z\end{array}\right]}^T=\mathit{F}\mathit{K}\left({\mathit{q}}_{ref}^2\right)-\mathit{F}\mathit{K}\left({\mathit{q}}^2\right)$$

(16)

Note that the maximum deviation is limited by the reach of the arm, $\Vert \epsilon \Vert <L_1+L_2-\sqrt{L_1^2+L_2^2}$. If the influence of the grabbing arm over the attitude controller is relatively low due to the zero-torque controller, this measurement can then be taken as input by the position controller of the multirotor platform, as represented in upper part of Figure 7, replacing the GPS (Global Positioning System) used for navigating to achieve better accuracy during the manipulation phase.

4.2. Impedance Control in Grabbing Conditions

Assuming that the aerial platform is close to the hover state during the grabbing maneuver ($\varphi \cong \theta \cong 0$), it is desired that the position deviation of the aerial platform is assimilated to an impedance behavior characterized as follows:

$${\mathit{M}}_{\mathit{d}}\ddot{\mathit{\epsilon }}+{\mathit{D}}_{\mathit{d}}\dot{\mathit{\epsilon }}\mathbf{+}{\mathit{K}}_{\mathit{d}}\mathit{\epsilon }={\mathit{F}}_{ext}$$

(17)

where ${\mathit{M}}_{\mathit{d}}$, ${\mathit{D}}_{\mathit{d}}$, and ${\mathit{K}}_{\mathit{d}}$ are the desired inertia, damping, and stiffness, respectively, and ${\mathit{F}}_{ext}$ is the external force exerted over the multi-rotor. This force can be estimated from the Cartesian deflection of the grabbing arm, as it will be seen in next subsection, and taken as input by the attitude controller of the aerial platform so it can be partially compensated with the wrenches generated by the rotors.

The grabbing arm will react to the external force exerting a pushing/pulling force at the end effector in the direction of the position deviation, relying on the Cartesian force controller described in the next subsection. The impedance control is then achieved, generating a force reference that cancels the dynamic behavior described by Equation $\left(17\right)$. Figure 7 represents the case of desired stiffness, ${\mathit{F}}_{e,TCP}^i={\mathit{K}}_{\mathit{d}}\mathit{\epsilon }$.

4.3. Force Control Based on Cartesian Deflection

The force control of the compliant arm is formulated in the Cartesian space and based on the Cartesian deflection, defined as the position deviation of the TCP of the compliant arm w.r.t. the same point in an equivalent stiff joint arm. That is,

$$\mathbf{\Delta }{\mathit{l}}_{TCPi}=\mathit{F}\mathit{K}\left({\mathit{\theta }}^i\right)-\mathit{F}\mathit{K}\left({\mathit{q}}^i\right)$$

(18)

This definition is useful for expressing the force at the end effector directly in the task space:

$${}_{}{}^{\mathit{i}}\mathit{F}{}_{TCPi}={\mathit{K}}_C^i\mathbf{\Delta }{\mathit{l}}_{TCPi}+{\mathit{D}}_C^i{\dot{\mathbf{\Delta }\mathit{l}}}_{TCPi}$$

(19)

where ${\mathit{K}}_C^i$ and ${\mathit{D}}_C^i$ are the Cartesian stiffness and damping matrices, respectively, whose values can be obtained from the physical joint stiffness and damping through the Jacobian:

$${\mathit{K}}_C^i={\left({\mathit{J}}^T\right)}^{-1}{\mathit{K}}_p^i {\mathit{J}}^{-1}$$

(20)

where ${\mathit{K}}_p^i=diag\left\{k_j^i\right\}$ is the physical joint stiffness matrix [17]. The Cartesian damping can be obtained analogously. The force controller, represented in Figure 7 relies on the inverse kinematics, giving as output an incremental position correction term for the TCP:

$${}_{}{}^{\mathit{i}}\mathit{r}{}_{TCPi}^{ref}={}_{}{}^{\mathit{i}}\mathit{r}{}_{TCPi}+\mathbf{\Delta }{\mathit{r}}_{TCP}^i=\mathit{F}\mathit{K}\left({\mathit{q}}^i\right)+\left({\mathit{K}}_P^F{\mathit{F}}_{e,TCP}^i+{\mathit{K}}_I^F{\displaystyle \int }{\mathit{F}}_{e,TCP}^{i}dt\right)$$

(21)

Here, $\mathbf{\Delta }{\mathit{r}}_{TCP}^i$ is the position increment that should be applied in the grabbing arm to achieve the desired force reference; ${\mathit{F}}_{e,TCP}^i={\mathit{F}}_{ref}^i-{\mathit{F}}_{TCP}^i$ is the force control error at the TCP; whereas ${\mathit{K}}_P^F=diag\left\{K_{Px}^F, K_{Py}^F, K_{Pz}^F\right\}$ and ${\mathit{K}}_I^F=diag\left\{K_{Ix}^F, K_{Iy}^F, K_{Iz}^F\right\}$ are the proportional and integral gain matrices, respectively, whose value is tuned experimentally knowing the nominal values of the forces (around $1 \left[N\right]$) and the Cartesian deflection (around $0.02 \left[m\right]$).

5. Experimental Results

5.1. Position Estimation with Zero Deflection/Torque Control

The goal of this experiment was to evaluate the accuracy of the position estimation provided by the grabbing arm when this was used as position sensor, enabling the zero-torque controller to reduce the reaction wrenches induced over the aerial platform on flight (see Section 4.1). The experiment, shown in the video provided as Supplementary Material, consisted of four phases:

(1)

The multirotor takes off.

(2)

The arms adopt the nominal operation pose given by Equation $\left(15\right)$.

(3)

The aerial robot approaches the contact point using the Opti Track system for navigating.

(4)

Once the arm grabs the support structure, the zero-torque controller and the estimator are enabled while the multirotor is controlled in position with the Opti Track system.

Figure 8, Figure 9 and Figure 10 represent the signals of interest during the grabbing maneuver, that is, the multirotor position (arm estimation and ground truth) relative to the nominal pose, the multirotor orientation, the estimation error, and the joint position and torque in the grabbing arm. In order to appreciate the ability of accommodation of the arm and the accuracy in the estimation, we intentionally deviated the aerial platform in the x- and z-axes, with small deviations in the y-axis. Note that in the experiments the end-effector is supposed to be firmly attached to the grabbing point in such a way that the displacement of the multirotor platform will force the displacement of the grabbing arm. However, the magnetic gripper may slip around the contact area, introducing errors in the position estimation. It was also assumed that the multirotor heading (yaw angle) was almost constant, with the ${\mathit{Y}}_{\mathit{B}}$-axis parallel to the linear structure. Otherwise, additional degrees of freedom should be integrated in the wrist joint to estimate the relative orientation, although this is out of the scope of this work. The influence of these two effects over the positioning accuracy can be observed in Figure 9, where the estimation error increased as the multirotor orientation in the yaw angle changed with respect to its initial value, causing mainly errors in the y-axis estimation.

5.2. Impedance Control in Test-Bench

Before its evaluation on flight, the impedance controller of the grabbing arm was firstly validated in test-bench in order to evidence more clearly the variation in the desired stiffness, as defined in Equation $\left(17\right)$. During the experiment, the arm adopted the L-shaped configuration (Figure 6 and Equation $\left(15\right)$), manually exerting an external force at the end effector that was estimated and compensated from the Cartesian deflection, as expressed by Equations $\left(19\right)$ and $\left(21\right)$. The desired stiffness was set to $K_d=50 Nm$ for the experiment illustrated in Figure 11, and to $K_d=100 Nm$ for the experiment represented in Figure 12. As it can be seen, the position deviation of the end effector was lower in the second case, as expected, and the instantaneous stiffness ($K_x=F_x/{\epsilon }_x$) was similar to the desired value. Note that the stiffness is affected by the singularity associated to the deflection when this tends to zero.

5.3. Grabbing Interaction on Flight with Impedance Control

The goal of this experiment was to analyze the behavior of the aerial manipulation robot on flight when the grabbing arm was actively controlling the impedance in the x- and z-axes, considering a desired Cartesian stiffness $K_{d,X}=K_{d,Z}=50 \left[N/m\right]$. The performance of the controller was evaluated by injecting a disturbance on the multirotor position that emulates the effect of an external force, such as a wind gust. The zero-torque controller of the first joint (shoulder yaw) was disabled, relying only on the passive compliance to support the lateral displacements (y-axis) and heading variations (yaw angle) of the multirotor. The flight test consists of eight phases:

• The aerial manipulator takes off with the arms resting in landing configuration.
• The grabbing arm adopts the nominal pose (L-shaped) while the operation arm is retracted.
• The impedance controller is enabled, imposing a zero reference for the first joint (${\theta }_1^2=0$).
• The aerial manipulator approaches to the linear structure until the magnetic gripper grabs it.
• The aerial manipulator hovers while the impedance controller of the grabbing arm is active.
• A position disturbance is injected through the radio controller to observe the response of the aerial manipulator and the reaction of the grabbing arm.
• The torque control of the servos is disabled to release the grabbing arm.
• The arms adopt the landing pose, and the platform goes back and lands.

Figure 13 and Figure 14 show the evolution of the system in the time intervals corresponding to phase 5 and phase 6, respectively. On the one hand, Figure 13 covers the time interval from t = 25 to t = 35 s, in which the multirotor was hovering at fixed position while the grabbing arm exerted a small force in the x-axis, around 0.5 N, due to the slight displacement of the platform with respect to the nominal pose. On the other hand, Figure 14 covers the interval from t = 48 to t = 70 s. At t = 49.6 [s], we intentionally applied a 10 cm displacement in the x-axis position of the multirotor, emulating a wind gust. The grabbing arm reacted, exerting a pushing force with a peak of 2.2 [N] that counteracted the disturbance, recovering the nominal operation pose in 2 s. The instantaneous stiffness varied w.r.t. the desired value (50 N/m) due to the singularity in the displacement ($K_{d,x}=F_x/{\epsilon }_x$). Note also that, since the grabbing arm was not aligned with the ${\mathit{X}}_{\mathit{B}}{\mathit{Z}}_{\mathit{B}}$ plane of the base but it was displaced a distance $D/2$ (see Section 3.1), then the force exerted during the interaction would cause a reaction torque in the yaw angle of the multirotor, and with it, a position deviation in the y-axis, as can be seen in Figure 14. The effect was accentuated as the pilot disturbed the multirotor controller between t = 52 and t = 60 s. Although the passive deflection of the shoulder yaw joint provided a certain level of accommodation, the motion constraint associated with the grabbing condition had a more significant effect on the y-axis and yaw angle, in accordance with the results shown in Figure 9.

6. Conclusions

The paper presented two approaches for the estimation and control of the physical interactions of a compliant dual arm aerial manipulation robot operating in grabbing conditions, in which one of the arms was used as position sensor relative to a grabbing point, implementing a zero-torque controller so the wrenches induced over the multirotor base were relatively low. The mechanical joint compliance of the arm was also exploited for the development of active compliance methods, achieving desired impedance behaviors that improved the response of the aerial manipulator when it was affected by external disturbances while interacting physically with the environment. The experimental results presented here validated the concepts, allowed us to evaluate the accuracy of the position estimation, and evidenced the convenience of combining the passive/active compliance on the three Cartesian axes.

Although the aerial manipulation robot will require a navigation system to reach the workspace, the positioning accuracy required to perform the manipulation task (which should be around 10% of the reach of the arm) cannot be achieved with typical sensors employed in outdoor environments, such as GPS, LIDAR (light detecting and ranging), or vision systems. In this sense, the estimation methods described in this work result from special interest to avoid the integration of additional positioning systems, taking into account the limited payload capacity of the aerial platform, being also an effective and reliable solution. In terms of future work, we propose the application of this system for the installation of bird flight diverters on power lines, or for inspection and maintenance of railways, chemical plants, and other linear infrastructure.