University of Twente

EEMCS / Electrical Engineering Control Engineering

Design, Realization and Analysis of a Manipulation system for UAVs

Arvid Keemink

MSc Report

Committee:

Prof. dr. ir. S. Stramigioli Dr. R. Carloni

Dr. M. Fumagalli MSc. A. Y. Mersha

January 2012

Report nr. 004CE2012

Control Engineering

EE-Math-CS

University of Twente

P.O. Box 217

7500 AE Enschede

The Netherlands

Contents

List of Figures 4

Abstract 5

1 Introduction 7

2 Requirements 92.1 General requirements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.2 Non-destructive testing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.3 Visual inspection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102.4 Object placing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102.5 Surface cleaning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3 Design of the Manipulator 113.1 Manipulator concepts . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3.1.1 Serial manipulators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113.1.2 Parallel manipulators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3.2 Manipulator specification . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113.3 End-effector specification . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

4 Realization of the Manipulator 154.1 Materials . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 154.2 Components . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

4.2.1 Manipulator Actuators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 164.2.2 Thighs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 164.2.3 Shins . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 164.2.4 NDT end-effector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

4.3 Electronics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184.3.1 Motor drivers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184.3.2 Strain gages . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 194.3.3 Low level hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 194.3.4 Communication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 194.3.5 High level hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

4.4 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 194.4.1 Low level software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 194.4.2 High level software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

5 Model of the System 235.1 Derivation of the simple model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 235.2 UAV dynamics as fictitious input force . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 255.3 DC motor model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 265.4 UAV model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

1

5.4.1 Application to a quadrotor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 275.4.2 Application to a ducted fan UAV . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

5.5 Virtual system multi-body model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 295.5.1 Rigid Body Dynamics Through Screw Theory . . . . . . . . . . . . . . . . . . . . . . . 295.5.2 Describing Dynamics in Screw Theory . . . . . . . . . . . . . . . . . . . . . . . . . . . 305.5.3 Bond Graph Representation of Rigid Body Dynamics . . . . . . . . . . . . . . . . . . 305.5.4 Kinematic Structure of the Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 315.5.5 Modeling Environmental Interaction . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

6 Manipulator and UAV Control 356.1 Position control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 356.2 Joint space impedance control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 366.3 Workspace impedance control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 366.4 UAV control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

6.4.1 Small Angle Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 386.4.2 Large Angle Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

6.5 Trajectory generation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 396.5.1 UAV offset reference . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 396.5.2 Reference limitation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

7 Simulation Results 437.1 Simple model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 437.2 Manipulator attached to a Ducted Fan UAV . . . . . . . . . . . . . . . . . . . . . . . . . . . . 447.3 Manipulator attached to a Quadrotor AUV . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

8 Experimental Results 478.1 Manipulator stand-alone Performance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 478.2 Manipulator attached to Ducted Fan UAV . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48

8.2.1 Free-flight with position control of the manipulator . . . . . . . . . . . . . . . . . . . . 488.2.2 Interaction During Flight . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

8.3 Manipulator attached to Quadrotor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 518.3.1 Free-flight With Position Control of the Manipulator . . . . . . . . . . . . . . . . . . . 518.3.2 Combined UAV and manipulator inertial frame trajectory tracking . . . . . . . . . . . 528.3.3 Interaction During Flight . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

9 Conclusions and Future Work 55

Bibliography 58

A Extra Control Strategies 59A.1 Virtual UAV Jacobian pseudo-inverse reference generation . . . . . . . . . . . . . . . . . . . . 59A.2 Decentralized combined control of manipulator and UAV . . . . . . . . . . . . . . . . . . . . . 60

B Relevant Code 61B.1 Limit to workspace matlab code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61B.2 Inverse kinematics matlab code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63B.3 Forward kinematics matlab code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65B.4 Gravity compensation matlab code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66

C Technical Drawings 69

2

List of Figures

2.1 Schematic view of boiler approach maneuver . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3.1 CAD of manipulator with nomenclature . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123.2 Delta manipulator local workspace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133.3 Kinematic structure of AIV system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

4.1 Photo of the prototype attached to a quadrotor and stand-alone . . . . . . . . . . . . . . . . 154.2 Finite Element Analysis of the Thigh . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 164.3 Photo of the end-effector in close up . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174.4 Exploded end-effector view . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174.5 FEM Analysis of end-effector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184.6 Control Network . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 204.7 Simulink Control Scheme . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 214.8 Simulink Workspace and Jointspace Control Scheme Submodel . . . . . . . . . . . . . . . . . 22

5.1 UAV and manipulator, indicating relevant frames and sizes . . . . . . . . . . . . . . . . . . . 245.2 Quadrotor UAV Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 275.3 Duted Fan UAV Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 285.4 A single rigid body model in multi-bond bondgraphs . . . . . . . . . . . . . . . . . . . . . . . 315.5 An interconnection of two rigid bodies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 315.6 Example of two simulated interconnected rigid bodies . . . . . . . . . . . . . . . . . . . . . . 325.7 Bondgraph of the UAV a Manipulator system . . . . . . . . . . . . . . . . . . . . . . . . . . . 335.8 Simulation frames for interaction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

6.1 PI position control scheme . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 356.2 Joint space impedance control scheme . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 366.3 Workspace impedance control scheme . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 376.4 UAV Reference Trajectory from Manipulator Reference Trajectory . . . . . . . . . . . . . . . 406.5 Projection of references onto the manipulator workspace . . . . . . . . . . . . . . . . . . . . . 41

7.1 Simulation Results of the Simple Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 437.2 Wall approach and touching of the Ducted Fan UAV in the multibody model . . . . . . . . . 447.3 Interaction force during touching in the Ducted Fan UAV model . . . . . . . . . . . . . . . . 447.4 Ducted Fan UAV x-reference and measured position . . . . . . . . . . . . . . . . . . . . . . . 457.5 Wall approach and touching of the quadrotor in the multibody model . . . . . . . . . . . . . 457.6 Interaction force during touching in the quadrotor model . . . . . . . . . . . . . . . . . . . . . 457.7 Quadrotor UAV x-reference and measured position . . . . . . . . . . . . . . . . . . . . . . . . 46

8.1 Position and Joint Space Impedance control results . . . . . . . . . . . . . . . . . . . . . . . . 488.2 Joint space position control tracking results with added mass of 500 g . . . . . . . . . . . . . 488.3 Manipulator on Ducted Fan and an Optitrack Camera Used for Tracking . . . . . . . . . . . 498.4 Results and statistics of the end-effector position tracking during hand held movement. . . . . 498.5 Results of the end-effector position tracking during free flight including statistics . . . . . . . 50

3

8.6 The estimated interaction force when the Ducted Fan UAV is in contact, with photo . . . . . 508.7 Pelican with onboard manipulator and vision trackables. . . . . . . . . . . . . . . . . . . . . . 518.8 Two inertial frame tracking experiments, including position statistics. . . . . . . . . . . . . . 518.9 Local manipulator end-effector position during tracking. . . . . . . . . . . . . . . . . . . . . . 528.10 Quadrotor End-Effector Inertial Frame Reference Tracking . . . . . . . . . . . . . . . . . . . . 528.11 Quadrotor UAV in contact through manipulator . . . . . . . . . . . . . . . . . . . . . . . . . 538.12 Top view of flight area, relevant positions shown, including local manipulator position. . . . . 538.13 The estimated interaction force when the Quadrotor UAV is in contact and UAV angles . . . 54

A.1 Total control scheme of UAV and manipulator combined . . . . . . . . . . . . . . . . . . . . . 60

4

Abstract

The main goal of the AIRobots collaboration is to come up with prototypes of a remotely controlled aerialvehicle which is capable of performing NDT tasks in a predefined mock-up environment. The chosen roboticairframes used in the project will be equipped with a robotic arm: an appropriate end-effector and elec-tromechanical arm-like manipulator.

This report presents the mechanical design, modeling and realization of a manipulation system placedon Unmanned Aerial Vehicles.

The innovation of the prototype lies in the use of a Cartesian Delta robotic manipulator, on-board a flyingvehicle, to physically interact with environments and perform ultrasonic non-destructive testing experimentsand other versatile tasks at unreachable locations for humans.

The novel NDT end-effector is composed of a spring loaded sensor to compliantly interact with environ-ments. A double spring loaded Cardan gimbal system provides two passive degrees of freedom in rotationwith defined equilibria to overcome gravity on the sensor and to define a stable zero reference. Actuation ofthe axis of the sensor provides actuation of the remaining DOF.

Simulation in rigid body bondgraphs and experimental in-flight interaction measurement results showthat the total system can properly exert a force in the range of 2 Newton with an ultrasonic testing sensor,which is sufficient for proper NDT measurements.

Inertial frame reference tracking experiments of the manipulator end-effector show the necessity for havinga manipulated tool, due to the coupled UAV dynamics a fixed rigid tool would not always suffice for taskswhere position accuracy is preferred.

5

6

Chapter 1

Introduction

Industrial inspections and rescue operations require considerable effort from humans. Large structures andinaccessible locations demand laborious support systems and possible risks of structural collapse create ahazardous working environment. Implementing security measures takes time and reduces inspection andrescue efficiency. These factors generated the need for remote controlled and (semi-)autonomous roboticaiding systems that can support the human in these tasks.

Robots with climbing possibilities have successfully been deployed in industrial structural inspection invisual or non-destructive testing. These autonomous systems are frequently based on magnetic or aerostatic(vacuum or low dynamic pressure) attractive forces with subsequent rolling or guiding on the surface to beinspected. Novel approaches on biologically-inspired and electro-adhesion might prove useful in overcomingchallenges set by navigating over unstructured irregular surfaces [1].

The use of Unmanned Aerial Vehicles (UAVs) in rescuing scenarios after natural disasters and generalsurveillance have proved to be successful. The advantage of large tether-free workspaces, small vehicle sizesand the possibility to carry different payloads make the UAV a suitable candidate for investigation andexploration. Recent research in UAV cooperation in object lifting, group navigation and fully autonomousnavigation opens up even more opportunities with useful applications [2].

An innovative field of application and research lies in the deployment of UAVs for industrial structuralinspections, such as remote non destructive testing of boilers and chimneys in power generation plants [3].As opposed to common UAVs, inspection UAVs are required to physically interact with their surroundingenvironment. This poses new challenges in UAV research for the stable interaction control and opens up a lotof research possibilities for integrated on-board electromechanical devices to interact with and manipulatethis environment. Interaction with rigid environments is not new, as Albers et al. [1] mention a design of aflying robot with an on-board structure to apply force to the environment for several tasks such as cleaning.Pounds et al. [4] provide the concept of a under-actuated gripper system to grab, transport and releaseobjects by helicopter and assess further stability issues that arise with such environmental interaction.

This report is a continuation of previous work [5] within the AIRobots project [3] and focuses as well onthe novel idea of physical interaction of UAVs that combines all aforementioned fields of research (inspec-tion, touching, grabbing etc.): the design and testing of a robotic sensor and tool manipulation system forstructural inspection UAVs.

The novelty lies in a manipulation system that can be placed on-board different kinds of UAV platformswith modular task-specific end-effectors that are thereby supplied with full Cartesian movement relative tothe UAV. The area of application of the on-board manipulator goes beyond structural inspection and can,e.g., also be incorporated in rescue and exploration UAVs.

The report will cover the design of the manipulator system following from the requirements and systemsimulations. Usefulness of the manipulator will is verified through experimental data. Recapitulation ofUAV nomenclature and general UAV information can be found in [6], and is hence not discussed here.

Due to the fact that the main focus was on realizing the manipulator, some ideas presented on control inthis report are not yet implemented on the physical systems due mainly time constraints. These ideas areto some extent used in simulation and could be quite useful in future research and implementations and willbe recaptured in the conclusions.

7

The report is organized as follows. In Chapter 2, the manipulator system requirements are presented.In Chapter 3, the design options, choices and mechanical dimensioning are discussed. Chapter 4, presents aspecification of the current realization of the prototype. In Chapter 5, mathematical models are discussedwhich are used to assess the dynamic behavior of the manipulation system on a UAV. Chapter 6 discussesused and future control strategies and ideas. The simulation results of the resulting controlled models arediscussed in Chapter 7. In Chapter 8 experimental results of in flight and interaction experiments arediscussed. Finally, conclusions and future research possibilities are discussed in Chapter 9.

8

Chapter 2

Requirements

Considering the UAV and the manipulation system together, there are several tasks to be performed. Withinthe research project AIRobots [3], the robot should inspect a section of an industrial boiler wall by means ofthe manipulator. Non-destructive testing (NDT) with an ultrasonic testing (UT) sensor and cameras, objectplacing and cleaning tasks should be able to be performed on the surface of the boiler tubes. From thesetasks, the requirements for the manipulator and the end-effector are derived. In this work the focus willmainly be on the NDT task, hence other manipulator tasks are not discussed further and are only consideredin the requirements during the design phase.

2.1 General requirements

The allowable weight of the manipulator and the end-effector is limited by the UAV payload specifica-tions [7][8]. The weight should be < 200 g using the weight of subsequent control electronics to balanceforces on the UAV. It should be placed on the side of the UAV since walls to be inspected are usuallyvertical.

2.2 Non-destructive testing

The system should be capable of performing UT-NDT on a section of a boiler wall. It should hence be ableto fly to a preferred location and dock either by using aerodynamic forces to push itself against a verticalsurface.

The manipulator system should move an UT-NDT sensor to a tube of the boiler wall and take measure-ments at points, lines or surfaces defined by the human operator. This requires the manipulator to movein Cartesian space. Since the UAV has, relatively, slow coupled dynamics, its positioning accuracy whilepushing the manipulator against the wall might be not sufficient without additional solutions. Therefore, therange of movement, i.e. the practical workspace Wp, is chosen to be Wp = 5 5 5 cm3 to compensate forthese positioning and subsequent movement errors of the UAV. When the manipulator is in contact with theboiler tube, it will apply a preferred contact force that ensures proper contact to do sensor measurements.This force should be in the same range of the maximum force the UAV can apply to the wall. Since the UAVlateral pushing force is in the range of 5 N [7], this is also the minimal force of what the manipulator shouldbe able to apply in most of its workspace. As an upper bound, a maximum impact force of 20 N should behandled without damage to the mechanics.

Small angle offsets in all rotary motions of the end-effectors due to surface roughness, tube curvature andUAV orientation error should be corrected by the end-effector itself as shown in Fig. 2.1. Peak impact forces,when the sensor hits a wall, need to be reduced by mechanically decoupling the sensor from the manipulatorand UAV.

9

Figure 2.1: Schematic top view (not to scale) of the UAV, manipulator (dotted line) and sensor (small box) mounted onthe UAV side. A.) Boiler approach maneuver, B.) Theoretical proper placement of the sensor with perfect UAV positioning,C.) Realistic faulty placement of UAV and sensor with some error, the end-effector should hence follow or adapt itself to thecurvature or surface irregularities of the tube.

2.3 Visual inspection

Apart from possible on board vision, a dedicated camera could be placed on the manipulator end-effectorfor close up visual inspection. This requires the end-effector to be modular, i.e. the NDT sensor must bereplaced with relative ease by a camera or something else.

2.4 Object placing

The possibility of placing switchable magnetic pulley systems to hoist wheeled robots up the boiler tube.This requires the end-effector to be modular, with a possible gripper or (electro-)magnet. It should then beable to carry objects of yet unknown weight. Taking only the UAV payload specifications [7] into account,objects in the order of a few hundred grams should be properly carried without the manipulator carrierstrength being the bottleneck.

2.5 Surface cleaning

Before doing NDT measurements, the boiler wall surfaces might need to be cleaned with a metal brush andmotor in the end effector. This also requires the end-effector to be modular. It needs to be able to apply anunknown force preferably transmit all reaction torque to the UAV body, since UAV attitude control is ableto apply significant torque.

10

Chapter 3

Design of the Manipulator

This chapter will cover the design of the manipulator. Different manipulator concepts will briefly be discussed.The chosen design of manipulator will be specified in terms of kinematics and features. The realization ofthis design is the topic of Chapter 4.

3.1 Manipulator concepts

Two main kinematic structures have been considered in the design process: serial manipulators and parallelmanipulators.

3.1.1 Serial manipulators

The serial kinematic chain consists of different links with usually their own motor actuating in one or twodegrees of freedom (DOFs). Carrying subsequent links requires a bigger motor on the base. If necessary,tendons can be used to place all motors on the base (or somewhere else on the UAV) instead of on the links,but a disadvantage of this approach could be high complexity and possible loss of positioning accuracy.

3.1.2 Parallel manipulators

Parallel manipulators usually are ideal for high speed applications where also rigidity and strength arepreferred. If tendons are not considered, another advantage is the placement of motors on the base insteadof in the links. Being more rigid than a serial manipulator of the same scale, due to the mechanical constraints,makes it an appropriate candidate to transmit forces to the UAV.

For the application discussed in Chapter 2, a parallel manipulator is more suitable than a serial one. Amanipulator with Delta [9] kinematics was chosen. The Delta is ideal since it provides the preferred Cartesianend-effector motion through three rotary motor motions and as preferred it can supply a significant amountof force to the end-effector, compared to a serial manipulator. The actuators are all on the base closer tothe UAV center of gravity than would be the case in a tendonless serial manipulator. This induces a smallertorque disturbance on the UAV and hence requires less counterbalancing. Since there is no need for themotors to carry the weight of the next one, their strength and, hence the total weight can stay low, makingthe mechanical bandwidth high.

3.2 Manipulator specification

The Delta is made up of five distinctive elements, shown in Fig. 3.1. If the angles of the legs are limited bymechanical design or control, the manipulator can be used outside of any singular configuration with a uniquekinematic mapping of actuator angles to end-effector position. The forward and inverse Delta kinematicscan be solved analytically with a projective geometry approach [10] explained in much detail in [5] and given

11

Figure 3.1: CAD of the stand alone manipulator prototype - (1) indicates the actuators, (2) the hips, (3) the thighs, (4) theknees, (5) the shin, (6) the ankle and (7) the end-effector.

in Appendix B.3 and B.2. The velocity and acceleration kinematics also have closed form solutions [11] thatcan be used in (model based) control and design, as used in Chapter 5.

Kinematic lengths for the thighs and shins are respectively 5 and 7 cm. The kinematic base plate diameteris 6 cm, with an end-effector diameter of 4.5 cm. This provides the manipulator with a workspace startingapproximately 3 cm from the base plate, extending 9 cm.

The theoretical local total workspaceW meets the requirements in Chapter 2 on the preferred workspaceWp, where Wp W for actuator angles in the range [90,+10]. The resultant workspace is shown inFig. 3.2. Moving to more positive angles is not possible due to constraints set by the UAV design.

Since the combination of UAV and manipulator is redundant in DOF, its workspace is theoreticallyunbounded but is in practice not completely dexterous due to angle constraints on the UAV.

The used kinematic structure of the UAV, manipulator and end-effector together can be seen in Fig. 3.3.

3.3 End-effector specification

An extra actuator is placed on the end-effector to supply roll movement for the cylindrical sensor. Two1-DOF rotational joints near the front of the sensor form a double Cardan joint (double gimbal) insideend-effector (see Fig. 3.3). Weak spring loading is used to define an equilibrium position and to counteractgravity, while extra spring loading ensures mechanical compliance during contact with the environment. Thissystem passively makes the sensor adjust to the surface or to orientation errors as illustrated in Fig. 2.1.

12

0.06 0.04 0.02 0 0.02 0.04 0.060.13

0.12

0.11

0.1

0.09

0.08

0.07

0.06

0.05

0.04

0.03

y [m]

z [m

]

0.1

0.2

0.3

0.4

0.5

0.6

0.06 0.04 0.02 0 0.02 0.04 0.060.13

0.12

0.11

0.1

0.09

0.08

0.07

0.06

0.05

0.04

0.03

x [m]

z [m

]

0.06 0.04 0.02 0 0.02 0.04 0.06

0.06

0.04

0.02

0

0.02

0.04

0.06

x [m]

y [m

]

0.050

0.050.05

0

0.050.12

0.1

0.08

0.06

0.04

y [m]

x [m]

z [m

]

Figure 3.2: Delta manipulator local workspace. In the top left is the total workspace W enveloping the preferred workspacecube Wp. The other three images are bottom, and two side views of the workspace. Gray scale values indicate the sum ofabsolute motor torques to withstand 5 N interaction force from the bottom of the workspace, which is a measure for the requiredmotor current.

Figure 3.3: The kinematic structure of the UAV, manipulator and end-effector combined, going from fixed world to sensor.Rotary joints are indicated with R, translational joint with T.

13

14

Chapter 4

Realization of the Manipulator

This chapter will cover the realization of the total system. The mechanical realization and further specifi-cation are presented. Furthermore, the used electronic hardware and software and software architecture arediscussed. Software communication protocols are discussed in project deliverables [3].

The design as described in Chapter 3 has been realized in a 190 grams prototype, which can be seen inFig. 4.1. Some crucial parts of the design had been validated a priori through the dynamic models describedin Chapter 5 and by a Finite Element Method (FEM) Analysis in ANSYS 12.0. The manipulator was notweight optimized, since the improvements would have been marginal considering the fact that the controlelectronics and wiring were not optimized.

The used kinematic structure for the delta is as described in Fig. 3.3. The technical drawings used toproduce the prototype are provided in Appendix C.

Figure 4.1: Left) Photo of the prototype attached to a quadrotor. Right) Photo of prototype stand-alone.

4.1 Materials

The prototype is composed of an aluminum alloy for all custom made parts. Pultruded carbon fiber tubesmake up the nine axial parts of the thighs and shins. Bronze is used in the end-effector in combination witha conical steel set screws to create a low friction free Cardan joint. Double sealed steel small ball bearingsare used in all other joints to provide rotational degrees of freedom.

15

4.2 Components

4.2.1 Manipulator Actuators

A triple of Maxon RE10 motors with GP10A (four stage 256:1) reduction gears and MEnc10 12 ticks encoderdrive the legs of the manipulator. These can deliver over 1.5 mNm of continuous torque at the motor sideand over 200 mNm at the output shaft, which is necessary following results from the model (Chapter 5) andmanipulator kinematic analysis [5]. Also other motors with lower gear rations have been used to improveimpedance controlled behavior of the robot.

4.2.2 Thighs

The thighs only take, dependent on the configuration, bending and axial loads. They are made up of a carbonfiber tube and two aluminum components. To measure motor torque, the bottom component has a milled-out section to facilitate a measurable strain of 0.0008 with strain gages in the highest load circumstance,see Fig. 4.2. This has been determined with the same data as shown in the table in Fig. 4.5. The higheststress and, hence, strain builds up on the outside of the material as expected, which makes the use of straingages possible.

Figure 4.2: FEM Analysis of the Thigh part with maximal load. The colors (online version) indicate the equivalent von Misesstress. The maximum stress is at the bottom left in the opening of the tapped hole which connects to the drive shaft. Maximumstrain in the bending part is 0.0008.

4.2.3 Shins

The shins form a parallelogram that constrains the movement of the end-effector in a way that it cannotrotate relative to the manipulator base. They only take axial loads because they are decoupled from rotationby the bearings.

4.2.4 NDT end-effector

The end-effector plate is the ring moved in space by the three legs of the manipulator. The ring has beendesigned such that any type of functional end-effector can be placed inside this ring. The NDT end-effectorconfiguration is shown as an exploded view in Fig. 4.4 with numbers referring to mentioned parts in thenext paragraph and as a photo in Fig. 4.3. Its function is to hold a sensor with four degrees of freedom.One degree of freedom is active rotation, two are passive elastic rotations and the final degree of freedom ispassive sliding with spring loading. To compensate for surface curvature, the passive elastic double gimbalsystem ensures adaption of sensor orientation to the surface normal.

16

Figure 4.3: Close up photo of the end-effector showing the gimbal system and sensor and connection to the legs.

NDT End-Effector Composition

The NDT end-effector is composed of six main parts with several sub-parts, labeled in Fig. 4.4:

Figure 4.4: Exploded view of the end-effector assembly indicating important parts.

1. The sensor: a custom made dual crystal ultrasonic testing NDT sensor (a), with coaxial plugs (b)which have stress relief and cables running from them to the signal processing electronics.

2. The outer ring: the stationary gimbal ring connected to the three parallelograms of the legs throughdouble sealed steel ball bearings (d). Small bronze cups are placed inside holes to form a Cardan joint(c). This outer ring is generic and contents can be changed depending on the task.

3. The inner ring: a quadruple of conical set screws, of which two are indicated by (e), form the doublegimbal system together with parts (c) and (f). This ring is necessary to supply an extra degree offreedom in rotation.

4. The probe sleeve: small bronze cups are placed inside holes to form a Cardan joint (f) as is also donein the outer ring. This part forms the third stage of the dual gimbal and has hence two rotationaldegrees of freedom. An Igus 2022-03 glide bearing (g) fits around the UT sensor (a) to aid in smoothsliding and rotational movement. Three elastic bands clamped between the bearing brackets (d) andthe bracket on the back of the sleeve (h) define a proper equilibrium for the sensor sleeve and make

17

it possible to generate a torque around the gimbal system counteracting gravity and defining a sensorequilibrium.

5. Probe springs: a set of 4 parallel springs with 3 mm diameter and 0.1 N/mm spring constant (k)partially decouples sensor impacts from the rest of the manipulator. Two plates (j) make sure theprobe can rest on the springs, and actuation through the connector sleeve (m) is still possible.

6. Probe actuation: a Faulhaber motor, model 0615C4,5S with 06/1K (three stage 64:1 reduction) andPA2-50 encoder (l) drives the sensor by using a connector sleeve (m) that is connected to a combinationof custom 12/25 ratio spur gears (n), of which one is attached to the motor. By rotating the motor,the spur gears rotate the sleeve that subsequently rotates the probe through its connectors and cables.

Since it is crucial that this part can take the prescribed load of a maximum impact force of 20 N, aFEM analysis of this part has been performed, see Fig. 4.5. This shows the part is strong enough, even overdimensioned, to withstand maximally prescribed loads.

Aluminum Part (20 N)Max [MPa] 280 5.05Max [m/m] 3.9 103 7.1 105

Figure 4.5: FEM Analysis of the end-effector under maximal load. The colors indicate the equivalent von Mises stress. Thetable on the right specifies the yield stress and strain for Aluminum and the maximum stress felt in the part with during amaximal load of 20 N.

4.3 Electronics

Next to the motors, the electronics used to operate the manipulator are divided into five other groups thatwill be treated separately: Motor drivers, Strain gages, Low level hardware, Communication, High levelhardware.

More in depth information on the software running on the low and high level hardware can be found inSec. 4.4.

4.3.1 Motor drivers

To supply power to the motors, four Pololu 755 High-Power Motor Drivers are used at 6 Volts, suppliedby an RC Hobbywing UBEC which acts as a voltage regulator. Each of the discrete MOSFET H-bridgemotor drivers enables bidirectional control of one brushed DC motor. The boards provide two fault flagpins that can be read by low level hardware to check proper operation (i.e. short circuit, over-heating orunder-voltage). A (logical NOT) reset pin provides the option to switch the drivers off, i.e. cutting the powerflow to the motor in case of emergencies. These motor driver boards are over dimensioned in the currentprototype, although this was not an issue. Much weight reduction can be gained by combining four motordrivers on a small PCB.

18

4.3.2 Strain gages

Although the strain gages to take joint-torque or 6D force-torque measurements are not yet implemented,they will be discussed here. An 8 channel multiplexer Maxim DG408DY with op-amp INA118 amplifies thedata of maximally 8 Wheatstone half-bridges. The multiplexer can be set to pass one of the 8 signals to the14 bits ADC converter ADS7279 by setting three inputs to either ON or OFF. This can be regulated by themicro controller ATmega328P. The micro controller collects the data of the ADC outputs and communicatesthrough either a serial interface or CAN bus to the user and/or motor control boards.

4.3.3 Low level hardware

An Arduino Mega 2560 with Atmel ATmega2560 processor is used for low level hardware, running custommade firmware. It takes care of sending PWM and direction signals to the motor drivers, supplying powerto encoders and reading encoder increment interrupts and external digital hardware.It receives input signalsover a serial communication line, of which the protocol has been defined during AIRobots IW1. It is chosento use a separate ADC for measuring strain gage voltages since it increases resolution and reduces noisecompared to the Arduino ADC.

4.3.4 Communication

Communication between high and low level is done through serial messages over USB to the UAV on boardcomputer and Wireless LAN to the ground station. The communications channel has been been tested andcan operate using the embedded Arduino serial to USB driver and with an X-Bee wireless device.

4.3.5 High level hardware

The high level hardware comprises any combination of computers with Linux and the UAV on-board Atomprocessor running Ubuntu 10.10. The ForceDimenson Omega.6 haptic device is used to control the UAVwith haptic feedback. A three-axis joystick is used to control the manipulator. A PhoeniX Technologies Inc.VisualEyez tracker system tracks active trackables placed on the UAV.

4.4 Software

Software concerning the manipulator runs on two levels, the low and high level. These are discussed sepa-rately. When assuming that a human operator or a supervisor is to be in high level control, the discussedhigh level in the subsequent paragraphs can be seen as being middle level. The total system control networkis shown in Fig. 4.6.

4.4.1 Low level software

The software running on the Arduino Mega 2560 (on the right in Fig. 4.6) is custom made, with severalfunctions obtained from the open source community. It can read analog and digital ports and set PWMand digital values through connected pins. Fast reading through the open source digitalReadFast anddigitalWriteFast functions1 can be used to read pins a lot faster than the similar functions from the standardArduino library.

When starting the low level software, it will calibrate by moving the joints until they hit a full stop.After that it will go and run the main control loop. At the start of the main loop loop, the processor checksfor serial messages sent by the high level control and applies these. Data in the received message headerspecifies a certain state of control, or operational mode, the Arduino should switch to. The five main modesare:

1. Joint space position control: PI control of motor angle position. The high level sends position setpoints as reference. The low level calculates the control signals.

1http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1267553811

19

Ground Stat ion

TelemanipulationCom

Pose Est imat ion

x = (x, y, z, , , )

Human Operator

VisualEyez3DTracker System

On Board

Com

Atom Board

Flight Ctrl

C(s) P(s)ur ey

ym

Pos Ctrl At t Ct rl

Asctec Pelican UAV

Manip-ulator

Manip CtrlHL Ctrl Inputs

Manip/ Cam DataRefFFMarker Data

UAV Pose

Ctrl Inputs

UAV Data

Data Exchange

LED Markers

Manipulator Control

JoystickHaptic Interface

Figure 4.6: Control network indicating all devices and interconnections.

2. Joint space position control with minimum jerk trajectory: PI control on motor angle position withinterpolated minimum jerk trajectory between given set points. This can be used to reduce the fre-quency of communication between high and low level hardware since it only requires the final desiredjoint configuration. The low level calculates the trajectory and control signals.

3. Joint space velocity control: PI control of motor angular velocity. The low level integrates the velocityto position and calculates the position control signals. This ensures proper velocity control at very lowspeeds and smoother manipulator movement.

4. Joint space impedance control: PD control of motor angle position with nonlinear gravity compensa-tion. The high level software processes the kinematics functions to estimate the gravity torque on themotors and sends a PWM value to compensate this. This PWM value is added to the internal PDcontroller.

5. PWM control: if complicated control (such as workspace based control) is done on the high levelhardware, the Arduino can be used to only set the communicated reference voltages from the high level.Hence in this mode no low level control is performed, and the Arduino is only used for communicationand measurement.

In any mode, the Arduino sends its values of measured motor angles and other relevant data to the highlevel back over the serial communications channel. At the end of the loop, the Arduino waits until the totalloop has taken at least 3 ms with an accuracy of 4 s.

Telemetry is transmitted every 12ms, data is received dependent on the ground level sample time, softwarepriority (since the OS is not rea-time) and Wireless LAN queue.

The received references and PWM values can be filtered by a 3 Hz first order low-pass filter to preventstaircasing behavior of the manipulator.

4.4.2 High level software

Interfaces running on the UAV transmit data within a standard, ROS [12] and YARP [13] network. The maincontrol code around the UAV position controller, together with high level manipulator control is runningas C++ compiled software in a Matlab Simulink 2011b as shown in Fig. 4.7 and Fig. 4.6. The workspacecontroller ( 4.8) loop described in Chapter 6 runs on the high level software. The kinematics are implemented

20

Joint Space Position Control

Joint Space Impedance Control

Work Space Impedance Control

Euler Angles

x,y,z

makeomega

local EE posV2PWM

UDP Receive

Tilde1

Tilde

Terminator

T

Roll,Pitch,Yaw

Reference Generation

Pos/IMPOutput

On/Off

Msg Handler JSPC

Msg Handler JSIC1

Msg Handler JSIC

Limit to Workspace Inverse Kinematics

Inverse Dynamics and Workspace Impedance

Inertial frame UAV position

IMP_Mode

H

Gravity Comp

Go to Calibration

Forward Kinematics

Fictitious Force on End Effector

Euler Angles

EM Switch

Calib_Switch

Base_Movement

Figure 4.7: Simulink Control Scheme. The left top part handles incoming messages and calculates references. The top rightgenerates a proper message for the Arduino to handle, according to what the operator wants. The bottom part calculates theUAV pose and the forces on the manipulator which could be used in the controller shown in Fig. 4.8. The gravity compensationis done in code in a block in the middle.

as shown in Sec. B.3 and Sec. B.2. Gravity compensation is calculated as described in Sec. B.4. A customwritten UDP-Serial interface converts messages on the UAV platform to those for the low level controller ofthe manipulator. Since the timing of the high level is unreliable, closing control loops on the high level leadsto unstable behavior. Therefor closing loops on the low level is highly preferred.

Any reference generator can be used to generate set points for the robot: analytical/numerical functions,joy pad or joystick input, mouse or keyboard input or haptic devices. The setup is such that the totalmanipulator controller can be incorporated in outer haptic tele-operation loops if preferred.

21

Workspace PD Force to motor torque

JS PD

gain6

gain5

gain4

gain3

gain2

gain1gain V2PWM1

Transpose1

To Workspace4

Saturation

SVF2

SVF1

PD WS

PD JS

Matrix Multiply3

Matrix Multiply2

Matrix Multiply1Mass_Matrix

Manual Switch

Load_to_Motor_Conv

Joint_FrictionDemuxSel1

Coriolis_Centrif_Matrix

Add1

Add

theta

Figure 4.8: Simulink Workspace and Jointspace Control Scheme Submodel. The gravity compensation is done in code asshown in the large control scheme in Fig. 4.7.

22

Chapter 5

Model of the System

This chapter will cover dynamic modeling of the manipulator, the UAV and both together as a system.Modeling the manipulator and UAV gives insight into the dynamic behavior after preliminary dimension-

ing. It is used for dimensioning the actuators, to assess system behavior during operation and to quickly testseveral control ideas that are laborious to implement in a real setup. Several models are discussed: a threestate equation joint-pace model for the manipulator which will be used for computed torque control. UAVmodels will be discussed in Sec. 5.4 and a model of the total system will be assessed in Sec. 5.5. Results ofnumerical simulations will be presented in Chapter 7.

5.1 Derivation of the simple model

This model is called a simple model, since it is a three state equation model of the manipulator, which isused in inverse dynamics control. The dynamics due to the degrees of freedom inside the end-effector areneglected in this model, since their influence is small. The UAV movements are modeled as external forceson the end effector, but manipulator influence on the UAV cannot be modeled in this way. This is overcomein the model described in Sec. 5.5.

In the following sections the subscript th will indicate a thigh property, subscript sh will describe a shinproperty, subscript ee will indicate an end-effector property and subscript par, h will describe the horizontalpart of the shin parallelogram. Subscript suffix t states total mass or total inertia. Variable I specifiesrotational inertia and m linear mass. Relevant locations or lengths can be found in Fig. 5.1.

The manipulator is described in its local frame in the negative z-direction with the actuator anglesbeing negative in that region (see Fig. 5.1.b). It is described locally from the base plate origin frame (b),hence UAV movement is described by external fictitious forces working on the end-effector and thighs.Since the manipulator will be placed horizontally on the side of the UAV (which has frame u in itsorigin, see Fig. 5.1.a), the local frame must be transformed to the inertial frame. The total homogeneoustransformation, assuming the manipulator attached in its local origin, is given by the UAV pose and thestationary transformation from the manipulator origin to the UAV, i.e.,

H0b(t) = H0u(t)H

ub (5.1)

Where the super- or subscript 0 indicates the inertial frame. Hence the time dependent rotation matrixis part of that pose, i.e., R0b(t) = R(H0b(t)). Time dependencies will be omitted in the remainder of thederivation.

Using the approach in [11], the shin inertia is added to the thighs and end-effector to reduce modelcomplexity. The inertia felt by the motors at the output shaft is an addition of the thighs own inertiarotating around its end-point (i.e. implying the parallel axis theorem) including the horizontal part of theparallelogram at the thighs end and 2/3 contribution of the parallelogram beams of the shin, i.e.,

Ith =[Ith,com +mthr2com] + [Ipar,h +mpar,ha

2]+

2/3(2msh)a2

23

Figure 5.1: a) UAV with manipulator, indicating relevant frames. Usually UAV reference frames are with the z-axis pointingdownward, this is merely a coordinate transformation. b) Stand alone manipulator indicating thigh length a and shin lengthb. The location of the thigh center of mass, rcom and effective center of mass rG,th are indicated, relative to the hip. Numbersnext to the knees indicate the number of the leg. Frame b floats a little above the real base plate at the height the planeformed by the axes of rotation of the actuators.

The moment arm of gravity on the thigh is hence:

rG,th = arcoma mth +mpar,h + 2/3(2msh)

mth +mpar,h + 2/3(2msh)

The total thigh mass is its own mass together with the horizontal parallelogram part and a contribution ofthe two shin beams:

mth,t = mth +mpar,h + 2/3(2msh)

The unit vector of a thigh in axial direction is denoted as ebi (i), the inertial frame gravity unit vector

as e0G = [0, 0,1]T

and the hip rotation axis is given by ebj,i = Rz,i(i) [1, 0, 0]T . The relative jointspacecontribution of the gravity force on the thigh is hence given by ( denoting the cross product):

Gth() =

ebj,1, eb1(1) Rb0e0Gebj,2, eb2(2) Rb0e0Gebj,3, eb3(3) Rb0e0G

with locally described unit vectors of the thigh given by:

ebi (i) = Rz,i(i)

0cos(i)sin(i)

, i = 0, 2/3, 4/3The total mass of the end-effector is the contribution of the end-effector itself and three times two beams

of the parallelogram and the horizontal parallelogram shin part:

mee,t = mee + 3

(1

32msh +mpar,h

)The vector describing the end-effector gravity contribution in the inertial frame is hence given by F0G =

[0, 0,mee,tg]T with g the gravitational acceleration.Taking the inertial force contributions to the system torque gives, respectively, for the end-effector and

the thighs (Jp being the 3 3 geometric Jacobian):

ee,I = JTp

(mee,tp

bee

) th,I = IthI3

The external, non-inertial, contributions to system torque arise from gravity (F0G, Gth()), external(F0ext) and fictitious forces (F

bf,ee, expressed in eq. 5.3). Their torque contributions are a follows:

ee,e = JTp R

b0(F

0G + F

0ext) + J

Tp F

bf,ee

th,e = gmth,trG,thGth()

24

Where Fbf,ee is the fictitious force on the end-effector due to UAV movement as described in Sec. 5.2.The balance of torques require all inertial torques to equal the external torques [11], including viscous

friction torques at load side fr = kd and motor load shaft torque l:

ee,I + th,I = ee,e + th,e + fr + l

Expanding the acceleration of the end-effector into joint space and subsequent collecting gives the motorload side torque:

=(IthI3 +mee,tJ

Tp Jp

) +

(mee,tJ

Tp Jp + kd

)

mth,tgrG,thGth() JTp Rb0(F0G + F

0ext

)where the torque and angles are specified on the, post-gear, load (subscript l) side. Changing this to themotor (subscript m) pre-gear side, the following relations hold:

l = m/kr l = krm

where is the gearbox efficiency and kr the gear ratio. When adding the motor and gear inertia (subscriptm, g), but keeping load angles (omitting the subscript l) and using motor pre-gear side torque m, one gets:

m =

(krIm,gI3 +

1

kr

(IthI3 +mee,tJ

Tp Jp

))+(

mee,tkr

JTp Jp + k

)

(mth,tgrG,th

krGth()+

1

krJTp R

b0F

0G

) 1kr

(JTp R

b0F

0ext

)(5.2)

This yields a lower felt inertia due to the gear ratio. It can be seen that eq. 5.2 is a standard robot dynamicsequation of the form:

m = M() + C(, ) + k + g() + f()

where M() states the system mass matrix, C(, ) the Coriolis and centrifugal terms, k the linear viscousfriction parameters, g() and f() the gravity and external contribution including interaction and fictitiousforces.

5.2 UAV dynamics as fictitious input force

In the following section rigid body twists Tb,ca , or their elements, are described by rotational and translationalvelocity (throug the frame origin b) of body a with respect to c expressed in b. More on screw theory canbe found in [14]. A tilde accent denotes the 3 3 skew-symmetric form of a 3 1 vector or the tilde formof the twist.

Assume the UAV as one rigid body, with twist:

T0,0u =

[0,0u0,0u

], T0,0u =

[0,0u

0,0u

0T3 0

]Time derivatives of the base pose can be derived, with the hat ( ) denoting a vector augmented with a 1

element for homogeneous transformations:

R0b = 0,0u R

0uR

ub

p0b =(

T0,0u + (T0,0u )

2)

H0uHub 03 = H

0b 03

From which one can get the second derivative of the rotation matrix and the linear acceleration of the baseexpressed in the inertial frame needed for the calculation of fictitious force on the end-effector. If the i-th

25

row of the manipulator Jacobian is denoted as Jp,i and the standard three unit vectors of an orthonormalframe by ui then the fictitious force (subscript f) felt by the end-effector described in the manipulator frameis felt as:

Fbf,ee = mee,t[p0b +

3i=1

((2Jp,iR

0b + p

bee,iR

0b

)ui

)](5.3)

The fictitious force felt at the thighs due to their inertia is neglected.

5.3 DC motor model

A proper model of the used DC motors in the manipulator are required. This model is used in the abovesimple model and the elaborate multi-body model discussed in 5.5 to compute voltage dependent drivetorque.

One assumes a linear DC motor described by the following dynamics:

u(t) = Ldi

dt(t) +Rti(t) +

1

kt(t) (5.4)

kti(t) = Jd

dt(t) + kd(t) (5.5)

Stating the electrical terminal voltage u(t), motor current i(t), electrical inertia (inductance L), terminalresistance (Rt) and motor constant kt relating the armature current and produced torque and rotationalspeed and back-EMF. Motor inertia is given by J and viscous damping by kd. Adding the rotation angle(t) to the couple of equations, the total state space model can be described as a standard linear system:

x = Ax + Bu (5.6)

y = Cx (5.7)

Which written out gives:

d

dt

i

= RtL 1kt 0kt

J kdJ 00 1 0

i

+ 1L0

0

u (5.8)y =

[0 0 1

]x (5.9)

The transfer function of the system from voltage to rotation angle is given by:

HDC(s) =(s)

V (s)=

kts((Js+ kd)(Ls+Rt) + k2t )

(5.10)

With motor specifications kt = 0.00428 [Nm/A], J = 0.12 107 [kgm2], Rt = 10.8 [], kd = 2.35 108 [Nms/rad].

5.4 UAV model

In this section a generic UAV model will be discussed. More detail is given about two types of miniatureUAVs: a Quadrotor UAV and a Ducted Fan UAV. This is done since the prototype of the manipulator hasbeen tested on both platforms.

To model a generic UAV, the North-East-Down (NED) orientation convention is considered for a singlerigid body frame. It is assumed that a single rigid body with control force mappings to actuators is sufficientto describe both UAVs, this approach is a little different from [7][6] and more in line with Quadrotorwork [15][16][17][18]. The equations describing the dynamics of the UAV are given by the Newton-Euler

26

equations for a rigid body, with two extra state (vector)equations for the orientation and position of theUAV:

x0 = v0

mv0 = mgz0 + fR0u [0, 0,1]TR0u = R

0u

u,0u

Ju,0u = u,0u Ju,0u + Mgy + Mu(5.11)

Where m is the linear UAV mass, J is the rotational inertia, g is the gravitational acceleration, R0u theorientation of the UAV body in the inertial frame, u,0u the rotational velocity of the UAV with respect tothe inertial frame, but expressed in the body-frame, and x0 and v0 the position of the UAV center of massdescribed in the inertial frame. The control forces are given by the total thrust f and the control torque Mu.The variable Mgy denotes any gyroscopic effects due to rotating propellers. Any external force or momentacting on the UAV can be put in the respective Newton or Euler equation stated above. Fig. 5.2 and Fig. 5.3give clarify conventions described above.

5.4.1 Application to a quadrotor

The model given in eq. 5.11 can be applied to a Quadrotor UAV. A Quadrotor UAV has two pairs of (usually)

f1u1u2

u3

f4

f3f2u

x00

y0

z0

x

^

^

^

Figure 5.2: Left) The AscTec Pelican Quadrotor [8] vehicle. Right) Quadrotor model indicating the inertial frame 0 andthe UAV frame u and the thrust vectors.

counter-rotating propellers. This architecture leads to four eccentric forces and a reaction moment at eachpropeller. By varying relative thrust a net torque can be applied in any direction. The constant linearmapping from the forces generated by the propellers (fi, i = 1, 2, 3, 4), see Fig. 5.2, to the total thrust f and

body moment Mu = [Mx,My,Mz]T

:fMxMyMz

=

1 1 1 10 d 0 dd 0 d 0c c c c

f1f2f3f4

u = A

Where d is the distance from the UAV center of mass to the propeller and c is the constant ratiobetween propeller reaction torque and the generated thrust. With non-zero value for these parameters,the determinant of the 4 4 matrix above is non-zero and the system is invertible, i.e. one can design acontrol law for the thrust and control torques and calculate the propeller force. From the propeller force thepropeller rotational speed i can be calculated through the relation fi = kl

2i , where kl is the lift constant

27

of the propeller [6][7][18]. Both pairs of the Quadrotor propellers counter rotate and with the assumptionthat they have low inertia, their gyroscopic influence (when propeller speeds are unequal) on the UAV isnegligible. Hence Mgy = 0.

5.4.2 Application to a ducted fan UAV

The model given in eq. 5.11 can be applied to a Ducted Fan UAV. The ducted fan UAV is an aerial vehicle

T

d

u1u2

u3fx

u

x00

y0

z0

x

fy^

^

^

Figure 5.3: Left) Ducted fan and the build up of the vane structure [7] a) frame/housing b) propeller c1) anti-torque/yawvanes, c2) roll and pitch vanes. Right) Ducted Fan UAV model indicating the inertial frame 0 and the UAV frame u, thethrust vector and the lateral flap forces at its bottom.

with one main propeller in a duct which provides thrust. Body moments are generated by deflecting theairflow by sets of control vanes [7][6]. One set (with vane angle ) deflects the air in such a way that thereaction torque of the propeller is cancelled. The two other sets of vanes generate a force and hence torquein lateral directions (torque about body x by the vanes with angle , and torque about body y by vaneswith angle .

If the velocity of the air incident on the control vanes is given by Vi, following [6], the lift force producedby the -set, -set and moment on the -set of vanes are respectively given by:

Fy =12clV

2i (2Si) = c1V

2i [N]

Fx =12clV

2i (2Si) = c1V

2i [N]

N = 12clV2i (8Si)(

dT2 ) = c2V

2idT2 [Nm]

(5.12)

Where denotes the density of the surrounding air, which is 1.225 kgm3, cl the vane lift constant, ideallyconsidered to be 2 from flat plate theory, Si the vane effective incident surface area and dT the distancebetween the centers of pressure on the anti-torque vanes. The above relations hold under the assumptionthat the vane angles are rather small and the lift constant is constant in that region.

The velocity of the incident air on the vanes is related to the reaction force propeller thrust, whosemodulus is just the same as that of the normal thrust T , through:

Vi =

T

2S

Where S is the surface of the propeller disk. Although the vane forces Fy and Fx are small compared tothe thrust T and could be neglected in hovering flight [7], it is decided to keep them in this analysis. Theyalso induce a torque around the vehicle COM over a distance d, see Fig. 5.2 for more information. The thrustdependent mapping from the force generated by the propeller and the control vane angles ([T, , , ]

T) to

28

the total thrust f and body moment Mu = [Mx,My,Mz]T

:MxMyMzfxfyfz

=

0 dTk1 0 00 0 dTk1 0

0 0 0 dT2 Tk20 0 dTk1 00 dTk1 0 01 0 0 0

T

u = A(T )

Which is left-pseudo-invertable when T 6= 0. Constants k1 and k2 depend on values in eq. 5.12. If lateralforces are assumed to be small, the system becomes square, which makes invertability easier. The thrust ofthe main propeller is given by T = kl

2p, with kl the propeller lift constant and p the propellers rotational

speed in radians per second. Since there is only one propeller, whose size and inertia are significant [6], thegyroscopic torque induced on the vehicle is given by:

Mgy = u,0u Jpuupp

Where uup is the axis of rotation of the propeller in the UAV body frame. Since by design one knows thepropeller inertia, and by control the rotational velocity of propeller, the gyroscopic precession torque canalways be estimated.

5.5 Virtual system multi-body model

Since the manipulator model derived in Sec. 5.1 is a minimum state description of the delta manipulatorin a joint space model, it is non-trivial to combine it with a model of the UAV to get an indication of thetotal system dynamics. This section will describe a complete virtual prototype, multi-body model of a UAV,manipulator, NDT end effector and environmental interaction. It is modeled with the help of screw theoryfor rigid body dynamics of which more can be found in [14] and in the following section. Simulation resultsof the models are discussed in Chapter 7.

5.5.1 Rigid Body Dynamics Through Screw Theory

Assume a rigid body b in Cartesian space with configuration matrix (homogeneous transformation matrix)H0b SE(3) relative to the inertial frame. This body can have some velocity, rotation al translational inthat frame. The so called twist is a 6-dimensional vector describing in the top the rotational () and in thebottom somewhat the translational velocity () of a rigid body:

Ta,bc =[a,bc

Ta,bc

T]T

Which is a twist of body/frame c relative to body/frame b expressed in frame/body a. Take care that thetranslational velocity is that one of a point through the origin of the frame in which the twist is expressed,and not the linear velocity of the body in that frame.

The rigid body b with twist relative to the inertial frame (0) expressed in the inertial frame is denotedas: T0,0b . This twist can be expressed (seen from) in any other frame c through the following coordinatetransformation for twists:

Tc,0b = AdHc0T0,0b

AdHc0 =

[Rc0 033

pc0Rc0 R

c0

]A 6-dimensional force/moment vector is called a wrench. In the definition used here the moment comprises

the upper part of a wrench vector, and the force the bottom part:

Wji =[ ji

Tf jiT]T

29

A wrench acting on body b expressed in frame 0, W0b , can be transformed to frame c, Wcb, through:

Wcb = AdTHc0

W0b

Although wrenches are the duals of twists and should hence be presented as row-vectors, they are pre-sented in the following text as column vectors for ease of notation.

The time-derivative of the Adjoint transformation can be described as:

(AdHji

)= AdHji

adTi,ji

adTi,ji=

[i,ji 033i,ji

i,ji

]Which will be useful later on.

5.5.2 Describing Dynamics in Screw Theory

Choosing an appropriate frame where the inertia-tensor is diagonal (i.e. the principal- or body-frame) makesthe equations of dynamics straightforward. This makes it possible to write the Newton-Euler equations forrigid body motion in compact form (treating the generalized momentum of body i, Pi, as a column vector):

Pi = adTTi,jiPi + Wii

Iii Ti,ji = adTTi,ji IiiT

i,ji + W

ii

(5.13)

The wrench Wii can be any wrench acting on body i. In a multi-body model this wrench is composed ofactuation (by motors, propellers etc.) forces and torques and of constraint forces and torques, convenientlydescribed in their own frame and transformed to the body frame. The final step is to find a way to relatethe body twist and its configuration. Since the configuration matrix Hji (t) SE(3), the following holds:HjiH

ij ,H

ijH

ji se(3), which is the Lie algebra of SE(3) containing the tilde form of the twists. Hence,

dependent on preference, there are two differential equations for the body configuration related to the tildeform of the twist:

Hji = Hji T

i,ji

Hji = Tj,ji H

ji

Any of these can be numerically integrated to get the configuration at time t, assuming a known startingconfiguration at t0 = 0. Using the twist of a body i expressed in the inertial frame:

H0i (t) =

t0

T0,0i ()H0i ()d + H

0i (t0) (5.14)

5.5.3 Bond Graph Representation of Rigid Body Dynamics

The bond graph modeling technique is a nice and structured way to generate and represent equations ofphysical system that need to be simulated. It makes interconnection of different physical domains in modelsmore intuitive and focuses on drawing the physical concepts rather than the equations (as would be in ablock diagram). More on bond graph modeling and its notation can be found in [19] and specifically forsimulation of rigid body dynamics [20].

A multi-bond bond graph model can be constructed with flow variables being the body twists expressed inany chosen frame. The effort variables are given by the wrenches acting on bodies. The 1-junction representsa (common) twist or summation of wrenches in that frame, a 0-junction represents a (common) wrench, orsummation of twists in that frame. Any I-element represents the bodys inertia tensor. Any C-elementrepresents potential storage in 6D DOFs.

Assuming a rigid body looked at from its local principal frame, the required physical concepts to describethe body dynamics are the mechanical inertia (I) and fictitious forces, since the frame is non-inertial, given

30

Figure 5.4: A single rigid body model in multi-bond bondgraphs

by a modulated gyrator. Gravity acts on the body and needs to be transformed from the inertial to thebody principal frame. See Fig. 5.4 for the bond graph implementation of a generic rigid body. In conclusion,these elements effectively simulate eq. 5.13 in the body frame with external forces.

Interconnecting bodies can be done through the fact that relative twists can be constrained, i.e. a wrenchcan act somewhere in space to keep bodies together in preferred DOFs. A twist difference between two bodiesi and j is given by:

T0,ji = T0,0i T0,0j = T0,ij

Which is given by a 0-junction in bond graph notation. This twist difference can be transformed to the jointlocation in frame k to get that same twist difference expressed in k, given by T

k,ji . At this location any

DOF can be constrained by using very stiff and damped springs. Compliant interconnection is also possibleby using the necessary compliance as a constraint.

In this way any serial and parallel chain of rigid bodies can be generated as shown in Fig. 5.5.

Figure 5.5: An interconnection of two rigid bodies. Notice the twist difference is transformed to the joint location whereconstraint forces are applied in the preferred DOF.

5.5.4 Kinematic Structure of the Model

The kinematic structure of the model is as given in Fig. 3.3, including elastic bands and spring loading for theend-effector and DC motors to drive the manipulator arm. An example of two rigid bodies interconnected

31

is shown in Fig 5.6. All rigid bodies start with an initial configuration respective to the inertial frame. Allrigid bodies are interconnected by joints. The relative coordinate transformations of the joints to the rigidbody principal frames are time-constant. The homogeneous transformation matrices describing the rigidbody configurations are not and are calculated through eq. 5.14.

x00

y0

z0

a

H0a

b

H0b

Haj j

Figure 5.6: Example of two simulated interconnected rigid bodies. Body a and b have some initial configuration in the inertialframe and are interconnected, partially constrained, at point j.

Through bondgraph modeling, multiple rigid bodies are interconnected in serial and parallel chains.The summation of twists transmits body velocities and the reverse summation of wrenches transmits allinterrelated interaction forces between rigid bodies [20]. The final bond graph of the manipulator can beseen in Fig. 5.7.Left and the UAV in Fig. 5.7.Right.

5.5.5 Modeling Environmental Interaction

Rigid bodies modeled in free space should feel contact wrenches when coming into contact with the environ-ment or another body. A method to calculate interaction frames can be found in [21]. It is assumed only theface of the cylindrical sensor touches the vertical surface, see Fig. 5.8. The vertical surface (a wall) is locatedat point p0w in the inertial frame and the center of the contact face of the sensor is given by p

sc, relative to

the sensor principal frame. Taking the unit vector of the contact surface normal n0w = [1, 0, 0]T (can alsobe any other), two additional variables are needed:

= ys,nsw = zs,nsw

Which give the projection of the wall normal in the sensor face frame. The contact face is a disk, expressedaround the center point of the contact face. Hence the mapping can be normalized and negated:

pss =1

2 + 2 +

0rr

Where is a small number to avoid division by zero. The point pci will give the location of the point closestto the wall expressed in the face center frame of the sensor, which can be transformed back to the inertialframe if preferred.

A Hunt-Crossley interaction model is adopted [22] to calculate the contact force normal to the wall. Ifthe two bodies collide, this normal reaction force is dependent on the penetration-depth x given by:

Fn(x) =

{kxn(t) + xn(t)x(t) x < 00 x 0

Where and k are respectively damping and stiffness coefficients. Power n is assumed to be 1. Penetrationx is trivially dependent on the difference of the previously calculated pci and given p

0w, compared in the same

frame. Then when the interaction force is calculated, an interaction wrench W0i (x) at the interaction pointof the wall can be calculated:

W0i (x) =[0T3 Fn(x)n0w

]TWhich can be transformed to the sensor body frame.

32

Figure 5.7: Left) Bondgraph of the Manipulator, notice all interconnected rigid bodies. Right) Bondgraph of the UAV, noticefour Wrenches acting at different locations through the MSe elements.

33

xs

z sysx0

z0

y0

nw0

ps pw

Figure 5.8: The sensor disk with its frame and the wall with its normal vector

34

Chapter 6

Manipulator and UAV Control

This chapter will cover the control of the manipulator and UAV. Methods used to generate references for theUAV and manipulator in the setup and models will be discussed. A suggestion will be made about how toimplement combined control, which has not been implemented yet due to time constraints and the absenceof force sensors. For more information the reader is hence referred to Appendix A.

Different control modes of the manipulator are available. For moving to set-points a PI(D) controlleris used. For touching inspection tasks, a proper candidate is impedance control. In this case the jointmotors are controlled with PD action on the position error (joint space impedance), or PD on the workspaceposition error (workspace impedance). Necessary (UAV-)configuration dependent counter gravity torque canalways be supplied through the simple model described in Chapter 5 through nonlinear feedback. Extensivediscussions on joint space and workspace impedance controllers for robotic manipulators and their stabilityconsiderations can be found in [23] and the following three sections are derived from that source. Hence, itis not referenced further in the following sections.

6.1 Position control

One of the manipulator control strategies is stiff position control with disturbance rejection. A PID-controllaw on the joint angles is a proper candidate for this since by design there is always a unique mapping formthe range space of the end effector to the joint angles through an inverse kinematics function.

Since the gearbox ratio is high (kr = 256) the felt inertia from the robot is reduced by a factor k2r . Hence

the robot mass inertia is assumed to be negigable through the use of the gearboxes. This simplifies the controlanalysis to one analysis for a single motor as given in Sec. 5.3. The back-EMF and mechanical friction of thereal motors are high. Using their absolute damping gave better results than using back-EMF compensationwith derivative control action. Hence a PI control has been implemented in the control electronics as shownin Fig. 6.1, where the T&L block relates to the Transformation And Limitation explained in sections 6.5.1and 6.5.2.

Figure 6.1: PI control scheme. The inertial frame position reference is transformed to a local position reference. This issubsequently passed through the inverse kinematics (IK) function to get joint angle references. The joint angles in the plant Pare position controlled by the controller CPI.

The controller gains in the Arduino hardware are defined for counts instead of radians: ki = 32, kp = 512.

35

Converting to SI units gives controller gains:

kp =kpumaxccpr

(28 1)22cr

ki =kiumaxccpr

(28 1)22cr2cr,i

Which can be used in simulation or analysis. The parameters are given by cr = 7, cr,i = 6, umax = 6 [V]and ccpr = 12 cts/turn. The controller tuning was course, since a proper in depth control analysis of themotor was not the goal of the work. The motors also saturate much. An elaborate analysis for saturatedactuators as can be found in other sources like [24].

6.2 Joint space impedance control

A second control mode is the joint space impedance control with nonlinear gravity compensation feedback.The assumption is made that the gravity compensation done by modelling is proper. This is useful forinteraction, since the system will behave as a damped compliance. Any non-modeled, and hence non-compensated, forces are seen as disturbances that might introduce a steady state error. In essence the jointspace impedance controller is a less stiff variant of the position controller without integral action and addeddifferential gain, hence it needs gravity compensation to approach a zero steady-state error. The gravitycompensation is discussed in Sec. 5.1. The control scheme is a shown in Fig. 6.2, where the T&L blockrelates to the Transformation And Limitation explained in sections 6.5.1 and 6.5.2.

Figure 6.2: Joint space impedance control scheme. The inertial frame position reference is transformed to a local positionreference. This is subsequently passed through the inverse kinematics (IK) function to get joint angle references. The jointangles in the plant P are position controlled by the controller CPD. Nonlinear feedback to counter-act gravity uses the UAVorientation R0u and the measured joint angles, together with the measured Jacobian Jp obtained from a forward kinematics(FK) function.

The discrete Arduino controller values are kp = 128 and kd = 170, where in SI units the damping constantis:

kd =kdumaxccpr

(28 1)22crWhich can be used in simulation or analysis with the same parameters a given for the previously described

PI-control. The controller tuning was course, since a proper in depth control analysis of the motor was notthe goal of the work.

6.3 Workspace impedance control

Workspace impedance control is a more elaborate control strategy, compared to the joint space impedancecontrol. The impedance is felt in the workspace and additional feedforward computed torque and feedbackdynamics make this the control strategy with the most control over the manipulator. The control schemeis shown in Fig. 6.3, where the T&L block relates to the Transformation And Limitation explained insections 6.5.1 and 6.5.2.

36

Figure 6.3: Workspace impedance control scheme with nonlinear gravity and dynamics feedback and feedforward accelerationterms. The inertial frame position reference is transformed to a local position reference. This is subsequently passed through theinverse kinematics (IK) function to get joint angle references and its derivatives. The reference Jacobian Jp is used to calculateacceleration torque in the inverse dynamics (ID) block. The joint angles in the plant P are controlled by the workspace controllerCPD which generates a force mapped to the joint space by the Jacobian transpose J

Tp . Nonlinear feedback to counter-act gravity

(g()) and centrifugal and Coriolis forces C(, )) uses the UAV orientation R0u and the measured joint angles, together withthe measured Jacobian Jp obtained from a forward kinematics (FK) function.

Assume that the total robot including motor dynamics is modeled in joint space by:

M(q)q + C(q, q) + kdq + g(q) = m

A Lyapunov candidate function Rn 7 R and its time derivative are:

V = 12qTMq +

1

2eTp Kpep

V = 12qTMq +

1

2qTMq + eTp Kpep

When one defines:

ep = pd pmep = pm = Jpq

Filling in yields:

V = 12qTMq +

1

2qTMq qTJTp Kpep

Filling in the dynamical model into Mq:

V = qT (m g(q)) +1

2qT (M 2C)q qTkdq qTJTp Kpep

= qTkdq qT(m g(q) JTp Kpep

)Taking as control law for the motor torque:

m = g(q) + JTp Kpep + J

Tp Kdcpm

Makes the derivative of the Lyapunov function negative semi definite, dependent on the joint speed:

V = qTkdq qTJTp KdcJpq

Hence the system is stable under such a control law.

37

6.4 UAV control

UAVs are underactuated systems. Hence they need to be controlled in a nested way: in some way lateralposition references should generate attitude (angle) references. For completion, this section will briefly discusstwo methods of controlling the UAV, a small angle approach [7][18][16][6] and a large angle approach [17][15].The large angle approach has major advantages over the small angle control approach since the UAV can becontrolled to any state from almost any initial state [17]. This controller is implemented in simulation sinceit is a more robust control strategy. It is not implemented on the real vehicles. The drawback, in practice,of a large angle approach that propellers might also need to be able to be used in reversed rotation modeand that the necessary state estimations might be difficult to achieve.

6.4.1 Small Angle Controller

As in [7][18][16][6] the UAV rigid body pose R0u is decomposed into Euler angles for vehicle roll (), pitch() and yaw () angles through the fact that:

R0u = Rz()Ry()Rx()

The Euler angle rates can be determined from the relation between the rigid body rotation vector and theEuler angles:

= 1 st ct0 c s

0 s/c c/c

u,0u,xu,0u,yu,0u,z

= u,0uWhere c = cos and similar for sin , s, and tan , t. With the use of small angles, one makes a properassumption that the Euler angles can be independently controlled. Controllers on the UAV altitude (z) andyaw can be any classic (saturated with high frequency roll off) P(I)D controller [18]. For example a PDcontroller without roll-of is stated as (the bar denotes a desired or reference variable):

f =gmUAV +kp,z(z

0z0)+kd,z(z0z0)cc

Mz = kp,( ) + kd,( )

The references for roll and pitch can be generated from a P(I)D controller on the preferred lateral positionand velocity and subsequently controlled by their own PD controller, dependent on the local reference anderror [16]:

= arcsin (kp,x(xu xu) + kd,x(xu xu)) = arcsin (kp,y(y

u yu) + kd,y(yu yu))Mx = kp,( ) + kd,( )My = kp,( ) + kd,( )

6.4.2 Large Angle Controller

An approach similar to [17] will be applied to stabilize a UAV on SE(3). Stability proof of this controlstrategy can be found in [17] and [15]. In shorter notation R = R0u and =

u,0u . Similarly R denotes the

desired orientation and denotes the desired rotational velocity.The skew symmetric form of the error in rotation is defined as:

eR =1

2(RTRRT RT )

The orientation velocity error as:

e = RT R

38

Taking the time derivative of the orientation velocity error (note that = 0):

e = + RT R RT R RT R

= J1( J + Mu) + RT R RT R

Therefore a control law candidate for the control torque Mu is:

Mu = kReR ke + J J(RT R RT R

)(6.1)

Which asymptotically stabilises the yaw-attitude of the UAV, see [17] for the proof. Omitting thefeedforward terms makes the control law easier, which still leads to sufficient performance [15]. The positioncontrol law is a controller for the total thrust with feedback into the desired orientation. Hence a propercontrol law for the total thrust f is given by using the position error ex = x x and velocity error ev = v x:

f = kxex + kvev +mg[0, 0, 1]T mx,R[0, 0, 1]T = F,R[0, 0, 1]T (6.2)

The F denotes the total control force the UAV should apply. Hence from this the desired orientation ofthe thrust axis u3c can be derived. A preferred heading vector u1 has lower priority and is mapped as goodas possible realizing mainly the correct direction. When the position error goes to zero, the heading vectorapproaches the desired one. Intermediate computed direction vectors are given by:

u3c =FF ,F 6= 0

u1c = 1u3cu1 (u3c (u3c u1))(6.3)

From this the desired rotation matrix and rotational velocity can be formed:

R = [u1c,u3c u1c,u3c] = RT R

(6.4)

Together, both control laws asymptotically stabilise the UAV position and yaw. As can be seen, thiscontrol law requires proper knowledge of UAV parameters. With the manipulator attached, this might proveto be difficult. Hence saturated integral control action on the UAV position could be applied. The proof ofstability might then need be reworked.

6.5 Trajectory generation

Apart from flying the UAV and keeping the manipulator steady, an operator might to move the end effectoras flying hand. Therefore it feels natural to have an operator give position and yaw references for the endeffector tip. Therefore UAV reference trajectories have to be generated from the manipulator reference.

6.5.1 UAV offset reference

Lets assume a constant preferred location of the end effector relative to the UAV, described by vector puee inthe UAV body frame. The time varying end effector reference trajectory p0ee is given in the inertial frame andis assumed to be smooth. A smooth yaw reference is given by including any offset angle for manipulatorsnot attached to the UAV front side. The direction in which heading vector u1 points, the UAV referenceposition and orientation can hence be specified through:

x0(t) = p0ee(t)Rz((t))pueex0(t) = p0ee(t) (t)uzRz((t))pueex0(t) = p0ee(t) ((t) + 2(t)uz)uzRz((t))pueeu1(t) = [cos(), sin(), 0]

T

(6.5)

Where Rz is a rotation matrix around local z and uz the skew-symmetric of a z unit vector.

39

puee^puee^x0

x0_

x0p0ee p

0ee

_

ex

EE TrajectoryUAV Trajectory

a) Ideal case b) Realistic case

Figure 6.4: UAV Reference Trajectory from Manipulator Reference Trajectory, notice the fixed offset and UAV error themanipulator should compensate for.

The UAV will have a reference trajectory it should follow. The manipulator should hence attempt to followits inertial frame reference. The manipulator works with local references only, so a simple transformationgives the local coordinate to track, which is highly dependent on the positioning of the UAV:

pbee(t) = R0b(t)

T(p0ee(t) x0(t)

)Rub Tpub (6.6)

Since the UAV is underactuated, it will need to tilt to translate. Hence the manipulator will compensatefor such movements as well as for wind and wobble due to other aerodynamic effects, as shown in Fig 6.4.

The end effector sensor roll can be rotated to angle:

4 = arcsin u2, z0+ pWhich is the roll angle derived from mapping the UAV y (u2) vector onto the inertial z vector (z

0). Thisensures that the sensor stays at an angle p relative to the fixed world horizon.

6.5.2 Reference limitation

The low level hardware cannot do the necessary computations and the high-to-low level communication istoo unreliable to always do complicated control on the high level. Since no (damped, i.e. through Jacobiansingular values) Jacobian method is used to control the manipulator some other approach is needed.

To transform workspace coordinates to joint space coordinates, the local end effector position referencepbee(t) R3 is converted to joint angles (t) R3 through an inverse kinematics mapping on the high level.

From joint angles to end effector position, the mapping is injective and non-surjective from the smallerjoint-position space to the larger end-effector space. Hence the reverse map from the end-effector space to thejoint-angle space might not always exist, i.e. the preferred reference lies outside the reachable workspace.To solve this, the local manipulator reference is approximately projected to the real preferred workspacewhen it lies outside it. In this manner the manipulator will always attempt to point towards the direction itshould go to, which is convenient since the absence of step responses or erratic behavior lowers the transientinfluence on UAV dynamics.

The workspace is approximated by four ellipsoids Si, i = 1, 2, 3, 4 as seen in Fig. 6.5. Ellipsoids Si, i =2, 3, 4 are the same but rotated by 120 degrees. The ellipsoid is a set of all points satisfying the equation(using p = [x, y, z]T ):

Si = {x, y, z R :(x ox,isx,i

)2+

(y oy,isy,i

)2+

(z oz,isz,i

)2= 1

}

Where we can denote oi = [ox,i, oy,i, oz,i]T as the ellipsoid origin and si = [sx,i, sy,i, sz,i]

T as its Cartesianradii. Any point in the base frame pb = [pbx, p

by, p

bz]T can be expressed in ellipsoid-i space (Si), by the

following transformation:

pSi = (pb oi) siWhere denotes elementwise division. If pSi < 1 the point lies inside the sphere Si. If a point does notlie inside the sphere, it might have to be projected onto the sphere. This is done by shooting a ray from the

40

Figure 6.5: Projection of references onto the manipulator workspace. Any reference point (black) is checked, when outsidethe workspace, it is projected onto an approximation the workspace (red) created by three ellipsoids.

point pSi to a point defined as the center of mass of the workspace CSi = (Cb oi) si. The line definedby these two points can be described by a Plucker-line LCp:

LCp ={pSi CSi : pSi CSi

}= {U : V}

Since any vector end-point q on the Plucker-line must obey the following double rank deficient system ofequations:

0 Uz Uy VxUz 0 Ux VyUy Ux 0 VzVx Vy Vz 0

[ q1]

= 0

This holds due to orthogonality of U and V and the location of the point on the line. The above equationsenables us to express the x- and y-coordinates of the point q in its z-coordinate. Filling this in into ahomogeneous sphere equation, it is reduced to a quadratic equation, which gives the two solutions of thez-coordinate of the point on the sphere where the line intersects the sphere. The quadratic equation to solvefor this coordinate for a unit sphere is:

0 = U2q2z + 2(UyVx UxVy)qz + V 2x +