Arvid Keemink
MSc Report
Committee:
Prof. dr. ir. S. Stramigioli
Dr. R. Carloni
Dr. M. Fumagalli
MSc. A. Y. Mersha
January 2012
Contents
List of Figures
Abstract
1 Introduction
2 Requirements
2.1 General requirements . .
2.2 Nondestructive testing
2.3 Visual inspection . . . .
2.4 Object placing . . . . .
2.5 Surface cleaning . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
9
9
9
10
10
10
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
11
11
11
11
11
12
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
15
15
16
16
16
16
16
18
18
19
19
19
19
19
19
20
.
.
.
.
23
23
25
26
26
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
1
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
5.5
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
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 standalone Performance . . . . . . . . . . . . . . .
8.2 Manipulator attached to Ducted Fan UAV . . . . . . . . . . . . .
8.2.1 Freeflight with position control of the manipulator . . . .
8.2.2 Interaction During Flight . . . . . . . . . . . . . . . . . .
8.3 Manipulator attached to Quadrotor . . . . . . . . . . . . . . . . .
8.3.1 Freeflight 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
55
Bibliography
58
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
C Technical Drawings
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
61
61
63
65
66
69
List of Figures
2.1
. . . . . . . . . . . . . . . . . . . . . . . . . . .
10
3.1
3.2
3.3
12
13
13
4.1
4.2
4.3
4.4
4.5
4.6
4.7
4.8
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
15
16
17
17
18
20
21
22
5.1
5.2
5.3
5.4
5.5
5.6
5.7
5.8
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
24
27
28
31
31
32
33
34
6.1
6.2
6.3
6.4
6.5
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
35
36
37
40
41
7.1
7.2
7.3
7.4
7.5
7.6
7.7
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
43
44
44
45
45
45
46
8.1
8.2
8.3
8.4
8.5
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
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 endeffector position during tracking. . . . . . . . . . . . . . . . . . .
Quadrotor EndEffector 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
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 mockup environment. The chosen robotic
airframes used in the project will be equipped with a robotic arm: an appropriate endeffector and electromechanical armlike 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, onboard a flying
vehicle, to physically interact with environments and perform ultrasonic nondestructive testing experiments
and other versatile tasks at unreachable locations for humans.
The novel NDT endeffector 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 inflight 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 endeffector 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 nondestructive 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 biologicallyinspired and electroadhesion 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 tetherfree 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 onboard 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 onboard structure to apply force to the environment for several tasks such as cleaning.
Pounds et al. [4] provide the concept of a underactuated 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 onboard different kinds of UAV platforms
with modular taskspecific endeffectors that are thereby supplied with full Cartesian movement relative to
the UAV. The area of application of the onboard 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. Nondestructive 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 endeffector 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 endeffector 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
Nondestructive testing
The system should be capable of performing UTNDT 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 UTNDT 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 endeffectors due to surface roughness, tube curvature and
UAV orientation error should be corrected by the endeffector 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 endeffector 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 endeffector
for close up visual inspection. This requires the endeffector 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 endeffector 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 endeffector 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
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
endeffector motion through three rotary motor motions and as preferred it can supply a significant amount
of force to the endeffector, 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 endeffector 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 endeffector.
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 endeffector 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 endeffector together can be seen in Fig. 3.3.
3.3
Endeffector specification
An extra actuator is placed on the endeffector to supply roll movement for the cylindrical sensor. Two
1DOF rotational joints near the front of the sensor form a double Cardan joint (double gimbal) inside
endeffector (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 endeffector combined, going from fixed world to sensor.
Rotary joints are indicated with R, translational joint with T.
13
14
Chapter 4
Figure 4.1: Left) Photo of the prototype attached to a quadrotor. Right) Photo of prototype standalone.
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 endeffector 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 endeffector 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 endeffector
The endeffector 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 endeffector can be placed inside this ring. The NDT endeffector
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 endeffector showing the gimbal system and sensor and connection to the legs.
Figure 4.4: Exploded view of the endeffector 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 202203 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
PA250 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 endeffector 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 HighPower Motor Drivers are used at 6 Volts, supplied
by an RC Hobbywing UBEC which acts as a voltage regulator. Each of the discrete MOSFET Hbridge
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, overheating or
undervoltage). 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 jointtorque or 6D forcetorque measurements are not yet implemented,
they will be discussed here. An 8 channel multiplexer Maxim DG408DY with opamp INA118 amplifies the
data of maximally 8 Wheatstone halfbridges. 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
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 XBee wireless device.
4.3.5
The high level hardware comprises any combination of computers with Linux and the UAV onboard Atom
processor running Ubuntu 10.10. The ForceDimenson Omega.6 haptic device is used to control the UAV
with haptic feedback. A threeaxis 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
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/cgibin/yabb2/YaBB.pl?num=1267553811
19
Ground St at ion
Ct rl Input s
Dat a Exchange
C(s)
P(s)
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
HL Ct rl Input s
Manipulat or
Haptic Interface
Joystick
LED Markers
VisualEyez
3DTracker Syst em
Human Operat or
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 reatime) and Wireless LAN queue.
The received references and PWM values can be filtered by a 3 Hz first order lowpass filter to prevent
staircasing behavior of the manipulator.
4.4.2
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
Pos/IMP
Calib_Switch
Output
Go to Calibration
Inverse Dynamics and Workspace Impedance
IMP_Mode
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
Tilde
T
Tilde1
Base_Movement
makeomega
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 UDPSerial 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 teleoperation 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
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
5.1
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 endeffector 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 endeffector 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 zdirection 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 endeffector 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 endeffector 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 endpoint (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 zaxis 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.
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 endeffector is the contribution of the endeffector 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 endeffector 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 endeffector and
the thighs (Jp being the 3 3 geometric Jacobian):
bee
ee,I = JTp mee,t p
th,I = Ith I3
The external, noninertial, 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
Where Fbf,ee is the fictitious force on the endeffector 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 endeffector into joint space and subsequent collecting gives the motor
load side torque:
+ mee,t JT J p + kd
= 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 pregear 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
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 endeffector. If the ith
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 endeffector 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 multibody 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 backEMF. 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)
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 NorthEastDown (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 NewtonEuler
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 bodyframe, 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.
counterrotating 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 nonzero value for these parameters,
the determinant of the 4 4 matrix above is nonzero 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
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) antitorque/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
(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 antitorque 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
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 leftpseudoinvertable 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
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 nontrivial 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, multibody 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
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 6dimensional 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 6dimensional 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 rowvectors, they are presented in the following text as column vectors for ease of notation.
The timederivative 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
5.5.2
Choosing an appropriate frame where the inertiatensor is diagonal (i.e. the principal or bodyframe) makes
the equations of dynamics straightforward. This makes it possible to write the NewtonEuler 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 multibody 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
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 multibond 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 1junction represents
a (common) twist or summation of wrenches in that frame, a 0junction represents a (common) wrench, or
summation of twists in that frame. Any Ielement represents the bodys inertia tensor. Any Celement
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 noninertial, given
30
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 0junction 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
The kinematic structure of the model is as given in Fig. 3.3, including elastic bands and spring loading for the
endeffector 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 timeconstant. 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
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 HuntCrossley 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 penetrationdepth 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
6.1
Position control
One of the manipulator control strategies is stiff position control with disturbance rejection. A PIDcontrol
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 backEMF and mechanical friction of the
real motors are high. Using their absolute damping gave better results than using backEMF 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
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
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 nonmodeled, 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 steadystate 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 counteract 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
PIcontrol. 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 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 counteract 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
= 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
=
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
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
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 rollof 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, ( )
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
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
TR
RT R
Mu = kR eR k e + J J R
(6.1)
Which asymptotically stabilises the yawattitude 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
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 skewsymmetric 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 hightolow 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 nonsurjective from the smaller
jointposition space to the larger endeffector space. Hence the reverse map from the endeffector space to the
jointangle 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 ellipsoidi 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
uckerline LCp :
LCp = pSi CSi : pSi CSi = {U : V}
Since any vector endpoint q on the Pl
uckerline 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 ycoordinates of the point q in its zcoordinate. Filling this in into a
homogeneous sphere equation, it is reduced to a quadratic equation, which gives the two solutions of the
zcoordinate 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
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 multibody 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 multibody 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 endeffector trajectory tracking is not presented here, since the
ideal simulation results are not comparable to the real setup.
7.1
Simple model
Figure 7.1: Simulation Results of the Simple Model. In the top left the reference for the local zcoordinate 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 endeffector, i.e.
43
the local x and ydirections are stationary. The UAV rotates the base around local ydirection with angle
= /9 sin (2t) and movement in xdirection 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
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 xreference 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 xreference 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
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 xreference 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. Inflight 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
This experiment presents the results on the position control and the joint space impedance control applied
to the standalone 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 zdirection. Due to the force on the endeffector, 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 endeffector. 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 nonconstant. 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 endeffector 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 endeffector.
8.2
The manipulator was attached to a Ducted Fan UAV developed by the CASY at the University of Bologna [7].
The UAV and manipulator endeffector are tracked by reflective markers by an OptiTrack system as can be
seen in both photos in 8.3.
8.2.1
This experiment presents the results of keeping the manipulator at a desired position, while the aerial vehicles
are moving. The manipulator jointspace position controller makes it possible for the endeffector 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 lowpass 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 endeffector
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 endeffector 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 endeffector, both described in the inertial frame (therefore denoted by superscript 0). The
standard deviation in the UAV and Endeffector 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
endeffector 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 endeffector 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 inflight 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 endeffector 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 endeffector
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 endeffector 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 handheld 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
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
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
endeffector. 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 ROSnode which was developed by a third party.
Since the endeffector position in the inertial frame was not measured in the current setup, any endeffector position can only be estimated through the local endeffector 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
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 endeffector 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 zdirection 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 endeffector position during tracking. Units in meters.
8.3.2
The inertial frame tracking performance of the endeffector 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 endeffector 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 endeffector 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 EndEffector Inertial Frame Reference Tracking. The endeffector 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
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.
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
endeffector
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 endeffector position during interaction, showing that the joint space impedance
controller allows for proper compression.
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
55
56
Bibliography
[1] A. Albers, S. Trautmann, T. Howard, T. Nguyen, M. Frietsch, and C. Sauter, Semiautonomous 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 ductedfan miniature aerial vehicle, Control Engineering Practice, vol. 18, no. 7, pp. 747760,
2010.
[8] Asctec pelican3 product website. [Online]. Available: http://www.asctec.de/asctecpelican3/
[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. ZsomborMurray, Descriptive geometric kinematic analysis of Clavels Delta robot, 2003.
[11] A. Codourey, Dynamic modeling of parallel robots for computedtorque 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 coordinatefree approach.
Springer Verlag, 2001, vol. 266.
[21] V. Duindam, Portbased 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. ZavalaRio and V. Santibanez, Simple extensions of the pdwithgravitycompensation 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
A.1
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 pseudoinverse 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
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 pseudoinverse for a least squares approximation:
h = C
(CT C)1 CT h
Such that:
A.2
=
(CT C)1 CT
036
043
I3
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 forcetorque (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 virtualreaction 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
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
42
43
44
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
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
17
18
19
20
21
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
31
32
33
34
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
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
112
113
114
115
116
117
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
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
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
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
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
end
B.4
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
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
31
32
33
34
35
36
37
38
39
40
41
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
2,5
5x45 (2x)
2,5
FACULTY OF EEMCS
PROJECTION
METHOD
7,3x0,6 (2x)
6J7 thru

SURFACE FINISH

5:1
A4
01
REV.
662011
SHEET 1 OF 1
Bearing_Bracket
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
FACULTY OF EEMCS
PROJECTION
METHOD

SURFACE FINISH

10:1
DIMENSIONS IN MILLIMETERS
SHEET 1 OF 1
A4
01
REV.
962011
Cardan_Joint_Sapphire_Inlay
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
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
DRAWING NO.
A4
01
REV.
962011
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
DRAWING NO.
SHEET 1 OF 1
A4
01
REV.
962011
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
15
27
FACULTY OF EEMCS
PROJECTION
METHOD
M2 thru (4x)

SURFACE FINISH
2:1

SHEET 1 OF 1
End_Effector_Inner_Gimbal
DRAWING NO.
A4
01
REV.
962011
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)
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.
962011
SHEET 1 OF 1
Motor_Bracket
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
DRAWING NO.
A4
01
REV.
962011
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
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
DRAWING NO.
A4
01
REV.
962011
TITLE
DATE
Arvid Keemink
DRAWN
DIMENSIONS IN MILLIMETERS
Al 6061

MATERIAL
UNLESS STATED
OTHERWISE:
TOLERANCES 0,1 MM
DD
If not stated, any corner drawn with rounded fillet has radius 1 mm
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
DRAWING NO.
A2
01
REV.
1462011
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
DRAWING NO.
Shin_End

5:1
A4
01
REV.
1062011
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
or 0,1
3,5
7J6 thru
,5
R4
10
7
6J
,
)
2x
(
6
ru
th
FACULTY OF EEMCS
PROJECTION
METHOD
0
5x
6
,
R3
3 (2x)

SURFACE FINISH
DRAWING NO.
Thigh_End

Thigh End
5:1
A4
01
REV.
1062011
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
1 (2x)
FACULTY OF EEMCS
PROJECTION
METHOD

SURFACE FINISH
DIMENSIONS IN MILLIMETERS
Thigh_Shaft
DRAWING NO.
5:1
A4
01
REV.
1062011
SHEET 1 OF 1
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
DRAWING NO.
Thigh Start
5:1
A4
01
REV.
1062011
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