On the Application of Extreme Interaction Torque with an Underactuated UAV

M.J.W. (Max) Snippe

Sc Report M

Committee: Prof.dr.ir. S. Stramigioli Dr.ir. J.B.C. Engelen H. Wopereis, MSc Dr.ir. R.G.K.M. Aarts Dr.ir. A. Yenehun Mersha

August 2018

033RAM2018 Robotics and Mechatronics EE-Math-CS

University of Twente P.O. Box 217 7500 AE Enschede The Netherlands

1

On the Application of Extreme Interaction Torque with an Underactuated UAV Marcus J.W. Snippe

Abstract—This paper aims at setting the initial steps towards as opening doors, pick-and-placing, and even building rope the application of extreme interaction torque using an underac- bridge-like structures [1]–[4]. tuated Unmanned Aerial Vehicle (UAV). Extreme torques are Although aerial manipulation has been a subject of research considered torques significantly larger than what UAVs can intrinsically generate. for several years, the manner of manipulation is limited to An optimization algorithm is designed that uses predetermined fairly low interaction wrenches. The main reason for this constraints, a desired application torque, and UAV parameters to is the fact that UAVs are free floating bodies, contrary to derive an optimal manipulator and input force and torques for for example a stationary robotic arm with a fixed base. the UAV. The optimization minimizes a nonlinearly constrained Therefore they are unable to close the force cycle with the cost function and returns an optimal homogeneous transforma- tion matrix and input covector. During optimization, the resulting environment and thus must intrinsically deliver the reaction torques and forces are scaled using a weighting function, which forces, which considerably limits the maximum interaction allows to assign priority to certain force or torque elements in forces and torques for UAVs. the application point. In 2012, a lightweight and versatile manipulator was devel- To validate the optimization, the specific use-case scenario of oped specifically for use on a ducted-fan UAV, which allowed fastening a bolt is used. The UAV parameters and desired appli- cation torque are selected for this use-case and the optimization compliant interaction with the environment for non-destructive is executed. The weighting function is used to prioritize that testing [5]. Scholten, Fumagalli, Stramigioli, et al. controlled undesired torques and forces remain small. this manipulator while in contact based on interaction control The optimal transformation is validated by means of a static to follow a path on a surface with the end-effector, whilst simulation. A certain theoretical maximum torque is considered simultaneously exerting a force on the surface [6]. For the use as reference. This is the maximum that can be reached with in free-flight, a control strategy was derived subsequently that the determined UAV weight, maximum thrust, and effective manipulator arm length. The results indicate that the achieved incorporates the dynamics of the manipulator and uses it to desired torque reaches 67% of its theoretical maximum. The improve path tracking performance and maneuverability [7]. undesired torque reaches only 41% of the value it has when the To counter the under-actuatedness of UAVs, McArthur, theoretical maximum of the desired torque is applied. For the Chowdhury, and Cappelleri endowed a tricopter UAV with an chosen UAV, the achieved desired torque is more than 7 times reversible rotor generating thrust that works in the horizontal larger than what the UAV can generate intrinsically. The applicability of the optimization for the chosen use-case plane, which adds an additional actuated direction [1]. Several is validated by a dynamic simulation with a specifically derived fully-actuated UAVs have been developed by tilting the rotor controller. To that end, a dynamic rigid body model of the axes slightly [8]–[10]. optimized manipulator on the UAV is derived using screw theory More in the direction of applying extreme forces and for rigid body dynamics. A Finite State Machine (FSM) is used torques, several control approaches for handling high energy to solve the problem of fastening a bolt with limited allowed displacement. Fuzzy Inference System (FIS) evaluation based on impacts were compared and a manipulator capable of con- an estimate of the current damping friction is used to adapt the verting the kinetic energy to potential energy and storing it controller to the unknown friction model of the bolt. permanently was realized [11], [12]. The results of the latter The results show that the chosen controller structure is where used to enable a UAV to perch on a smooth surface to capable of fastening the bolt to a desired tightening torque. The stretch battery-life and consequently the possible air-time of a adaptive FIS PID controller effectively deals with the complex friction torque, but performance can probably be improved by UAV [13]. reconsidering the semantic rule-set. The simulation results imply UAVs can intrinsically generate limited torques about their that the chosen motion profile setpoint might not be as effective xˆ and yˆ axes that are mainly used for maneuverability. By as a critically damped PID controller with step reference input. making UAVs able to execute tasks that demand significantly Index Terms—UAV, aerial interaction, torque application larger interaction torques than those that UAVs can intrinsi- cally generate, their range of applications can also include tasks like drilling holes, tightening or loosening bolts, or I.INTRODUCTION grinding or cleaning surfaces. To that end, this study will HE range of applications of UAVs is extensive, due attempt to answer the following question: T to their mobility and agility. Until shortly, UAVs were “How can an underactuated UAV be used to apply mostly used for passive tasks, such as photography/filming, an extreme torque on an axis perpendicular to a passive inspection, and surveillance. Aerial interaction has vertical surface?” increased the range of applications even more. UAV now are Where ‘extreme’ is used to indicate that the torque is sig- able to actively interact with their environment to execute tasks nificantly larger than what UAVs can generate intrinsically. 2

section III-B. The payload of a UAV is limited further by the gravitational acceleration of its own mass and the UAV mass of the intended manipulator. The mass and inertia of the manipulator will also con- • Manipulator tribute to the dynamics of the system. The UAV must still be able to fly in free-flight, without completely saturating Torque axis its input force and torques. The UAV with the intended manipulator will have to be • positioned in an optimal state to exert maximum torque. Application point This optimal state must be either directly reachable or reachable through a control procedure. Additionally, the following assumptions are made in this study. Fig. 1. Example of a UAV endowed with a rigid manipulator that is able to Typically, half of the maximum inputs is used as a safety • use its thrust to exert a torque on the application point. margin to maintain some level of maneuverability. This rule of thumb will also be kept in this study. To ensure that the UAV’s rotors will not hit the surface, A more specific definition of these torques will be provided • further on. a safe distance has to be kept at all times. This distance Firstly, the problem is elaborated and the proposed approach will have to be kept between the bounding box of the and used notation are described in the following section. UAV and the surface. This study is confined to applying a torque about an Subsequently, an optimization of a rigid manipulator is derived • in section III and a controller is designed for a specific axis perpendicular to a virtual flat vertical surface that realistic use-case in section IV. The simulated experiments stretches out infinitely. Phenomena as the ground-, wall-, and ceiling-effect are will be described alongside their results in section V. Finally • sections VI and VII will discuss and conclude the paper. not considered. The manipulator has a uniform mass distribution, thus a • II.PROBLEM ANALYSIS constant mass per unit length. Its inertial properties are assumed to be equal to those of a thin rod. A. Problem description The manipulator has an end effector with a fixed mass. • Miniature to small scale UAVs (with a gross take-off weight Its inertial properties are assumed to be equal to those of up to 10 kg and an arm length of up to 0.5 m) can intrinsically a point mass. generate torques in the order of 0 to 15 N m. Therefore, ± these UAVs are unable to execute tasks with higher required torques, e.g. tightening a metric M10 bolt to its recommended B. Approach tightening torque requires at least 30 N m, depending on its 1 The displacement and orientation of the UAV imposed by grade and material . the manipulator as seen from the application point is described In this paper an attempt is made to derive a method to exert with a generic homogeneous transformation. The kinematics of an extreme torque on an axis perpendicular to a vertical surface the system are determined based on this transformation. These by using an underactuated UAV of miniature to small scale. kinematics are used to determine the transformation of the A torque is considered extreme if it is at least five times as force and torques applied by and on the UAV to the application large in magnitude as the torque that the UAV can intrinsically point. generate about any of its principle axes. To amplify and An optimization procedure is executed to determine what concentrate the force and torques that the UAV can generate, the optimal transformation is that maximizes the desired torque the choice is made to endow the UAV with a rigid manipulator. at the application point. This manipulator can be attached to the UAV either with a The constraints of the optimization are determined based on joint, or rigidly fixed. Figure 1 shows an example of how the the constraints and limits mentioned in section II-A. These can UAV would be positioned with respect to the surface and the be ordinary boundary conditions, but could also contain highly application point. non-linear constraints due to the coupling of thrust force and To be able to exert extreme torques using an underactuated torques that the UAV can generate. UAV, the following points have to be taken into account. A specific hypothetical UAV with a certain desired torque UAVs in single contact2 cannot close the force cycle with • on the application point is used to validate the optimization. their environment and therefore must be able to withstand The optimization is solved numerically. The results are used the torques that they apply to maintain an equilibrium to derive a dynamical model of the UAV with the manipulator, state. in contact. The maximum torques and thrust of an underactuated • The transformation is validated with a static simulation of UAV are limited and coupled, as will be shown in the dynamic model. For this simulation it is assumed that 1https://www.blacksfasteners.co.nz/assets/Metric-8 12466 1.pdf the application point is unmovable and resists all forces and 2Contact without anchoring to another contact point. torques that the UAV applies. The results of this simulation 3

j j k are the measured forces and torques on the application point, used as mapping from one frame to another, e.g. P = HkP . which can be compared with the theoretical result. For twists, this mapping is defined as To validate that the manipulator can be used in a realistic k,l m,l T = AdHk T , use-case scenario, a custom controller is designed for the use j m j case of tightening a bolt. The friction model of the bolt is where k assumed unknown to the controller. Multiple levels of damping Rm 0 Ad k = . friction are simulated to verify the generic applicability of the Hm p˜k Rk Rk  m m m manipulator and controller. For the wrench, due to duality, the mapping is

k T T m T W = AdHm (W ) . (1) C. Notation k H-matrices are constructed
as The notation used in this work follows the notation used Rk ok by Stramigioli and Bruyninckx [14]. Generally, subscripts Hk = j j , denote a ‘name’ indicator and superscripts denote a reference j 0 1 k   frame (e.g. pj denotes the point j expressed in frame Ψk). k 3 where o R denotes the origin of Ψj expressed in Ψk Quantities without superscript are purely geometric quantities, j ∈ and Rk SO(3) is the rotation matrix that describes the e.g. (co-)vectors and points, or scalar entities, e.g. power. j ∈ 3 rotation of Ψj with respect to Ψk. A more detailed introduction Capital Pj PR is generally used to denote the point pj ∈ to screw theory in robotics is provided by Stramigioli and in three dimensional projected space, e.g. Bruyninckx [14]. T T All coordinate systems used in this study consist of an Pj = pj 1 , origin o and the xˆ, yˆ, and zˆ axes. The coordinate systems where T denotes the transpose operator.
Due to this notation, are right-handed and orthonormal. The hat (ˆ) accent is used any scalar multiplication λPj where λ = 0 still represents the for (co-)vectors of unit magnitude. The same notation is used 6 same point in R3. for unit twists and in section IV-A the definition of a unit twist The tilde accent (˜) is used to denote the skew-symmetric will be given. matrix representation of three-dimensional (co-)vectors. This In the schematic drawings shown in this study, are skew-symmetric matrix is build from a vector such that x y = forces, are torques, and xˆ-, yˆ- and zˆ-axes are drawn as ∧ xy˜ . Thus , , and respectively, unless noted otherwise. 0 x x The maximum value of an arbitrary variable x is denoted − 3 2 x˜ = x3 0 x1 . x¯, the minimum value is denoted x.  x x −0  − 2 1   III.OPTIMIZATION Screw theory is used to describe rigid body dynamics. Unless noted otherwise, every rigid body has its coordinate To optimize the transformation imposed by the manipulator, frame defined in its Center of Gravity (CoG). Generalized first the kinematics are determined in section III-A. Then 6 1 velocity of a frame is denoted T R × for twist. Twists the constraints and limits for the optimization are determined ∈ are composed of the angular velocity of a body and its virtual in section III-B. A new coordinate frame Ψf is defined linear velocity through the origin of the frame in which it is in section III-C, in which the resulting optimal wrench is expressed, expressed during the optimization. This additional frame sim- ωk,l plifies the cost function. Subsequently, a cost function is T k,l = j , derived that will be minimized in section III-D. Finally, the j vk,l j ! results of the optimization of the specific UAV are presented where ω and v are three-dimensional vectors. The superscript in section III-E. notation of twists is slightly different from what was men- k,l tioned before, i.e. Tj denotes the twist of frame Ψj with A. Kinematics respect to frame Ψl, expressed in frame Ψk. 1 6 In this study the system is considered as shown in figure 3. Generalized forces are denoted W R × for wrench. ∈ The frames introduced in figure 3 are: Wrenches are the dual of twists, and are composed of torques k k, Ψ0 inertial non-moving frame which coincides with the and forces, such that W T • = P where P is power as scalar, • application point; W k = τ k f k , Ψb UAV body-fixed frame placed in its CoG and aligned with its principal axes; where τ and f are the torques and forces
respectively, both Ψm similar to Ψb but for the manipulator; are three-dimensional co-vectors. Ψgb UAV’s gravity frame, also placed in its CoG but The position and orientation of frame Ψk with respect to aligned with the axes of Ψ0 such that the gravita- frame Ψ , can be expressed in a transformation matrix Hl tional force always points along the negative zˆgb- l k ∈ SE(3). Transformation matrices also conform to the sub- and axis; superscript rules as mentioned above. Additionally, they are Ψgm similar to Ψgb but for the manipulator. 4

f3 Ψ0 f4 τ4 Ψ gm Ψm τ3 f 2 Ψb Ψgb τ2 Ψb pz f5 la fgm f1 τ5 p py z f6 x τ6 fgb fgb τ1 Ψ0 y Fig. 3. Schematic representation of the UAV with generic manipulator. The x dashed axes compose the frames for the gravitational forces, the solid axes Fig. 2. Forces and torques generated by-, and gravity working on an under- the body fixed frames. The violet line represents the manipulator. actuated UAV. The length of the UAV arms is denoted la.

application point (Ψ0) in the body fixed coordinate system Ψb. 0 The UAV is treated as a single rigid body that generates pb will also transform the gravitational wrench, the forces and torques as shown in figure 2. These forces and I p0 H0 = 3 b , torques can be described as a single thrust force ft along the gb 0 1 body’s z-axis and three torques about the three body axes τx,   τy, and τz, following and the gravitational wrench of the manipulator will be trans- 0 formed by half pb f 1 I 1 p0 1 1 1 1 1 1 f f H0 = 3 2 b .  2 t gm 0 1 1 1 1 1 1 1 f τ   K  2 2 − 2 − − 2  3 =  x , 1 0 1 1 0 1 f  τ Using equation (1), the wrenches from equation (3) can be − −  4 y  1 1 1 1 1 1 f  τ  transformed to the application point Ψ0. When expressed in  − − −   5  z    f6   the same frame, the wrenches can be summed element-wise,   0   (2) giving one total wrench Wtot. where 0 T T gm T T gb T T b T gm (Wtot) = AdH (Wgm ) + AdHgb (Wgb ) + AdHb (Wb ) . K = diag √3 0 0 0 1 la 2 la kd , 0 n o To clearly show the dependencies of Wtot on inputs and and kd 1 is the ratio between generated thrust and drag the gravitational forces, it can be expressed as the following  torque of the rotors, which is assumed constant and equal for matrix product all rotors. la denotes the length between the rotors and the T T W 0 = G τ τ τ f f f , (4) CoG. For every rotor i Z [1, 6], the thrust force of that rotor tot x y z t gb gm ¯ ∈ ∩¯ is fi R [0, frot], where frot is the maximum thrust that one where G is a
non-linear function of φ, θ, ψ, p , p
, and p ∈ ∩ ¯ x y z rotor can generate. The total thrust ft thus is ft R [0, 6frot] ∈ ∩ and will be provided in section III-D in simplified form. and 6f¯rot = f¯t. The torques and force on the right hand side of equation (2) B. Constraints and limits are the controllable inputs of the system. Besides the inputs, two separate gravitational wrenches It must be taken into account that the forces in equation (4) are acting in the system. All forces and torques add to the are not of the same order of magnitude. fgb is proportional total system wrench as three separate wrenches, expressed as to the UAV’s mass, thus fixed for a chosen UAV. fgm is follows proportional to the length of the manipulator, determined by px, py, and pz and thus cannot be controlled directly. The b Wb = τx τy τz 0 0 ft , maximum torques τ¯x and τ¯y and the maximum thrust f¯t gb W = 0 0 0 0 0 fgb , (3) are mutually coupled. This coupling is described elaborately gb −
gm in appendix A. In short, this coupling allows the co-vector Wgm = 0 0 0 0 0 fgm , −
τx τy ft to be in the (approximated) 3D space shown in
figure 13b, assuming that τz is zero. This is captured in the where fgb and fgm are the gravitational forces acting on the
following inequality constraints UAV body and the manipulator respectively. The frames in which these wrenches are expressed are shown in figure 3. 2 2 3la ft + τ 6f¯rot, τ ft, τ f¯rot, The generic rigid manipulator is used to transform the 3la ≤ 3la ≤ ≤ 2 wrench generated by the UAV to the application point. To where that end, the manipulator can be defined as a generic trans- τ = τ 2 + τ 2 0 x y formation which can be fully described with the Hb -matrix. 0 0 ¯ q This matrix contains the position pb and orientation Rb of the and frot is the maximum thrust of a single rotor. 5

The UAV must be able to maintain in-flight stability with the manipulator equipped. To that end, the additional load 0 f torque due to Wgm and the gravitational force acting on the zˆ yˆ end-effector cannot be greater than the torque that can be xˆ0 =x ˆf generated with the thrust needed to maintain hovering. To α maintain maneuverability, an additional safety factor of 1/2 yˆ0 is used, which places the following constraint on the length zˆf and mass of the manipulator Fig. 4. The manipulator (again in violet) without UAV to show how Ψ is 1 3 f l g m + m l m g, defined. m,xy ee 2 m ≤ 4 a tot   where mtot = mee + mm + mb, mee is the end-effector mass, mm is the manipulator mass, mb is the UAV body mass, and C. Additional wrench frame l xy m,xy is the length of the manipulator projected on the - Any force generated by the UAV will be directly transferred plane of the world frame, thus the effective arm gravity makes to the application point. In addition, the transformation of Ψ from the end effector’s CoG and the manipulator’s CoG to b. torques from Ψ to Ψ cannot cancel forces in Ψ . Therefore, Additionally, the total gravitational force may not exceed b 0 0 minimizing the force in Ψ directly minimizes the force in Ψ . f¯ /2, again to maintain maneuverability. This places the b 0 t This will bring the optimal manipulator back to where only following constraint τx is exerted on the application point and the thrust is used to 1 cancel gravity. gmtot f¯t. ≤ 2 To properly define a constraint on the force, it must con- Another maneuverability constraint is defined to limit the strain the components of the force that are not contributing to moment of inertia of the manipulator, including the mass of the desired torque. To that end, the wrench can be expressed the end-effector. Due to the relatively low maximum yaw in a virtual frame Ψf , which shares its origin with Ψ0 and b torque about zˆ , the additional moment of inertia due to aligns its yˆ-axis with the projection of the manipulator on the the end effector and the manipulator should not reduce the 0 yz-plane of Ψ0. To transform Wtot to Ψf , equation (1) can maneuverability too much. When the UAV is in free-flight be used with AdH0 . with the manipulator endowed, there is no need for aggressive f H0 can be parameterized with six parameters3. The orienta- maneuvering, but an acceptable angular acceleration must b tion is parameterized with three angles ψ, θ and φ correspond- still be feasible. Because angular acceleration is inversely ing to the sequential rotations about the zˆ-, yˆ - and xˆ -axis proportional with the total moment of inertia, the maximum 0 00 respectively. These angles are Tait-Bryan angles, commonly moment of inertia about zˆb is chosen to be limited to twice known as yaw, pitch, and roll in aviation. The three position the moment of inertia of the UAV about zˆb parameters are px, py and pz. Jee + Jm 2Jzz,b, Due to the rotational symmetry of the UAV as explained ≤ such that the total moment of inertia about zˆb remains smaller in appendix A, under the assumption that τz is kept zero, ψ is not influencing the value of W 0 . Therefore, during than 3Jzz,b. Jee and Jm are the moments of inertia of the end- tot effector and the manipulator respectively, both about zˆb, and the optimization ψ is taken 0 rad to decrease the needed computations. Afterwards, ψ is taken such that the manipulator Jzz,b is the third diagonal element of the UAV’s inertia matrix. The UAV needs to have a certain distance from the surface always lies in the xz-plane of Ψb, where x is positive. This on which the torque is applied to safeguard the rotors from way, the manipulator is always pointing ‘forward’ which will hitting the surface. The bounding box of the UAV is approx- ease flying in free-flight. f imated by a cylinder shape with a radius of la + rr, where When py and pz are known, H0 can be determined with the la is the length of the UAV’s arms and rr the radius of the atan2 function with py and pz as arguments. See equation (27) rotors. An additional safety margin is determined to be 5 cm. in appendix D for the definition of atan2 used in this paper. f The minimum absolute distance p from the UAV CoG to H0 can then be expressed as | x| the surface is therefore constraint as 1 0 0 0 px la + rr + 0.05. (5) f 0 cos α sin α 0 | | ≥ H0 =  −  , Finally, the maximum angle that the UAV is able to make 0 sin α cos α 0 0 0 0 1 with the horizontal plane will have to be constraint. For   applications in which the manipulator can suddenly break   where loose from the application point, the UAV must maintain controllability. To that end, the maximum angle is limited by α = atan2(pz, py). constraining the pitch angle θ and roll angle φ to π/8 = 22.5◦ as follows π θ2 + φ2 . (6) ≤ 8 3Provided that θ = π/2 but with the constraint in equation (6), θ is not p allowed to reach π/62. 6

D. Cost function The only constant boundary constraint that is of importance The goal of the manipulator is to transform the UAV wrench is the minimum length of px, from equation (5). in such a way that the total wrench in Ψf conforms to the px < (la + rr + 0.05), following: − where l is the length of the UAV arm and r the radius of its τ f is maximized, because this is the goal of this study; a r • x rotors. This constraint is limiting the maximum of p , because τ f , τ f , and f f are minimized, because these torques x • y z y the positive xˆ0-axis points into the wall. are undesired and can damage the end-effector or the For the following, it is assumed that the manipulator has application point; uniform mass distribution and a mass density per length unit f f = f 0 is also minimized, but must be positive, because • x x of ρ , the end-effector can be represented as point mass, and a positive force along xˆ0 is needed to prevent the UAV m the manipulator as a slender rod. from drifting from the application point. The intermediate variable values that are calculated are τ f f In this, fz is not constrained, because it directly contributes as in equation (7) and f to τx . 2 2 2 Due to the fact that k 1, the maximum τ¯ is significantly lm = p + p + p , d  z x y z smaller than τ¯x and τ¯y. Increasing τz is also very costly fgm =q gρmlm, in terms of thrust. Therefore, we choose to neglect τz as a 2 (8) Jee = meel , potential input. With τz = 0, equation (4) loses a column and m can be expressed as 1 2 Jm = mmlm, 0 T T 3 Wtot = G τx τy ft fgb fgm , where g is the gravitational acceleration constant. where G is

With the constraints from section III-B and the interme-

py diate variables from equation (8), the following inequality cθ sφsθ pzsφ + pycφcθ py 2 − −p constraints are set. The inequality constraints are limiting the 0 c c (p c p s ) p x φ φ x θ z θ x 2 maximum torque in the xy-plane with equation (9a), the thrust  s c s − p s p− c s 0 0  G = − θ θ φ − x φ − y φ θ , based on figure 13b with equations (9b) and (9c), the total sys-  0 0 c s 0 0   φ θ  tem weight with equation (9d), the maximum roll/pitch angle  0 0 s 0 0   − φ  combined, with equation (6), and the manipulator moment of  0 0 cφcθ 1 1   − −  inertia, including the end-effector mass mee with equation (9f).   where sin i and cos i are denoted si and ci respectively. An optimal manipulator can be determined by minimizing 3 τ l f¯ 0 (9a) an objective function, taking into account the (non-linear) − 2 a rot ≤ constraints set on the decision variables. The objective function 2 τ ft 0 (9b) can be defined as 3la − ≤ 2 ˇ ft 6f¯rot + τ 0 (9c) min QU O(qd) O 2, qd − − 3la ≤
f + f 3f¯ 0 (9d) where gb gm rot T − ≤ f f f f 2 2 π O(qd) = τx τ fx fy , φ + θ 0 (9e) − 8 ≤ 2 2
J p+ J 2J 0 (9f) f f f ee m zz,b τ = τy + τz , (7) − ≤ R r The optimization is carried out by using MATLAB [15],     4 4 fmincon4 qd is a vector containing the decision variables, Q R × in particular the function . This function is chosen ∈4 4 is a constant diagonal weighing function, and U R × is to allow for non-linear constraints, using an interior point ∈ a diagonal matrix that scales only the force entries of O(qd) algorithm for nonlinear programming [16]. with lm. The latter is used to obtain equal units for all entries, which allows to sum them in calculating the norm. E. Results ˇ The desired value of O(qd) is given in O: The numerical values used in this optimization are listed in T table I. The value for ρ corresponds to the weight per unit Oˇ = τ b 0 0 0 , m x,des length of a typical carbon fiber tube. The remainder of values b 0 where τx,des is the desired target value for
τx . correspond to typical values for a small-scale hexacopter. b The following decision variables are combined in qd: For the chosen use case, the desired value of τx,des is chosen to be 10¯τ which equals 15laf¯rot. Substituting the numerical qd = ft τx τy px py pz φ θ . values from table I, this equals 60.75 N m, which provides the Two types of constraints are used in the optimization,
UAV with enough torque to fasten an M10 metric bolt to its constant boundary constraints, and inequality constraints. Ad- recommended fastening torque. ditionally, some intermediate variables are used to make the 4See https://mathworks.com/help/optim/ug/fmincon.html for the documen- R calculations more uncluttered. tation of the MATLAB fmincon function. 7

TABLE I Ψ0 NUMERICAL VALUES USEDINTHE OPTIMIZATION FUNCTION. Ψm Ψb Parameter Value Unit ¯ -0.2 -0.6 frot 13.5 N 0 -0.4 m 2.5 kg 0.2 -0.2 b 0.4 0.6 l 0 0.8 0 0 a 0.3 m y 1 0.2 x rr 0.1 m 1.2 −1 ρm 0.06 kg m Fig. 5. The optimal transformation of Ψ and consequently of Ψ . g 9.806 65 m s−2 b m 2 −1 Jzz,b 0.1 N m s rad

TABLE II The second element can never be zero when the thrust is used OPTIMAL VALUESOFTHE DECISION VARIABLES. to generate additional xˆ0 torque, due to the minimum length of px. Additionally, in this configuration the last element of Parameter Value Unit 0 f Wtot is aligned with zˆ and is therefore not constrained. The ft 60.909 N first element is nearly 7.3 times as big as τ¯, which comes close px −0.45 m py 1.1807 m to the desired 10 times and is still large enough to fasten an pz 0 m M10 bolt. φ 0 rad θ 0 rad The optimal input covector is τ 2.1636 N m x b b τ −5.6767 N m τ τ ft 2.2 5.7 60.9 y x y ≈ − which is also plotted as
in figure 13b. It shows
that the endpoint is placed on the surface of the potential space, The goal of the optimization is to maximize τ f , but keep the x indicating that the input is completely saturated when applying other elements of O(q) close to zero. Because it is unknown f the maximal torque. on beforehand what a reachable maximum for τx is, the focus is put on keeping the other elements close to zero. To this end, the weighing function Q is chosen such that less focus is given IV. CONTROL to the first term of O(q) Oˇ. − Most possible use cases that require high torques, require 1 it to obtain a certain displacement at high damping or to Q = diag 2 1 1 1 overcome stiction. The former needs a controller that allows Interestingly, setting the first element of Q, henceforth referred movement and the latter needs a certain amount of caution to as Q[1], to any value above 0.1, will not change the decision when increasing the applied torque, due to a possible over- 0 variables that define Hb . The decision variables that change shoot when the friction suddenly decreases drastically. This b b are ft, τx, and τy . A balance is found between using thrust to illustrates that the control of a UAV in torque applying mode f f b b increase τx and consequently increase τy or using τx and τy to is not straightforward and demands a dedicated controller. To f decrease τy . When setting Q[2] to 0, the maximum thrust will achieve this, first the dynamic behavior of the system in the f f be used to generate τx , because τy is no longer considered. chosen use case is determined. Next a FSM is introduced to The resulting optimal values of the decision variables are overcome the problem of rotating back and forth in the chosen 0 listed in table II. These result in a transformation Hb of use case. Controllers are tuned for the separate states, and finally the problem of stiction is treated by introducing a FIS 0.35614 0.93443 0 0.45 to determine an additional controller gain. 0.93443 0.35614 0− 1.1807 H0 = (10) b − 0 0 1 0   0 0 0 1  A. Dynamics     and A dynamic model has been developed to validate the kinematics of the optimal manipulator. Both the UAV and 0.35614 0.93443 0 0.225 − the manipulator are considered rigid bodies, rigidly fixed to 0 0.93443 0.35614 0 0.59035 each other. For the use case of fastening a bolt, the Degree of Hm = −  , (11) 0 0 1 0 Freedom (DoF) that is to be controlled is the rotation about  0 0 0 1  0   xˆ , which is denoted q1. The formerly static transformation   matrices are now functions of q in this use case. The inputs which result in the frames shown in figure 5 as Ψb and Ψm 1 respectively. are ft, τx and τy, again assuming τz to be kept zero. As expected, the optimal transformation matrix places the Additionally, a ratchet-like joint is assumed between the thrust perpendicular to the manipulator arm to maximize its manipulator and the bolt. This joint ensures that the UAV is f f able to move back after tightening the bolt, to keep the needed contribution to τx . This also ensures that fy is zero. The resulting wrench in the application point then is stroke length small. To determine the Jacobian function that maps from the 0 Wtot = 44.69 10.53 0 0 0 35.65 . (12) change rate of the generalized coordinates to the rigid body
8

twists, the unit twist of q1 is used. The unit twist of a rotational where 0 T 0 T 0 1 (R ) (R ) p joint is defined as H0 − = b − b b ωˆ b 0 1 Tˆ = ,   r˜ωˆ
1 0 −   and likewise for Hm . where ωˆ denotes the unit angular velocity of the joint and r˜ the Euler-Lagrangian equations can be solved for the complete
tilde representation of the arm r that reaches from the origin system to find the equations of motion. Substituting the numer- to the rotation axis of ω. Note that this can be any point on ical values provided by the optimization the Euler-Lagrangian that axis [14]. results in 0 The generalized coordinate q1 only rotates about xˆ , which d ∂ ∂ 0 gives a unit twist of L L = τx + τtd dt ∂q91 − ∂q T   Tˆq = 1 0 0 0 0 0 0 1 3.57q:1 + 29.39 cos q1 = τx + τtd, (15) and a constant Jacobian mapping of
where τtd denotes the auxiliary torques due to friction. 0,0 ˆ Due to the aforementioned ratchet joint, the bolt friction Ti = Jq91 = Tq1 q91. torque τ is only considered when the bolt is tightened, i.e. Now this Jacobian can be used to express the kinetic co- b when q91 > 0. Additionally, when the bolt is completely energy E of the two bodies as follows: ∗ fastened (when the head of the bolt touches the surface) it T 1 i,0 i i,0 will start elongating, adding a spring element to the dynamics. Ei∗ = Ti Ti 2 I Consequently, the head of the bolt is pulled towards the wall 1   T 0,0 T i 0,0 by its elongation, increasing the normal friction of the head. = Ti AdHi AdHi Ti 2 0 I 0 This also increases the friction torques significantly. 1 T T  T i = q9 J AdHi AdHi Jq9, (13) 0 0 0, if q91 0, 2 I ≤ where i can be either m for manipulator or b for the UAV and τtd = τb, if q91 > 0 and xbolt 0, (16)  ≤ denotes the inertia tensor. The choice of expressing every  I τb + τe, if q91 > 0 and xbolt > 0, rigid body frame in the CoG of that body with its axes aligned τ τ with the principal inertial axes of the body, ensures that i is where b denotes the torque due to friction of the bolt and e I diagonal and can be composed as denotes the additional torques due to reaching the end of the bolt. Therefore, there are three separate system models with J 0 i = i . significantly different dynamic behavior. I 0 miI3   To ensure a realistic bolt model, the friction torque τb cannot Besides the kinetic co-energy, the potential energy Vi(q) of be considered merely viscous. Using a discontinuous friction every body can be determined as follows model that includes a static friction term, can be problematic when using numeric simulation techniques, therefore the con- V (q) = m gh i i tinuous friction model from Specker, Buchholz, and Dietmayer 0 T = mig 0 0 1 0 Hi (q) 0 0 0 1 . (14) is used [17]. Figure 6 shows how the friction force of this With equations (13) and
(14) the system’s Lagrangian
rotation can be build from three separate terms, viz. viscous-, equation can be determined as Coulomb-, and Stribeck friction. The separate terms can be expressed as: (q1, q91) = E∗(q1, q91) V (q1) L − τv(ω) = Dω, 1 T T T m = q91J AdHm m AdHm Jq91 2 0 I 0 ω τc(ω) = τc,c tanh , 1 T T T b ωt + q J Ad b Ad b Jq1   91 H0 b H0 9 2 I τst(ω) = g(ω)[τst,c τc(ωsp) τv(ωsp)] , 0 T − − mmg 0 0 1 0 H (q1) 0 0 0 1 − m where m g 0 0 1 0 H0(q ) 0 0 0 1 T 2 b
b 1
ω ω 1 − g(ω) = exp + and to that end Hm and Hb must be expressed in terms 0 0

ωsp − √2ωsp ! 2 of q1. This can be done by transforming the matrices in equations (10) and (11) as follows: and the total bolt friction torque is 

1 0 0 0 τb(ω) = τv(ω) + τc(ω) + τst(ω). (17) b 0 cos q1 sin q1 0 0 1 H (q1) = H − , In the above τ and τ denote the Coulomb- and Stribeck 0 0 sin q −cos q 0 b c,c st,c 1 1 friction constants respectively, D denotes the viscous damping 0 0 0 1
  constant, and ωt determines the steepness of the tanh function 1 0 0 0 in the expression for the Coulomb friction. Similarly, ωsp 0 cos q sin q 0 1 m 1 1 0 − determines the region in which the Stribeck friction is present. H0 (q1) =  −  Hm , 0 sin q1 cos q1 0 Note that ωsp is not the Stribeck velocity ωst, but is typically 0 0 0 1
  chosen ωsp ωst/3 [17].   ≈ 9

τ (N m) 0/0 Stribeck τst 0/0 Coulomb τc Viscous τv 1/0 Total τb

ROTATING 1/1 start FASTENING FINISHED 0 −1 -/1 BACK 0 ωx (rad s )

1/0

0/1 Fig. 7. Finite state machine structure of the controller. The conditions are Fig. 6. Description of a continuous model for the friction forces τb as a 0 0 shown in the form desired angle q1 reached / desired torque τ reached , function of ωx = q91[17]. x e.g. 1/0 denotes that theh end of the position strokei h is reached without reachingi the desired torque and 1/1 means that both the desired position and the desired torque are reached. A dash (-) denotes that the concerning state transition can B. Finite state machine happen with either value for that condition. Due to the significantly different dynamic models resulting from equations (15) and (16), the control approach is chosen to representing a moving mass, i.e. a double integrator plant. contain a FSM with separately tuned controllers. In state FAS- For the chosen third order motion profile and the considered TENING, q1 will start at 0 and rotate toward the desired angle. moving mass, PD controller gains can be determined based When this angle is reached or when the desired application on path following criteria [18], [19]. The maximum tracking torque is reached, the state will change to ROTATING BACK, error is chosen to be 5% of the maximum stroke. The cross- in which the q1 setpoint is set to 0 again. When q1 is back at over frequency ωpd of the PD controller is then determined 0, the state will change to FINISHED if the desired application following torque was reached the last time in FASTENING and back to ¯ q:1√β FASTENING if the application torque was not reached. Note ωpd = , that the desired application torque must be smaller than or s e¯t equal to the maximum torque deliverable. ¯ where e¯t is the desired maximum error and q:1 is the maximum The inertia of the UAV makes that its angular velocity acceleration of the third order motion profile, which can be cannot be changed instantaneously, which means that the determined as follows: switching between states will lower average velocity. Increas- ¯ 8hm ing the stroke length of the motion thus increases the time q:1 = , t2 efficiency of the control approach. Therefore, the desired m position is the maximum q1 provided that the conditions in where hm and tm denote the amplitude and the duration of section III-D still hold. To that end, the reference setpoint is the motion profile respectively. determined to be π/8 rad, which is the limit of the UAV’s The continuous PD controller can be described with the roll/pitch angle. following transfer function If the UAV will descend past q1 = 0 and reach the end of τˇ s + 1 C (s) = K z the bolt before being rotated back to q1 = 0, it is impossible pd c τˇps + 1 for the system to return and thus it is stuck in an impasse. 5 To prevent this, the minimum allowed angle is set to 0 and and the parameters can be determined with the maximum to π/8. To ensure that overshoot is minimal 2 meqωpd √β 1 and movement is smooth, the reference input of the system is Kc = , τˇz = , τˇp = , (19) √β ω √βω chosen to be a third order motion profile. pd pd where β = 10 is a tameness factor and meq is the equivalent mass as seen by the controller (equal to 3.57) [18]. C. Synthesis The resulting PD controller is The dynamic equation of motion of the system was derived 1.147s + 5.437 C (s) = , in section IV-A and is pd 0.02109s + 1 0 3.57q:1 + 29.39 cos q1 = τx + τtd. (18) which is tuned to state ROTATING BACK, where the UAV is moving back with (ideally) zero friction. The nature of this equation allows the non-linearity of the Considering that in state FASTENING the bolt adds a com- differential equation to be canceled by an input component. plex friction component to the dynamics of the system, a To accomplish that, a feed-forward controller is used. This different controller is needed. Firstly, there is the possibility reduces equation (18) for state ROTATING BACK to 5 The accents of τˇz and τˇp have no other function than to distinguish the 0 3.57q:1 = τx , time constants from the various torque variables. 10 that the end of the bolt is reached just before the end of the 1.6 motion profile is reached. The small remaining error will lead 1.6 to a small plant input, but the large friction torque τe will a

immobilize the UAV when only using a PD controller. To reach K 1.2 1.2 the end of the stroke or saturate the input without reaching the

Gain 0.8 end of the stroke, an integral action is added. 0.8 For a PID controller the desired crossover frequency ωpid 0.4 can be determined with 0 0.4 ¯ 0.2 40 3 q;12β 20 30 ωpid = , 0.4 0 10 s e¯t q9 (rad s−1) τ (N m) ¯ where q;1 is the maximum jerk of the third order motion profile Fig. 8. Evaluation of the FIS with the rules as in appendix C and membership [19]. The maximum jerk can be determined with functions as in figure 20.

¯ 32hm q;1 = 3 . tm The total wrench applied to the application point during the The PID controller can be described with static test was 0 τˇzs + 1 τˇis + 1 Wtot = 45.16 10.73 0.00 0.00 0.00 35.71 , (21) Cpid(s) = Kc . τˇps + 1 τˇis which indeed is very close to the optimal value shown
in The parameters from equation (19) can be recalculated for the equation (12). PID controller by substituting ωpd for ωpid and additionally For a dynamic test, the parameters mentioned in equa- τˇi can be chosen tion (17) must first be determined. The needed accuracy for τˇi 2ˇτz these parameters is low, therefore it is sufficient to make ≥ a rough approximation of the order of magnitude of the to prevent that the integral action deteriorates the phase parameters. By initializing the UAV in the optimal state and advance of the D action[18]. This results in the following PID setting the input to zero, it should intuitively behave as a controller: damped pendulum. The parameters have been set such that 0.6045s2 + 4.174s + 5.881 the response was roughly equal to that of a physical damped C (s) = . (20) pid 0.01028s2 + 0.5069s pendulum. It will be shown later that a high accuracy for these parameters is not significant for the validation of the optimal To prevent that the state transitions de-stabilize the system, manipulator and derived controller. the start of the motion profile when entering a state is set to To ensure that the FSM controller works as intended for a the current position. Additionally, the I wind-up of the PID variety of possible bolts, the bolt friction torque τ is scaled controller is reset every state transition. b between 1/2 and 3/2 times its initial value.

D. Fuzzy inference system B. Results

To generalize the PID controller for state FASTENING for Three simulations have been run, with τb scaled by 1/2, multiple possible bolt friction shapes, an additional configu- 1, and 3/2 respectively. Figure 10 shows the results of these ration gain Ka is used to make the controller adaptive. By three simulations, i.e. in- and output of the plant u and q1, the considering the rate of change of q1 and measured torque reference angle q1,des, the error between reference and output 0 on the application point, a convenient choice for Ka is made e, the applied torque τx and the additional controller gain Ka based on a set of rules that command a FIS. This will allow as returned by the FIS. The latter is set to 1 if the FSM is not the controller to e.g. lower Ka when a small force results in in state FASTENING. Note that the input u is normalized with a large rate of change and increase Ka when a large force the maximum torque from equation (12). results in a small rate of change q91. As can be seen in figure 10, the differences between the Ka will scale the controller in equation (20) with a value different scalings of the bolt friction torque are marginal. The 3 between 0.25 and 1.75. The value for Ka as function of q91 simulation with 2 τˆb shows that the increased Stribeck friction and τ is shown in figure 8. The FIS is elaborately discussed causes the FIS to return a higher additional gain to overcome 1 in appendix C. this friction. The simulation with 2 τˆb shows that overall the additional gain is lower. For all three simulations, at t the end of the bolt is reached V. SIMULATION EXPERIMENTS ∗ and the elongation and additional friction enforce that the UAV A. Simulation description can no longer follow the motion profile accurately. This causes To validate the kinematics of the model derived in sec- the error signal to rise, consequently increasing u until it is tion IV-A, a static simulation was done with q1 fixed. The saturated. When u is saturated for some nonzero time, the inputs of the model were set to the values in table II. Figure 9 controller switches to ROTATING BACK, moves back to q1 = 0 shows a 3D render of the UAV in the optimal configuration. and stoppes the simulation by switching to state FINISHED. 11

FASTENING ROTATING BACK 1 3 ft 2 τˆb τˆb 2 τˆb Ψb 0 (mm)

blt 0.05

Ψm x Ψ0 −

f 0.1 gb − Bolt pos.

0.4 (a) (rad) 1 q 0.2 Angle 0

Ψ0 Ψb 0.1 Ψm (rad)

e 0 Error 0.1 −

(b) 0.4 (rad) des

, 0.2 1

ft q Ψb Ref. 0

Ψm 1.5 Ψ0 a K fgb 1 Gain 0.5

(c) 200 Fig. 9. A 3D rendering of the UAV with the optimal manipulator, during the static simulation test. (a) shows an overview, (b) shows a top view from zˆb, and (c) a front view from xˆ0. (N m) 0 x − τ 100

VI.DISCUSSION Torque 0 Without constraints on φ and θ, the optimal state would 1 be when the thrust ft and gravity fgb pointed in the same direction, thus with the UAV placed upside down. In this study u it is assumed that this state would be an unacceptable risk for 0 the system and therefore φ and θ have been limited.

The second element of the optimal wrench in equation (12), Plant input 0 0 i.e. the applied τ , is nearly one fourth of the desired τ . 1 y x − Due to the maneuverability constraints, the length of the 0 2 4 t∗ 6 8 manipulator is limited. Additionally, the bounding box of the Time t (s) UAV places a minimum on px. The combination of these two constraints leads to the significant τ 0 that can only be canceled Fig. 10. Simulation results of the dynamic simulations. Three tests have been y conducted, with 3 τˆ , τˆ , and 1 τˆ . The jumps in the reference signal are b b 2 b b 2 b by the UAV’s τy or τx. These torques again are limited due due to the switching between states. The applied torque shows a similar spike 3 to the coupling of the thrust and the torques as described in for all three simulations, although the one for 2 τˆb is slightly higher than the 0 0 other two. appendix A. Consequently, to decrease τy , τx will have to 12 decrease too. A proper balance between these torques can be The torque curve in figure 10 shows three minor peaks: one found by adjusting the weighting function Q. For the use case at the start and two more at the two first state transitions. 0 considered in this study, the value of τy is acceptable. This additional torque is probably required to overcome the 0 Interestingly, the optimal transformation matrix Hb stays Stribeck friction component. The higher Stribeck friction for 3 the same for any value above 0.1 for Q[1]. Even when changing the simulation where τb = 2 τˆb, seems to cause a stick-slip 0 Q[2] to 0, thus omitting the penalty on τy , results in the same behavior after the second state transition. This in turn causes 0 Hb . It seems that the transformation matrix is optimized to a Ka to increase quickly, but considering the curves of the plant global minimum of the cost function, within the set constraints. input u and the error e, it seems to overreact. The optimal roll φ and pitch θ angles are zero, which positions At the point in time when the bolt hits the end of its the thrust direction perpendicular to the effective arm of the thread, t = t∗ in figure 10, a significant spike is detected f 0 manipulator. Consequently this ensures that fy = 0 and that in the applied torque τx which exceeds the torque of the 0 all thrust force is contributing to τx . optimal wrench excessively. This could indicate that even b b When setting Q[2] to 0, τx and τy will no longer be used higher application torques could be achieved by building up 0 and the maximum thrust is used to generate τx , because the UAV’s momentum and transferring it in a short time f τy is no longer considered. With the optimization results in interval to the application point. In a real-world test this spike section III-E, the target torque of 60.75 N m is not reached, will probably be tempered by the internal damping of the bolt, but the obtained torque is near to being considered extreme, which is not considered in this simulation. following the definition in section II-A. The additional gain Ka was expected to change smoothly As shown in figure 13b, the endpoint of the input covector is with varying damping in state FASTENING due to its con- placed on the surface of the potential space. More specifically, tinuous shape shown in figure 8, but in simulation it changes it is placed on the top face, which indicates that the input erratic. This might be due to the inputs of the FIS. One input is 0 covector is fully saturated. When changing Q, the endpoint the measured torque τx , which changes almost instantaneously will move over this face but will remain coincident. Only with adjusting Ka. The other input is the rate of change 0 0 when Q[1] is smaller than 0.1, Rb will be identity and pb q91, which needs time to change due to the UAV’s inertial T will be 0.45 0 0 . The thrust is then used to cancel the properties. This could cause the following development of Ka − b gravitational force and only the intrinsically generated τx is during simulation:
0 transferred to the application point. The endpoint of the input 1) τx is normal and q91 is still zero initially 0 covector is still located on the surface of the potential space 2) This causes Ka to increase, in turn increasing τx instan- in figure 13b, but on the middle cylinder section. taneously 0 Considering the maximum thrust of the specific UAV and 3) q91 does not change directly, but the measured τx is larger the length of the optimal manipulator perpendicular to the xˆ0 than during the former time step 0 0 axis, the theoretically maximum torque τx would be 4) This causes Ka to increase more, increasing τx more too 2 2 (f¯t fgb) p + p 66.7 N m − y z ≈ 5) Repeating 2-4 causes Ka to quickly reach its limit q 0 The same reasoning can be followed for when q1 is too small and the unwanted maximum torque τy would be 9 and Ka is quickly decreased. (f¯ f ) p2 + p2 25.4 N m. t − gb x z ≈ 0 0 VII.CONCLUSION The achieved torque τx ofp 44.7 N m and τy of 10.7 N m reach 0 approximately 67% of the theoretical maximum τx and only In this paper the initial steps have been taken towards 0 41% of the undesired theoretical maximum τy . the application of extreme interaction torques with underac- Considering the controller, the FIS to determine Kc adds tuated UAVs. The kinematics have been derived of a generic complexity to the control system, but also contributes greatly transformation of the UAV. Constraints for the optimization to the performance. In comparison with a conventional PD algorithm have been determined based on input saturation, controller, tuned for both state FASTENING and ROTATING maneuverability, and safety margins. A optimization algorithm 0 BACK, the FIS FSM shows less overshoot, a better tracking of has been designed that takes the desired torque τx , the UAV the motion profile, and a better response on stick-slip behavior. parameters, and a weighting function as input and derives 0 On the other hand, the structure of the controller is still the optimal Hb matrix and input covector to achieve that P(I)D tuned for a relatively slow motion profile. An alternative torque taking into account the weighting of the desired and might be to tune a P(I)D controller such that it is critically the undesired forces and torques. damped when applied to the considered system. Supplying a To validate the algorithm, an optimal rigid manipulator has step reference input to the controlled system would prevent been derived for a specific use case scenario of fastening a bolt. overshoot and offer high immediate gain due to a large The manipulator has been validated by means of a dynamic immediate error. The slow initial increase of the currently UAV model, derived using screw theory for rigid body dynam- applied motion profile leads to a longer time period where the ics. Subsequently the dynamic model has been used to apply Stribeck friction is active. Due to this slow start the controller the manipulator in the realistic use-case scenario. To that end, has to catch up when the Stribeck friction is overcome, still a Finite State Machine (FSM) controller was designed with a causing some overshoot. PD controller for one state and a PID controller for the other 13 state, both in addition to a feed-forward controller canceling the gravitational influence. The PID controller is adaptive by 1 6 means of an additional gain, which is determined by means of a Fuzzy Inference System (FIS) based on current angular velocity and torque. x The controlled system has proven to be able to fasten a variety of bolts with different friction parameters. The FIS has y proven to successfully counteract the complex nonlinearity of 2 5 the bolts friction model, but its performance can probably be improved with other semantic rules that prevent the quickly la increasing or decreasing resulting Ka. The optimal manipulator and input covector of the UAV balance the desired and undesired wrench elements in Ψ0 and 3 4 0 0 achieve a τx of 67% of its theoretical maximum and a τy of only 41% of its theoretical maximum. For the specific UAV, Hexagon formed by the rotors the achieved τ 0 is more than 7 times larger than the torque x Possible space for τx τy ft , view A that it can generate intrinsically. Approximated space for τx τy
ft A. Future work Fig. 11. UAV motor layout for a generic hexacopter.
This study was focused on achieving a static maximum torque. Future work can use the optimization of the manipu- τy are also zero. The line segment to point (1.5,2) can be found lator as presented and possibly further increase the achievable by maximizing the torque with the minimum amount of thrust. torque by utilizing the dynamic properties of the UAV. For The segment from point (1.5,2) to point (1.5,4) follows from example, the inertia and mass of the UAV can be used as the fact that the torque is kept the same if the forces on both a momentum storage, increasing momentum by building up sides of the axis are increased equally. The maximum thrust in (angular) velocity. This momentum can be transferred to the (0,6) can be found by increasing all forces to their maximum. application point in a short time interval, to achieve a resulting The total thrust is then 6f¯rot, but the forces in equations (23) torque that can be added to the static torque achieved in this and (24) cancel out so there is no torque. The limits can be study. mirrored in zˆ, completing the symmetric shape wherein T and Another possible addition to this area of study could be τx can be controlled. the application of a powerful external actuator on the end For τy this shape is similar, but stretched out in horizontal effector of the manipulator. Using the powerful actuator and direction. This is due to the fact that the average arm length the ‘slowness’ of the UAV’s inertia, a short powerful torque at maximum τx is only 3/2la which is slightly smaller than could be applied to the application point. the average arm length at maximum τ of √3l 1.73l . y a ≈ a A virtual axis running through rotor 1 and 4, or through APPENDIX A rotor 3 and 6, has the same thrust-torque space as the yˆ COUPLINGBETWEENTHRUSTANDTORQUE axis. Similarly, the axes perpendicular to these mentioned axes Underactuated UAVs use the thrust from multiple parallel have the same thrust-torque space as the xˆ axis. By linearly rotors to directly control their altitude and attitude. The altitude interpolating between these six planes, the three-dimensional can be directly controlled by increasing or decreasing all thrust-torque space can be defined as shown in figure 13a. thrusts. To control attitude, torques about the body fixed xˆb, yˆb, The space in figure 13a can be approximated by a rota- and zˆb can be generated, which follow from the rotor thrusts tionally symmetric space, which is obtained by rotating the and the UAV motor layout. shape in figure 12a about the τx = 0 line. This gives the For a hexacopter as shown in figure 11, these four forces largest rotationally symmetric space that fits in the space of are expressed in the rotor thrusts as follows: figure 13a.

T = f1 + f2 + f3 + f4 + f5 + f6 (22) APPENDIX B 1 τ = l (f f ) + l (f + f f f ) (23) BONDGRAPHMODEL x a 2 − 5 2 a 1 3 − 4 − 6 √3 Modeling the dynamic equations of section IV can be done τ = l (f + f f f ) (24) graphically by means of (multi) bond graphs in conjuncture y 2 a 3 4 − 1 − 6 with screw theory. For an introduction in bond graph theory, τ = k (f f + f f + f f ) (25) z d 1 − 2 3 − 4 5 − 6 readers are referred to [20]. The models in this section were Three (symmetric) limits can be recognized when express- derived using bond graph terminology and the simulations R ing the thrust T as a function of τx and τy, as shown in from section V have been done using 20SIM [21] in combi- R figure 12. The point (0,0) in figures 12a and 12b can be found nation with the controller in MATLAB [15]. by noticing from equation (22) that at zero thrust, all fi must In the figures provided in this section, the port s1 denotes be zero, thus from equations (23) and (24) follows that τx and a signal (s) input (circle) numbered 1 and p2 denotes a power 14

ft( f¯rot) ft( f¯rot) 6 · I 6 · IV II V III VI

4 4 A A A A

2 2

τx( laf¯rot) τy( laf¯rot) · · 3 3 3 3 √3 1 √3 1 √3 √3 − 2 − 4 4 2 − − 2 2 (a) (b)

f3 + f4 z z f4 + f6 f5 y y x x la √3 2 la (c) (d)

f4 + f6 f3 + f4 f2 + f5 z z f1 + f3 f5 y y x x (e) (f)

f1 + f3 f4 + f6 f3 + f4 f2 + f5 f1 + f6 z z f2 f5 y y x x (g) (h)

Fig. 12. Graphical representation of the dependency of T on τx and τy. The graph in (a) shows the plotted relation. (c), (e) and (g) show (in orange) the thrust changes that follow the line segments of I, II, and III in (a) for positive τx respectively, and (in violet) the corresponding torque change. For negative τx, the thrusts are identical, but mirrored in zˆ. Similarly, (d), (f) and (h) show the thrust changes that follow the line segments of IV, V, and VI in (b) for positive τy respectively, and the corresponding torque change. Note that the maximum τy is slightly bigger than τx due to the longer average arm length.

6 6

4 4 ) ) rot rot ¯ ¯ f f · · ( ( t t f 2 f 2

2 2 0 0 2 0 2 0 − 1 0 − 1 0 − 1 2 2 − 1 2 2 − − τy ( laf¯rot) τy ( laf¯rot) τx ( laf¯rot) τx ( laf¯rot) · · · · (a) (b)
Fig. 13. (a) Possible space for τx τy ft , parameterized in maximum thrust per rotor f¯rot and length of the UAV’s arms la. A greater torque magnitude can be achieved, but this also result in a net yaw torque. (b) Thrust space approximated as rotationally symmetric space. The mark indicates the endpoint of the covector after the optimization in section III-E. 15

(p) output (square) numbered 2. These ports have no dynamic behavior and only transmit either a signal or a (multi) flow- effort pair. s The high level model in figure 14 consists of a UAV- R 2 block, which holds a rigid body bond graph structure with a Rr : modulated source of effort for the input force and torques [22]. MR The Joint-block contains a bond graph generic joint, but without degrees of freedom, effectively fixing the manipulator in the Body-block to the UAV with a high stiffness and high p4 1 0 p5 damping. Ratchet The Bolt-block is shown in figure 15 and the Ratchet- Fig. 16. Bond graph of the -model. block is shown in figure 16. The former consists of a switching X1-junction that switches closed when the end of the bolt’s thread is reached. The latter is used as sensory input for the controller and holds a dissipative element that is switched on p1 0 p2 when q91 > 0 and switched off otherwise.

s 0 0 b 3 MTF : Hj = Hb Hj UAV p1 p1 Joint s3

s1 p2 RC 1 Fig. 17. Bond graph of the Joint-model.

s1 p2

s3 FSM Manipulator T b T adT b,0 b − b P s2 p3 : I : MGY
I

s gb 2 W : 1 s1 : gb Se RTF: MSe Hb τ b τ b f Ratchet p4 gb x y t b
p5 MTF : H0

1 p2 p5 RC Fig. 18. Bond graph of the UAV-model. Bolt

Fig. 14. The high level block diagram of the dynamic model. T m T ad m,0 ( ) m − Tm P : I : MGY I Re(q1) Rb(q91) : : MR MR gm Wgm : 1 Se RTF: m Hgm 1 : 1 1 p5 : Ke C X1 TF: s m x < 0 λ 3 MTF : H0

: I

p2 p3 Jbolt 1 Manipulator Fig. 15. Bond graph of the Bolt-model. Fig. 19. Bond graph of the -model. 16

Measured torque τ (N m) of the Gaussian function is located. For SM this is at x = 0, 0 5 10 15 20 25 30 35 40 45 for LA this is at x =x ¯.

µ SM LA 1 A. Example For a given pair (q91, τ), the corresponding value for Ka can be determined as follows. Firstly, the degree of membership 0.5 µ for all membership functions is determined as follows: τ 2 τ 2 µτ,SM(τ) = exp −τ¯ 2 = exp − ,

Degree of membership 2( ) 253.125 0  4    (τ τ¯)2 (τ 45)2 0 0.1 0.2 0.3 0.4 0.5 µ LA(τ) = exp − − = exp − − , τ, 2( τ¯ )2 253.125 Angular velocity q9 (rad s−1)  4    q2 q2 Fig. 20. Membership functions of the input of the Fuzzy Inference System 91 91 µq91,SM(q91) = exp −¯ = exp − , (FIS). q91 2 0.03125 2( 4 ) !   ¯ 2 2 (q91 q91) (q91 0.5) µq91,LA(q91) = exp − ¯− = exp − − . APPENDIX C q91 2 0.03125 2( 4 ) !   FUZZY INFERENCE SYSTEM Then the fuzzy logic value of the antecedents in the FIS rule- A Fuzzy Inference System (FIS) is used to adjust the set are determined. The fuzzy logic equivalent of the logical proportional gain of the PID controller in state FASTENING, AND is to take the minimum of the two degrees of membership due to the unknown friction of the bolt when tightening. The as follows: friction cannot be measured directly, therefore the gain is a1 = min(µq91,LA, µτ,SM), a2 = min(µq91,LA, µτ,LA), adjusted based on the rate of change of q1 and the measured torque on the application point. Four straightforward semantic a3 = min(µq91,SM, µτ,SM), a4 = min(µq91,SM, µτ,LA). K rules are used to determine a, which is used to scale the Then the resulting value of Ka can be determined with gain K of the PID controller. These rules are of the structure c 4 IF (antecedent) THEN (consequent) : aici i=1 1) If q91 = LA and τ = SM then Ka = 0.25 K = , a P4 2) If q = LA and τ = LA then K = 0.75 91 a ai 3) If q91 = SM and τ = SM then Ka = 1.25 i=1 SM LA P 4) If q91 = and τ = then Ka = 1.75 where ci are the consequents as given by rule 1 to 4. In order of the list above, if the current velocity is LA, although the measured torque is SM, the friction of the bolt is most APPENDIX D probably small. In that case the control signal can and should DEFINITION OF ATAN2 be lowered to prevent that the UAV will overshoot its setpoint. In this study, the following definition for the atan2 function If both the measured torque and the current velocity are LA, is used: the gain can be slightly lowered, because the velocity should atan y for x > 0, be limited. If both signals are SM, the gain is slightly increased x atan y + π for x < 0 and y 0, to help overcome the Stribeck friction of the bolt. If on the  x
≥  y other hand the current velocity is SM and the measured torque atan x π for x < 0 and y < 0, atan2(y, x) = 
− (27) is LA, the friction is probably high. In that case, the control  π for x = 0 and y > 0,  2
signal can be increased within the limits of what the UAV is  π 2 for x = 0 and y < 0, able to apply. − 0 for x = 0 and y = 0 The membership functions that correspond to the values   SM (small) and LA (large) for the input signals are shown  in figure 20. Note that the scale for the measured torque is REFERENCES shown on the top x-axis. The surface plot in figure 8 shows [1] D. R. McArthur, A. B. Chowdhury, and D. J. Cappelleri, “Design of the evaluation of the FIS for the possible combinations of τ the I-Boomcopter UAV for environmental interaction,” in 2017 IEEE International Conference on Robotics and Automation (ICRA), May and q91. 2017, pp. 5209–5214. DOI: 10.1109/ICRA.2017.7989611. The Gaussian membership functions are described by [2] G. Heredia, A. E. Jimenez-Cano, I. Sanchez, D. Llorente, V. Vega, J. Braga, J. A. Acosta, and A. Ollero, “Control of a multirotor outdoor (x c)2 µ(x) = exp − − , (26) aerial manipulator,” in 2014 IEEE/RSJ International Conference on 2σ2 Intelligent Robots and Systems, Sep. 2014, pp. 3417–3422. DOI: 10.   1109/IROS.2014.6943038. where σ is chosen τ/¯ 4 = 11.25 for the torque membership [3] A. Mirjan, F. Gramazio, M. Kohler, F. Augugliaro, and R. D’Andrea, ¯ “Architectural fabrication of tensile structures with flying machines,” functions and q91/4 = 0.125 for the rate of change membership Green Design, Materials and Manufacturing Processes, pp. 513–518, functions. The constant c is the value of x where the center 2013. 17

[4] F. Augugliaro, A. Mirjan, F. Gramazio, M. Kohler, and R. D’Andrea, “Building tensile structures with flying machines,” in 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, Nov. 2013, pp. 3487–3492. DOI: 10.1109/IROS.2013.6696853. [5] A. Q. L. Keemink, M. Fumagalli, S. Stramigioli, and R. Carloni, “Mechanical design of a manipulation system for unmanned aerial vehicles,” in 2012 IEEE International Conference on Robotics and Automation, May 2012, pp. 3147–3152. DOI: 10.1109/ICRA.2012. 6224749. [6] J. L. J. Scholten, M. Fumagalli, S. Stramigioli, and R. Carloni, “Interaction control of an UAV endowed with a manipulator,” in 2013 IEEE International Conference on Robotics and Automation, May 2013, pp. 4910–4915. DOI: 10.1109/ICRA.2013.6631278. [7] A. Y. Mersha, S. Stramigioli, and R. Carloni, “Exploiting the dynamics of a robotic manipulator for control of UAVs,” in 2014 IEEE Inter- national Conference on Robotics and Automation (ICRA), May 2014, pp. 1741–1746. DOI: 10.1109/ICRA.2014.6907086. [8] R. Voyles and G. Jiang, “Hexrotor UAV platform enabling dextrous interaction with structures — preliminary work,” in 2012 IEEE Inter- national Symposium on Safety, Security, and Rescue Robotics (SSRR), Nov. 2012, pp. 1–7. DOI: 10.1109/SSRR.2012.6523891. [9] G. Jiang and R. Voyles, “Hexrotor UAV platform enabling dextrous interaction with structures — flight test,” in 2013 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR), Oct. 2013, pp. 1–6. DOI: 10.1109/SSRR.2013.6719377. [10] R. A. M. Rashad Hashem, P. Kuipers, J. Engelen, and S. Stramigioli, “Design, modeling, and geometric control on SE(3) of a fully-actuated hexarotor for aerial interaction,” arXiv.org, Sep. 2017. [11] J. T. Bartelds, H. W. Wopereis, S. Stramigioli, and M. Fumagalli, “A comparison of control approaches for aerial manipulators handling physical impacts,” in 2016 24th Mediterranean Conference on Control and Automation (MED), Jun. 2016, pp. 646–652. DOI: 10.1109/MED. 2016.7535915. [12] J. T. Bartelds, A. Capra, S. Hamaza, S. Stramigioli, and M. Fumagalli, “Compliant aerial manipulators: Toward a new generation of aerial robotic workers,” IEEE Robotics and Automation Letters, vol. 1, no. 1, pp. 477–483, Jan. 2016, ISSN: 2377-3766. DOI: 10.1109/LRA.2016. 2519948. [13] H. W. Wopereis, T. D. van der Molen, T. H. Post, S. Stramigioli, and M. Fumagalli, “Mechanism for perching on smooth surfaces using aerial impacts,” in 2016 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR), Oct. 2016, pp. 154–159. DOI: 10.1109/ SSRR.2016.7784292. [14] S. Stramigioli and H. Bruyninckx, “Geometry and screw theory for robotics,” in Proceedings IEEE International Conference on Robotics and Automation, vol. 2001, Citeseer, 2001, p. 75. [15] MathWorks, MATLAB, version 9.4.0.857798 (R2018a) Update 2. Nat- ick, Massachusetts, 2018. [Online]. Available: http://www.mathworks. com/. [16] R. H. Byrd, J. C. Gilbert, and J. Nocedal, “A trust region method based on interior point techniques for nonlinear programming,” Mathematical Programming, vol. 89, no. 1, pp. 149–185, 2000. [17] T. Specker, M. Buchholz, and K. Dietmayer, “A new approach of dynamic friction modelling for simulation and observation,” IFAC Proceedings Volumes, vol. 47, no. 3, pp. 4523–4528, 2014, 19th IFAC World Congress, ISSN: 1474-6670. DOI: 10.3182/20140824- 6- ZA- 1003 . 01711. [Online]. Available: http : / / www. sciencedirect . com / science/article/pii/S1474667016423116. [18] J. van Dijk and R. Aarts, “Analytical one parameter method for PID motion controller settings,” in Proceedings of the IFAC Conference on Advances in PID Control, ser. WeC2.4, University of Brescia, Mar. 2012, pp. 1–6. [19] T. S. Tadele, T. de Vries, and S. Stramigioli, “PID motion control tuning rules in a damping injection framework,” in 2013 American Control Conference, Jun. 2013, pp. 4957–4962. DOI: 10.1109/ACC. 2013.6580607. [20] J. F. Broenink, “Introduction to physical systems modelling with bond graphs,” University of Twente, Tech. Rep., 1999. [Online]. Available: https : / / www. ram . ewi . utwente . nl / bnk / papers / BondGraphsV2 . pdf (visited on 03/09/2018). [21] Controllab Products, 20-sim, version 4.6.2 build:7308. 2016. [Online]. Available: http://www.20sim.com/. [22] J. Engelen and G. Folkertsma, “Control for UAVs,” course manual, Jun. 2018.