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

applied sciences Article Aerial Physical Interaction in Grabbing Conditions with Lightweight and Compliant Dual Arms Alejandro Suarez * , Pedro J. Sanchez-Cuevas, Guillermo Heredia and Anibal Ollero GRVC Robotics Labs, University of Seville, 41092 Seville, Spain; [email protected] (P.J.S.-C.); [email protected] (G.H.); [email protected] (A.O.) *Correspondence: asuar[email protected] Received: 4 November 2020; Accepted: 12 December 2020; Published: 14 December 2020   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. Keywords: aerial manipulation; dual arm; compliance 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 – 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. Appl. Sci. 2020,10, 8927; doi:10.3390/app10248927 www.mdpi.com/journal/applsci Appl. Sci. 2020,10, 8927 2 of 17 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 – 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 1illustrates 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. Appl. Sci. 2020, 10, x FOR PEER REVIEW 2 of 16 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–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. Figure 1. Dual arm aerial manipulation robot grabbed a linear structure with the right (grabbing) arm. The innovative aspects of this paper with respect to our previous published works [17,21,22] can be summarized in the following points: 1. 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. 2. 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. 3. 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. 4. The experimental evaluation and qualitative analysis of the effects of the grabbing arm and the injected disturbances over the stability of the multirotor controller. Figure 1. Dual arm aerial manipulation robot grabbed a linear structure with the right (grabbing) arm. The innovative aspects of this paper with respect to our previous published works [ 17 , 21 , 22 ] can be summarized in the following points: 1. 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. 2. 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. 3. 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. Appl. Sci. 2020,10, 8927 3 of 17 4. 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 3covers 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 4is 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 1its main features. Each arm provides three degrees of freedom for end effector positioning in the following kinematic configuration [ 22 ]: shoulder yaw at the base ( q1 ), shoulder pitch ( q2 ), and elbow pitch ( q3 ). 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. 1 Figure 2. Compliant dual arm used in the experiments. 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) Appl. Sci. 2020,10, 8927 4 of 17 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 1for 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 3shows 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. Appl. Sci. 2020, 10, x FOR PEER REVIEW 4 of 16 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. Figure 3. Dual arm aerial manipulation robot. Figure 4. Hardware architecture of the aerial manipulation robot. 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 {𝑬} (inertial), the multirotor body frame {𝑩}, and the manipulator Figure 3. Dual arm aerial manipulation robot. Appl. Sci. 2020, 10, x FOR PEER REVIEW 4 of 16 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. Figure 3. Dual arm aerial manipulation robot. Figure 4. Hardware architecture of the aerial manipulation robot. 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 {𝑬} (inertial), the multirotor body frame {𝑩}, and the manipulator Figure 4. Hardware architecture of the aerial manipulation robot. Appl. Sci. 2020,10, 8927 5 of 17 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 {E} (inertial), the multirotor body frame {B} , and the manipulator frame {i} , with i={1, 2} for the left/right arms. In the following, ArB denotes the position of a certain point B w.r.t. (with respect to) reference frame {A} . In this way, ErB=[x,y,z]T and EηB=[φ,θ,ψ]T represent the multirotor position and orientation relative to {E} , and irTCP,i is the position of the tool center point (TCP) of the i -th manipulator expressed in its own frame. The origin of {i} 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. Appl. Sci. 2020, 10, x FOR PEER REVIEW 5 of 16 frame {𝒊}, with 𝑖={1,2} for the left/right arms. In the following, 𝒓 𝑨𝑩 denotes the position of a certain point 𝑩 w.r.t. (with respect to) reference frame {𝑨}. In this way, 𝒓 𝑬𝑩=[𝑥,𝑦,𝑧]𝑇 and 𝜼 𝑬𝑩= [𝜙,𝜃,𝜓]𝑇 represent the multirotor position and orientation relative to {𝑬}, and 𝒓 𝒊𝑇𝐶𝑃,𝑖 is the position of the tool center point (TCP) of the 𝑖-th manipulator expressed in its own frame. The origin of {𝒊} is located at the intersection of the shoulder joints, with the x-axis pointing forwards, the yaxis parallel to the baseline of the two arms, and the z-axis pointing upwards. Figure 5. Kinematic model of the dual arm aerial manipulator. The three reference frames are related through the corresponding transformation matrices: 𝑻𝑩= 𝑬[𝑹𝑩(𝜙,𝜃,𝜓) 𝑬𝒓 𝑬𝑩 𝟎1×3 1] ; 𝑻𝒊= 𝑩[𝑰3×3 𝒓 𝑩𝒊 𝟎1×3 1] ; 𝑻𝒊= 𝑬𝑻𝑩· 𝑻𝒊 𝑩𝑬 (1) where 𝑹𝑩 𝑬 is the multirotor rotation matrix; 𝒓 𝑩𝒊=[𝐷𝑥,±𝐷/2,𝐷𝑧]𝑇 is the origin of {𝒊} relative to {𝑩}, with 𝐷𝑥 and 𝐷𝑧 being the displacement of the arms with respect to {𝑩} in the x- and z-axes, respectively; and 𝐷 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 𝑗-th joint of the 𝑖-th arm is denoted as 𝑞𝑗𝑖, whereas 𝜃𝑗𝑖 is the corresponding servo shaft position. The difference between these two variables is the deflection angle, ∆𝜃𝑗𝑖=𝜃𝑗𝑖−𝑞𝑗𝑖, measured by the encoders [17,21]. The forward and inverse kinematic models are computed as follows (superscript 𝑖 is omitted for clarity reasons): 𝒓 𝒊𝑇𝐶𝑃,𝑖=𝑭𝑲(𝒒𝑖)=[ 𝑟(𝑞2,𝑞3)·cos(𝑞1) 𝑟(𝑞2,𝑞3)·sin(𝑞1) 𝐿1cos(𝑞2)+𝐿2cos⁡(𝑞2+𝑞3)] (2) 𝒒𝑖=𝑰𝑲(𝒓 𝒊𝑇𝐶𝑃,𝑖)= [ 𝑎𝑡𝑎𝑛2(𝑦,𝑥) cos−1(𝑥2+𝑦2+𝑧2−𝐿1 2−𝐿2 2 2𝐿1√𝑥2+𝑦2) cos−1(𝑥2+𝑦2+𝑧2−𝐿1 2−𝐿2 2 2𝐿1𝐿2) ] (3) where 𝐿1 and 𝐿2 are the upper arm/forearm link lengths, and 𝑟(𝑞2,𝑞3) is given by 𝑟(𝑞2,𝑞3)=𝐿1sin(𝑞2)+𝐿2sin⁡(𝑞2+𝑞3) (4) Figure 5. Kinematic model of the dual arm aerial manipulator. The three reference frames are related through the corresponding transformation matrices: ETB="ERB(φ,θ,ψ)ErB 01×31#;BTi="I3×3Bri 01×31#;ETi=ETB·BTi(1) where ERB is the multirotor rotation matrix; Bri=[Dx,±D/2, Dz]T is the origin of {i} relative to {B} , with Dx and Dz being the displacement of the arms with respect to {B} in the x- and z-axes, respectively; and Dis 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 qi j , whereas θi j is the corresponding servo shaft position. The difference between these two variables is the deflection angle, ∆θi j=θi j−qi j , measured by the encoders [ 17 , 21 ]. The forward and inverse kinematic models are computed as follows (superscript iis omitted for clarity reasons): irTCP,i=FKqi= r(q2,q3)·cos(q1) r(q2,q3)·sin(q1) L1cos(q2)+L2cos(q2+q3)  (2) Appl. Sci. 2020,10, 8927 6 of 17 qi=IKirTCP,i= atan2(y,x) cos−1 x2+y2+z2−L2 1−L2 2 2L1√x2+y2! cos−1x2+y2+z2−L2 1−L2 2 2L1L2  (3) where L1and L2are the upper arm/forearm link lengths, and r(q2,q3)is given by r(q2,q3)=L1sin(q2)+L2sin(q2+q3)(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 {2} to {B} , taking into account that the position of the TCP referred to {2} is directly obtained from the forward kinematic model given by Equation (2) . 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) d dt       ∂L ∂. ξ      −(∂L ∂ξ)=Γ+Γext (6) where L is the Lagrangian; K and V are the kinetic and potential energies, respectively; ξ is the vector of generalized coordinates; and Γ and Γ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, θi=hθi 1θi 2θi 3i and qi=hqi 1qi 2qi 3i, respectively, and thus ξis defined as follows. ξ=hErBEηBθ1q1θ2q2iT∈ <18 (7) Analogously, the vector of generalized forces comprises the forces and torques acting over the multirotor and the joints of the manipulator. Γ=hFBτBτ1 mτ1τ2 mτ2iT∈ <18 (8) The vector of external forces Γext models the disturbance wrenches exerted on the multirotor base [ 18 – 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]. Appl. Sci. 2020,10, 8927 7 of 17 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=KUAV +Karms (9) Each of these components comprises two terms corresponding to the translation and rotation of the masses with respect to the inertial frame {E}: KUAV =1 2mUAVkE. rBk2+1 2 EωT BIUAVEωB(10) Karms = 2 X i=1 4 X j=11 2mi jkE. rj ik2+1 2 Eωi,T jIi j Eωi j(11) where mUAV and IUAV are the mass and inertia tensor of the aerial platform, respectively, whereas mi j and Ii j 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        mUAVzUAV + 2 X i=1 4 X j=1 mi jzi j        + 2 X i=1 4 X j=1 ki jθi j−qi j2(12) Here, g is the gravity constant, and ki j is the corresponding joint stiffness. After some work, it is possible to express the dynamic model in the usual compact matrix form [14]: M.. ξ+Cξ,. ξ+G(ξ)+K(ξ)+D. ξ=Γ+Γext (13) where M∈ <18×18 is the generalized inertia matrix; C and G∈ <18 represent the centrifugal, Coriolis, and gravity terms; and K and D∈ <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 Appl. Sci. 2020,10, 8927 8 of 17 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 6illustrates. Appl. Sci. 2020, 10, x FOR PEER REVIEW 8 of 16 Here, 𝜃𝑗2 is the position of the 𝑗-th servo of the right arm, whereas the term on the right side is the incremental position correction. The proportional and integral gains, 𝐾𝑝 and 𝐾𝑖, 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): 𝒓𝑇𝐶𝑃2 𝑟𝑒𝑓 =[𝐿2 0 −𝐿1]=𝑭𝑲(𝒒𝑟𝑒𝑓 2)=𝑭𝑲([ 00 −𝜋/2]) 𝟐 (15) Figure 6. Model considered in grabbing conditions. Nominal operation pose (left), and displacement due to external force (right). The grabbing arm compensates the disturbance exerting a reaction force. 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, 𝜺=[𝜀𝑋𝜀𝑌𝜀𝑍]𝑇=𝑭𝑲(𝒒𝑟𝑒𝑓 2)−𝑭𝑲(𝒒2) (16) Note that the maximum deviation is limited by the reach of the arm, ‖𝜀‖<𝐿1+𝐿2−√𝐿1 2+𝐿2 2. If the influence of the grabbing arm over the attitude controller is relatively low due to the zerotorque 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. Figure 7. Control scheme of the aerial manipulator with grabbing arm (𝑖=2). DUAL ARM SYSTEM AERIAL PLATFORM Force Controller IK 𝜽𝑟𝑒𝑓 𝑖 ∆𝒓𝑇𝐶𝑃 𝑖 FK FK ⁡+ 𝜽 𝒒 Grabbing Arm − 𝑲𝐶∆ 𝑇𝐶𝑃 𝑖 𝑭𝑇𝐶𝑃 𝑖 ⁡+ − 𝒒𝑟𝑒𝑓 𝑖 Multirotor Attitude Controller Position Controller ⁡+FK − 𝜺 ⁡+ 𝑲𝐶𝑭𝑟𝑒𝑓 𝑖 𝜺 𝑭𝑒𝑥𝑡 Figure 6. Model considered in grabbing conditions. Nominal operation pose (left), and displacement due to external force (right). The grabbing arm compensates the disturbance exerting a reaction force. 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: θ2 j,ref =θ2 j+ Kpθ2 j−q2 j+KiZθ2 j−q2 jdt!(14) Here, θ2 j 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, Kp and Ki , 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): 2rref TCP2= L2 0 −L1  =FKq2 ref =FK           0 0 −π/2            (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, ε=hεXεYεZiT=FKq2 ref −FKq2(16) Note that the maximum deviation is limited by the reach of the arm, kεk<L1+L2−qL2 1+L2 2 . If the influence of the grabbing arm over the attitude controller is relatively low due to the zero-torque Appl. Sci. 2020,10, 8927 9 of 17 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. Appl. Sci. 2020, 10, x FOR PEER REVIEW 8 of 16 Here, 𝜃𝑗2 is the position of the 𝑗-th servo of the right arm, whereas the term on the right side is the incremental position correction. The proportional and integral gains, 𝐾𝑝 and 𝐾𝑖, 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): 𝒓𝑇𝐶𝑃2 𝑟𝑒𝑓 =[𝐿2 0 −𝐿1]=𝑭𝑲(𝒒𝑟𝑒𝑓 2)=𝑭𝑲([ 00 −𝜋/2]) 𝟐 (15) Figure 6. Model considered in grabbing conditions. Nominal operation pose (left), and displacement due to external force (right). The grabbing arm compensates the disturbance exerting a reaction force. 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, 𝜺=[𝜀𝑋𝜀𝑌𝜀𝑍]𝑇=𝑭𝑲(𝒒𝑟𝑒𝑓 2)−𝑭𝑲(𝒒2) (16) Note that the maximum deviation is limited by the reach of the arm, ‖𝜀‖<𝐿1+𝐿2−√𝐿1 2+𝐿2 2. If the influence of the grabbing arm over the attitude controller is relatively low due to the zerotorque 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. Figure 7. Control scheme of the aerial manipulator with grabbing arm (𝑖=2). DUAL ARM SYSTEM AERIAL PLATFORM Force Controller IK 𝜽𝑟𝑒𝑓 𝑖 ∆𝒓𝑇𝐶𝑃 𝑖 FK FK ⁡+ 𝜽 𝒒 Grabbing Arm − 𝑲𝐶∆ 𝑇𝐶𝑃 𝑖 𝑭𝑇𝐶𝑃 𝑖 ⁡+ − 𝒒𝑟𝑒𝑓 𝑖 Multirotor Attitude Controller Position Controller ⁡+FK − 𝜺 ⁡+ 𝑲𝐶𝑭𝑟𝑒𝑓 𝑖 𝜺 𝑭𝑒𝑥𝑡 Figure 7. Control scheme of the aerial manipulator with grabbing arm (i=2). 4.2. Impedance Control in Grabbing Conditions Assuming that the aerial platform is close to the hover state during the grabbing maneuver ( φθ 0), it is desired that the position deviation of the aerial platform is assimilated to an impedance behavior characterized as follows: Md .. ε+Dd . ε+Kdε=Fext (17) where Md , Dd , and Kd are the desired inertia, damping, and stiffness, respectively, and Fext 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 (17) . Figure 7represents the case of desired stiffness, Fi e,TCP =Kdε. 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 stiffjoint arm. That is, ∆lTCPi =FKθi−FKqi(18) This definition is useful for expressing the force at the end effector directly in the task space: iFTCPi =Ki C∆lTCPi +Di C . ∆lTCPi (19) Appl. Sci. 2020,10, 8927 16 of 17 8. Rhudy, M.B.; Chao, H.; Gu, Y. Wide-field optical flow aided inertial navigation for unmanned aerial vehicles. In Proceedings of the IEEE/RSJ Int. Conference on Intelligent Robots and Systems, Chicago, IL, USA, 14–18 September 2014; pp. 674–679. 9. Schmid, K.; Tomic, T.; Ruess, F.; Hirschmüller, H.; Suppa, M. Stereo vision based indoor/outdoor navigation for flying robots. In Proceedings of the 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, Tokyo, Japan, 3–7 November 2013; pp. 3955–3962. 10. Faessler, M.; Fontana, F.; Forster, C.; Mueggler, E.; Pizzoli, M.; Scaramuzza, D. Autonomous, vision-based flight and live dense 3D mapping with a quadrotor micro aerial vehicle. J. Field Robot. 2016 ,33, 431–450. [CrossRef] 11. Huh, S.; Shim, D.H.; Kim, J. Integrated navigation system using camera and gimbaled laser scanner for indoor and outdoor autonomous flight of UAVs. In Proceedings of the IEEE/International Conference on Intelligent Robots and Systems, Tokyo, Japan, 3–7 November 2013; pp. 3158–3163. 12. Ramon Soria, P.; Arrue, B.C.; Ollero, A. A 3D-printable docking system for aerial robots: Controlling aerial robotic manipulators in outdoor industrial applications. IEEE Robot. Autom. Mag. 2019 ,26, 44–53. [CrossRef] 13. Ruggiero, F.; Trujillo, M.A.; Cano, R.; Ascorbe, H.; Viguria, A.; Perez, C.; Lippiello, V.; Ollero, A.; Siciliano, B. A multilayer control for multirotor UAVs equipped with a servo robot arm. In Proceedings of the IEEE Int. Conference on Robotics and Automation (ICRA), Seattle, WA, USA, 26–30 May 2015; pp. 4014–4020. 14. Suarez, A.; Jimenez-Cano, A.E.; Vega, V.M.; Heredia, G.; Rodriguez-Castaño, A.; Ollero, A. Design of a lightweight dual arm system for aerial manipulation. Mechatronics 2018,50, 30–44. [CrossRef] 15. Fumagalli, M.; Naldi, R.; Macchelli, A.; Forte, F.; Keemink, A.Q.L.; Stramigioli, S.; Carloni, R.; Marconi, L. Developing an aerial manipulator prototype: Physical interaction with the environment. IEEE Robot. Autom. Mag. 2014,21, 41–50. [CrossRef] 16. Hamaza, S.; Georgilas, I.; Fernandez, M.; Sanchez, P.; Richardson, T.; Heredia, G.; Ollero, A. Sensor installation and retrieval operations using an unmanned aerial manipulator. IEEE Robot. Autom. Lett. 2019 ,4, 2793–2800. [CrossRef] 17. Suarez, A.; Heredia, G.; Ollero, A. Physical-virtual impedance control in ultralightweight and compliant dual-arm aerial manipulators. IEEE Robot. Autom. Lett. 2018,3, 2553–2560. [CrossRef] 18. Bodie, K.; Brunner, M.; Pantic, M.; Walser, S.; Pfändler, P.; Angst, U.; Siegwart, R.; Nieto, J. An omnidirectional aerial manipulation platform for contact-based inspection. In Proceedings of Robotics: Science and Systems. arXiv 2019, arXiv:1905.03502. 19. Ruggiero,F.; Cacace, J.; Sadeghian, H.; Lippiello, V. Impedance control of VToL UAVs with a momentum-based external generalized forces estimator. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Hong Kong, China, 31 May–7 June 2014; pp. 2093–2099. 20. Tomi´c, T.; Haddadin, S. A unified framework for external wrench estimation, interaction control and collision reflexes for flying robots. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Chicago, IL, USA, 14–18 September 2014; pp. 4197–4204. 21. Suarez, A.; Real, F.; Vega, V.M.; Heredia, G.; Rodriguez-Castaño, A.; Ollero, A. Compliant bimanual aerial manipulation: Standard and long reach configurations. IEEE Access 2020,8, 88844–88865. [CrossRef] 22. Suarez, A.; Heredia, G.; Ollero, A. Lightweight compliant arm with compliant finger for aerial manipulation and inspection. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Daejeon, Korea, 9–14 October 2016; pp. 4449–4454. 23. AERIAL-CORE Project Home Page. Available online: https://aerial-core.eu/(accessed on 12 October 2020). 24. Real, F.; Torres-Gonz á lez, A.; Ramon-Soria, P.; Capitan, J.; Ollero, A. UAL: An abstraction layer for unmanned aerial vehicles. In Proceedings of the 2nd International Symposium on Aerial Robotics, Philadelphia, PA, USA, 11–12 June 2018. 25. Perez-Jimenez, M.; Montes-Grova, M.A.; Ramon-Soria, P.; Arrue, B.C.; Ollero, A. POSITRON: Lightweight active positioning compliant joints robotic arm in power lines inspection. In Proceedings of the 2020 International Conference on Unmanned Aircraft Systems (ICUAS), Athens, Greece, 1–4 September 2020; pp. 729–736. 26. Sanchez-Cuevas, P.J.; Heredia, G.; Ollero, A. Characterization of the aerodynamic ground effect and its influence in multirotor control. Int. J. Aerosp. Eng. 2017,2017, 1823056. [CrossRef] 27. Liu, T.; Lei, Y.; Han, L.; Xu, W.; Zou, H. Coordinated resolved motion control of dual-arm manipulators with closed chain. Int. J. Adv. Robot. Syst. 2016,13, 80. [CrossRef] Appl. Sci. 2020,10, 8927 17 of 17 28. Lee, J.; Chang, P.H.; Jamisola, R.S. Relative impedance control of dual-arm robots performing asymmetric bimanual tasks. Trans. Ind. Electron. 2014,61, 3786–3796. [CrossRef] 29. Stolfi, A.; Gasbarri, P.; Sabatini, M. A combined impedance-PD approach for controlling a dual-arm space manipulator in the capture of a non-cooperative target. Acta Astronaut. 2017,139, 243–253. [CrossRef] Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations. © 2020 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).