Professional Documents
Culture Documents
2
< <
2
).
In addition, let the position of the shaft of the tilt servo be given by , the angle between the
z-axis of B and l
tilt
(
2
< <
2
) (see Figure 3.2).
Furthermore, let d
pan
and d
tilt
be the lengths of the pan and the tilt lever.
1
http://www.hitl.washington.edu/artoolkit/documentation/cs.htm
8
3.2 Pan/tilt Platform Model
Figure 3.2: Outline of the pan/tilt platform model showing the body and camera coordinate system.
Furthermore, the position of the tilt servos motor shaft and the pan servos motor shaft
is displayed.
The two vectors l
pan
and l
tilt
are then calculated by:
l
pan
=
_
_
0
0
d
pan
_
_
(3.2.1)
l
tilt
=
_
_
d
tilt
sin cos
d
tilt
sin sin
d
tilt
cos
_
_
. (3.2.2)
In addition, the rotation matrix R
C
B
, which transforms camera coordinates to body coordinates
can be computed through:
e
C
x
=
_
_
cos /2
sin /2
0
_
_
(3.2.3)
e
C
y
= l
tilt
/|l
tilt
| (3.2.4)
e
C
z
= e
C
x
e
C
y
(3.2.5)
R
C
B
=
_
e
C
x
, e
C
y
, e
C
z
, (3.2.6)
where equation (3.2.3) is based on the observation that by construction the x-axis of C is always
parallel to the xy-plane of B. Therefore, this axis can be calculated through rotating the x-axis
of B clock-wise according to the angle /2.
9
3 Mathematical Representation
0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45
80
60
40
20
0
20
40
60
80
time [s]
p
o
s
i
t
i
o
n
o
f
t
h
e
s
e
r
v
o
s
m
o
t
o
r
s
h
a
f
t
[
]
Step response of a servo
Figure 3.3: The step response of a servo, averaged over 10 samples.
3.2.1 Servo Model
To control the motion of the the pan/tilt platformwe used two servos. In general, a servo consists
of a control circuit and an electro motor, which is connected to a gear box and a feedback
potentiometer
2
. A position can be provided to it modulated as a PWM signal. The control
circuit decodes this position and compares it to the position measured by the potentiometer.
If these two position differ the control circuit starts to pulse the motor (switches it on/off) at
a xed rate per second, where the speed of the motor can be controlled through the length of
these pulses. Therefore, the motor moves to the set position and then remains there.
In order to create a model of a servo, we read out its feedback potentiometer to measure its step
response. We did so ten times and then calculated an average (for details see Chapter 6.2), the
result can be seen in Figure 3.3.
Based on this average step response, we made the following assumptions:
If the motor of the servo is turned on it quickly accelerates with a constant angular acceleration
s
until it reaches a maximum angular velocity
max
, where the sign of
s
and
max
is deter-
mined by the direction in which the motor should turn.
If the motor is turned off it is decelerated with
s
.
2
http://www.futaba-rc.com/servos/digitalservos.pdf
10
3.2 Pan/tilt Platform Model
Let now be the position of the servos motor shaft, then the motion of a servo can be modelled
by:
If the motor is turned on
=
_
s
if | | < |
max
|
0 otherwise
and if the motor is turned off
=
_
s
if = 0
0 otherwise
3.2.2 Marker Detection Model
The location of the marker in camera coordinates is measured by the marker detection system
if the marker is inside the eld of view (FOV) of the camera.
Let the position of the marker in camera coordinates be r
CM
. Furthermore, let and be the
horizontal and the vertical angle of view of the camera (see Figure 3.4).
One can now calculate the relative position of the quadrotor to the marker in its body coordi-
nates:
r
BM
=
_
l
pan
+l
tilt
+R
C
B
r
CM
if
arctan
_
r
CM
x
r
CM
z
_
2
and
arctan
_
r
CM
y
r
CM
z
_
2
0 otherwise
, (3.2.7)
where l
pan
and l
tilt
are calculated according to (3.2.1) and (3.2.2) and R
C
B
according to (3.2.6).
Figure 3.4: Field of view of the camera.
11
3 Mathematical Representation
Figure 3.5: Overview over the distance sensor model.
3.3 Distance Sensor Model
We used ultrasonic distance sensors to measure the distance towards an object. They emit
ultrasound and then measure the time it needs to be reected back to them. Since the sound
waves expand as they travel through the air, these sensors have a cone-shaped detection area,
where the opening angles of this cone is predened by the manufacturer. The distance to the
nearest object inside this cone is then measured.
Hence, we modelled a distance sensor as a cone dened through t, d, h, (see Figure 3.5),
where t W is the apex of the cone (the position of the sensor) in world coordinates, d W
is its normalized axis direction (the direction in which the sensor is oriented), h is its height and
is half its opening angle. Furthermore, let l
max
=
h
cos()
be the reach of the sensor.
The obstacles were modelled as planes, given in Hesse normal form. Therefore, a plane is
specied by its unit normal vector n W, and d its distance to the origin of W, where d =
p
T
n, with p W a point on the plane. We dened the unit normal vectors, such that they point
towards the quadrotors starting point.
Since the quadrotor should never y through such a plane, one can now check if the sensor
detected an obstacle by testing if there exists a point on the cone, which lies in the negative
halfspace of the plane. For that, we used the procedure outlined in [Ericson 2004, Section 5.2.4]:
First, one must calculate q the point furthermost in the direction of n, located on the circular
endcap of the cone:
q = t + hd + r m,
where r = h tan and m = (n d) d.
Second, it must be tested if this point is in the negative halfspace of the plane:
n
T
q < d
If this is true, the modelled distance sensor detected an obstacle.
In this case, the intersection of a line between t and q, and the plane is found, where the
12
3.3 Distance Sensor Model
intersection is given as:
0 = (t +l
O
v)
T
n d,
with v =
qt
|qt|
. After that the distance from the apex t to this intersection is calculated.
Otherwise, if no object was detected, it is assumed that the sensor reports its maximum reach
l
max
.
Therefore, the distance measurement of the sensor is given through:
l
O
=
_
dt
T
n
v
T
n
if n
T
q < d
l
max
otherwise
(3.3.1)
13
3 Mathematical Representation
14
4
Controller
In this chapter the controllers used for simulating the pan and the tilt servo and to control the
pan/tilt platform are shown. Furthermore, the extension of the quadrotor position controller is
described.
Figure 4.1: The controlled system of the pan/tilt platform, with and
the set points for, and , the
current position of the pan servos and the tilt servos motor shaft, respectively.
x
and
z
are the angular velocities around the z- and the x-axis of the quadrotors body frame. r
CM
is a vector in camera coordinates pointing from the camera to the marker.
15
4 Controller
4.1 Pan/tilt Platform
The controlled system of the pan/tilt platform can be seen in Figure 4.1. Since its behaviour
should be simulated, we devised a controller for the servo model presented in Chapter 3.2.1.
After that the controller of the pan/tilt platform is shown.
4.1.1 Bang-bang Servo Motor Controller
The task of this controller was to simulate the behaviour of a servo, hence, it was not used to
control an actual servo motor.
With this in mind, we simplied the controllers design by omitting the pulsing behaviour men-
tioned in Chapter 3.2.1. Furthermore, we made the assumption that the controller can observe
the rate at which the motor decelerates (
s
) when it is turned off.
Therefore, it can compute the time the motor stops its movement if it is turned off
t
h
=
s
s
and the position the servos motor shaft will be at this time
h
= +
s
t
h
s
2
t
2
h
,
where the current position of the servos motor shaft and
s
its current angular velocity.
Based on this we devised a simple on-off controller, which calculates the input to the motor as:
u =
_
_
1 if > 0 and =
h
1 if < 0 and =
h
0 otherwise
, (4.1.1)
where is the set point for the position of the servos motor shaft and the sign of u determines
the direction in which the motor should turn. Notice that if the set position is equal to the
position
h
, where the servos motor shaft will halt if the motor is turned off, then the motor is
turned off (u = 0).
4.1.2 Position Controller for the Pan/tilt Platform
A quadrotor, which uses a marker detection system with a rigidly attached camera, might not
always be able to follow the marker, if it is moved rapidly sideways. Furthermore, a quadrotor
needs to tilt in order to perform translative movements, which might move the marker out of the
FOV of the camera.
Both of these problems can be mitigated through attaching the camera to a pan/tilt platform,
which can follow the movements of the marker independently from the movements of the
quadrotor. Therefore, the pan/tilt platform should always be oriented in such a way that the
marker remains in the middle of the cameras FOV.
16
4.2 Extension of a Quadrotors Position Controller: Following a Leader While Avoiding Obstacles
To achieve this, we used a controller, which calculates a position set point for each of the pan
and the tilt servo, where it was assumed that the camera is directly attached to them, i.e. |l
pan
| =
|l
tilt
| = 0. Therefore, changing the position of a motor shaft does only cause a rotation of the
camera coordinate systemC, but not any translation of its origin, relative to the quadrotors body
coordinate system B.
Let now
C
be the angle between the projection of r
CM
onto the zy-plane of C and the z-axis
of C (see Figure 4.2 (a)). Similarly, let
C
be the angle between the projection of r
CM
onto the
zx-plane of C and the z-axis of C (see Figure 4.2 (b)):
C
= arctan
_
r
CM
y
r
CM
z
_
C
= arctan
_
r
CM
x
r
CM
z
_
If these two angles are zero then r
CM
coincides with the z-axis of C and the marker is in the
midpoint of the cameras FOV as required.
Hence, the error term for the controller of the tilt servo is dened as
e
C
= 0
C
and the error term for the controller of the pan servo is dened as
e
C
= 0
C
In order to compensate for changes of the quadrotors attitude, its angular velocities around the
z- and the x-axis of its body frame,
z
and
x
, are integrated and subtracted.
The set point for the tilt servo is then given as
= K
C
x
dt (4.1.2)
and, analogous, for the pan servo
= K
C
z
dt, (4.1.3)
where dt the sample time of the respective controller and , the current position of the pan
servos and the tilt servos motor shaft, respectively. The controller gains K
and K
were
found through manual tuning.
4.2 Extension of a Quadrotors Position Controller:
Following a Leader While Avoiding Obstacles
The quadrotor should keep a predened position relative to the marker and deviate from this
position if the measured distance towards an obstacle becomes too small. In order to achieve
17
4 Controller
(a) (b)
Figure 4.2: (a)
C
is the angle between the projection of r
CM
onto the zy-plane of C and the z-axis of C.
(b)
C
is the angle between the projection of r
CM
onto the zx-plane of C and the z-axis of C.
this, an extension for an existing quadrotor controller (see [Mellinger and Kumar 2011]) was
devised.
Given a set point specied through a position p W and a yaw angle
, the original controller
rst computes the errors on position and velocity
e
p
= p p (4.2.1)
e
v
= p
p
and then the desired force vector:
F
des
= K
p
e
p
K
v
e
v
+ mg e
W
z
+m
p, (4.2.2)
where K
p
and K
v
are positive denite gain matrices, m the mass of the quadrotor and e
W
z
the
unit vector for the z-axis of the world coordinate system W.
After that the desired attitude of the quadrotor R
des
is calculated:
e
B
z
,des
=
F
des
|F
des
|
x
des
=
_
_
cos(
)
sin(
)
0
_
_
e
B
y
,des
=
e
B
z
,des
x
des
|e
B
z
,des
x
des
|
e
B
x
,des
= e
B
y
,des
e
B
z
,des
R
des
=
_
e
B
x
,des
e
B
y
,des
e
B
z
,des
_
18
4.2 Extension of a Quadrotors Position Controller: Following a Leader While Avoiding Obstacles
Figure 4.3: Display of the various vectors involved in calculating the set point position p for the exten-
sion of the quadrotors position controller. Only distance sensor 1 has detected an obstacle,
therefore, all other force vectors F
dist
i
i {2, ..., N} have length zero.
F
des
and R
des
are then used to compute the desired inputs u.
Based on this we wanted to use the measured relative position of the marker to set p such that
it points from the origin of W to the predened position relative to the marker.
For that let r
p
W be a vector pointing from the marker to the predened position relative to
it. Furthermore, let p
M
W be the position of the marker in world coordinates and r
WM
=
R
W
B
r
BM
a vector pointing from the quadrotor to the marker, with r
BM
calculated according to
(3.2.7) (see Figure 4.3). It follows:
p = p
M
+ r
p
= p +r
WM
+ r
p
From this the error term (4.2.1) can now be dened as:
e
p
=
_
p p = (r
WM
+ r
p
) if r
WM
= 0
0 otherwise
(4.2.3)
Furthermore, since the quadrotor should always be "looking" at the marker, we set
= arctan
_
r
WM
y
r
WM
x
_
19
4 Controller
This concludes the rst part of the extension. However, we also wanted the quadrotor to deviate
from p if the measured distance towards an obstacle becomes too small. For this we had the
idea to let the obstacles exert a force on the quadrotor which is inversely proportional to the
distance towards it. Therefore, for each distance sensor attached to the quadrotor a force vector
F
dist
, which acts opposite to the orientation of the sensor, is calculated.
Let now d
i
be the orientation of the i
th
distance sensor attached to the quadrotor, l
O
i
its current
distance measurement and l
max
i
its maximum reach, as specied in Chapter 3.3, then its force
vector is computed as:
F
dist
i
= d
i
_
1
l
max
i
1
l
O
i
_
(4.2.4)
Notice that the force goes to innity as the distance becomes smaller.
Finally, one can combine these force vectors and the new error term (4.2.3) with the equation
of the old desired force vector (4.2.2) to get:
F
des
= K
p
e
p
K
v
e
v
+mg e
W
z
+m
p +
N
i=0
K
s
F
dist
i
,
where K
s
is a diagonal gain matrix found through manual tuning and N the number of attached
distance sensors.
20
5
Hardware and Implementation
In this chapter we give an overview over the hardware we used and the software we imple-
mented.
5.1 Pixhawk 4 Flight Management Unit (PX4FMU)
The PX4FMU was developed by the PIXHAWK Reasearch Project and "is a high-performance
autopilot-on-module suitable for xed wing, multi rotors, helicopters, cars, boats and any other
robotic platform that can move."
1
It runs the NuttX
2
real-time OS and applications can be de-
veloped for it in C or C++. The communication between the various applications is realised
through an micro object request broker (uORB), which implements the publish-subscribe pat-
tern. The PX4FMU offers, among others, four UART and two I2C ports, which we used to
interact with the ultrasonic distance sensors and the pan/tilt camera platform. Furthermore, it
has four different sensors: MPU-6000 (3D ACC / Gyro), L3GD20 (3D Gyro), HMC5883L (3D
Mag) and MS5611 (barometric pressure), the measurements of which are fused and published
via the uORB.
5.2 Pan/tilt platform
The requirements for the pan/tilt platform were:
1
http://pixhawk.org/modules/px4fmu
2
http://www.nuttx.org/
21
5 Hardware and Implementation
(a) (b)
(c)
Figure 5.1: The 3D models of the pan (a) and the tilt (b) lever. The assembled pan/tilt platform (c).
22
5.2 Pan/tilt platform
1. compact dimension
2. low weight
3. simple adaptation to different servos and cameras
Based on these we designed a platform consisting of a pan and a tilt lever in Autodesk Inventor
and then 3D printed it on a MakerBot
3
Replicator 2 (see Figure 5.1). This approach allowed
us to adjust the weight and stability of the platform through controlling the inll parameter of
the 3D printer. Furthermore, the shape of the levers could easily be changed to allow the use of
different servos and cameras.
5.2.1 Servos
For moving the pan/tilt platform two Graupner
4
DES 488 BB MG servos were used, which we
connected to a 12 channel servo controller IC (SERVO12C) from Hobbytronics
5
. This chip
is connected to the PX4FMU via I2C and allows to specify the position of a servo through a
dimensionless scalar value ranging from 0 to 255. In order to refer to this value we invented the
unit ABS.
We implemented a driver for the SERVO12C, which allows an application to change the position
of the pan/tilt camera platform. For that it sends a message via uORB to the driver, which
contains the desired position of the pan and the tilt servo, where the position can be specied
either in ABS or in radian. If the latter is the case, the position is converted to ABS by the
driver.
In both cases the driver must check if the positions lie inside the predened range of positions
the pan/tilt platform can assume and needs to adjust them if necessary.
Finally, the positions are sent to the SERVO12C via the I2C protocol. The chip then converts
them to PWM signals which are applied to the corresponding servos.
In order to enable the driver to convert from radians to ABS we wrote a calibration application.
This application uses the marker detection system to rst place the marker in the midpoint of
the cameras FOV and then gradually changes the position of the pan servos motor shaft while
measuring the corresponding angle. After that it does a linear regression on these data points
and provides the computed parameters to the driver. This procedure is then repeated for the tilt
servo.
5.2.2 Marker Detection System: PointGrey Firey Camera and
ODROID-U2
The marker detection system we utilized was already set up by another work [Ngeli 2013]
and uses a 0.3 MP PointGrey
6
black and white Firey camera running at 30 Hz to observe the
3
http://www.makerbot.com/
4
http://www.graupner.de/
5
http://www.hobbytronics.co.uk/datasheets/slave_servo_datasheet_v02.pdf
6
http://ww2.ptgrey.com/
23
5 Hardware and Implementation
Figure 5.2: The marker detection system consists of a PointGrey black and white Firey camera and an
ODROID-U2 single-board computer
marker. Captured frames from this camera are transmitted via USB to an ODROID-U2
7
single-
board computer, which runs the Ubuntu operating system. If a marker is present, its 3D position
in camera coordinates is then extracted from these frames by the ARToolKitPlus
8
library and
sent to the PX4FMUover an UART connection. On the PX4FMUa receiver application extracts
the relevant information and publishes it via uORB.
5.2.3 Implementation of the Position Controller
The implementation of position controller followed the ideas presented in Chapter 4.1.2. The
sample time dt was, thereby, set to 33 ms, since the Firey camera was running at 30 Hz and
we made the simplifying assumption that the time needed to extract the position of the marker
was constant.
A problem was that the current position of the servo motor shafts, and , could not be ob-
served. We, therefore, estimated them as the last position, which was set. However, to do so it
had to be ensured that
and lie inside the predened range of positions the pan/tilt platform
could assume. If this check would be omitted the position sent to the driver might be adjusted
by it, which led to wrong estimates.
5.3 Distance sensors
For measuring the distance towards an obstacle, we used the MB1222 I2CXL-MaxSonar-EZ2
ultrasonic distance sensor. It is suggested for collision avoidance by its manufacturer and can
detect persons up to approximately 2.4 m.
9
Furthermore, its measurements can be triggered and
read out via I2C.
7
http://www.hardkernel.com
8
http://studierstube.icg.tugraz.at/handheld_ar/artoolkitplus.php
9
http://www.maxbotix.com/documents/I2CXL-MaxSonar-EZ_Datasheet.pdf
24
5.3 Distance sensors
We extended an already existing driver for this sensor, such that it can trigger the measurement
and read out multiple sensors. However, when using multiple sensors they can inuence each
other, since one sensor might record the ultrasound emitted by another sensor. A save method to
avoid this is to let one sensor complete its measurement before starting the measurement of the
next sensor. Even so, this method rapidly reduces the measurement rate of the sensors, which,
if only one sensor is used, can vary between 10 and 40 Hz depending on the environment and
the farthest distance one wants to measure.
Thus, we decided to implement a function, which allows to group sensors together, such that
the measurements of all the sensors in one group must be completed before the measurements
of the next group are started. With this it is possible to group sensors together, which are
unlikely to inuence each other, for example, sensors which are oriented in opposite directions.
Furthermore, we designed and 3D printed funnels, which we attached to the sensors, thereby,
physically shielding them from each other. In addition, we also designed brackets, which can
be attached to the quadrotors frame, to mount the sensors on the quadrotor (see Figure 5.3).
25
5 Hardware and Implementation
(a) (b)
Figure 5.3: The MB1222 I2CXL-MaxSonar-EZ2 ultrasonic distance sensor attached to the quadrotor
with (a) and without (b) funnel
26
6
Experiment Setup and Simulation
In the next sections we describe the experiments, which were carried out to investigate how
the ultrasonic distance sensors might inuence the measurements of each other and to test the
functionality of the pan/tilt platforms position controller.
For the latter experiment the servos had to be modied such that the position of their motor
shafts could be read out directly. This modication is also described.
Furthermore, we explain the simulation of the quadrotor system and how this simulation was
evaluated.
6.1 Investigating the Inuence Ultrasonic Distance
Sensor Might Have on the Measurements of Each
Other
As mentioned previously one ultrasonic distance sensor might inuence the measurements of
another ultrasonic distance sensor if both are measuring at the same time.
We conducted an experiment, where we tried to nd out how strong this inuence was. We then
went on and attached funnels to the sensors, thereby, shielding them from each other. It was
then tested if this reduces the inuence they had on one another.
For this experiment, two distance sensors were attached to a quadrotors body frame, using
the brackets described in Chapter 5.3, which was then placed in a corner (see Figure 6.1), rst
at a distance of 70 cm and afterwards of 100 cm from each of the two corner walls. The
measurements of the sensors were then started at the same time and their reported distances
were recorded.
27
6 Experiment Setup and Simulation
Figure 6.1: Experiment setup for testing the inuence two ultrasonic distance sensors might have on
each other, when used at the same time. The body frame was rst placed at a distance of
70 cm and then of 100 cm from each of the two walls.
28
6.2 Servo Modication
Figure 6.2: Modication of a servo. The white, green and black wires are connected to the control circuit
of the servo. The blue and the second black wire were added in order to measure the position
of the servos motor shaft.
6.2 Servo Modication
A servo contains a feedback potentiometer, which informs its position controller about the cur-
rent position of its motor shaft.
We opened the pan and the tilt servo and soldered a wire to each of their potentiometers (see Fig-
ure 6.2), which we then lead out of the servo casing. The voltage measured on these wires is
directly related to the position of the servos motor shafts. To convert it to radian we measured
the corresponding voltage for different positions of the servos motor shaft and then calculated
a linear regression to obtain the conversion factor.
6.3 Testing the Functionality of the Pan/tilt Platforms
Position Controller
In order to test the functionality of the pan/tilt platforms position controller a marker was placed
at different positions relative to the platform and it was then measured how long the controller
needed to orient the platform, starting from a known initial position, such that in the end the
marker was placed in the middle of the PointGrey Firey cameras FOV.
Because the position of the pan and the tilt servo was controlled independently by the controller,
we decided to test its functionality for each of the platforms two degrees of freedom separately
(see Figure 6.3). Therefore, the rst two marker positions were chosen such that the pan servos
motor shaft needed to be rotated by 24
and by 16
and by 10
,
respectively.
Thereby, we measured the angle of view of the PointGrey Firey camera to be 48
horizontally
and 32
vertically.
Because of the sensitivity of the marker detection system to a changing illumination of the
29
6 Experiment Setup and Simulation
(a) (b)
Figure 6.3: Experiment setup for testing the functionality of the pan/tilt platforms position controller.
The marker was positioned such that the platform either moved in a tilting (a) or a panning
(b) motion.
marker, it was important to control the lighting conditions while these measurements were
taken.
6.4 Simulating the Quadrotor System
We gradually implemented a simulation based on the models and controllers described in Chap-
ter 3 and 4. First, the bang-bang position controller for the servo model was implemented and
the model was tted to our measurements of the servos system responds. This was then used
as a building block for the simulation of the pan/tilt platform.
We then extended a simulation of a quadrotor model provided to us according to the ideas de-
scribed in Chapter 4.2. Finally, this simulation was combined with the simulation of the pan/tilt
platform.
6.4.1 Simulating and Evaluating the Servo Model
The servo model should be used in the simulation of the pan/tilt platform. Therefore, we
recorded the system response of the two servos ten times with an oscilloscope while they were
incorporated in the pan/tilt platform with the PointGrey Firey camera attached.
These measurements were then put on top of each other and an average curve was calculated.
In order to do that the time delays between them were estimated through computing their cross-
correlation. For each of the two servos the model parameters
s
and
max
were then read out
from their corresponding averaged system responds.
For evaluating the model the movements of the servos were recorded while they headed for var-
30
6.4 Simulating the Quadrotor System
ious positions from a known starting position. For each of the provided positions the recording
was repeated ten times and an average was calculated as described above. Then the same posi-
tions were provided to the model and the differences between the simulation and the averaged
measurements were calculated.
6.4.2 Evaluating the Pan/tilt Platform Model
To evaluate the pan/tilt platform model we used a similar procedure to the one described in the
previous section. We, thereby, calculated an average of the measurements obtained through the
experiment for testing the functionality of the pan/tilt platforms position controller and then
compared it to the simulated movement of the pan/tilt platform model.
6.4.3 Simulating the Quadrotor Flight
The pan/tilt camera platform simulation was incorporated in an already existing simulation of a
quadrotor, which we extended as described in Chapter 4.2.
With this simulation we then tested the functionality of the extension for the quadrotors position
controller, therefore, if it could follow a marker while avoiding obstacles. In addition, we tested
if the problems the marker detection system might has through using a camera rigidly attached
to the quadrotor, namely, the loss of the marker, due to rapid sideways or forwards/backwards
movement of the marker (compare Chapter 1.2), could be mitigated with the pan/tilt platform.
31
6 Experiment Setup and Simulation
32
7
Results and Discussion
We now present and discuss the results of the experiments and the simulations described in the
previous chapter.
7.1 Inuence Ultrasonic Distance Sensor Might Have
on the Measurements of Each Other
As one can see from Figure 7.1 both distance sensors reported 1 m when they were used with
funnels. This corresponded to the actual distance at which the quadrotors body frame was
placed from the corner walls. However, if the sensors were used without the funnels, one sensor
repeatedly reported 38 cm (see Figure 7.2).
After the quadrotors frame was placed at a distance of 70 cm from the corner walls, the
experiment was repeated and we obtained a similar result. With funnels both sensors re-
ported 70 cm (see Figure 7.3), without funnels one sensor started to report distances of around
38 cm (see Figure 7.4).
The reason for why only the measurements of one sensor started to oscillate if no funnels were
used, while the measurements of the other one remained relatively stable, was that these sensors
received their take measurement command via I2C. Hence, they were not exactly triggered at
the same time, but with a slight delay, which we measured with a logic analyzer to be smaller
than half a millisecond. One can, therefore, see from these measurements, which sensor was
triggered rst and which one second, since the rst one inuenced the measurements of the
second one.
It is interesting that the measurement error, which one could see if no funnels were used, was of
33
7 Results and Discussion
a similar magnitude for both experiments. So far we have no explanation for this phenomena.
We tried to exclude other sources of measurement errors through ensuring that the sensors, when
used individually, reported the correct distance every time we attached a funnel or removed one
from them. Furthermore, to rule out a programming error, we also made an experiment where
we removed the funnels while not stopping the measurements. The results were similar to the
ones already discussed.
From these experiments we concluded that funnels can diminish the inuence ultrasonic dis-
tance sensor might have on the measurement of each other. However, they were conducted in
a static environment and the use of these sensors in a dynamic environment might introduce
additional noise. To deal with this a lter, as suggested by [Bouabdallah et al. 2007], might be
required.
34
7.1 Inuence Ultrasonic Distance Sensor Might Have on the Measurements of Each Other
0 2 4 6 8 10 12 14 16 18 20
0
10
20
30
40
50
60
70
80
90
100
110
Time [s]
M
e
a
s
u
r
e
d
d
i
s
t
a
n
c
e
[
c
m
]
Sensor 2
Sensor 1
Figure 7.1: 1 m with funnels.
0 2 4 6 8 10 12 14 16 18 20
0
10
20
30
40
50
60
70
80
90
100
110
Time [s]
M
e
a
s
u
r
e
d
d
i
s
t
a
n
c
e
[
c
m
]
Sensor 2
Sensor 1
Figure 7.2: 1 m without funnels.
35
7 Results and Discussion
0 2 4 6 8 10 12 14 16 18 20
0
10
20
30
40
50
60
70
80
Time [s]
M
e
a
s
u
r
e
d
d
i
s
t
a
n
c
e
[
c
m
]
Sensor 2
Sensor 1
Figure 7.3: 70 cm with funnels.
0 2 4 6 8 10 12 14 16 18 20
0
10
20
30
40
50
60
70
80
Time [s]
M
e
a
s
u
r
e
d
d
i
s
t
a
n
c
e
[
c
m
]
Sensor 2
Sensor 1
Figure 7.4: 70 cm without funnels.
36
7.2 Functionality of the Pan/tilt Platforms Position Controller
7.2 Functionality of the Pan/tilt Platforms Position
Controller
Through the measurements we obtained, it could be veried that the pan/tilt platforms position
controller we implemented was indeed able to control the position of the platform such that the
marker is placed in the FOV of the marker detection systems camera. Its average precision,
thereby, was better than 2
.
However, as one can see from the red and the green curve in Figure 7.5 the performance of
the controller could vary noticeably from test to test. Furthermore, it could happen that the
controller might overshot for one test (see the blue curve in Figure 7.5) and then not produced
any overshoots for the following tests.
We also observed that the time the controller needed to orient the pan/tilt platform, such that
the marker is placed in the FOV of the camera, might not get shorter if the distance, by which
servos motor shaft needed to be moved, was reduced (see Figure 7.6).
A possible explanation for this variance of the pan/tilt platforms position controllers perfor-
mance might be that the marker detection system used the Ubuntu OS, which is not a real-time
OS and so there are no guarantees given for the time needed to compute the position of the
marker from a captured frame. Hence, the system has not a constant rate at which it sends the
messages, containing the measurements of the markers position, to the PX4FMU, even though
the frame rate of the camera was set to 30 Hz. Therefore, it is not possible for the pan/tilt
platforms position controller to synchronize its execution with the image-taking cycle of the
camera or, for that matter, to predict when the camera might take an image of the marker.
This could lead to two different problems:
First, since the positions of the servos motor shafts could not be observed by the controller
and, thus, their last set positions were used as an estimation, it was implicitly assumed that
the marker detection system took its measurements always after the servos had nished their
movement. Violating this assumption might be the reason why the controller, from time to time,
produced overshoots even though its gain was not changed.
The second problem was that the interval between two position messages could vary and since
the controller was only able to carry out its computation when a position message arrived from
the marker detection system, the time between two outputs from the controller could also vary
to the same extent. This might explain the variance of the time needed to orient the platform.
37
7 Results and Discussion
0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4
1
1
3
5
7
9
11
13
15
17
19
time [s]
p
o
s
i
t
i
o
n
o
f
t
h
e
p
a
n
s
e
r
v
o
s
m
o
t
o
r
s
h
a
f
t
[
]
test run 1 (target pos.: 16)
test run 2 (target pos.: 16)
test run 3 (target pos.: 16)
Figure 7.5: Three measurements of the movement of the pan servos motor shaft, where the position of
the marker and the controllers gain were left unchanged. The measurements were ltered
in order to remove measurement noise.
0 0.05 0.1 0.15 0.2 0.25 0.3 0.35
2
0
2
4
6
8
10
12
14
16
18
20
22
24
26
28
30
time [s]
p
o
s
i
t
i
o
n
o
f
t
h
e
p
a
n
s
e
r
v
o
s
m
o
t
o
r
s
h
a
f
t
[
]
test run 1 (target pos.: 16)
test run 2 (target pos.: 24)
Figure 7.6: Two measurements of the motion of the pan servos motor shaft, where the marker was rst
positioned such that the servo needed to rotate its shaft by 16
(red curve). The controllers gain remained unchanged. The measurements were ltered in
order to remove measurement noise.
38
7.3 Simulation of the Quadrotor System
7.3 Simulation of the Quadrotor System
We now present the results obtained through simulating the quadrotor system. Furthermore, in
case of the servos and the pan/tilt platforms simulation, these results were also compared with
the measured real-world behaviour.
7.3.1 Evaluation of the Servo Model
A comparison between the simulated and the averaged measured movement of the tilt servos
motor shaft by 20
is displayed in Figure 7.7. One can see a jump at the start of the movement of
the servos motor shaft and spikes at its end. These phenomena are present in all measurements
of movements of the servos motor shaft, however, we have no explanation for them. Ignoring
these, the biggest difference between the simulation and the averaged measurements in Figure
7.7 has a magnitude of around 2.7
s
m
o
t
o
r
s
h
a
f
t
[
]
Average of 10 measurements
Simulation
0 0.02 0.04 0.06 0.08 0.1 0.12 0.14
2
1
0
1
2
3
4
5
time [s]
D
i
f
f
e
r
e
n
c
e
b
e
t
w
e
e
n
t
h
e
r
e
a
l
a
n
d
t
h
e
s
i
m
u
l
a
t
e
d
p
o
s
i
t
i
o
n
[
]
(Average of 10 measurements simulation)
Figure 7.7: Validation of the servo model. The movement of the tilt servos motor shaft was recorded
while it should move by 20
s
m
o
t
o
r
s
h
a
f
t
[
]
Average of 10 measurements
Simulation
0 0.2 0.4 0.6 0.8 1 1.2
1
0
1
2
3
4
5
6
7
8
9
10
time [s]
D
i
f
f
e
r
e
n
c
e
b
e
t
w
e
e
n
t
h
e
r
e
a
l
a
n
d
t
h
e
s
i
m
u
l
a
t
e
d
p
o
s
i
t
i
o
n
[
]
(Average of 10 measurements simulation)
Figure 7.8: Validation of the pan/tilt platform model. The movement of the pan servos motor shaft was
recorded while it should move 24