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

Abstract

1 Introduction

2 Requirements
2.1 General requirements . .
2.2 Non-destructive testing
2.3 Visual inspection . . . .
2.4 Object placing . . . . .
2.5 Surface cleaning . . . . .

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

9
9
9
10
10
10

3 Design of the Manipulator

3.1 Manipulator concepts . . . .
3.1.1 Serial manipulators . .
3.1.2 Parallel manipulators
3.2 Manipulator specification . .
3.3 End-effector specification . .

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

11
11
11
11
11
12

4 Realization of the Manipulator

4.1 Materials . . . . . . . . . . . .
4.2 Components . . . . . . . . . . .
4.2.1 Manipulator Actuators .
4.2.2 Thighs . . . . . . . . . .
4.2.3 Shins . . . . . . . . . .
4.2.4 NDT end-effector . . . .
4.3 Electronics . . . . . . . . . . .
4.3.1 Motor drivers . . . . . .
4.3.2 Strain gages . . . . . . .
4.3.3 Low level hardware . . .
4.3.4 Communication . . . . .
4.3.5 High level hardware . .
4.4 Software . . . . . . . . . . . . .
4.4.1 Low level software . . .
4.4.2 High level software . . .

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

15
15
16
16
16
16
16
18
18
19
19
19
19
19
19
20

.
.
.
.

23
23
25
26
26

.
.
.
.
.

.
.
.
.
.

5 Model of the System

5.1 Derivation of the simple model . . . .
5.2 UAV dynamics as fictitious input force
5.3 DC motor model . . . . . . . . . . . .
5.4 UAV model . . . . . . . . . . . . . . .

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.
1

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

5.5

5.4.1 Application to a quadrotor . . . . . . . . . . . . . . .

5.4.2 Application to a ducted fan UAV . . . . . . . . . . . .
Virtual system multi-body model . . . . . . . . . . . . . . . .
5.5.1 Rigid Body Dynamics Through Screw Theory . . . . .
5.5.2 Describing Dynamics in Screw Theory . . . . . . . . .
5.5.3 Bond Graph Representation of Rigid Body Dynamics
5.5.4 Kinematic Structure of the Model . . . . . . . . . . .
5.5.5 Modeling Environmental Interaction . . . . . . . . . .

6 Manipulator and UAV Control

6.1 Position control . . . . . . . . .
6.2 Joint space impedance control .
6.3 Workspace impedance control .
6.4 UAV control . . . . . . . . . . .
6.4.1 Small Angle Controller .
6.4.2 Large Angle Controller .
6.5 Trajectory generation . . . . .
6.5.1 UAV offset reference . .
6.5.2 Reference limitation . .

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

27
28
29
29
30
30
31
32

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

35
35
36
36
38
38
38
39
39
40

7 Simulation Results
43
7.1 Simple model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
7.2 Manipulator attached to a Ducted Fan UAV . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
7.3 Manipulator attached to a Quadrotor AUV . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
8 Experimental Results
8.1 Manipulator stand-alone Performance . . . . . . . . . . . . . . .
8.2 Manipulator attached to Ducted Fan UAV . . . . . . . . . . . . .
8.2.1 Free-flight with position control of the manipulator . . . .
8.2.2 Interaction During Flight . . . . . . . . . . . . . . . . . .
8.3 Manipulator attached to Quadrotor . . . . . . . . . . . . . . . . .
8.3.1 Free-flight With Position Control of the Manipulator . . .
8.3.2 Combined UAV and manipulator inertial frame trajectory
8.3.3 Interaction During Flight . . . . . . . . . . . . . . . . . .

. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
tracking
. . . . .

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

47
47
48
48
50
51
51
52
53

9 Conclusions and Future Work

55

Bibliography

58

A Extra Control Strategies

59
A.1 Virtual UAV Jacobian pseudo-inverse reference generation . . . . . . . . . . . . . . . . . . . . 59
A.2 Decentralized combined control of manipulator and UAV . . . . . . . . . . . . . . . . . . . . . 60
B Relevant Code
B.1 Limit to workspace matlab code .
B.2 Inverse kinematics matlab code . .
B.3 Forward kinematics matlab code .
B.4 Gravity compensation matlab code

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

C Technical Drawings

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

61
61
63
65
66
69

List of Figures
2.1

Schematic view of boiler approach maneuver

. . . . . . . . . . . . . . . . . . . . . . . . . . .

10

3.1
3.2
3.3

CAD of manipulator with nomenclature . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

Delta manipulator local workspace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Kinematic structure of AIV system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

12
13
13

4.1
4.2
4.3
4.4
4.5
4.6
4.7
4.8

Photo of the prototype attached to a quadrotor and stand-alone

Finite Element Analysis of the Thigh . . . . . . . . . . . . . . . .
Photo of the end-effector in close up . . . . . . . . . . . . . . . .
Exploded end-effector view . . . . . . . . . . . . . . . . . . . . .
FEM Analysis of end-effector . . . . . . . . . . . . . . . . . . . .
Control Network . . . . . . . . . . . . . . . . . . . . . . . . . . .
Simulink Control Scheme . . . . . . . . . . . . . . . . . . . . . .
Simulink Workspace and Jointspace Control Scheme Submodel .

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

15
16
17
17
18
20
21
22

5.1
5.2
5.3
5.4
5.5
5.6
5.7
5.8

UAV and manipulator, indicating relevant frames and sizes

Quadrotor UAV Model . . . . . . . . . . . . . . . . . . . . .
Duted Fan UAV Model . . . . . . . . . . . . . . . . . . . . .
A single rigid body model in multi-bond bondgraphs . . . .
An interconnection of two rigid bodies . . . . . . . . . . . .
Example of two simulated interconnected rigid bodies . . .
Bondgraph of the UAV a Manipulator system . . . . . . . .
Simulation frames for interaction . . . . . . . . . . . . . . .

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

24
27
28
31
31
32
33
34

6.1
6.2
6.3
6.4
6.5

PI position control scheme . . . . . . . . . . . . . . . . . . . . . . .

Joint space impedance control scheme . . . . . . . . . . . . . . . .
Workspace impedance control scheme . . . . . . . . . . . . . . . .
UAV Reference Trajectory from Manipulator Reference Trajectory
Projection of references onto the manipulator workspace . . . . . .

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

35
36
37
40
41

7.1
7.2
7.3
7.4
7.5
7.6
7.7

Simulation Results of the Simple Model . . . . . . . . . . . . . . . . . . . . .

Wall approach and touching of the Ducted Fan UAV in the multibody model
Interaction force during touching in the Ducted Fan UAV model . . . . . . .
Ducted Fan UAV x-reference and measured position . . . . . . . . . . . . . .
Wall approach and touching of the quadrotor in the multibody model . . . .
Interaction force during touching in the quadrotor model . . . . . . . . . . . .
Quadrotor UAV x-reference and measured position . . . . . . . . . . . . . . .

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

43
44
44
45
45
45
46

8.1
8.2
8.3
8.4
8.5

Position and Joint Space Impedance control results . . . . . . . . . . . . . . . . . . . . .

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

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

48
48
49
49
50

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

8.6
8.7
8.8
8.9
8.10
8.11
8.12
8.13

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

.
.
.
.
.
.
.
.

50
51
51
52
52
53
53
54

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

60

.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.

Abstract
The main goal of the AIRobots collaboration is to come up with prototypes of a remotely controlled aerial
vehicle which is capable of performing NDT tasks in a predefined mock-up environment. The chosen robotic
airframes used in the project will be equipped with a robotic arm: an appropriate end-effector and electromechanical arm-like manipulator.
This report presents the mechanical design, modeling and realization of a manipulation system placed
on Unmanned Aerial Vehicles.
The innovation of the prototype lies in the use of a Cartesian Delta robotic manipulator, on-board a flying
vehicle, to physically interact with environments and perform ultrasonic non-destructive testing experiments
and other versatile tasks at unreachable locations for humans.
The novel NDT end-effector is composed of a spring loaded sensor to compliantly interact with environments. A double spring loaded Cardan gimbal system provides two passive degrees of freedom in rotation
with defined equilibria to overcome gravity on the sensor and to define a stable zero reference. Actuation of
the axis of the sensor provides actuation of the remaining DOF.
Simulation in rigid body bondgraphs and experimental in-flight interaction measurement results show
that 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 having
a manipulated tool, due to the coupled UAV dynamics a fixed rigid tool would not always suffice for tasks
where position accuracy is preferred.

Chapter 1

Introduction
Industrial inspections and rescue operations require considerable effort from humans. Large structures and
inaccessible locations demand laborious support systems and possible risks of structural collapse create a
hazardous working environment. Implementing security measures takes time and reduces inspection and
rescue efficiency. These factors generated the need for remote controlled and (semi-)autonomous robotic
aiding systems that can support the human in these tasks.
Robots with climbing possibilities have successfully been deployed in industrial structural inspection in
visual 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 be
inspected. Novel approaches on biologically-inspired and electro-adhesion might prove useful in overcoming
challenges set by navigating over unstructured irregular surfaces [1].
The use of Unmanned Aerial Vehicles (UAVs) in rescuing scenarios after natural disasters and general
surveillance have proved to be successful. The advantage of large tether-free workspaces, small vehicle sizes
and the possibility to carry different payloads make the UAV a suitable candidate for investigation and
exploration. Recent research in UAV cooperation in object lifting, group navigation and fully autonomous
navigation opens up even more opportunities with useful applications [2].
An innovative field of application and research lies in the deployment of UAVs for industrial structural
inspections, 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 surrounding
environment. This poses new challenges in UAV research for the stable interaction control and opens up a lot
of research possibilities for integrated on-board electromechanical devices to interact with and manipulate
this environment. Interaction with rigid environments is not new, as Albers et al. [1] mention a design of a
flying 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 release
objects 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 on
the novel idea of physical interaction of UAVs that combines all aforementioned fields of research (inspection, touching, grabbing etc.): the design and testing of a robotic sensor and tool manipulation system for
structural inspection UAVs.
The novelty lies in a manipulation system that can be placed on-board different kinds of UAV platforms
with modular task-specific end-effectors that are thereby supplied with full Cartesian movement relative to
the 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 system
simulations. Usefulness of the manipulator will is verified through experimental data. Recapitulation of
UAV 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 in
this report are not yet implemented on the physical systems due mainly time constraints. These ideas are
to some extent used in simulation and could be quite useful in future research and implementations and will
be 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 a
specification of the current realization of the prototype. In Chapter 5, mathematical models are discussed
which are used to assess the dynamic behavior of the manipulation system on a UAV. Chapter 6 discusses
used and future control strategies and ideas. The simulation results of the resulting controlled models are
discussed in Chapter 7. In Chapter 8 experimental results of in flight and interaction experiments are
discussed. Finally, conclusions and future research possibilities are discussed in Chapter 9.

Chapter 2

Requirements
Considering the UAV and the manipulation system together, there are several tasks to be performed. Within
the research project AIRobots [3], the robot should inspect a section of an industrial boiler wall by means of
the manipulator. Non-destructive testing (NDT) with an ultrasonic testing (UT) sensor and cameras, object
placing and cleaning tasks should be able to be performed on the surface of the boiler tubes. From these
tasks, the requirements for the manipulator and the end-effector are derived. In this work the focus will
mainly be on the NDT task, hence other manipulator tasks are not discussed further and are only considered
in 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 specifications [7][8]. The weight should be < 200 g using the weight of subsequent control electronics to balance
forces on the UAV. It should be placed on the side of the UAV since walls to be inspected are usually
vertical.

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 able
to fly to a preferred location and dock either by using aerodynamic forces to push itself against a vertical
surface.
The manipulator system should move an UT-NDT sensor to a tube of the boiler wall and take measurements at points, lines or surfaces defined by the human operator. This requires the manipulator to move
in Cartesian space. Since the UAV has, relatively, slow coupled dynamics, its positioning accuracy while
pushing the manipulator against the wall might be not sufficient without additional solutions. Therefore, the
range of movement, i.e. the practical workspace Wp , is chosen to be Wp = 5 5 5 cm3 to compensate for
these positioning and subsequent movement errors of the UAV. When the manipulator is in contact with the
boiler 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 UAV
lateral pushing force is in the range of 5 N [7], this is also the minimal force of what the manipulator should
be able to apply in most of its workspace. As an upper bound, a maximum impact force of 20 N should be
handled without damage to the mechanics.
Small angle offsets in all rotary motions of the end-effectors due to surface roughness, tube curvature and
UAV 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 manipulator
and UAV.
9

Figure 2.1: Schematic top view (not to scale) of the UAV, manipulator (dotted line) and sensor (small box) mounted on
the 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 the
curvature 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-effector
for close up visual inspection. This requires the end-effector to be modular, i.e. the NDT sensor must be
replaced 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 be
able 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 carrier
strength being the bottleneck.

2.5

Surface cleaning

Before doing NDT measurements, the boiler wall surfaces might need to be cleaned with a metal brush and
motor in the end effector. This also requires the end-effector to be modular. It needs to be able to apply an
unknown force preferably transmit all reaction torque to the UAV body, since UAV attitude control is able
to 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 of
this 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 parallel
manipulators.

3.1.1

Serial manipulators

The serial kinematic chain consists of different links with usually their own motor actuating in one or two
degrees 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 are
preferred. If tendons are not considered, another advantage is the placement of motors on the base instead
of 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. A
manipulator with Delta [9] kinematics was chosen. The Delta is ideal since it provides the preferred Cartesian
end-effector motion through three rotary motor motions and as preferred it can supply a significant amount
of force to the end-effector, compared to a serial manipulator. The actuators are all on the base closer to
the UAV center of gravity than would be the case in a tendonless serial manipulator. This induces a smaller
torque disturbance on the UAV and hence requires less counterbalancing. Since there is no need for the
motors to carry the weight of the next one, their strength and, hence the total weight can stay low, making
the 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 by
mechanical design or control, the manipulator can be used outside of any singular configuration with a unique
kinematic mapping of actuator angles to end-effector position. The forward and inverse Delta kinematics
can 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) the
knees, (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] that
can 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 diameter
is 6 cm, with an end-effector diameter of 4.5 cm. This provides the manipulator with a workspace starting
approximately 3 cm from the base plate, extending 9 cm.
The theoretical local total workspace W meets the requirements in Chapter 2 on the preferred workspace
Wp , where Wp W for actuator angles in the range [90 , +10 ]. The resultant workspace is shown in
Fig. 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 theoretically
unbounded 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. Two
1-DOF rotational joints near the front of the sensor form a double Cardan joint (double gimbal) inside
end-effector (see Fig. 3.3). Weak spring loading is used to define an equilibrium position and to counteract
gravity, while extra spring loading ensures mechanical compliance during contact with the environment. This
system 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
y [m]

0.04

z [m]

0.06
0.08

0.02

0.1
0.04

0.12

0.05
0

0.05

0.06

y [m]
0.06

0.05

0.05

0.03

0.03

0.04

0.04

0.05

0.05

0.06

0.06

0.07

0.07

z [m]

z [m]

0
x [m]

0.08
0.09

0.02

0
x [m]

0.02

0.04

0.06

0.6

0.5

0.4

0.08

0.3

0.09

0.1

0.1

0.11

0.11

0.12

0.12

0.13

0.04

0.06

0.04

0.02

0
x [m]

0.02

0.04

0.06

0.13

0.2

0.1

0.06

0.04

0.02

0
y [m]

0.02

0.04

0.06

Figure 3.2: Delta manipulator local workspace. In the top left is the total workspace W enveloping the preferred workspace
cube Wp . The other three images are bottom, and two side views of the workspace. Gray scale values indicate the sum of
absolute motor torques to withstand 5 N interaction force from the bottom of the workspace, which is a measure for the required
motor 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 specification are presented. Furthermore, the used electronic hardware and software and software architecture are
discussed. 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 in
Fig. 4.1. Some crucial parts of the design had been validated a priori through the dynamic models described
in Chapter 5 and by a Finite Element Method (FEM) Analysis in ANSYS 12.0. The manipulator was not
weight optimized, since the improvements would have been marginal considering the fact that the control
electronics and wiring were not optimized.
The used kinematic structure for the delta is as described in Fig. 3.3. The technical drawings used to
produce 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 tubes
make up the nine axial parts of the thighs and shins. Bronze is used in the end-effector in combination with
a conical steel set screws to create a low friction free Cardan joint. Double sealed steel small ball bearings
are used in all other joints to provide rotational degrees of freedom.
15

4.2
4.2.1

Components
Manipulator Actuators

A triple of Maxon RE10 motors with GP10A (four stage 256:1) reduction gears and MEnc10 12 ticks encoder
drive the legs of the manipulator. These can deliver over 1.5 mNm of continuous torque at the motor side
and over 200 mNm at the output shaft, which is necessary following results from the model (Chapter 5) and
manipulator kinematic analysis [5]. Also other motors with lower gear rations have been used to improve
impedance 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 carbon
fiber tube and two aluminum components. To measure motor torque, the bottom component has a milledout 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 highest
stress and, hence, strain builds up on the outside of the material as expected, which makes the use of strain
gages possible.

Figure 4.2: FEM Analysis of the Thigh part with maximal load. The colors (online version) indicate the equivalent von Mises
stress. The maximum stress is at the bottom left in the opening of the tapped hole which connects to the drive shaft. Maximum
strain 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 cannot
rotate relative to the manipulator base. They only take axial loads because they are decoupled from rotation
by 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 been
designed such that any type of functional end-effector can be placed inside this ring. The NDT end-effector
configuration is shown as an exploded view in Fig. 4.4 with numbers referring to mentioned parts in the
next 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 is
passive sliding with spring loading. To compensate for surface curvature, the passive elastic double gimbal
system 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 through
double 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 double
gimbal system together with parts (c) and (f). This ring is necessary to supply an extra degree of
freedom in rotation.
4. The probe sleeve: small bronze cups are placed inside holes to form a Cardan joint (f) as is also done
in the outer ring. This part forms the third stage of the dual gimbal and has hence two rotational
degrees of freedom. An Igus 2022-03 glide bearing (g) fits around the UT sensor (a) to aid in smooth
sliding and rotational movement. Three elastic bands clamped between the bearing brackets (d) and
the 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 sensor
equilibrium.
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 the
probe 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) and
PA2-50 encoder (l) drives the sensor by using a connector sleeve (m) that is connected to a combination
of 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, a
FEM analysis of this part has been performed, see Fig. 4.5. This shows the part is strong enough, even over
dimensioned, to withstand maximally prescribed loads.

Max [MPa]
Max  [m/m]

Aluminum
280
3.9 103

Part (20 N)
5.05
7.1 105

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

4.3

Electronics

Next to the motors, the electronics used to operate the manipulator are divided into five other groups that
will be treated separately: Motor drivers, Strain gages, Low level hardware, Communication, High level
hardware.
More in depth information on the software running on the low and high level hardware can be found in
Sec. 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, supplied
by an RC Hobbywing UBEC which acts as a voltage regulator. Each of the discrete MOSFET H-bridge
motor drivers enables bidirectional control of one brushed DC motor. The boards provide two fault flag
pins that can be read by low level hardware to check proper operation (i.e. short circuit, over-heating or
under-voltage). A (logical NOT) reset pin provides the option to switch the drivers off, i.e. cutting the power
flow to the motor in case of emergencies. These motor driver boards are over dimensioned in the current
prototype, although this was not an issue. Much weight reduction can be gained by combining four motor
drivers 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 the
data of maximally 8 Wheatstone half-bridges. The multiplexer can be set to pass one of the 8 signals to the
14 bits ADC converter ADS7279 by setting three inputs to either ON or OFF. This can be regulated by the
micro controller ATmega328P. The micro controller collects the data of the ADC outputs and communicates
through 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 custom
made firmware. It takes care of sending PWM and direction signals to the motor drivers, supplying power
to encoders and reading encoder increment interrupts and external digital hardware.It receives input signals
over a serial communication line, of which the protocol has been defined during AIRobots IW1. It is chosen
to use a separate ADC for measuring strain gage voltages since it increases resolution and reduces noise
compared 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 board
computer and Wireless LAN to the ground station. The communications channel has been been tested and
can 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 Atom
processor running Ubuntu 10.10. The ForceDimenson Omega.6 haptic device is used to control the UAV
with 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 separately. When assuming that a human operator or a supervisor is to be in high level control, the discussed
high level in the subsequent paragraphs can be seen as being middle level. The total system control network
is 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 several
functions obtained from the open source community. It can read analog and digital ports and set PWM
and digital values through connected pins. Fast reading through the open source digitalReadFast and
digitalWriteFast functions1 can be used to read pins a lot faster than the similar functions from the standard
Arduino 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 checks
for serial messages sent by the high level control and applies these. Data in the received message header
specifies a certain state of control, or operational mode, the Arduino should switch to. The five main modes
are:
1. Joint space position control: PI control of motor angle position. The high level sends position set
points as reference. The low level calculates the control signals.
1 http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1267553811

19

Ground St at ion
Ct rl Input s

Dat a Exchange

C(s)

P(s)

Asctec Pelican UAV

ym

UAV Dat a

Telemanipulation

Com

On Board

Flight Ct rl

Com

Pos Ct rl

At t Ct rl

UAV Pose
Pose Est imat ion

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

Manipulator Control

At om Board
FF

Marker Dat a

Ref

Manip/ Cam Dat a

Manip Ct rl

HL Ct rl Input s

Manipulat or

Haptic Interface

Joystick
LED Markers
VisualEyez
3DTracker Syst em

Human Operat or

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 with
interpolated minimum jerk trajectory between given set points. This can be used to reduce the frequency of communication between high and low level hardware since it only requires the final desired
joint 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 velocity
to position and calculates the position control signals. This ensures proper velocity control at very low
speeds and smoother manipulator movement.
4. Joint space impedance control: PD control of motor angle position with nonlinear gravity compensation. The high level software processes the kinematics functions to estimate the gravity torque on the
motors and sends a PWM value to compensate this. This PWM value is added to the internal PD
controller.
5. PWM control: if complicated control (such as workspace based control) is done on the high level
hardware, 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 communication
and measurement.
In any mode, the Arduino sends its values of measured motor angles and other relevant data to the high
level back over the serial communications channel. At the end of the loop, the Arduino waits until the total
loop 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, software
priority (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 prevent
staircasing 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 main
control code around the UAV position controller, together with high level manipulator control is running
as C++ compiled software in a Matlab Simulink 2011b as shown in Fig. 4.7 and Fig. 4.6. The workspace
controller ( 4.8) loop described in Chapter 6 runs on the high level software. The kinematics are implemented
20

Joint Space Position Control

Msg Handler JSPC

Pos/IMP

Joint Space Impedance Control

Calib_Switch

Output

Go to Calibration
Inverse Dynamics and Workspace Impedance

Msg Handler JSIC

IMP_Mode

Work Space Impedance Control

Reference Generation

Limit to Workspace

Inverse Kinematics
Msg Handler JSIC1
EM Switch

Forward Kinematics

Gravity Comp

V2PWM

On/Off

local EE pos

Euler Angles
Roll,Pitch,Yaw

H
UDP Receive

Terminator

Euler Angles

x,y,z

Inertial frame UAV position

Tilde
T
Tilde1
Base_Movement
makeomega

Fictitious Force on End Effector

Figure 4.7: Simulink Control Scheme. The left top part handles incoming messages and calculates references. The top right
generates a proper message for the Arduino to handle, according to what the operator wants. The bottom part calculates the
UAV pose and the forces on the manipulator which could be used in the controller shown in Fig. 4.8. The gravity compensation
is 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 custom
written UDP-Serial interface converts messages on the UAV platform to those for the low level controller of
the manipulator. Since the timing of the high level is unreliable, closing control loops on the high level leads
to 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 total
manipulator controller can be incorporated in outer haptic tele-operation loops if preferred.

21

theta

DemuxSel1

SVF1

Joint_Friction

gain1
Add

Mass_Matrix

SVF2

Coriolis_Centrif_Matrix

Transpose1

gain4

Workspace PD

PD WS

Add1

Matrix Multiply1

Matrix Multiply2

Load_to_Motor_Conv

Saturation

gain

V2PWM1

gain2

gain3

To Workspace4

Matrix Multiply3

Force to motor torque

gain5
Manual Switch

JS PD

PD JS

gain6

Figure 4.8: Simulink Workspace and Jointspace Control Scheme Submodel. The gravity compensation is done in code as
shown 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 dimensioning. It is used for dimensioning the actuators, to assess system behavior during operation and to quickly test
several control ideas that are laborious to implement in a real setup. Several models are discussed: a three
state equation joint-pace model for the manipulator which will be used for computed torque control. UAV
models will be discussed in Sec. 5.4 and a model of the total system will be assessed in Sec. 5.5. Results of
numerical 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 is
used in inverse dynamics control. The dynamics due to the degrees of freedom inside the end-effector are
neglected in this model, since their influence is small. The UAV movements are modeled as external forces
on the end effector, but manipulator influence on the UAV cannot be modeled in this way. This is overcome
in the model described in Sec. 5.5.
In the following sections the subscript th will indicate a thigh property, subscript sh will describe a shin
property, subscript ee will indicate an end-effector property and subscript par, h will describe the horizontal
part of the shin parallelogram. Subscript suffix t states total mass or total inertia. Variable I specifies
rotational 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 angles
being 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 its
origin, see Fig. 5.1.a), the local frame must be transformed to the inertial frame. The total homogeneous
transformation, assuming the manipulator attached in its local origin, is given by the UAV pose and the
stationary transformation from the manipulator origin to the UAV, i.e.,
H0b (t) = H0u (t)Hub

(5.1)

Where the super- or subscript 0 indicates the inertial frame. Hence the time dependent rotation matrix
is part of that pose, i.e., R0b (t) = R(H0b (t)). Time dependencies will be omitted in the remainder of the
derivation.
Using the approach in [11], the shin inertia is added to the thighs and end-effector to reduce model
complexity. The inertia felt by the motors at the output shaft is an addition of the thighs own inertia
rotating around its end-point (i.e. implying the parallel axis theorem) including the horizontal part of the
parallelogram at the thighs end and 2/3 contribution of the parallelogram beams of the shin, i.e.,
2
Ith =[Ith,com + mth rcom
] + [Ipar,h + mpar,h a2 ]+

2/3(2msh )a2
23

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

The moment arm of gravity on the thigh is hence:

rG,th = a

rcom
a 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 of
the two shin beams:
mth,t = mth + mpar,h + 2/3(2msh )
bi (i ), the inertial frame gravity unit vector
The unit vector of a thigh in axial direction is denoted as e
T
0
b
G = [0, 0, 1] and the hip rotation axis is given by e
j,i = Rz,i (i ) [1, 0, 0]T . The relative jointspace
as e
contribution of the gravity force on the thigh is hence given by ( denoting the cross product):
b

b1 (1 ) Rb0 e
0G i
h
ej,1 , e
b2 (2 ) Rb0 e
0G i
ebj,2 , e
Gth () = h
b
b
b
3 (3 ) R0 e
0G i
h
ej,3 , e
with locally described unit vectors of the thigh given by:

0
bi (i ) = Rz,i (i ) cos(i ) , i = 0, 2/3, 4/3
e
sin(i )
The 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:


1
2msh + mpar,h
mee,t = mee + 3
3
The vector describing the end-effector gravity contribution in the inertial frame is hence given by F0G =
T
[0, 0, mee,t g] 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):


bee
ee,I = JTp mee,t p

th,I = Ith I3
The external, non-inertial, contributions to system torque arise from gravity (F0G , Gth ()), external
and fictitious forces (Fbf,ee , expressed in eq. 5.3). Their torque contributions are a follows:

(F0ext )

ee,e
th,e

= JTp Rb0 (F0G + F0ext ) + JTp Fbf,ee

= gmth,t rG,th Gth ()
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 f r = kd and motor load shaft torque l :
ee,I + th,I = ee,e + th,e + f r + l
Expanding the acceleration of the end-effector into joint space and subsequent collecting gives the motor
load side torque:




+ mee,t JT J p + kd

= Ith I3 + mee,t JTp Jp

p


mth,t grG,th Gth () JTp Rb0 F0G + F0ext
where the torque and angles are specified on the, post-gear, load (subscript l) side. Changing this to the
motor (subscript m) pre-gear side, the following relations hold:
l
l

= m /kr
= kr m

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




1
T

Ith I3 + mee,t Jp Jp +
m = kr Im,g I3 +
kr



mth,t grG,th
mee,t T

Jp Jp + k
Gth ()+
kr
kr



1 T b 0
1
J R F

JT Rb F0
(5.2)
kr p 0 G
kr p 0 ext
This yields a lower felt inertia due to the gear ratio. It can be seen that eq. 5.2 is a standard robot dynamics
equation of the form:
+ C(, )
+ k + g() + f ()
m = M()
the Coriolis and centrifugal terms, k the linear viscous
where M() states the system mass matrix, C(, )
friction parameters, g() and f () the gravity and external contribution including interaction and fictitious
forces.

5.2

UAV dynamics as fictitious input force

In the following section rigid body twists Tb,c

a , or their elements, are described by rotational and translational
velocity (throug the frame origin b) of body a with respect to c expressed in b. More on screw theory can
be found in [14]. A tilde accent denotes the 3 3 skew-symmetric form of a 3 1 vector or the tilde form
of the twist.
Assume the UAV as one rigid body, with twist:
 0,0

 0,0 
u
u

0,0
0,0
0,0
u

Tu =
, Tu =
0,0
0T3
0
u
Time derivatives of the base pose can be derived, with the hat () denoting a vector augmented with a 1
element for homogeneous transformations:
0
R
b

0b
p

0,0 R0 Ru
=
u u b

0,0 + (T
0,0 )2 H0 Hu 0
0

=
T
u
u
u b 3 = Hb 03

From which one can get the second derivative of the rotation matrix and the linear acceleration of the base
expressed 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 orthonormal
frame by ui then the fictitious force (subscript f ) felt by the end-effector described in the manipulator frame
is felt as:
"
#
3 
 
X
b
0
0
b
0

b +
Ff,ee = mee,t p
2Jp,i Rb + pee,i Rb ui
(5.3)
i=1

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 above
simple model and the elaborate multi-body model discussed in 5.5 to compute voltage dependent drive
torque.
One assumes a linear DC motor described by the following dynamics:

u(t)
kt i(t)

1
di
(t) + Rt i(t) + (t)
dt
kt
d
= J
(t) + kd (t)
dt
= L

(5.4)
(5.5)

Stating the electrical terminal voltage u(t), motor current i(t), electrical inertia (inductance L), terminal
resistance (Rt ) and motor constant kt relating the armature current and produced torque and rotational
speed 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
y = Cx

(5.6)
(5.7)

Which written out gives:

1

Rt
L k1t 0
i
i
L
d
= kJt
kJd 0 + 0 u
dt

0
0
1
0


0 0 1 x
y =

(5.8)
(5.9)

The transfer function of the system from voltage to rotation angle is given by:
HDC (s) =

(s)
kt
=
V (s)
s((Js + kd )(Ls + Rt ) + kt2 )

(5.10)

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

5.4

UAV model

In this section a generic UAV model will be discussed. More detail is given about two types of miniature
UAVs: a Quadrotor UAV and a Ducted Fan UAV. This is done since the prototype of the manipulator has
been tested on both platforms.
To model a generic UAV, the North-East-Down (NED) orientation convention is considered for a single
rigid body frame. It is assumed that a single rigid body with control force mappings to actuators is sufficient
to describe both UAVs, this approach is a little different from [7][6] and more in line with Quadrotor
work [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 the
UAV:
x 0
mv 0
0
R
u
J u,0
u

= v0
T
= mg
z0 + f R0u [0, 0, 1]
0 u,0
= Ru u
u,0
u
= u,0
u J u + Mgy + M

(5.11)

Where m is the linear UAV mass, J is the rotational inertia, g is the gravitational acceleration, R0u the
orientation of the UAV body in the inertial frame, u,0
u the rotational velocity of the UAV with respect to
the inertial frame, but expressed in the body-frame, and x0 and v0 the position of the UAV center of mass
described 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 moment
acting on the UAV can be put in the respective Newton or Euler equation stated above. Fig. 5.2 and Fig. 5.3
give 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)

f3

x0
^

f4
u

u3

u1

u2 f2

f1

y0
^

z0
^

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

counter-rotating propellers. This architecture leads to four eccentric forces and a reaction moment at each
propeller. By varying relative thrust a net torque can be applied in any direction. The constant linear
mapping from the forces generated by the propellers (fi , i = 1, 2, 3, 4), see Fig. 5.2, to the total thrust f and
T
body moment Mu = [Mx , My , Mz ] :

f
1
1
1 1
f1
Mx
0 d 0 d f2

My = d
0 d 0 f3
Mz
c c c c
f4
u = A
Where d is the distance from the UAV center of mass to the propeller and c is the constant ratio
between 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 a
control law for the thrust and control torques and calculate the propeller force. From the propeller force the
propeller rotational speed i can be calculated through the relation fi = kl i2 , where kl is the lift constant
27

of the propeller [6][7][18]. Both pairs of the Quadrotor propellers counter rotate and with the assumption
that they have low inertia, their gyroscopic influence (when propeller speeds are unequal) on the UAV is
negligible. 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

x0
^

y0
^

z0
^

u1
u u2

u3

fx
fy

Figure 5.3: Left) Ducted fan and the build up of the vane structure [7] a) frame/housing b) propeller c1) anti-torque/yaw
vanes, c2) roll and pitch vanes. Right) Ducted Fan UAV model indicating the inertial frame 0 and the UAV frame u , the
thrust 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 the
airflow by sets of control vanes [7][6]. One set (with vane angle ) deflects the air in such a way that the
reaction torque of the propeller is cancelled. The two other sets of vanes generate a force and hence torque
in lateral directions (torque about body x by the vanes with angle , and torque about body y by vanes
with angle .
If the velocity of the air incident on the control vanes is given by Vi , following [6], the lift force produced
by the -set, -set and moment on the -set of vanes are respectively given by:
Fy
Fx
N

= 12 cl Vi2 (2Si ) = c1 Vi2 [N]

= 21 cl Vi2 (2Si ) = c1 Vi2 [N]
= 12 cl Vi2 (8Si )( d2T ) = c2 Vi2 d2T [Nm]

(5.12)

Where denotes the density of the surrounding air, which is 1.225 kgm3 , cl the vane lift constant, ideally
considered to be 2 from flat plate theory, Si the vane effective incident surface area and dT the distance
between the centers of pressure on the anti-torque vanes. The above relations hold under the assumption
that 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, whose
modulus is just the same as that of the normal thrust T , through:
s
Vi =

T
2S

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

the total thrust f and body moment Mu

Mx
My

Mz

fx =

fy
fz

= [Mx , My , Mz ] :

0 dT k1
0
0
0
dT
k1

0
0
0

0
0
dT k1

0 dT k1
0
1
0
0

0
0
dT
2 T k2
0
0
0

u = A(T )
Which is left-pseudo-invertable when T 6= 0. Constants k1 and k2 depend on values in eq. 5.12. If lateral
forces are assumed to be small, the system becomes square, which makes invertability easier. The thrust of
the main propeller is given by T = kl p2 , 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], the
gyroscopic torque induced on the vehicle is given by:
u
Mgy = u,0
u Jp up p

Where uup is the axis of rotation of the propeller in the UAV body frame. Since by design one knows the
propeller inertia, and by control the rotational velocity of propeller, the gyroscopic precession torque can
always 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 manipulator
in a joint space model, it is non-trivial to combine it with a model of the UAV to get an indication of the
total 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 theory
for rigid body dynamics of which more can be found in [14] and in the following section. Simulation results
of 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 in
that frame. The so called twist is a 6-dimensional vector describing in the top the rotational () and in the
bottom somewhat the translational velocity () of a rigid body:
h
i
T T
a,b T
Ta,b
a,b
c = c
c
Which is a twist of body/frame c relative to body/frame b expressed in frame/body a. Take care that the
translational 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 denoted
as: T0,0
b . This twist can be expressed (seen from) in any other frame c through the following coordinate
transformation for twists:
Tc,0
=
AdHc0 T0,0
b
b

Rc0
033
AdHc0 =
c0 Rc0 Rc0
p
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:
h T T iT
Wij = ji fij
29

A wrench acting on body b expressed in frame 0, Wb0 , can be transformed to frame c, Wbc , through:
Wbc = AdTHc0 Wb0
Although wrenches are the duals of twists and should hence be presented as row-vectors, they are presented in the following text as column vectors for ease of notation.
The time-derivative of the Adjoint transformation can be described as:

AdHj
= AdHj adTi,j
i
i
i
 i,j

i

033
adTi,j
=
i
i,j
i,j

i
i

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) makes
the equations of dynamics straightforward. This makes it possible to write the Newton-Euler equations for
rigid body motion in compact form (treating the generalized momentum of body i, P i , as a column vector):
P i

i,j
Iii T
i

= adTTi,j P i + Wii
i

i
= adTTi,j Iii Ti,j
i + Wi

(5.13)

Wii

The wrench
can be any wrench acting on body i. In a multi-body model this wrench is composed of
actuation (by motors, propellers etc.) forces and torques and of constraint forces and torques, conveniently
described in their own frame and transformed to the body frame. The final step is to find a way to relate
the body twist and its configuration. Since the configuration matrix Hji (t) SE(3), the following holds:
j Hi , Hi H
j
H
j i se(3), which is the Lie algebra of SE(3) containing the tilde form of the twists. Hence,
i j
dependent on preference, there are two differential equations for the body configuration related to the tilde
form of the twist:
j = Hj T
i,j
H
i
i i
j =T
j,j Hj
H
i
i
i
Any of these can be numerically integrated to get the configuration at time t, assuming a known starting
configuration at t0 = 0. Using the twist of a body i expressed in the inertial frame:
Z t
0,0 ( )H0 ( )d + H0 (t0 )
H0i (t) =
T
(5.14)
i
i
i
0

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 of
physical system that need to be simulated. It makes interconnection of different physical domains in models
more intuitive and focuses on drawing the physical concepts rather than the equations (as would be in a
block diagram). More on bond graph modeling and its notation can be found in [19] and specifically for
simulation of rigid body dynamics [20].
A multi-bond bond graph model can be constructed with flow variables being the body twists expressed in
any chosen frame. The effort variables are given by the wrenches acting on bodies. The 1-junction represents
a (common) twist or summation of wrenches in that frame, a 0-junction represents a (common) wrench, or
summation of twists in that frame. Any I-element represents the bodys inertia tensor. Any C-element
represents potential storage in 6D DOFs.
Assuming a rigid body looked at from its local principal frame, the required physical concepts to describe
the 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 the
body 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 wrench
can act somewhere in space to keep bodies together in preferred DOFs. A twist difference between two bodies
i and j is given by:
0,0
0,i
T0,j
= T0,0
i
i Tj = Tj
Which is given by a 0-junction in bond graph notation. This twist difference can be transformed to the joint
location in frame k to get that same twist difference expressed in k , given by Tk,j
i . At this location any
DOF can be constrained by using very stiff and damped springs. Compliant interconnection is also possible
by 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 where
constraint 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 the
end-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. All
rigid bodies are interconnected by joints. The relative coordinate transformations of the joints to the rigid
body principal frames are time-constant. The homogeneous transformation matrices describing the rigid
body configurations are not and are calculated through eq. 5.14.

x0
z0

H aj

H0a

y0

Hb0

Figure 5.6: Example of two simulated interconnected rigid bodies. Body a and b have some initial configuration in the inertial
frame 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 all
interrelated interaction forces between rigid bodies [20]. The final bond graph of the manipulator can be
seen 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 environment or another body. A method to calculate interaction frames can be found in [21]. It is assumed only the
face of the cylindrical sensor touches the vertical surface, see Fig. 5.8. The vertical surface (a wall) is located
at point p0w in the inertial frame and the center of the contact face of the sensor is given by psc , relative to
the sensor principal frame. Taking the unit vector of the contact surface normal n0w = [1, 0, 0]T (can also
be any other), two additional variables are needed:

= hys , nsw i
= hzs , nsw i

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

0
1
r
pss = p
2 + 2 +  r
Where  is a small number to avoid division by zero. The point pci will give the location of the point closest
to the wall expressed in the face center frame of the sensor, which can be transformed back to the inertial
frame if preferred.
A Hunt-Crossley interaction model is adopted [22] to calculate the contact force normal to the wall. If
the two bodies collide, this normal reaction force is dependent on the penetration-depth x given by:

kxn (t) + xn (t)x(t)

x<0
Fn (x) =
0
x0
Where and k are respectively damping and stiffness coefficients. Power n is assumed to be 1. Penetration
x is trivially dependent on the difference of the previously calculated pci and given p0w , compared in the same
frame. Then when the interaction force is calculated, an interaction wrench Wi0 (x) at the interaction point
of the wall can be calculated:
T

Wi0 (x) = 0T3 Fn (x)n0w
Which can be transformed to the sensor body frame.
32

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

y0

z0

xs
x0

ys

ps

n0w

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 the
UAV and manipulator in the setup and models will be discussed. A suggestion will be made about how to
implement combined control, which has not been implemented yet due to time constraints and the absence
of 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) controller
is used. For touching inspection tasks, a proper candidate is impedance control. In this case the joint
motors are controlled with PD action on the position error (joint space impedance), or PD on the workspace
position error (workspace impedance). Necessary (UAV-)configuration dependent counter gravity torque can
always be supplied through the simple model described in Chapter 5 through nonlinear feedback. Extensive
discussions on joint space and workspace impedance controllers for robotic manipulators and their stability
considerations can be found in [23] and the following three sections are derived from that source. Hence, it
is 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-control
law on the joint angles is a proper candidate for this since by design there is always a unique mapping form
the 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 kr2 . Hence
the robot mass inertia is assumed to be negigable through the use of the gearboxes. This simplifies the control
analysis to one analysis for a single motor as given in Sec. 5.3. The back-EMF and mechanical friction of the
real motors are high. Using their absolute damping gave better results than using back-EMF compensation
with derivative control action. Hence a PI control has been implemented in the control electronics as shown
in Fig. 6.1, where the T&L block relates to the Transformation And Limitation explained in sections 6.5.1
and 6.5.2.

Figure 6.1: PI control scheme. The inertial frame position reference is transformed to a local position reference. This is
subsequently passed through the inverse kinematics (IK) function to get joint angle references. The joint angles in the plant P
are 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

ki

kp umax ccpr
(28 1)22cr
ki umax ccpr
(28 1)22cr 2cr,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 the
motor was not the goal of the work. The motors also saturate much. An elaborate analysis for saturated
actuators 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 for
interaction, since the system will behave as a damped compliance. Any non-modeled, and hence noncompensated, forces are seen as disturbances that might introduce a steady state error. In essence the joint
space impedance controller is a less stiff variant of the position controller without integral action and added
differential gain, hence it needs gravity compensation to approach a zero steady-state error. The gravity
compensation is discussed in Sec. 5.1. The control scheme is a shown in Fig. 6.2, where the T&L block
relates 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 position
reference. This is subsequently passed through the inverse kinematics (IK) function to get joint angle references. The joint
angles in the plant P are position controlled by the controller CPD . Nonlinear feedback to counter-act gravity uses the UAV
orientation 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 constant
is:
kd =

kd umax ccpr
(28 1)22cr

Which 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 not
the goal of the work.

6.3

Workspace impedance control

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

Figure 6.3: Workspace impedance control scheme with nonlinear gravity and dynamics feedback and feedforward acceleration
terms. The inertial frame position reference is transformed to a local position reference. This is subsequently passed through the
p is used to calculate
inverse kinematics (IK) function to get joint angle references and its derivatives. The reference Jacobian J
acceleration torque in the inverse dynamics (ID) block. The joint angles in the plant P are controlled by the workspace controller
CPD which generates a force mapped to the joint space by the Jacobian transpose JT
p . Nonlinear feedback to counter-act gravity
uses the UAV orientation R0 and the measured joint angles, together with
(g()) and centrifugal and Coriolis forces C(, ))
u
the 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)
+ kd q + g(q) = m
A Lyapunov candidate function Rn 7 R and its time derivative are:
V

1 T
q Mq +
2
1 T
q M
q+
2

1 T
e Kp ep
2 p
1 T
q Mq + e Tp Kp ep
2

When one defines:

ep
e p

= pd pm
= p m = Jp q

Filling in yields:
1
1
q q T JTp Kp ep
V = q T M
q + q T M
2
2
Filling in the dynamical model into M
q:
V

1
2C)q q T kd q q T JTp Kp ep
q T ( m g(q)) + q T (M
2


= q T kd q q T m g(q) JTp Kp ep
=

Taking as control law for the motor torque:

m = g(q) + JTp Kp ep + JTp Kdc pm
Makes the derivative of the Lyapunov function negative semi definite, dependent on the joint speed:
V = q T kd q q T JTp Kdc Jp q
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 lateral
position references should generate attitude (angle) references. For completion, this section will briefly discuss
two 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 be
controlled to any state from almost any initial state [17]. This controller is implemented in simulation since
it 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 mode
and 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
Euler angles:

1 s t
= 0
c
0 s /c

relation between the rigid body rotation vector and the

u,0
u,x
c t
u,0
s u,y
= u,0
u
u,0
c /c
u,z

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

gmU AV +kp,z (
z 0 z 0 )+kd,z (z0 z 0 )
c c

= kp, ( ) + kd, ( )

The references for roll and pitch can be generated from a P(I)D controller on the preferred lateral position
and velocity and subsequently controlled by their own PD controller, dependent on the local reference and
error [16]:
u x u ))
= arcsin (kp,x (
xu xu ) + kd,x (x
= arcsin (kp,y (
y u y u ) + kd,y (yu y u ))

Mx = kp, ( ) + kd, ( )
My

6.4.2

= kp, ( ) + kd, ( )

Large Angle Controller

An approach similar to [17] will be applied to stabilize a UAV on SE(3). Stability proof of this control

strategy can be found in [17] and [15]. In shorter notation R = R0u and = u,0
u . Similarly R denotes the
denotes the desired rotational velocity.
desired orientation and
The skew symmetric form of the error in rotation is defined as:
R =
e

1 T
T)
(R R RT R
2

The orientation velocity error as:

e = RT R
38

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

TR
RT R
RT R
= + R

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

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

TR
RT R
Mu = kR eR k e + J J R

(6.1)

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

, R[0, 0, 1]T i
= hkx ex + kv ev + mg[0, 0, 1]T mx
T
= hF, R[0, 0, 1] i

(6.2)

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

F
, F 6= 0
= kFk
1 ))
= ku3c1u1 k (u3c (u3c u

(6.3)

From this the desired rotation matrix and rotational velocity can be formed:
= [u1c , u3c u1c , u3c ]
R
TR

=R

(6.4)

Together, both control laws asymptotically stabilise the UAV position and yaw. As can be seen, this
control law requires proper knowledge of UAV parameters. With the manipulator attached, this might prove
to be difficult. Hence saturated integral control action on the UAV position could be applied. The proof of
stability 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 effector
as flying hand. Therefore it feels natural to have an operator give position and yaw references for the end
effector tip. Therefore UAV reference trajectories have to be generated from the manipulator reference.

6.5.1

UAV offset reference

uee in
Lets assume a constant preferred location of the end effector relative to the UAV, described by vector p
0
ee is given in the inertial frame and
the UAV body frame. The time varying end effector reference trajectory p
is assumed to be smooth. A smooth yaw reference is given by including any offset angle for manipulators
not attached to the UAV front side. The direction in which heading vector u1 points, the UAV reference
position and orientation can hence be specified through:
0 (t) = p
0ee (t) Rz ((t))
x
puee
0
0

(t) = p
ee (t) (t)
x
uz Rz ((t))
puee
0
0
2
+ (t)

(t) = p
ee (t) ((t)
x
uz )
uz Rz ((t))
puee
T
1 (t) = [cos(), sin(), 0]
u
z the skew-symmetric of a z unit vector.
Where Rz is a rotation matrix around local z and u
39

(6.5)

a) Ideal case

b) Realistic case
_

x0

p^ eeu

x0

0
ee

ex
x0

^u
ee

UAV Trajectory

pee0

EE Trajectory

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

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


bee (t) = R0b (t)T p
0ee (t) x0 (t) Rub T pub
p
(6.6)
Since the UAV is underactuated, it will need to tilt to translate. Hence the manipulator will compensate
for 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:
0 i + p
4 = arcsin hu2 , z
Which is the roll angle derived from mapping the UAV y (u2 ) vector onto the inertial z vector (
z0 ). This

ensures 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 is
too unreliable to always do complicated control on the high level. Since no (damped, i.e. through Jacobian
singular 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 reference
R3 through an inverse kinematics mapping on the high level.
b (t) R3 is converted to joint angles (t)
p
ee

From joint angles to end effector position, the mapping is injective and non-surjective from the smaller
joint-position space to the larger end-effector space. Hence the reverse map from the end-effector space to the
joint-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 workspace
when it lies outside it. In this manner the manipulator will always attempt to point towards the direction it
should go to, which is convenient since the absence of step responses or erratic behavior lowers the transient
influence 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 :
)
2 
2 
2

y oy,i
z oz,i
x ox,i
+
+
=1
sx,i
sy,i
sz,i

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 Cartesian
radii. Any point in the base frame pb = [pbx , pby , pbz ]T can be expressed in ellipsoid-i space (Si ), by the
following transformation:
pSi = (pb oi ) si

Where denotes elementwise division. If kpSi k < 1 the point lies inside the sphere Si . If a point does not
lie 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 outside
the 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 defined
by these two points can be described by a Pl
ucker-line LCp :


LCp = pSi CSi : pSi CSi = {U : V}
Since any vector end-point q on the Pl
ucker-line must obey the following double rank deficient system of
equations:

0
Uz Uy Vx 

Uz
0
Ux Vy

q =0
Uy Ux
0
Vz 1
Vx
Vy
Vz
0
This holds due to orthogonality of U and V and the location of the point on the line. The above equations
enables us to express the x- and y-coordinates of the point q in its z-coordinate. Filling this in into a
homogeneous sphere equation, it is reduced to a quadratic equation, which gives the two solutions of the
z-coordinate of the point on the sphere where the line intersects the sphere. The quadratic equation to solve
for this coordinate for a unit sphere is:
0
qy
qx

= kUk2 qz2 + 2(Uy Vx Ux Vy )qz + Vx2 + V y 2 Uz2

U
Vx
, Uz 6= 0
= Uyz qz + U
z
V
Ux
= Uz qz Uyz , Uz 6= 0

There are two

for q and the proper solution for the projection is given by the qj , j = 1, 2 that
 solutions
1
2
belongs to min hq , Ui, hq , Ui , i.e. the vector that points from the workspace center of mass in the proper
direction towards pSi . The proper projected qj = qSi can be transformed back to the base frame:
qb = qSi si + oi
Where denotes elementwise multiplication.
The total limitation algorithm (shown in Matlab code in Appendix B.1) is set up as follows:
bee is inside S1 .
1. Check if the reference p
(a) If yes, do nothing and continue.
(b) If no, project the reference onto S1 and continue.
1
i. If CzS1 = pSee,z
, then Uz = 0. In this case

r

2
T
1 ,p
1
S
[hpSee,x
S1
ee,y ]
iT
1 Cz

S1
1 ,p
q = k pSee,x

ee,y
k
S1
Cz
41

2. Check if the resultant point q of the previous step, either inside S1 or on its boundary, inside Si , i =
2, 3, 4.
(a) If yes, calculate for every ellipsoid in where q lies inside, the projected point q0i onto the proper
Si . From all candidates, take the solution with the smallest kCb q0i k. The algorithm is finished.

(b) If no, the point q is inside the workspace or already properly projected. The algorithm is finished.

42

Chapter 7

Simulation Results
This chapter covers the simulation results of the used models described in 5. These models were used to
assess whether the whole idea of interaction with such a manipulator on a UAV was doable before moving on
and trying it in practice. The simple model and multi-body model were both used for dimensioning and to
check the validity of the design and several algorithms. The first sections will describe some results of stand
alone interaction with the three state equation model and multi-body model. The last sections will focus on
in flight tests on a quadrotor UAV and ducted fan UAV. This has been done to show that the system works
on multiple UAV platforms. Inertial frame end-effector trajectory tracking is not presented here, since the
ideal simulation results are not comparable to the real set-up.

7.1

Simple model

Figure 7.1: Simulation Results of the Simple Model. In the top left the reference for the local z-coordinate and its measured
value are given. In the top right the deviation from the ideal trajectory due to the UAV movement. The modeled interaction
force and necessary motor torque are stated in the middle and bottom plot.

The simple model was implemented in 20SIM in code. Some results of the model are shown in Fig. 7.1
where a wall at z = 0.095 m blocks a sinusoid reference movement only in local z for the end-effector, i.e.
43

the local x- and y-directions are stationary. The UAV rotates the base around local y-direction with angle
= /9 sin (2t) and movement in x-direction p0b,x = 0.05 sin (2t).
Workspace impedance control is modeled without compensation for the UAV movement, which leads to
a 1 cm compression and hence 5 N interaction force with maximally 1.25 mNm of motor torque, which fits
within design specifications. The simulated control scheme is that of Fig. 6.3 described in Chapter 6. The
UAV movement, relatively aggressive compared to real UAVs, is seen as a mild influence on end effector
position error and on the motor torque.

7.2

Manipulator attached to a Ducted Fan UAV

The Bologna Ducted Fan vehicle was modeled, having a weight of 1.567 kg, a propeller lift constant of
kl = 7.6 10 5 Ns2 rad2 , control vanes at distance d = 0.2075 m with a distance between the center of
pressure dT = 0.16 m [6][7]. The manipulator was attached to one of the UAV sides with a static counter
mass at the opposite side. Interaction forces are only modeled for the end effector sensor with the HuntCrossley model described in Sec. 5.5.5. Two steps in the simulation progression can be seen in Fig. 7.2, the
approach maneuver and the actual touching.
The UAV is controlled with the large angle controller explained in Sec. 6.4.2 with first order propeller
behavior and small integral action on the position error. Gyroscopic forces are taken into account in the
model, not in the control of the UAV. The manipulator is kept stationary under joint space impedance
control. This was due to the fact that inverse dynamics control resulted in long simulations times, which
were > 100 hours.
Low pass filtered Gaussian white noise is used as disturbance force (not torque) on the UAV to simulate
random airflow and changes in air density. A wall to interact with is relatively 1.5 meter away from the
inertial frame origin. The UAV lifts off and its global x-reference is moved such that the manipulator touches
the surface for 4.5 seconds, as shown in Fig. 7.4. The resulting simulated interaction force is shown in Fig. 7.3.

Figure 7.2: Left) Wall approach of the Ducted Fan UAV with manipulator, showing the nearest interaction point on the wall
with the coordinate frame. Right) Ducted Fan UAV pushing against the wall with impedance controlled manipulator. Notice
the sensor compression and gimbal adaption.

Figure 7.3: Interaction force normal to the surface during environmental touching in the Ducted Fan UAV model. Peaks at
the start and end of the contact are due to bouncing and large damping values on the contact surface. Wavy behavior is due
to disturbance forces acting on the Ducted Fan UAV.

44

Figure 7.4: Ducted Fan UAV x-reference and measured position. Notice that during touching the desired location can not be
reached, leading to the interaction force shown in Fig. 7.6

The interaction force is in the order of 1.5 to 2 N, which is good for the task. The current model gives a
good impression that proper interaction for NDT measurements is possible with a Ducted Fan UAV.

7.3

Manipulator attached to a Quadrotor AUV

Figure 7.5: Left) Wall approach of the Quadrotor UAV with manipulator, showing the nearest interaction point on the wall
with the coordinate frame. Right) Quadrotor UAV pushing against the wall with impedance controlled manipulator. Notice
the sensor compression and gimbal adaption.

Figure 7.6: Interaction force normal to the surface during environmental touching in the Quadrotor model. Peaks at the
start and end of the contact are due to bouncing and large damping values on the contact surface. Wavy behavior is due to
disturbance forces acting on the Quadrotor.

The Asctec Pelican [8] was modeled by approximation, having a battery free 900 grams mass, a lift
constant of kl = 1 10 5 Ns2 rad2 [25] and the distance from the Quadrotor center to the propeller of
d = 0.225 m. The manipulator was attached to one of the diagonal UAV sides with a counter mass at the
opposite side. For this reason the UAV effectively flies under a yaw angle of 45 . Interaction is modeled in
the same way as for the Ducted Fan UAV. Two steps in the simulation progression can be seen in Fig. 7.5,
the approach maneuver and the actual touching.
45

Figure 7.7: Quadrotor UAV x-reference and measured position. Notice that during touching the desired location can not be
reached, leading to the interaction force shown in Fig. 7.6

Similar as in the Ducted Fan UAV experiment, the manipulator is kept stationary, but under inverse
dynamics workspace impedance control. The UAV is controlled with the large angle controller, with first
order propeller behavior and small integral action on the position error without taking into account any
gyroscopic effects. The same low frequency distrubances and the same references are used as for the Ducted
Fan UAV shown in Fig. 7.7. The resulting modeled interaction force is shown in Fig. 7.6.
The interaction force is in the order of 1.5 to 2 N, similar to results from the Ducted Fan UAV interaction.
The current model gives a good impression that proper interaction for NDT measurements is possible with
a Quadrotor UAV.

46

Chapter 8

Experimental Results
This chapter will cover results from carried out experiments of a stand alone manipulator and a manipulator
attached to the UAVs. In-flight movement and interaction have been done with the real setup on both a
Ducted Fan and on a Quadrotor UAV. Custom bases have been created to attach the manipulator to the
UAV side. The weight of the manipulator system has been statically balanced by counterweight in the
form of control electronics, battery shifting and placement cabling. In this chapter the stand alone tracking
performance and in flight tracking and interaction performance are discussed. The results of the ducted fan
experiments are those from IW2 in Bologna and are the similar as described in project deliverable D6.2. The
Quadrotor experiments were carried out at the UT and are hence interpreted differently.

8.1

Manipulator stand-alone Performance

This experiment presents the results on the position control and the joint space impedance control applied
to the stand-alone manipulator by means of an analytical reference. The manipulator was directly connected
to a laptop over USB, lowering latency compared to communication in free flight.
T

The manipulator is moved to a local reference position of [0, 0, 0.07] . From there it performs a full
sinusoid period of 2.5 seconds with an amplitude of 3 cm for each of the three Cartesian directions. The
results of position tracking for the (joint space) position control and for the joint space impedance control
with gravity compensation can be seen in respectively Fig. 8.1.Left and Fig. 8.1.Right and are described in
the manipulator local frame (therefore denoted by superscript b).
The position tracking presented in Fig. 8.1 is a bit delayed (variable, up to 100 ms in the worst case) due to
communication and processing, but the tracking is good. The impedance tracking uses lower feedback gains,
hence the performance is good, but worse compared to the stiff position tracking presented in Fig. 8.1.Left.
The position error is estimated to be in the range of one millimeter, estimated through the Jacobian and
known joint angle resolution.
A feature of the joint space impedance control strategy is that is allows for compliant interaction. Hence
during the impedance control mode, a weight of 500 grams was put onto an upright manipulator. This
introduced a 4.9 Newton force in local positive z-direction. Due to the force on the end-effector, the joints of
the manipulator are deflected. The value of 500 grams was chosen since it resembles the maximum interaction
force the UAVs can apply. The results are shown in Fig. 8.2.
The manipulator was moved to a different location between the two instances the weight was added to
the end-effector. The deflection in the first position was 8.9 mm and 11.1 mm in the second position. Due
to the fact the impedance was in the joint space the impedance in the workspace is non-constant. The first
position was stiffer due a more stretched out manipulator configuration closer to singularity, i.e. more force
was hence transmitted by the mechanical structure and not by the impedance controlled motors.
47

Figure 8.1: Left) Joint space position control tracking results, with end-effector position calculated through forward kinematics.
Right) Joint space impedance position control tracking results, with end effector position calculated through forward kinematics.
Both reference (added subscript d) and measured values for the Cartesian coordinates are given.

Figure 8.2: Joint space position control tracking results. At the location of the vertical dotted lines a weight of 500 grams
was added or removed from to the end-effector.

8.2

Manipulator attached to Ducted Fan UAV

The manipulator was attached to a Ducted Fan UAV developed by the CASY at the University of Bologna [7].
The UAV and manipulator end-effector are tracked by reflective markers by an OptiTrack system as can be
seen in both photos in 8.3.

8.2.1

Free-flight with position control of the manipulator

This experiment presents the results of keeping the manipulator at a desired position, while the aerial vehicles
are moving. The manipulator joint-space position controller makes it possible for the end-effector to track
a position in the manipulator local frame. To do this an inverse kinematics function is used to transform
end effector position references to joint position references. The local motor angle references are afterwards
smoothed by a first order 3 Hz low-pass filter to accommodate smooth, i.e. no staircasing, high gain feedback
behavior. The low level controllers control the joint position by means of the PI feedback loop.
There exists a homogeneous coordinate transformation from the base frame to the inertial frame and vice
versa to transform manipulator references.
48

Figure 8.3: Left) Manipulator placed on Ducted Fan AUV, no markers attached to the manipulator EE. Right) Used OptiTrack
Camera

The relative orientation and position offset of the manipulator base origin with with respect to the UAV
origin have been estimated by using a visual tracker system.
A function was implemented such that when a joypad button is pressed, the current inertial end-effector
coordinate is saved. The UAV pose is calculated by IMU and/or an external tracker. The saved reference
can hence be calculated (i.e. estimated) to the base frame and is used as a reference until the button is
released. As a consequence the end-effector will track a point in the inertial frame as long as that button is
pressed. An external tracker measured the end effector position merely for evaluation purposes.
The experiment was carried out in two different settings: once during hand held movement of the UAV,
and once during free flight movement of the UAV. Fig. 8.4 shows the position of the UAV COG and the
position of the end-effector, both described in the inertial frame (therefore denoted by superscript 0). The
standard deviation in the UAV and End-effector position (during a phase in the experiment where the
manipulator is inside its local workspace) are shown in the table in Fig. 8.4. It can be seen that the
end-effector compensates for the UAV movement. Small errors are due to delays in communication, which
introduce some lag, and due to the fact that the UAV to manipulator coordinate frame transformation was
estimated. The resultant behavior is as good expected and fits the design criteria. With a well functioning
UAV controller any desired trajectory in space can be properly tracked.

x [mm]
y [mm]
z [mm]

UAV
11.0
15.2
19.3

EE
2.3
2.4
3.6

Figure 8.4: Left) Results of the end-effector position tracking during hand held UAV movement. The vertical dotted lines
respectively indicate the moment of starting and stopping the inertial frame position tracking. Right) Standard deviations in
inertial frame directions for hand held UAV movement.

49

The same results for an in-flight test are shown in Fig. 8.5. The manipulator again attempted to compensate for UAV movement. It can be seen that deviations in movement for the end-effector during hand
held movement of the UAV are lower than for the UAV in free flight. Delays in communication and a too
sensitive (in the sense of poor disturbance rejection) UAV both contribute to deviations in the end-effector
position. The UAV reacted quite strongly to manipulator movement and drifted away. The manipulator
afterwards moved most of the time outside its local workspace.

x [mm]
y [mm]
z [mm]

UAV
48.9
55.7
9.3

EE
27.6
35.9
8.5

Figure 8.5: Left) Results of the end-effector position tracking during free flight UAV movement. The vertical dotted lines
respectively indicate the moment of starting and stopping the inertial frame position tracking. Right) Standard deviations in
inertial frame directions for free flight UAV movement.

The UAV controller was so sensitive and not well tuned, such that it is difficult to assess the performance
quality during free flight. Still the hand-held experiment suggests that with proper UAV position control
(or a different UAV) any location on a surface could potentially be properly tracked to do a measurement.

8.2.2

Interaction During Flight

This experiment presents the results of interaction with a vertical wall through the manipulator attached to
the miniature ducted fan UAV, as in Fig. 8.6.Right. The manipulator is joint space impedance controlled
and the aerial vehicle was position controlled by means of a PID control. The force on the environment was
estimated through the measured joint errors and known control data and is shown in Fig. 8.6.Left.

Figure 8.6: Left) The estimated interaction force when the Ducted Fan UAV is in contact with a vertical surface. Right)
Ducted fan and manipulator in interaction with a vertical surface. Notice attached trackable markers.

50

The estimated contact force is in the order of 2 N and agrees with results from the dynamical models
presented in Chapter 7, which is very good for taking NDT measurements. Due to a conservative estimation
of the interaction force, the real interaction force is most likely higher, which is a good result. The contact
was proper, i.e. the sensor did not bounce much after some transient behavior and did not lose contact with
the wall. It is most likely that one is able to do proper NDT measurements with this setup.

8.3

Manipulator attached to Quadrotor

The manipulator was also attached to an AscTec Pelican Quadrotor UAV as shown in Fig. 8.7. It was
attached on one of the long sides of the UAV, such that the UAV had to fly with 45 yaw. The Quadrotor
pose was tracked by a VisualEyez tracker system with active trackables. There was no trackable on the
end-effector. The control scheme is the one as shown in Fig. 4.6. As with the ducted fan, the the vehicle was
PID controlled. The autopilot attitude control has an outer control running a position controller running as
a ROS-node which was developed by a third party.
Since the end-effector position in the inertial frame was not measured in the current set-up, any endeffector position can only be estimated through the local end-effector position estimation, UAV pose estimation and relative coordinate transformation estimation.

Figure 8.7: Quadrotor Pelican UAV with onboard manipulator and vision trackables.

8.3.1

Free-flight With Position Control of the Manipulator

HOVERING

TRACKING 1

X[m]

0.65

HOVERING

HOVERING

TRACKING 2

0.6

0.55

10

20

30

40

50

60

manipulator
virtual tool

Y [m]

0.05
0
-0.05
-0.1
-0.15

10

20

30

40

50

60

Z[m]

0.58

manipulator
virtual tool

0.56
0.54
0.52
0.5

10

20

30

40

50

ex
(mm)
-0.18
0.92
x
(mm)
3.9
16

ey
(mm)
0.34
4.9
y
(mm)
7.7
28

ez
(mm)
0.97
-15
z
(mm)
6.2
8.0

k
ek
(mm)
9.3
34
kek
(mm)
5.3
14

60

T ime[s]

Figure 8.8: Left) Two inertial frame tracking experiments, notice that the measured position (blue) is better than the virtual
tool position (dotted red). Right) Evaluation of the tracking errors with the manipulator and a virtual fixed tool during the
first tracking experiment shown in the left figure. The e denotes mean error of a vector element of an error vector e. The s
denote standard deviation of the measurements.

51

Inertial frame tracking experiments have been carried out on the Quadrotor platform. The tracking
performance is evaluated by comparing the estimated inertial frame position of the end-effector compared
to that of a virtual fixed tool (a stick) that would be placed in that same location as the manipulator on
the UAV. In Fig. 8.8 and its table on the right, it can be seen that the manipulator compensates a lot for
vehicle movement during both tracking moments.
Since the Quadrotor is directly controllable in z-direction and compensates a lot, the loop necessary
for calculating the new manipulator reference is not fast enough and a 3 Hz low pass filter to smooth the
reference make the manipulator too slow to properly cope with these disturbances as seen in Fig. 8.9.
TRACKING 1

HOVERING

0.06

HOVERING

TRACKING 2

HOVERING

px
py
pz
-0.05

Zee

Xee , Yee

0.03

0
-0.08

-0.03

-0.06

10

20

30

T ime[s]

40

50

-0.11
60

Figure 8.9: Local manipulator end-effector position during tracking. Units in meters.

8.3.2

Combined UAV and manipulator inertial frame trajectory tracking

The inertial frame tracking performance of the end-effector was tested on the Pelican Quadrotor. The ana e e0 = [0, 0.15 cos (2t/30), 0.5 +
lytical inertial frame reference generated for the end-effector was given by p
T
0.15 sin (2t/30)] with an offset between UAV and manipulator expressed in the inertial frame (see Sec. 6.5.1)
uee = [0.42, 0.03, 0.01]T . The tracking results are shown in Fig. 8.10. The end-effector position
given by p
is compared to a fixed virtual tool attached at a point where the manipulator would be extended to 42 cm
from the UAV center of mass.
ff

Figure 8.10: Quadrotor End-Effector Inertial Frame Reference Tracking. The end-effector position is compared to a fixed
virtual tool attached at a point where the manipulator would be extended to 42 cm from the UAV center of mass.

The standard deviation of the position errors in all the Cartesian inertial frame directions are shown in
Table 8.1, showing reduction of the position error, compared to the virtual tool.
The manipulator tracks the inertial reference quite well regarding the relatively poor tracker system
performance. The usefulness of a manipulation system on a UAV platform is shown by the fact that the
manipulator system can move to locations in the inertial frame which a rigid fixed tool would not be able
to. Tracking performance can be greatly improved by a better tracking system and/or pose estimation, not
sensitive to occluded markers.
52

ex
(mm)
6.0
19
-2.1
x
(mm)
7.8
16
15

manipulator
virtual tool
UAV

manipulator
virtual tool
UAV

ey
(mm)
-3.8
17
-5.6
y
(mm)
10
27
20

ez
(mm)
-0.75
-4.2
-0.21
z
(mm)
6.3
14
13

k
ek
(mm)
13
41
28
kek
(mm)
9.3
13
10

Table 8.1: Statistics of the inertial frame trajectory tracking. The e denotes mean error of a vector element of an error vector
e. The s denote standard deviation of the measurements.

8.3.3

Interaction During Flight

This section presents an interaction experiment during Quadrotor flight. Using an independent joint level
impedance control the UAV, position controlled, suddenly impacts a wall around xw = 1.2 m from the origin
of the inertial frame. Proper contact can be established during flight as is seen in Fig. 8.11 and Fig. 8.12.

Figure 8.11: Quadrotor UAV in contact through manipulator

0.2
1

x 10

0.18

CONTACT 2

CONTACT 1

-3

px
py
pz

-0.075

0.16
0.14

Xee , Yee

Y [m]

0.12
0.1
0.08
0.06
0.04
0.02
0
-0.2

-5

-0.078

Zee

-2

-8

end-effector
UAV

-0.081

WALL
0

0.2

0.4

0.6

X[m]

0.8

1.2

-11
0

1.4

10

20

30
T ime[s]

40

50

60

Figure 8.12: Left) Top view of the flight area, where the UAV and manipulator positions are shown. During contact the
manipulator is stationary. Right) Local frame end-effector position during interaction, showing that the joint space impedance
controller allows for proper compression.

As can be seen in Fig. 8.12.Right the manipulator is moved due to interaction.

The estimated contact force resulting from the use of an impedance controller during interaction is shown
in Fig. 8.13.Left. The UAV tilt can be seen in Fig. 8.13.Right,showing that both roll and pitch contribute
to generating an interaction force.
The estimated contact force is in the order of 2 N, similar to the results achieved with the Ducted Fan
and very similar to simulation results. The contact was proper and long, i.e. the sensor did not bounce
53

CONTACT 2

CONTACT 1
6

Y aw 45

[ ]

-2

-4

-6

-8

Roll
P itch
0

10

20

30

T ime[s]

40

50

60

Figure 8.13: Left) The estimated interaction force when the Quadrotor UAV is in contact with a vertical surface. The offset
after t = 33 s is due to the static friction in the manipulator joints and due to the fact that it is an estimation. Right) UAV
Euler angles during interaction experiment. Notice that the yaw is 45 and hence both roll and pitch contribute to generating
an interaction force. The increase in angle is due to the integral action on the UAV position.

much and did not lose contact with the wall. Hence it is most likely that one is able to do proper NDT
measurements with this setup.

54

Chapter 9

Conclusions and Future Work

This report demonstrated the concept, analysis and first prototype of the 3-DOF manipulator and (3 DOF)
sensor system for NDT to be placed on UAVs. A small, lightweight (around 190 grams) but versatile design
has been proposed which meets the set requirements. Delta parallel kinematics provide a fast and robust
manipulator that can perform Cartesian movement to compensate for UAV dynamics and place sensors and
objects at preferred locations. The NDT end-effector corrects its orientation to the normal of the interaction
surface to have proper placement of the ultrasonic sensor. Kinematic and dynamical models have been
proposed to dimension the manipulator according to the requirements and to be used in proper versatile
control strategies.
Flight tests have shown the validity and versatility of the system, even though through time constraints
the vehicle control was still completely separate from the manipulator control. Experimental data verifies the
usefulness of the developed models for evaluation and control, since interaction forces are around 2 Newton
in all cases. This interaction force is proper to do good NDT measurements.
During experiments the control of the UAV and manipulator was still separate. The logical step is to
approach a control strategy where the UAV does most of the pushing or counteracts the manipulator pushing
force, using information about interaction forces between the UAV and manipulator. A 6D force and torque
sensor under development could be properly applied here.
A decoupled workspace controller for the manipulator could rigidly track a plane of the wall in the inertial
frame, while being compliant or force controlled in the interaction direction. In this way the current method
of interaction can be improved to ensuring contact more close to a desired point. More elaborate control
strategies mentioned in the report could be properly analyzed, implemented and tested on the real setups
for better control over interaction, movement, force and system dynamics.
Further development could involve testing of new end-effector modules, i.e. the use of cameras, cleaning
equipment for visual inspection and to clean dirty surfaces, grippers and magnetic switches. An ultrasonic
couplant liquid or gel supply could be added to make the system more autonomous and to be able to do
significantly more measurements per flight.

55

56

Bibliography
[1] A. Albers, S. Trautmann, T. Howard, T. Nguyen, M. Frietsch, and C. Sauter, Semi-autonomous flying
robot for physical interaction with environment, in in Proceedings of the IEEE International Conference
on Robotics Automation and Mechatronics, 2010.
[2] D. Mellinger, M. Shomin, N. Michael, and V. Kumar, Cooperative grasping and transport using
multiple quadrotors, in Proceedings of the International Symposium on Distributed Autonomous Robotic
Systems, 2010.
[3] AIRobots. [Online]. Available: http://www.airobots.eu
[4] P. Pounds, D. Bersak, and A. Dollar, Grasping from the air: hovering capture and load stability, in
Proceedings of the IEEE International Conference on Robotics and Automation, 2010.
[5] A. Keemink, Conceptual investigations on a manipulator system for inspection uavs, 2011.
[6] A. Mersha, Modeling and robust control of an unmanned aerial vehicle, 2010.
[7] R. Naldi, L. Gentili, L. Marconi, and A. Sala, Design and experimental validation of a nonlinear control
law for a ducted-fan miniature aerial vehicle, Control Engineering Practice, vol. 18, no. 7, pp. 747760,
2010.
[8] Asctec pelican-3 product website. [Online]. Available: http://www.asctec.de/asctec-pelican-3/
[9] F. Pierrot, C. Reynaud, and A. Fournier, Delta: a simple and efficient parallel robot, Robotica, vol. 8,
no. 1, pp. 105109, 1990.
[10] P. Zsombor-Murray, Descriptive geometric kinematic analysis of Clavels Delta robot, 2003.
[11] A. Codourey, Dynamic modeling of parallel robots for computed-torque control implementation, The
International Journal of Robotics Research, vol. 17, no. 12, pp. 13251336, 1998.
[12] Robotic operation system documentation. [Online]. Available: http://www.ros.org/wiki/
[13] Yet another robotic platform documentation. [Online]. Available: http://eris.liralab.it/yarp/
[14] R. Murray, Z. Li, and S. Sastry, A mathematical introduction to robotic manipulation.
Ranton, 1993.

CRC, Boca

[15] D. Mellinger and V. Kumar, Minimum snap trajectory generation and control for quadrotors, in
Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011, pp. 2520
2525.
[16] B. Erginer and E. Altug, Modeling and PD control of a quadrotor VTOL vehicle, in Intelligent
Vehicles Symposium, 2007 IEEE. IEEE, 2007, pp. 894899.
[17] T. Lee, M. Leoky, and N. McClamroch, Geometric tracking control of a quadrotor UAV on SE(3), in
Decision and Control (CDC), 2010 49th IEEE Conference on, dec. 2010, pp. 5420 5425.
57

[18] T. Bresciani, Modelling, identification and control of a quadrotor helicopter. Department of Automatic
Control, Lund University, 2008.
[19] P. Breedveld, Modeling and simulation of dynamic systems using bond graphs, In Control Systems,
Robotics and Automation, from Encyclopedia of Life Support Systems (EOLSS), Developed under the
Auspices of the UNESCO. EOLSS Publishers, pp. 136, 2008.
[20] S. Stramigioli, Modeling and IPC control of interactive mechanical systems: a coordinate-free approach.
Springer Verlag, 2001, vol. 266.
[21] V. Duindam, Port-based modeling and control for efficient bipedal walking robots, 2006.
[22] N. Diolaiti, C. Melchiorri, and S. Stramigioli, Contact impedance estimation for robotic systems,
Robotics, IEEE Transactions on, vol. 21, no. 5, pp. 925935, 2005.
[23] B. Siciliano, L. Sciavicco, and L. Villani, Robotics: modelling, planning and control.
2009.

Springer Verlag,

[24] A. Zavala-Rio and V. Santibanez, Simple extensions of the pd-with-gravity-compensation control law
for robot manipulators with bounded inputs, Control Systems Technology, IEEE Transactions on,
vol. 14, no. 5, pp. 958 965, sept. 2006.
[25] V. Kumar and N. Michael, Opportunities and challenges with autonomous micro aerial vehicles.

58

Appendix A

Extra Control Strategies

This Appendix covers a section on combined control and reference generation that were not implemented
but might prove to be very powerful when applied to the total system.

A.1

Virtual UAV Jacobian pseudo-inverse reference generation

The approach presented below has not yet been tested in practice or in models. Future research should point
out whether it is useful in full system control.
The main goal of this section is to derive a full controller of the UAV or a virtual UAV from which
references are derived. The UAV is seen as a fully actuated 6D joint such that the total redundant system
can be controlled and finally mapped with the Jacobian pseudo-inverse into the real control signals or to
those of a virtual vehicle.
Assuming the UAV as a rigid body that is fully actuated, such that the controlled degrees of freedom of
the UAV are related to the globally defined twist of the UAV T0,0
u :
q u
T0,0
u

= [x , y , z , vx , vy , vz ]T
= Ju q u = AdH0u q u

The twist of the manipulator end effector, expressed in the inertial frame can be described as:
T0,0
ee

0,u
= T0,0
u + Tee
= Ju q u + AdH0u Tu,u
ee


I3
033
= Ju q u + AdH0u
033
Jp

 

 q u
R0u
Ju Jm
=
=
0 R0u
x
{z
}
|

03

033
R0u

033
R0u Jp



q u

69

= Js q s

This would imply a redundant system. Assuming there are a spring and dampener attached to a point
0,0 , the virtual kineostatic control wrench W0,0 acting on the end effector tip can be
with reference twist T
ee
c
modelled by a spring dampener (or PD) in 6D:
Z t




0,0 (t) T0,0 (t) + kp
0,0 ( ) T0,0 ( ) d
Wc0,0 (t) = kd T
T
ee
ee
ee
ee
0

This virtual control wrench can be mapped to the generalized forces of the system:
f0

[Mxu , Myu , Mzu , fxu , fyu , fzu , 1b , 2b , 3b ]T = [hT , T ]

= JTs Wc0,0 (t)

59

Still, the system is not redundant but redundant and underactuated. Hence the virtual joint forces of
the UAV have to be mapped to the real UAV joint forces = [f1 , f2 , f3 , f4 ]T for the Quadrotor for example.
Inversion comes down to using the left pseudo-inverse for a least squares approximation:
h = C
(CT C)1 CT h
Such that:

A.2


=

(CT C)1 CT
036

043
I3



Decentralized combined control of manipulator and UAV

This section will give an idea about how elaborated combined control can be achieved in another way,
compared to Sec. A.1. The main idea is that the UAV and manipulator will use a 6D force-torque (FT)
sensor between them at the manipulator base. The UAV will try to control the wrench felt at its center of
mass to zero.

Figure A.1: Total control scheme of UAV and manipulator combined. Notice the force controller, UAV attitude controller,
UAV position controller and manipulator workspace impedance controller. The system is still described as two separate plants.
Block describes all dynamic feedback torques due to base (UAV) movement, gravity and Coriolis and centrifugal forces.

Under the assumption no free flight (i.e. minimal UAV acceleration) the UAV should be able to be
controlled in such a way it behaves as a fixed base. The large angle controller is assumed for the UAV and
is hence shown in Fig. A.1.
For a normal robotic manipulator, the fixed world generates according to Newtons third law proper
reaction forces and moments at the attachment point due to its immense inertia. When the manipulator
base is in flight, this base has to actively generate these virtual-reaction forces. Assume the FT sensor
T
b T T
b
= [ bm , fm
] at the manipulator base. This wrench has to
measures a gravity compensated wrench Wm
be transformed to the UAV center of mass. When assuming a base frame with orientation Rub relative to
the UAV frame, with an offset (expressed in the UAV frame) of pub . The measured (subscript m) torque and
force felt at the UAV center of mass are:
um
u
fm

b
= Rub bm + pub fm
u b
= Rb fm

(A.1)

Still, as with any controlled manipulator, the force can not be set independently of position if the directions
of force and position control are not decoupled. Hence an outer force P(I) control loop can be constructed
that should control the calculated wrench at the UAV COM to 0 N/Nm as seen in Fig. A.1.

60

Appendix B

Relevant Code
B.1
1
2
3
4
5
6
7

Limit to workspace matlab code

function
e0 =
q0 =
ei =
qi =
Rr =
Rl =

Pp = LimitToWorkspace(P,C)
[0.062;0.062;0.0685];
[0;0;0.0477];
[0.068;0.078;0.063];
[0;0.063;0.0015];
[0.5 sqrt(3)/2 0; sqrt(3)/2 0.5 0; 0 0 1];
[0.5 sqrt(3)/2 0; sqrt(3)/2 0.5 0; 0 0 1];

8
9
10

Cqs = (Cq0)./e0;
Pqs = (Pq0)./e0;

11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41

if abs(abs(P(3))abs(C(3))) < 1e5 % in plane U(3) = 0

if (norm(Pqs) > 1) % if outside
Q = [Pqs(1);Pqs(2)];
Q = Q/norm(Q)*sqrt(1Cqs(3)2);
Pp = [Q;Cqs(3)].*e0+q0;
else
Pp = P; %if inside do nothing
end
else
%Express all in q0, and project onto q0
I = Pqs;
if (norm(Pqs) > 1)
U = PqsCqs;
V = cross(Pqs,Cqs);
abc = [norm(U)2 2*(U(2)*V(1)U(1)*V(2)) V(1)2+V(2)2U(3)2];
z1 = (abc(2)+sqrt(abs(abc(2)24*abc(1)*abc(3))))/(2*abc(1));%z(1);
z2 = (abc(2)sqrt(abs(abc(2)24*abc(1)*abc(3))))/(2*abc(1));%z(2);
y1 = U(2)/U(3)*z1+V(1)/U(3);
y2 = U(2)/U(3)*z2+V(1)/U(3);
x1 = U(1)/U(3)*z1V(2)/U(3);
x2 = U(1)/U(3)*z2V(2)/U(3);
I1 = [x1;y1;z1];
I2 = [x2;y2;z2];
I = I2;
if dot(I1,U) > 0
I = I1;
end
end
%scale back
I = I.*e0+q0;

42
43
44

%Check whether inside other spheres and project on those if necessary

chk = 0;

61

for i = 1:3
Pqi = (Rr(i1)*Iqi)./ei;
chk = chk + (i*21)*double(norm(Pqi)<1);
end

45
46
47
48
49

Ppt = zeros(3,3);
val = 0; ind = 0; va = 0; vb = 0; vc = 0;
switch(chk)
case 1 %1
Pp = getProjectedPoint(C,I,ei,qi); %in the inertial space
case 3 %2
Pp = Rl*getProjectedPoint(C,Rr*I,ei,qi);
case 5 %3
Pp = Rr*getProjectedPoint(C,Rl*I,ei,qi);
case 4 %1 2
Ppt(:,1) = getProjectedPoint(C,I,ei,qi);
Ppt(:,2) = Rl*getProjectedPoint(C,Rr*I,ei,qi);
va = norm(CPpt(:,1));
vb = norm(CPpt(:,2));
[val,ind] = min([va vb]);
Pp = (Ppt(:,ind));
case 6 %1 3
Ppt(:,1) = getProjectedPoint(C,I,ei,qi);
Ppt(:,2) = Rr*getProjectedPoint(C,Rl*I,ei,qi);
va = norm(CPpt(:,1));
vb = norm(CPpt(:,2));
[val,ind] = min([va vb]);
Pp = (Ppt(:,ind));
case 8 %2 3
Ppt(:,1) = Rl*getProjectedPoint(C,Rr*I,ei,qi);
Ppt(:,2) = Rr*getProjectedPoint(C,Rl*I,ei,qi);
va = norm(CPpt(:,1));
vb = norm(CPpt(:,2));
[val,ind] = min([va vb]);
Pp = (Ppt(:,ind));
case 9 %1 %2 %3
Ppt(:,1) = getProjectedPoint(C,I,ei,qi);
Ppt(:,2) = Rl*getProjectedPoint(C,Rr*I,ei,qi);
Ppt(:,3) = Rr*getProjectedPoint(C,Rl*I,ei,qi);
va = norm(CPpt(:,1));
vb = norm(CPpt(:,2));
vc = norm(CPpt(:,3));
[val,ind] = min([va vb vc]);
Pp = (Ppt(:,ind));
otherwise %inside
Pp = I;
end
end

50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

end

function [Q] = getProjectedPoint(C,P,e,q)

Cq = (Cq)./e;
Pq = (Pq)./e;
U = PqCq;
V = cross(Pq,Cq);
abc = [norm(U)2 2*(U(2)*V(1)U(1)*V(2)) V(1)2+V(2)2U(3)2];
z1 = (abc(2)+sqrt(abs(abc(2)24*abc(1)*abc(3))))/(2*abc(1));%z(1);
z2 = (abc(2)sqrt(abs(abc(2)24*abc(1)*abc(3))))/(2*abc(1));%z(2);
y1 = U(2)/U(3)*z1+V(1)/U(3);
y2 = U(2)/U(3)*z2+V(1)/U(3);
x1 = U(1)/U(3)*z1V(2)/U(3);
x2 = U(1)/U(3)*z2V(2)/U(3);
I1 = [x1;y1;z1];
norm(I1);
I2 = [x2;y2;z2];

62

norm(I2);
if dot(I1,U) < 0
I = I1;
else
I = I2;
end
Q = I.*e+q;

16
17
18
19
20
21
22
23

end

B.2
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

Inverse kinematics matlab code

function [theta, theta dot, theta ddot, J p, J p dot , solved] ...

= IK(p, p dot, p ddot, theta previous)
%Analytical Forward Kinematics: EE position to actuator angles
%Input variables
%p = EE origin position
%p dot = EE origin velocity
%p ddot = EE origin acceleration
%theta previous = previous angles
%
%Output Variables:
%theta = actuator angles [rad]
%theta dot = actuator angles derivative [rad]
%theta ddot = actuator angles second derivative [rad]
%J p = Jacobian [3 x 3]
%J p = Jacobian derivative [3 x 3]
%solved = was there a solution? 1 = yes, 0 = no

17
18
19
20
21

a = 0.05; b = 0.07; D = 0.03; d = 0.0225;

A = [0; D; 0];
Rl = [cosd(120) sind(120) 0; sind(120) cosd(120) 0; 0 0 1];
Rr = Rl';%[0.5 sqrt(3)/2 0; sqrt(3)/2 0.5 0; 0 0 1];

22
23
24
25
26

theta = zeros(4,1);
theta c = zeros(4,1);
B = zeros(3,1); C = zeros(3,1); P = zeros(3,1);
solved = 1;

27
28
29
30

for i = 1:3 %Three legs, so three times

%The rotated frame, rotate everything 120 degrees to the left
C = Rl(i1)*p(1:3)+[0;d;0];

31
32
33
34

%Sphere intersection plane

R = (CA)';
r = (b2a2 + norm(A)2norm(C)2)/2;

35
36
37
38

%Actuator plane
P1 = [1 0 0];
p1 = 0;

39
40
41
42

%Pluecker Line
U = cross(P1,R);
V = r*P1p1*R;

43
44
45
46
47
48

%Solve quadratic eqaution describing two points on the Pluecker line

alpha = norm(U)2;
beta = 2*U(2)*(U(3)*D+V(1));
gamma = (D2a2)*U(3)2+2*U(3)*V(1)*D+V(1)2;
discriminant = beta24*alpha*gamma;

49
50
51
52

if (discriminant < 0)
discriminant = 0;
end

63

53
54
55
56
57
58

%Two tries
Bz1 = (beta+sqrt(discriminant))/(2*alpha);
Bz2 = (betasqrt(discriminant))/(2*alpha);
By1 = (U(2)*Bz1V(1))/U(3);
By2 = (U(2)*Bz2V(1))/U(3);

59
60
61
62
63
64
65
66
67
68
69

B(3) = 0;
[val,ind] = max([By1 By2]);
if (ind == 1) %Take the bow legged solution
B(3) = Bz1;
else
B(3) = Bz2;
end
if (B(3) < a)
B(3) = a;
end

70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87

%Check
if discriminant 0 %Redudant
theta c(1:3) = theta previous(1:3);
i = 4;
solved = solved*0;%Was not solved
else
theta c(i) = asin(B(3)/a);
solved = solved*1;
if imag(theta c(i)) 6= 0
theta c(1:3) = theta previous(1:3);
i = 4;
solved = solved*0;%Was not solved
end
end
end
theta c(4) = p(4); %pass through
theta = [theta c(1);theta c(2);theta c(3);theta c(4)];

88
89
90

pp = zeros(3,1);
pp = p(1:3);

91
92
93
94
95
96
97
98
99
100
101
102
103

%Calc Jacobian
s1 = pp+[0;d;0][0; a*cos(theta(1))+D; a*sin(theta(1))];
s2 = pp+Rr*[0;d;0]Rr*[0; a*cos(theta(2))+D; a*sin(theta(2))];
s3 = pp+Rr*Rr*[0;d;0]Rr*Rr*[0; a*cos(theta(3))+D; a*sin(theta(3))];
g1 = [0; a*sin(theta(1)); a*cos(theta(1))];
g2 = Rr*[0; a*sin(theta(2)); a*cos(theta(2))];
g3 = Rr*Rr*[0; a*sin(theta(3)); a*cos(theta(3))];
J p = [s1';s2';s3']\diag([s1'*g1;s2'*g2;s3'*g3]);
J theta = inv(J p);
pp dot = zeros(3,1);
pp dot = p dot(1:3);
theta dot= J p\pp dot;

104
105
106
107
108
109
110
111

%Calculate Jacobian Derivative

s1 dot = J p *theta dotg1*theta dot(1);
s2 dot = J p *theta dotg2*theta dot(2);
s3 dot = J p *theta dotg3*theta dot(3);
g1 dot = a*theta dot(1)*[0;cos(theta(1));sin(theta(1))];
g2 dot = a*theta dot(2)*Rr*[0;cos(theta(2));sin(theta(2))];
g3 dot = a*theta dot(3)*Rr*Rr*[0;cos(theta(3));sin(theta(3))];

112
113
114
115
116
117

J p dot = [s1';s2';s3']\(diag([s1 dot'*g1+s1'* g1 dot;s2 dot'*g2+...

s2'* g2 dot;s3 dot'*g3+s3'* g3 dot])[s1 dot';s2 dot';s3 dot']* J p);
J theta dot = diag([s1'*g1;s2'*g2;s3'*g3])\([s1 dot';s2 dot';s3 dot']...
diag([s1 dot'*g1+s1'* g1 dot;s2 dot'*g2+...
s2'* g2 dot;s3 dot'*g3+s3'* g3 dot])/J p);

118
119

pp ddot = zeros(3,1);

64

pp dot = p dot(1:3);
theta ddot = J p\pp ddot + J theta dot * pp dot;

120
121
122

end

B.3
1
2
3
4
5
6
7
8
9
10
11
12
13
14

Forward kinematics matlab code

function [p, p dot, J p, J p dot] = FK(theta, theta dot)
%Input Variables:
%p = EE origin position
%p dot = EE origin velocity
%
%Output Variables:
%theta = actuator angles [rad]
%theta dot = actuator angles derivative [rad]
%J p = Jacobian [3 x 3]
%J p dot = Jacobian derivative [3 x 3]
a = 0.05; b = 0.07; D = 0.03; d = 0.0225;
A = [0; D; 0];
Rl = [cosd(120) sind(120) 0; sind(120) cosd(120) 0; 0 0 1];
Rr = Rl';

15
16
17

%Empty variables
aa = zeros(3,3); bb = zeros(3,3);% cc = zeros(3,3);

18
19
20
21
22
23
24
25
26
27

%Location of A in own frame

A = [0; D; 0];
B = [0;0;0];
for i = 3:1:1
B = [0;a*cos(theta(i))+Dd;a*sin(theta(i))]; %B', not B
aa(:,i) = Rr(i1)*A;
bb(:,i) = Rr(i1)*B; %B', not B
end
i=1;

28
29
30
31
32
33

%Sphere intersection planes [Rn:rn], notation: bb(x,n)

R1 = (bb(:,2)bb(:,1))';
R2 = (bb(:,3)bb(:,1))';
r1 = ((norm(bb(:,1)))2(norm(bb(:,2)))2)/2;
r2 = ((norm(bb(:,1)))2(norm(bb(:,3)))2)/2;

34
35
36
37

%Pluecker line of intersection L = {U:V} of both planes

U = cross(R1,R2);
V = (r1*R2r2*R1);

38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53

%Solve quadratic system

c1 = V(2)/U(3)+B(1);
c2 = B(2)V(1)/U(3);
alpha = norm(U)2/U(3)2;
beta = 2/U(3) * (c1*U(1)+c2*U(2)+B(3)*U(3));
gamma = (c12 + c22 + B(3)2 b2);
discriminant = beta24*alpha*gamma;
if discriminant<0
discriminant=0;
end
p = [0;0;0;0];
p(3) = (betasqrt(discriminant))/(2*alpha);
p(1) = U(1)/U(3)*p(3)V(2)/U(3);
p(2) = V(1)/U(3)+U(2)/U(3)*p(3);
p(4) = theta(4); %Pass through

54
55
56
57

%Calc Jacobian
pp = p(1:3);
s1 = pp+[0;d;0][0; a*cos(theta(1))+D; a*sin(theta(1))];

65

s2 = pp+Rr*[0;d;0]Rr*[0; a*cos(theta(2))+D; a*sin(theta(2))];

s3 = pp+Rr*Rr*[0;d;0]Rr*Rr*[0; a*cos(theta(3))+D; a*sin(theta(3))];
g1 = [0; a*sin(theta(1)); a*cos(theta(1))];
g2 = Rr*[0; a*sin(theta(2)); a*cos(theta(2))];
g3 = Rr*Rr*[0; a*sin(theta(3)); a*cos(theta(3))];
J p = [s1';s2';s3']\diag([s1'*g1;s2'*g2;s3'*g3]);
p dot = J p *theta dot;
J theta = inv(J p);

58
59
60
61
62
63
64
65
66

s1 dot = J p *theta dotg1*theta dot(1);

s2 dot = J p *theta dotg2*theta dot(2);
s3 dot = J p *theta dotg3*theta dot(3);
g1 dot = a*theta dot(1)*[0;cos(theta(1));sin(theta(1))];
g2 dot = a*theta dot(2)*Rr*[0;cos(theta(2));sin(theta(2))];
g3 dot = a*theta dot(3)*Rr*Rr*[0;cos(theta(3));sin(theta(3))];
J p dot = ([s1(1),s1(2),s1(3);s2(1),s2(2),s2(3); ...
s3(1),s3(2),s3(3)])\(diag([transpose(s1 dot)*g1+...
transpose(s1)* g1 dot;transpose(s2 dot)*g2+...
transpose(s2)* g2 dot;transpose(s3 dot)*g3+...
transpose(s3)* g3 dot])[s1 dot(1),s1 dot(2),...
s1 dot(3);s2 dot(1),s2 dot(2),s2 dot(3);s3 dot(1),...
s3 dot(2),s3 dot(3)]* J p);
J theta dot = inv(diag([transpose(s1)*g1;transpose(s2)*g2;...
transpose(s3)*g3]))*([s1 dot(1),s1 dot(2),s1 dot(3);...
s2 dot(1),s2 dot(2),s2 dot(3);s3 dot(1),s3 dot(2),s3 dot(3)]...
diag([transpose(s1 dot)*g1+transpose(s1)* g1 dot;...
transpose(s2 dot)*g2+transpose(s2)* g2 dot;transpose(s3 dot)*g3+...
transpose(s3)* g3 dot])* J theta);
theta ddot = J theta * p ddot + J theta dot * p dot;

67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87

end

B.4
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

Gravity compensation matlab code

function [gravity torque, gravity voltage] = ...

gravity(theta, J p, Rb u, Hu 0,args, k1, k2)
%Input:
%theta = joint angles in rad
%J p = current manipulator Jacobian [3 x 3]
%Rb u = constant rotation from base expressed in UAV frame
%Hu 0 = time variant homogeneous UAV pose
%args = vector containing gravity constant, thigh mass, thigh
%
gravity radius, end effector mass
%k1 = empirically defined constant for grativy compensation
%k2 = empirically defined constant for grativy compensation
%
%Output:
%gravity torque = estimated gravity torque
%gravity voltage = estimate voltage necessary for the motor to counter
%
gravity
g = args(1);
m thigh = args(2);
r gt = args(3);
m ee = args(4);

21
22
23

Rl = [0.5 sqrt(3)/2 0; sqrt(3)/2 0.5 0; 0 0 1];

Rr = Rl';

24
25
26
27

e = zeros(3,3);
Gth = zeros(3,1);
tau th = zeros(3,1);

28
29

R base = Hu 0(1:3,1:3) * Rb u;

30

66

%Arm unit vectors

for i = 1:3
e(:,i) = Rr(i1)*[0;cos(theta(i));sin(theta(i))];
Gth(i) = dot(cross(e(:,i),R base'*[0;0;1]),Rr(i1)*[1;0;0]);
end
%Arm gravity torque
tau th = k1*g*m thigh * r gt *Gth;

31
32
33
34
35
36
37
38

%End effector Gravity Force:

tau ee = J p'*R base'*[0;0;m ee *g];

39
40
41

gravity torque = (tau ee + tau th); %Reduced with gear ratio

gravity voltage = k2*(tau ee + tau th);

42
43
44

end

67

68

Appendix C

Technical Drawings
On the following pages structural drawings of all manipulator parts are given. The number of necessary
parts and part name of each part is stated in the table below.
Name
Base Plate
Bearing Bracket
End Effector Back
End Effector Bearing Bracket
End Effector Outer Ring
End Effector Gimbal Ring
Gear Big
Gear Small
Motor Bracket
Parallelogram Short Side Large
Parallelogram Short Side Small
Probe Sleeve
Shin End Part
Spring plate Holes
Spring plate No Holes
Thigh End
Thigh Shaft
Thigh Start

69

Amount
1
3
1
3
1
1
1
1
3
6
6
1
12
2
2
3
3
3

11

10

7,5

Comment: debur and break sharp edges

2,5

5x45 (2x)

2,5

FACULTY OF EEMCS

PROJECTION
METHOD

7,3x0,6 (2x)
6J7 thru

--

SURFACE FINISH

--

5:1

A4

01

REV.

6-6-2011

SHEET 1 OF 1

Bearing_Bracket

FILE / PART NAME

DRAWING NO.

Bearing Bracket

SCALE

Matteo Fumagalli
CHECKED

TITLE

DATE

Arvid Keemink
DRAWN

M2x4 (2x)

DIMENSIONS IN MILLIMETERS

Al 6061
--

MATERIAL

UNLESS STATED
OTHERWISE:
TOLERANCES 0,1 MM
or 0.1

3 (2x)

9
16

To be used with M2x3 conical set screw

FACULTY OF EEMCS

PROJECTION
METHOD

--

SURFACE FINISH

--

10:1

DIMENSIONS IN MILLIMETERS

SHEET 1 OF 1

A4

01

REV.

9-6-2011

Cardan_Joint_Sapphire_Inlay

FILE / PART NAME

DRAWING NO.

Sapphire Joint

SCALE

Matteo Fumagalli
CHECKED

TITLE

DATE

Arvid Keemink
DRAWN

Sapphire
--

MATERIAL

UNLESS STATED
OTHERWISE:
TOLERANCES 0,1 MM

3,9

10,5

Comment: debur and break sharp edges

VIEW B

2,1

4,

36,9

FACULTY OF EEMCS

PROJECTION
METHOD

4,5

90

5,4

13,2
6

3,6

9
4,5
--

SURFACE FINISH

SCALE

Matteo Fumagalli
CHECKED

5:1

--

SHEET 1 OF 1

EE_plate_bearing_bracket

FILE / PART NAME

DRAWING NO.

A4

01

REV.

9-6-2011

End Effector Bearing Bracket

TITLE

DATE

Arvid Keemink
DRAWN

DIMENSIONS IN MILLIMETERS

Al 6061
--

MATERIAL

UNLESS STATED
OTHERWISE:
TOLERANCES 0,1 MM
or 0,1

6J7 thru
7,3x0,6 (2x)

thr
2
M

)
3x
(
u

FACULTY OF EEMCS

PROJECTION
METHOD

18 (3x)

0
4 thru (2x)

12

--

SURFACE FINISH

2:1

-EE_plate_ring

FILE / PART NAME

DRAWING NO.

SHEET 1 OF 1

A4

01

REV.

9-6-2011

End Effector Outer Ring

SCALE

Matteo Fumagalli
CHECKED

TITLE

DATE

Arvid Keemink
DRAWN

DIMENSIONS IN MILLIMETERS

Al 6061
--

MATERIAL

UNLESS STATED
OTHERWISE:
TOLERANCES 0,1 MM
or 0,1

9
R1
32

0
12

15

R1
5,5

Comment: debur and break sharp edges

15

27

FACULTY OF EEMCS

PROJECTION
METHOD

M2 thru (4x)

--

SURFACE FINISH

2:1

--

SHEET 1 OF 1

End_Effector_Inner_Gimbal

FILE / PART NAME

DRAWING NO.

A4

01

REV.

9-6-2011

End Effector Gimbal Ring

SCALE

Matteo Fumagalli
CHECKED

TITLE

DATE

Arvid Keemink
DRAWN

DIMENSIONS IN MILLIMETERS

Al 6061
--

MATERIAL

UNLESS STATED
OTHERWISE:
TOLERANCES 0,1 MM

2 (4x)

2,5

ru
th

x)
2
(

11
9x45 (2x)

Comment: debur and break sharp edges

View C
Scale 2:1

95
,
4

x)

(2

FACULTY OF EEMCS

PROJECTION
METHOD

17

9
--

SURFACE FINISH

--

DIMENSIONS IN MILLIMETERS

5:1

A4

01

REV.

9-6-2011

SHEET 1 OF 1

Motor_Bracket

FILE / PART NAME

DRAWING NO.

Motor Bracket

SCALE

Matteo Fumagalli
CHECKED

TITLE

DATE

Arvid Keemink

M2x4 (2x)
DRAWN

2 (2x)

10,1x3,5
Al 6061
--

MATERIAL

UNLESS STATED
OTHERWISE:
TOLERANCES 0,1 MM
or 0,1

2,5

4
2 (2x)

9 thru

,5
R1 ,3
2
R

0,8
FACULTY OF EEMCS

PROJECTION
METHOD

--

SURFACE FINISH

SCALE

Matteo Fumagalli

5:1

--

SHEET 1 OF 1

Parallelogram_Horizontal_Big

FILE / PART NAME

DRAWING NO.

A4

01

REV.

9-6-2011

Parallelogram Short Side Large

TITLE

CHECKED

DIMENSIONS IN MILLIMETERS

Al 6061
--

MATERIAL

DATE

Arvid Keemink

4,5

DRAWN

M2 thru

UNLESS STATED
OTHERWISE:
TOLERANCES 0,1 MM

21,5
10,5
10

1,75

R3

1,75

Comment: Debur and break sharp edges

2,
R

R3

M2 thru (2x)

FACULTY OF EEMCS

PROJECTION
METHOD

3x5

11,5

--

SURFACE FINISH

SCALE

Matteo Fumagalli
CHECKED

10:1

--

SHEET 1 OF 1

Parallelogram_Horizontal_Small

FILE / PART NAME

DRAWING NO.

A4

01

REV.

9-6-2011

Parallelogram Short Side Small

TITLE

DATE

Arvid Keemink
DRAWN

DIMENSIONS IN MILLIMETERS

Al 6061
--

MATERIAL

UNLESS STATED
OTHERWISE:
TOLERANCES 0,1 MM

D-D

2,2 thru all

If not stated, any corner drawn with rounded fillet has radius 1 mm

Comment: debur and break sharp edges

x)

R2
(2

M2 thru (2x)

5
2,

(2

x)

6,25

10

6,25

3,5x45 (2x)

5
9
15,05
16,41
19,91

6,5 (2x)
6,5 (2x)

11,25

12

FACULTY OF EEMCS

PROJECTION
METHOD

4 thru all

-10

SURFACE FINISH

--

Al 6061

MATERIAL

UNLESS STATED
OTHERWISE:
TOLERANCES 0,1 MM
or 0,1

15

10

6,1

-DIMENSIONS IN MILLIMETERS
11

Probe_Sleeve

FILE / PART NAME

DRAWING NO.

A2

01

REV.

14-6-2011
3:1

SHEET 1 OF 1

Probe Sleeve

Matteo Fumagalli
TITLE

DATE
SCALE

Arvid Keemink
DRAWN

11

CHECKED

18

33

45

22x31
24

2x45 (2x)

2,5x45 (2x)

3,5

5
10
FACULTY OF EEMCS

PROJECTION
METHOD

15
5
--

SURFACE FINISH

FILE / PART NAME

DRAWING NO.

Shin_End

--

5:1

A4

01

REV.

10-6-2011

SHEET 1 OF 1

Shin End Part

SCALE

Matteo Fumagalli
CHECKED

TITLE

DATE

Arvid Keemink
DRAWN

DIMENSIONS IN MILLIMETERS

Al 6061
--

MATERIAL

UNLESS STATED
OTHERWISE:
TOLERANCES 0,1 MM
or 0,1

3,5

7J6 thru

,5
R4

Comment: debur and breark sharp edges

10

7
6J
,

)
2x
(
6

ru
th

FACULTY OF EEMCS

PROJECTION
METHOD

0
5x
6
,
R3

3 (2x)

--

SURFACE FINISH

FILE / PART NAME

DRAWING NO.

Thigh_End

--

Thigh End

5:1

A4

01

REV.

10-6-2011

SHEET 1 OF 1

SCALE

Matteo Fumagalli
CHECKED

TITLE

DATE

Arvid Keemink
DRAWN

DIMENSIONS IN MILLIMETERS

Al 6061
--

MATERIAL

UNLESS STATED
OTHERWISE:
TOLERANCES 0,1 MM

5,5 (2x)

12
13
20

Comment: debur and break sharp edges

1 (2x)

FACULTY OF EEMCS

PROJECTION
METHOD

--

SURFACE FINISH

DIMENSIONS IN MILLIMETERS

-Thigh_Shaft

FILE / PART NAME

DRAWING NO.

5:1

A4

01

REV.

10-6-2011

SHEET 1 OF 1

Thigh Coupling Shaft

SCALE

Matteo Fumagalli
CHECKED

TITLE

DATE

Arvid Keemink
DRAWN

AISI 316L SS
--

MATERIAL

UNLESS STATED
OTHERWISE:
TOLERANCES 0,1 MM

R1
,5
4

13

1
R

x)

(4

7,75
4

8,5
FACULTY OF EEMCS

PROJECTION
METHOD

R4,5

hr
2t

--

SURFACE FINISH

-Thigh_Start

FILE / PART NAME

DRAWING NO.

Thigh Start

5:1

A4

01

REV.

10-6-2011

SHEET 1 OF 1

SCALE

Matteo Fumagalli
CHECKED

TITLE

DATE

Arvid Keemink
DRAWN

DIMENSIONS IN MILLIMETERS

Al 6061
--

MATERIAL

UNLESS STATED
OTHERWISE:
TOLERANCES 0,1 MM

3 thru