Stabilization of a Quadrotor using Image-based

Processing

Romeu Paulo Santos Chumbo ∗

∗Department of Mechanical Engineering - IDMEC, Instituto SuperiorTecnico, Technical University of Lisbon (TULisbon) Av. Rovisco Pais,

1049-001 Lisboa, Portugal; e-mail: romeu.chumbo@ist.utl.pt

Abstract:The control of the horizontal position of a quadrotor when in hovering, requires the observationof the translation. One of the possible solutions is the use of GPS, but its accuracy is unsatisfying.It is thus necessary to develop a solution that warrants enough accuracy.This work develops an image-based control strategy. It places a camera under the quadrotortowards the ground, which identifies the UAV motion using image tracking. The result is thenintegrated into a cascade controller, regulating its position back to the original pose.Two LQR controllers are projected and simulation-validated, one for each loop in the cascadecontrol. Then, we proceed to the integration of the homography-based position control, testingfor multiple values of image resolution and vision angles.Once the control method is validated, the image tracking algorithm is implemented in Matlab.This algorithm searches the reference image over a video sequence, and returns the homographywhich expresses the motion.After program optimization, the speed was still not enough. For this reason, and because theSRV-1 requires a C implementation, the program is converted into that language first on aPC platform. The C code performs the real-time tracking up to 30Hz. Then, the programperformance is tested using multiple types of image patterns.The algorithm was ported on the SRV-1 but the resulting processing rate of 0.17Hz is far belowthe 5Hz limit that was necessary for the position control. For this reason, the projected controlwas not taken to implementation on the UAV.

Keywords: Quadrotor, Image Tracking, Visual Servoing, Real Time Control.

1. INTRODUCTION

This work was suggested by the company UAVision, whichis developing a quadrotor, and needed an additional con-trol system. The quadrotor is quipped with:

• Self stabilization with an IMU, compass and baro-metric height sensor• Take-off and landing in automatic mode• Autonomous navigation with GPS• Control from a ground station and real-time image

transmission

However, when in hovering, the quadrotor drifts in thehorizontal plane. This can be caused by multiple reasons(side wind, small accuracy of the IMU data, small accu-racy in the actuation, noise, among others). This work isproposed to solve this problem, coming up with an image-based solution which regulates the position, avoiding un-desired movements. For the implementation, UAVision hasavailable the DSP micro controller surveyor SRV-1, whichallows the image processing directly from a camera. Weneeded to execute each of the following steps:

(1) Design of a horizontal translation controller for thequadrotor

(2) Simulation of the controller with homography-feedback

(3) Development, validation and test of the image-tracking algorithm

(4) Implementation of the tracking algorithm in C, notonly on PC but also on SRV-1

(5) Integration of SRV-1 into the quadrotor, validatingthe projected controller

This article is organized as follows: in section 2, a briefstate of the art about quadrotors and image tracking is pre-sented; in section 3 the model of quadrotor is introducedas well as the control design; in section 3 the homography-based control is integrated as an outer loop for horizontalposition control and the close loop behavior is studied;in section 4 the image-tracking algorithm is introduced,implemented in Matlab and its performance is evaluatedagainst the configuration parameters. It is then imple-mented in C language exhibiting real time performance;in a final section, the main conclusions of this work arepresented as well as some suggestions for the next steps ofthe project.

2. STATE OF THE ART

2.1 Quadrotor

The quadrotor concept began in the beginning of theXX century, when in 1907 Louis and Jacques Breguet

developed the ”Gyroplane No.1” [aviastar, 2011]. Recently,the development in technology and electronics, the appear-ing of sensors and miniature microprocessors allowed theconstruction of smaller quadrotors, light and economicallyaccessible, where it is possible to implement and developoriginal solutions, allowing new scientific achievements.

2.2 Visual Servoing

Visual servoing is also known as Visual Servo Control. Ituses the visual information to control the position of arobot relatively to a target or a set of target-characteristics[Corke, 1994]. In literature the visual servoing methods arein general classified as follows, according to its trajectory:

• 3D visual servoing (PBVS): the task is expressed inCartesian space, i.e. the information obtained fromtwo images (using reference image and current image)is used explicitly to rebuild the position and attitudeof a camera [Wilson et al., 1996], [Basri et al., 1998],[Taylor and Ostrowski, 2000] e [Malis and Chaumette,2002].• 2D visual servoing (IBVS): This method does not

need of the explicit prediction of position’s error inCartesian space [Espiau et al., 1992] [Chaumette,2004], because it builds an isomorphic task for thecamera’s position.• 2D 1/2 visual servoing : the path is expressed from

the image Cartesian space, i.e. the rotation errorof estimated explicitly, and the translation error isexpress from the image.

Where PBVS and IBVS stand for Position-Based VisualServoing and Image-Based Visual Servoing, defined by[Sanderson and Weiss, 1980]. The solution used here for thecontrol of a quadrotor is based on [BenHimane and Malis,2007]. It is a 3D visual servoing method that controlsthe robot control building an isomorphic path for cameraposition in the Cartesian space, based on homography.

Image tracking Image tracking is the process to findand track a reference image along a sequence of imagesor video, through image analysis. It is classified in twomain groups [BenHimane and Malis, 2007]. The first iscomposed by methods which search features, such asline segments or contours [Isard and Blake, 1996], [Torrand Zisserman, 2000], [Drummond and Cipolla, 1999].The second group contains the methods which use theinformation relative to the image intensity.

[BenHimane and Malis, 2004] proposes the use of anEfficient Second-Order Minimization. The ESM uses anhomography to identify the relation between two pictures.In 2010, [Tahri and Mezouar, 2010] proposes a variantbased in ESM method, where he changes the method tominimize the cost function. The approach in this work usesnot only the ESM and Tahri method, but also the simpleJacobian method for image tracking.

3. MODELING THE PROBLEM AND CONTROLLERDESIGN

The solution we are looking for integrates control andimage processing. In this part it is presented the control

solution. In the following one, we expose the image-processing solution and integration.

The UAV open-loop dynamic model is introduced throughthe dynamics equations, and the variables which makeit. The next step is the construction of the model andlinearization about the equilibrium point, allowing us tohave a linearized model of the UAV. The low-level controlstabilizes the attitude and height, the existent sensorsin the UAVision model.The high-level controller controlsthe horizontal translation through image feedback. Thisfeedback is made using the homography.

3.1 Quadrotor

In this work we define two frames, the inertial (or fixed)frame and the UAV frame, shown in figure 1.

((a)) Inertial frame (NED) ((b)) Quadrotor frame

Fig. 1. Example of quadrotor and frames used (from[Henriques, 2011])

The rotation matrix from equation 1 presents the rota-tion from the earth fix frame (the North-East-Down orNED frame) to the UAV body frame.. This matrix isthe successive multiplication of three elementary rotationmatrices (R), one for each Euler angle, as follows: S =R(φ)R(θ)R(ψ).

S =

[cψ cθ cθ sψ −sθ

cψ sφ sθ − cφ sψ cφ cψ + sφ sψ sθ cθ sφsφ sψ + cφ cψ sθ cφ sψ sθ − cψ sφ cφ cθ

](1)

The kinematic equations of the quadrotor are:

p = [x, y, z]T (2)

V = [U, V, W ]T (3)

Ψ = [φ, θ, ψ]T (4)

Ω = [P, Q, R]T (5)

where p is the position of the quadrotor in the NED frame,V is the linear speed of the quadrotor in the body frame,Ψ are the Euler angles, and Ω is the angular speed in thebody frame.

3.2 Dynamic Model

From Newton’s second law, we have the following equa-tions for dynamics:

m(V + Ω×V) = F

I(Ω + Ω× IΩ) = M(6)

where m is the quadrotor mass, I is the quadrotor inertiamatrix, F and M are the total force and moments appliedto the quadrotor. The position and attitude of the quadro-tor in the NED frame is given by the cinematic equations:

p =ST V

Ψ =Rt Ω(7)

The propulsion forces in UAV frame are the following:

Fz =− b (W 21 +W 2

2 +W 23 +W 2

4 )

Fm =[0 0 Fz]T

Mx =b (W 24 −W 2

2 ) l

My =b (W 21 −W 2

3 ) l

Mz =d

2(W 2

2 +W 24 −W 2

3 −W 21 )

Mm =[Mx My Mz]T

(8)

where Wi is the speed of motor i, Fz is the thrust force, Fmis the total force due to motors propulsion and Mm is theresultant moment due to motor’s action. The constants be d are respectively the propeller propulsion and torquecoefficients. The aerodynamic forces in the UAV frame arecalculated as follows:

va =Ω− wbsFwx =−Kaxy vax |vax |Fwy =−Kaxy vay |vay |Fwz =−Kaz vaz |vaz |Fw =[Fwx Fwy Fwz ]

T

(9)

where Fw is the force due the interaction UAV-air. Kaxye Kaz are the UAV aerodynamic coefficients. The weightof the UAV is given by:

Fg =S [0 0 g]T , (10)

where S is the matrix presented in equation 1 and g thegravitic acceleration. The global resultant of forces andmoments acting on quadrotor are:

Ft =Fg + Fm + FwtMt =Mm

(11)

The parameters of the model (table 1) were providedby UAVision and were considered to elaborate the UAVdynamic model. The mathematic model of the UAV is

Designation Value

m 1.5 kgl 0.375 mb 2.9023e−6 N.s2/radd 2.9023e−7 N.m.s2/rad

I

[78.5938 −0.0080 −0.0387−0.0080 145.4917 −0.0041−0.0387 −0.0041 70.9423

]10−4 kg.m2

Table 1. Parameters used for the dynamic model.

implemented in the function dynamic model.m. The modelwas linearized at hovering, when the propulsion of the fourrotors equals the weight of the quadrotor, being the speedof each rotor given by:

Wi = 2√mg/(4 b) (12)

Although the system has twelve states, which is the sixdegrees of freedom and the respective time derivatives, wedon’t consider the horizontal positioning states (U V x y)in the low-level model. The states considered for thedynamic model are thus [W P QRz φ θ ψ]. The dynamic

1

saida

MATLAB

Function

dynamic_model

1

s

Integrator1

2

WindSpeed

1

motors' speed

Fig. 2. Open loop model

model matrices, using numerical linearization are the fol-lowing:

A =

0 0 0 0 0 −0.000491 −0.000491 0

0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0

1 0 0 0 0 0 0 0

0 1 0 0 0 0 0 0

0 0 1 0 0 0 0 0

0 0 0 1 0 0 0 0

(13)

B =

−0.00436 −0.00436 −0.00436 −0.00436

−0.00000555 −0.312 −0.0000398 0.312

0.168 −0.0000158 −0.168 0.0000185

−0.0460 0.0459 −0.0461 0.0462

0 0 0 0

0 0 0 0

0 0 0 0

0 0 0 0

(14)

Design of the Low-Level Controller The weights R eQ used to project the LQR regulator for the presenteddynamic system are the following:

R =

[1/20 0 0 0

0 1/20 0 0

0 0 1/20 0

0 0 0 1/20

]2

Q =

1 0 0 0 0 0 0 0

0 1 0 0 0 0 0 0

0 0 1 0 0 0 0 0

0 0 0 1 0 0 0 0

0 0 0 0 20 0 0 0

0 0 0 0 0 50 0 0

0 0 0 0 0 0 50 0

0 0 0 0 0 0 0 20

2

(15)

For a sample frequency of 50Hz, the gain matrix Kobtained is presented in equation 16:[

−0.01498 0.004827 59.07 −17.43 −194.8 −0.02393 566.2 −19.36

−0.01498 −42.17 0.001426 17.43 −194.8 −520.4 0.008488 19.36

−0.01498 0.008872 −59.07 −17.43 −194.8 −0.01300 −566.2 −19.36

−0.01498 42.15 −0.002619 17.43 −194.8 520.5 −0.002445 19.35

](16)

The model with the implementation of the low-level

[0 0 0]'

wind speed (NS/EW/DU)

sqrt(m*g/(4*b))*[1 1 1 1]'

motor's hovering speed

K

feedback gain

motors' speed

WindSpeed

states

dynamic model

(0 0 0 0)

Const

W P Q R z phi theta psi

2

phi theta psi_ref

1

z_ref

selector

Fig. 3. Simulink model containing the low-level controller

controller is presented in figure 3. This diagram im-plements a LQR regulator to the open-loop system

(shown in figure 2). The simulation whose results are

0 2 4 6−1

−0.5

0

0.5

time (s)

position (m)

x

y

z

0 2 4 6−0.05

0

0.05

0.1

0.15

time(s)

angle(rad)

φ

θ

ψ

Fig. 4. Simulation to test the low-level regulator

shown in figure 4 tests the controller from [φ θ ψ z] =[0.1(rad) 0.1(rad) 0.1 (rad),−0.5(m)] to [0(rad) 0(rad)

0(rad) − 0.6(m)].

Project of the High-level Controller The high-level con-troller receives the information from image analysis andtranslate them into φ e θ references, which provides to thelow-level loop. The project of this LQR regulator is similarto the one projected before, in equation 16. The dynamicsystem found via numerical linearization is given by:[

x

y

x

y

]=

[0 0 1 0

0 0 0 1

0 0 0 0

0 0 0 0

][x

y

x

y

]+

[0 0

0 0

0 −9.81

9.81 0

][φrefθref

](17)

The Q and R weights used for the project of the controllerare shown in equation 18.

R =

(1

4 π180

[1 0

0 1

])2

Q =

[1/(0.8) 0 0 0

0 1/(0.8) 0 0

0 0 1 0

0 0 0 1

]2(18)

The regulator gain matrix obtained for the sample fre-quency of 5Hz is shown in equation 19. The block dia-gram which contains the cascade control implementationis shown in figure 5. There we can see the block diagram ofthe high-level loop. It regulates the quadrotor to a givenx y U V reference, using the projected LQR controller. Theselector blocks xy and psi are meant to select the variableswe want from the internal model. The rectifies x y blocktranslates the x y error from the inertial frame to the bodyframe.

K =

[0 0.2343 0 0.2880

−0.2343 0 −0.2880 0

](19)

The results of the the regulation from an initial position

z_refx y U V_ref

x y

x y U V

psi

Out

recti!es_x_y

psi_ref

psi

z psi_ref

phi theta_ref

xy z

phi theta psi

U V

LowLevel

K

Fig. 5. Simulink model of the high-level loop

error x = 0.2my = 0.2m are shown in figure 6. Therewe can verify that the system reaches the stable target

in approximately 2s. Once this controller validated, weconsider validated as well the cascade control proposed.

0 1 2 3 4 5−1.5

−1

−0.5

0

0.5

time(s)

position(m)

x

y

z

0 1 2 3 4 5−4

−2

0

2

4

time(s)

angle (degrees)

φ

θ

ψ

Fig. 6. Results of the high-level model simulation

3.3 Homography

Definition An homography is a geometric transforma-tion which maps entities in a projective space. It is usefulbecause it can identify the same region of an image, whena camera observes from different positions. According to[Kanatani, 1998], from the homography between two im-ages from the same planar surface, it is possible to extractthe relative position of the camera. Let pa and pb be thecoordinates of the point p in the perspectives a and b. LetHab be the homography which contains the transformationbetween both perspectives, as presented in equation 20.

pa =

[xaya1

], pb =

[xbyb1

],Hab =

[h11 h12 h13h21 h22 h23h31 h32 h33

](20)

The pb coordinates can be obtained from pa by using theequations 21 and 22:

p′b =

[w′ xbw′ ybw′

]= Habpa (21)

pb = p′b/w′ =

[xbyb1

](22)

Reconstruction of Motion We want to validate the pos-sibility of control the quadrotor dynamic model basedon homography analysis. In this part, the homography iscalculated relating two projections of points into imageplane. The implementation of such system into Simulink,is done using an s-function, which executes the shown infigure 8.

x_target

target_ x y z phi theta psi

e_xy

x y z

phi theta psi

QuadrotorSelector

sfunction_image

s-function

Fig. 7. Block diagram with homography-based closed loop.The selector block is meant to choose only x y fromthe 6DOF given by the s-function

Figure 7 shows the block diagram which makes thehomography-based control in x y. The block designed byQuadrotor, contains the model of figure 5. In the func-tional block diagram of figure 8 are shown the operationsexecuted by the s-function. The parameters to evaluate arethe following:

(1) Pixelization/resolution

Reference pose Actual pose

Points on the ground

Points on image plane

for the reference pose

Points on image plane

for the actual pose

Homography which relates both

set of points

Normal vector

A!tude of the Vehicle

Result of homography decomposi"on:

Transla"on and rota"on of the quadrotor

Fig. 8. Block diagram of the sfunction image

(2) View angle

The results shown in figure 9 are taken from a simulationbeginning in [x y z ψ] = [0.3m − 0.3m − 3m 0.2 rad] to[0m 0m − 2m 0 rad].

0 2 4 6 8 10−3

−2

−1

0

1

2

3

time(s)

angular position error(degrees)

ephi

eteta

epsi

((a)) Error to 1024 pixels, 120degrees

0 2 4 6 8 10−10

−5

0

5

10

tempo(s)

erro

de

posi

ção

angu

lar

(gra

us)

ephietetaepsi

((b)) Error to 240 pixels, 120 de-grees

Fig. 9. Angular position estimation errors for differentresolutions, using homography

Analyzing figure 9 we can verify that the estimation erroris smaller when the system stabilizes. This is justifiedbecause the reconstruction of the movement is non-linear,and it is then sensitive to the UAV attitude angles. Insteady-state we can conclude that the error decreases forbigger resolutions.

0 2 4 6 8 10−8

−6

−4

−2

0

2

4

6

time(s)

angular position error (degrees)

ephi

eteta

epsi

((a)) Error to 240 pixels, 90 de-grees

0 2 4 6 8 10−10

−5

0

5

10

tempo(s)

erro

de

posi

ção

angu

lar

(gra

us)

ephietetaepsi

((b)) Error to 240 pixels, 120 de-grees

Fig. 10. Angular position estimation errors for differentvision angles, using homography

Figure 10 shows that for the same other conditions, theaccuracy get is better if the view angle is smaller. Capturea smaller area of the image implies that the points on theground are projected onto smaller area of the image. Thegraph of figure 11 allows us to conclude that for a biggerresolution, the final control has better accuracy. To the

smaller resolution (240 pixels), a bad performance couldbe expected, but the system oscillates x y (less than 5cm),which is an acceptable result. We can thus deduce that the

1 2 3 4 5 6 7 8 9 10

−0.1

−0.05

0

0.05

0.1

time(s)

position(m)

x

y

((a)) Error to 240 pixels, 90 de-grees

1 2 3 4 5 6 7 8 9 10

−0.1

−0.05

0

0.05

0.1

time(s)

position(m)

x

y

((b)) Error to 1024 pixels, 120degrees

Fig. 11. Estimation error in x y, for different resolutionsand enlarged for the position between −0.1 and 0.1m

homography-based control is achievable, as the simulationhas shown positive results.

4. TEXTURED-IMAGE TRACKING

4.1 Tracking as a mathematic minimization problem

Once we have a controlling scheme based on homography,we need then to obtain it from the image-tracking homog-raphy. In this work, the homography is calculated fromthe parameters vector x. The goal of this image trackingalgorithm is to find the correction ∆x of x, such thats(∆x + x) coincides with the reference image, as shownin [BenHimane and Malis, 2004]. The vector ∆x can becalculated from the minimization of the quadratic error inthe vector s:

f(∆x) =1

2‖(s(∆x + x)− s(e))‖2 (23)

The solution used by [S. BenHimane, 2004] was to estimatethe Hessian matrix with a second order Taylor series of thevectorial function of s(x) over x, calculated to x = 0.The solution of the minimization problem proposed by[S. BenHimane, 2004] leads to solve eq. 24, which maybe done using the pseudo-inverse (eq. 25).

∆s ≈ −1

2(J(e) + J(xc)) ∆x (24)

∆x ≈ −(

1

2J(e) +

1

2J(xc)

)+

∆s (25)

The method is called ESM, and was validated in [S. BenHi-mane, 2004] and [BenHimanhe, 2006], however, [Tahri andMezouar, 2010] suggested to edit the way ∆x is calculated,as shown in equation 26. The difference is that he uses thesum of the pseudo-inverse of each jacobian, instead of thepseudo-inverse of the jacobians average.

∆x ≈ −(

1

2J(e)+ +

1

2J(xc)+

)∆s (26)

Some alternative ways to calculate ∆x can tried by usingthe J(x)

∣∣x=0

, reference jacobian, or by J(x), the currentjacobian, or a combination of both. For each of thecombinations, we have the following equations:

∆x ≈ −(J(e)+

)∆s (27)

∆x ≈ −(J(xc)+

)∆s (28)

Equation 29 allows to get G from x, where A(x) =∑8i=1 xiAi. The matrixes present in 30 are the base used

to represent x.

G(x) = exp(A(x)) =

∞∑i=0

1

i!A(x)

i(29)

A1 =

[0 0 1

0 0 0

0 0 0

], A2 =

[0 0 0

0 0 1

0 0 0

], A3 =

[0 1 0

0 0 0

0 0 0

], A4 =

[0 0 0

1 0 0

0 0 0

]A5 =

[1 0 0

0 −1 0

0 0 0

], A6 =

[0 0 0

0 −1 0

0 0 1

], A7 =

[0 0 0

0 0 0

1 0 0

], A8 =

[0 0 0

0 0 0

0 1 0

] (30)

4.2 Evaluation of the Algorithm in Matlab

The set of images used for testing is the sequence of imagesused by [BenHimanhe, 2006], and presented in figure 12.

Fig. 12. Sequence of images used for testing

Minimization Method To minimize the function f(∆x)we have four options, as follows:

• Algorithm ESM, equation 25• ESM version modified by Tahri, equation 26• Reference jacobian, equation 27• Current jacobian, equation 28

Pyramid of images Let an image be resized by half ntimes having now n+1 images, representing one level of thepyramid each. The use of the pyramid of images consists indoing the search level after level, from the smaller image tothe full size image, instead of searching only in the full-sizeimage. The simulation results are shown in figure 13 wherewe can verify that the minimization by current jacobianwith a 5-level pyramid minimizes the total amount oftime spent along the sequence (50s). In figure 14 we cansee the evolution of one parameter of x when we searchthe reference image in figure 11. As the current jacobianconverges faster than the other methods we choose it toproceed in the work.

Bicubic Interpolation Function The bicubic interpola-tion comes from the need to calculate the value of theimage in non-integer pixels. This can be done using severalmethods, but the bicubic interpolation was proven to be

0

10

20

30

40

50

60

70

80

1 1/2 1/4 1/8

Tim

e t

o s

ea

rch

th

e s

eq

ue

nce

(s)

Maximum scale searching

ESM

Tahri

Reference Jacobian

Current Jacobian

Fig. 13. Processing time for different minimization types,with image pyramid, to search a 152×158 pixel imagereference in a 512 × 512 image, along the sequenceshown in 12. Across the abscissa axis, we have havethe scale of the image where the program stopped tosearch, relatively to the original 512 × 512 image. Inthe bigger scale, the pyramid has 5 levels.

0 10 20 30 40 50−20

−15

−10

−5

0

Iterations number

Value of the parameters

ESM

Tahri

JI0

JI

((a)) x(1)

Fig. 14. Evolution of the first parameter of x when search-ing the reference in the image 5, after image 1

the most effective. The bicubic interpolation algorithmhas a constant, a, which varies the performance of theprogram. This part is meant to find the a value which leadsthe program to faster results. To find the a value that

Fig. 15. Interpolated point and the points used to calculatethe interpolation

−1 −0.9 −0.8 −0.7 −0.6 −0.5330

340

350

360

370

380

390

value for a

Nu

mb

er

of

ite

rati

on

Fig. 16. Number of iteration for some values of a.

gives the best performance to the program we’ve madesome tests to the sequence of images 12, varying a. Infigure 16 we can see that the best value for a is −0.74 asit minimizes the total iterations number required.

Jacobian step, ∆z A derivative is calculated by doinga difference of the image intensity in two near points,divided by the distance between both points. The jacobianstep, as we call it, is the distance between the point wherewe are measuring the derivative and the second point. Tosimplify, we call it ∆z. To find the best ∆z, the simulation

0 0.005 0.01 0.015 0.02231

232

233

234

235

236

237

ddz value (pixels)

Pro

cess

ing

tim

e (

s)o

((a)) Variation of processing timefor ∆z

0 0.005 0.01 0.015 0.02332

334

336

338

340

ddz

Ite

rati

on

s n

um

be

r

((b)) Variation in the total numberof iterations for ∆z

Fig. 17. Program performance for different values of ∆z.

was done for multiple values of the parameter, getting theplots in figure 17. We consider that the best value for ∆zis 0.006, as it have the smaller processing time and alsosmall iterations number.

Threshold coefficient Jthcoef The implementation ofthreshold consists in use only the high-gradient points todo image tracking.

330

340

350

360

370

0 0.1 0.2 0.3 0.4 0.5 0.6

Nu

mb

er o

f it

era

o

ns

Threshold

0

200

400

600

800

1000

0 0.1 0.2 0.3 0.4 0.5 0.6

Sim

ula

o

n

me

(s)

Threshold

Fig. 18. Program’s performance in function of Jthcoef

-0.015

-0.01

-0.005

0

0.005

0.01

0 0.1 0.2 0.3 0.4 0.5 0.6

Deviaon

Threshold

Parameter1

Parameter2

Parameter3

Parameter4

Parameter5

Parameter6

Fig. 19. Deviation of x for multiple values of Jthcoef,relatively to the values obtained for Jthcoef =0. Theseparameters are obtained for the searching in the image4

In figure 18 are presented the processing time and theiterations number for multiple values of Jthcoef. In figure19 are presented the variations in the 6 first parametersof vector x. There we can verify that the simulationtakes less time for bigger values of threshold and that theiterations number is minimum for thresholds between 0.2and 0.35. Analyzing figure 20 we can conclude that ψ isthe less affected by Jthcoef, keeping the error near to zero.Analyzing figures 18 and 20 we choose the value 0.3 forJthcoef, as with it the program runs faster, keeping a smalliterations number.

Maximum number of iterations The analysis of the plots(figure 21) shows that one iteration is enough to performtracking. However, three or four iterations would be moreadequate as it can guarantee more robustness for biggerdisplacements. Hereafter we will use the fixed numberof four iterations by level. When searching the wholesequence, the final displacement found was −33.75 pixelsin x −2.14 pixels in y e 2.14 degrees in z (ψ), as we cansee in figure 22.

Fig. 22. Result of image tracking along the sequence 12

4.3 Evaluation with Image Pairs

The three images from figure 23 were tested. The resultsshow that for displacements until 15 pixels and rotation of6 degrees, the algorithm can perform tracking for any ofthe tested images. For displacements above 20 pixels, onlywith the third image (figure 23) could perform tracking.Figure 24 presents one of the examples of divergence ofthe program.

((d)) Im. 1 ((e)) Im. 2 ((f)) Im. 3

Fig. 23. Images used for algorithm testing, and its mostimportant points (Jthcoef = 0.3)

Fig. 24. Example where the algorithm converges to a localminimum. In the left we can see the original imagewith the reference selected, and in the left we cansee the displaced image with the current trackingconverging to a local minimum

-0.0005

-0.0004

-0.0003

-0.0002

-0.0001

0

0.0001

0.0002

0.0003

0 0.1 0.2 0.3 0.4 0.5 0.6

Lin

ea

r d

ev

ia

on

(m

)

Threshold

x

y

z

-0.06

-0.04

-0.02

0

0.02

0.04

0.06

0 0.1 0.2 0.3 0.4 0.5 0.6

An

gu

lar

de

via

o

n (

de

gre

es)

Threshold

phi

theta

psi

((a)) Image 2 → 3, [x y z φ θ ψ] = [ 0.328 −0.440 0.996 −0.111 0.129 0.172][(3×m)(3× degrees)]

Fig. 20. Absolute error when threshold is applied, for one of the images where the search was made. The referencecoordinates are calculated without applying threshold, and are present in the caption of each sub-figure.

-0.0002

0

0.0002

0.0004

0.0006

0.0008

0 2 4 6 8 10 12

Lin

ea

r d

ev

ia

on

(m)

Maximum number of iteraons

x

y

z

-0.05

0

0.05

0.1

0.15

0 5 10 15

An

gu

lar

de

via

o

n (

de

gre

es)

Maxumum iteraons number

phi

theta

psi

Fig. 21. Results deviation in in function of the maximum allowed number of iterations.

4.4 C Implementation

Once the Matlab program is validated we proceed to Cimplementation, because of two reasons: the Matlab ver-sion is not fast enough, and because the SRV-1 requires aC written program. The C version was done in CodeBlocks[codeblocks, 2011], which uses the GNU GCC compiler andthe blackfin toolchain.

Figure 25 has a fluxogram with the layout used to developthe program in C.

Start of the Program

framecolorcvQueryFramecapture

framecvResizeframecolor

bwcvCvtColorframe

imagebuffer2structbw

makeI1pyr

image

I10

I11

I12

I13

I14

I10

I11

I12

I13

I14

pyramid

xhat

ghat

Rl0

Rl1

Rl2

Rl3

Rl4

I10

I11

I12

I13

I14

xhat

ghat

x!l

mydrawsearch

Rl0

Il0

ghattoview

todisplayImageStr2CVtoview

shows

trackingcvShowImagetodisplay

image

coli

rowi

ncols

nrows

Rl0

Rl1

Rl2

Rl3

Rl4

Rl0

Rl1

Rl2

Rl3

Rl4

makeI0pyr

framecolorcvQueryFramecapture

framecvResizeframecolor

bwcvCvtColorframe

Loop, while it

converges

Fig. 25. Fluxogram of the C program

4.5 Evaluation of the C implementation in PC

The testing results are shown in the table 2. There we cansee the program performance for each set of parameters.

((a)) Chess ((b)) Floor1 ((c)) Floor2

Fig. 26. Scenes where the program was tested

Analyzing the table, we verify that the number of levels

Window Res. N. lev.Time/frame(ms)ch. fl. 1 fl. 2

40× 64 160× 120

0 30.20 - -1 29.24 30.26 -2 31.22 28.36 -3 30.78 - -

80× 128 240× 320

1 57.18 42.33 -2 57.25 56.12 -3 68.62 56.08 -4 61.32 - -

160× 256 480× 640

1 87.58 70.18 -2 99.78 70.14 -3 108.67 82.9 -4 104.66 - -

Table 2. Performance of the C implementation into PC, where– means the program could not do image tracking

considered for the pyramid can affect the convergence ofthe program, as we can see in the floor 1 sample.

4.6 Implementation into Surveyor SRV-1

The SRV-1 belongs to UAVision, so that it is used only toimplement this program. As nobody in the company hadany touch with it before, we had to start from zero. Theinsertion of the C program in the SRV-1 board was donethrough the re-compilation of its firmware. The compilerwe used to compile the firmware, the mingw, did not have

(or had, but incomplete) some functions (as max, sqrt orprintf ), which could receive only for integers.

Fig. 27. Log of SRV-1 tracking one image

Once the program is implemented in the SRV-1, thenwe could test it. As implemented, it can perform imagetracking but it is too slow doing it. The processing of animage with 40×64 pixels takes about 6 seconds (0.17Hz),which is way too long for the control requisites we had(5Hz).

Knowing that there are GCC modules optimized for Black-fin, perhaps if we try to implement them in the surveyormodule it would have a better performance, but we didnot have time to try that.

5. CONCLUSION

With the aim of controlling the horizontal position in aquadrotor we proposed the control architecture integratingthe previous implemented stabilization with the externalloop to stabilize horizontally using image-based feedback.We built a dynamic model for a quadrotor, based on dataprovided by UAVision. Two LQR controllers were used forthe cascade control system.

Then, we integrated the homography-based feedback intothe cascade control. It was also tested for multiple config-urations of view angle and image resolution.

We have also implemented an image-tracking algorithmin Matlab. To improve its performance, we optimized itsparameters one by one, and then we tested its performancenot only with an image sequence, but also with imagepairs, in order to have a better tracking speed.

Once fully developed in Matlab, we proceeded to theimplementation of the same program in C language, tomake it faster and to implement into SRV-1 later. Thetests made in the PC revealed that the program can doreal-time image tracking, as it can search at a rate of 30frames per second.

Last, we proceeded to the implementation of the C pro-gram into the SRV-1, but the processing speed was notenough for the control requisites and we did not proceedto the experimental implementation.

5.1 Future Work

Owing to keep developing this project, we leave somepoints as suggestion of future work:

• Optimize the program, whether using other optimizedlibraries (such as GCC for blackfin) or programmingtechniques, so it can be used in the SRV-1 class ofembedded systems.

• Implement in a device with better processing/memorycapabilities.

• Since the video quality which arrives to processingdoes not depend only on resolution, a better qualitycamera may give a less noise image, allowing betterresults on image tracking.

• Make the algorithm functional in a bigger quantity ofscenarios, with more robustness.

• Use processing on base, instead of processing on boardof the UAV. Despite this process can have somedrawbacks, such as the image transmission UAV-base,and then the transmission of the control signal backto the UAV, this is a way to override the on boardprocessing capacities, since the image processing ismade on base.

• Try to use other image tracking techniques, such asfeature based methods [Torr and Zisserman, 2000].

REFERENCES

All the worlds’ rotorcraft - helicopters, autogyros, tilt-wingand tilt-rotor aircraft. 2011. URL http://aviastar.org/helicopters.html.

Code::blocks. 2011. URL http://www.codeblocks.org/.Ronen Basri, Ehud Rivlin, and Ilan Shimshoni. Visual

homing: Surfing on the epipoles. In ICCV98, pages 863–869, 1998.

S. BenHimane and E. Malis. Real-time image-based track-ing of planes using efficient second-order minimization.In Intelligent Robots and Systems, 2004. (IROS 2004).Proceedings. 2004 IEEE/RSJ International Conferenceon, volume 1, pages 943 – 948 vol.1, sept.-2 oct. 2004.doi: 10.1109/IROS.2004.1389474.

S. BenHimane and E. Malis. Homography-based 2d vi-sual tracking and servoing. Int. J. Rob. Res., 26:661–676, July 2007. ISSN 0278-3649. doi: 10.1177/0278364907080252. URL http://portal.acm.org/citation.cfm?id=1270220.1270222.

S. BenHimanhe. Vers une approche unifiee pour le suivitemps-reel et l’asservissement visuel. PhD thesis, coleNationale Superieure des Mines de Paris, 2006.

Francois Chaumette. Image moments: A general anduseful set of features for visual servoing, 2004.

Peter I. Corke. Visual control of robot manipulators – areview. In Visual Servoing, pages 1–31. World Scientific,1994.

T. Drummond and R. Cipolla. Visual tracking and controlusing lie algebras. In Computer Vision and PatternRecognition, 1999. IEEE Computer Society Conferenceon., volume 2, pages 652 –657 Vol. 2, june 1999. doi:10.1109/CVPR.1999.784996.

B. Espiau, F. Chaumette, and P. Rives. A new approachto visual servoing in robotics. Robotics and Automation,IEEE Transactions on, 8(3):313–326, 1992. doi: 10.1109/70.143350. URL http://dx.doi.org/10.1109/70.143350.

Bernardo Sousa Machado Henriques. Estimation and con-trol of a quadrotor attitude. Master’s thesis, InstitutoSuperior Tecnico, 2011.

Michael Isard and Andrew Blake. Contour trackingby stochastic propagation of conditional density. InBernard Buxton and Roberto Cipolla, editors, Com-puter Vision ECCV ’96, volume 1064 of Lecture Notesin Computer Science, pages 343–356. Springer Berlin /Heidelberg, 1996. URL http://dx.doi.org/10.1007/BFb0015549. 10.1007/BFb0015549.

Kenichi Kanatani. Optimal homography computationwith a reliability measure. IAPR Workshop on Ma-chine Vision Applications, Nov. 17-19. 1998, Makuhari,Chiba, Japan, 1998.

Ezio Malis and Francois Chaumette. Theoretical improve-ments in the stability analysis of a new class of model-free visual servoing methods, 2002.

Ezio Malis S. BenHimane. Real-time image-based trackingof planes using efficient second-order minimization. Pro-ceedings of the IEEE/RJS International Conference onIntelligent Robots and Systems, pages 943–948, October2004.

A.C. Sanderson and L.E. Weiss. Image-based visual servocontrol using relational graph error signals. In IEEEInternational Conference on Cybernetics and Society,page 10741077, Cambridge, Massachusetts, oct 1980.

Omar Tahri and Youcef Mezouar. On visual ser-voing based on efficient second order minimization.Robotics and Autonomous Systems, 58(5):712 – 719,2010. ISSN 0921-8890. doi: 10.1016/j.robot.2009.11.003. URL http://www.sciencedirect.com/science/article/pii/S0921889009002000.

Camillo J. Taylor and James P. Ostrowski. Robust vision-based pose control. In In International Conference onRobotics and Automation, pages 2734–2740. IEEE, 2000.

P. Torr and A. Zisserman. Feature based methods forstructure and motion estimation. In Vision Algorithms:Theory and Practice, volume 1883 of Lecture Notes inComputer Science, pages 278–294. Springer Berlin /Heidelberg, 2000. URL http://dx.doi.org/10.1007/3-540-44480-7_19.

W. J. Wilson, Williams C. C. Hulls, and G. S. Bell.Relative end-effector control using Cartesian positionbased visual servoing. 12(5):684–696, 1996.