Rapport de PFE -...

Rapport de PFEVision Processing on Parrot ARDrone 2.0

Student: Maxime COLLETTE

INSA Strasbourg Génie Électrique - Option Systèmes

Dates: From the 3rd of March 2014 to the 15th ofAugust 2014

Company:

Micro Air Vehicle Laboratory - TU Delft

Kluyverweg 1

2619 HS Delft

The Netherlands

Company Supervisor : Guido DE CROON School Supervisor : Renaud KIEFER

Goals of the internship

Goals of the internship

During this internship, work has been done on the Parrot ARDrone 2.0 in order to improve itsstate estimation by adding visual information. The goals of the internship are:

• Use the onboard DSP for vision processing:Vision processing needs a lot of computingpower. To perform complex computer vision algorithm, a DSP can be used in order torelease the general purpose processor.

• Improving the state estimation by adding visual information to the current state estima-tion: Vision on UAV is source of multiple information like speed, position, obstacle detec-tion for autonomous navigation. In order to improve the state estimation, vision informa-tion must be added to the current state estimation.

Rapport de PFE Génie Électrique - Option Systèmes i

Abstract

Abstract

UAVs (Unmanned Air Vehicles) are becoming more and more autonomous. Having an estima-tion of the groundspeed and also position is important for navigation or just for hovering anobserving a target. In most case, speed and position are estimated by an on-board GPS. Butin some cases, GPS signal can be unavailable, like by indoor flying. The drone has to rely onon-board sensors to estimate its speed. A common task for a UAV is the target observation. GPSsystem, when available, is not accurate enough to make the drone hovering on the target.In this report, a real-time method for computing the optical flow on a 320x240 picture using his-togram comparaison will be presented. To improve the computation capability of the ARDrone2,which is used as test platform, work has first been done in order to use the onboard DSP forvision processing. The optic flow computation method has then been implemented on the DSPand used for making the drone hovering.

Rapport de PFE Génie Électrique - Option Systèmes ii

Contents

Goals of the internship i

Abstract ii

I Presentation of the MAVLab 1

1 The MAVLab 2

2 The ARDrone 2.0 32.0.1 Cameras output format . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

3 Paparazzi 5

II Vision Processing on ARDrone 2.0 DSP 6

1 The TMS320C64X+ DSP 8

2 DSPBridge 11

3 Userspace files build 13

4 Standalone DSP Project 15

5 Shared Memory between ARM and DSP 17

6 Image exchange between ARM and DSP 20

7 First vision processing on the DSP 21

8 DSP Optimzations 22

III State estimation on the ARDrone 2.0 24

1 Existing state estimation for the ARDrone 2.0 261.1 Parrot’s state estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 261.2 Paparazzi’s state estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 271.3 Complementary filter for state estimation . . . . . . . . . . . . . . . . . . . . . . 28

2 The Optic Flow 292.1 Getting metric speed from optical flow . . . . . . . . . . . . . . . . . . . . . . . . 30

iii

CONTENTS

3 Experiment conditions 31

4 Current optic flow estimation on the ARDrone 2.0 33

5 New optic flow estimation on the ARDrone 2.0 355.1 Off-board implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 365.2 On-board implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

6 Extention to vertical speed 426.1 Off-board implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 436.2 On-board implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

7 Stabilisation with optic flow 47

8 Conclusion 51

Bibliography 52

List of Figures 53

IV Appendix 54

A DSP Optic flow main loop 55

B Horizontal optic flow computing code 58

C Optic flow code on ARM Side with control loop 62

Rapport de PFE Génie Électrique - Option Systèmes iv

Part I

Presentation of the MAVLab

1

The MAVLab

1 The MAVLab

The MAVLab (Micro Air Vehicle LABoratory) is a research laboratory of the Aerospace Engineer-ing Faculty of the TU Delft. It aims is the study and the creation of autonomous small drones.The most important realisation of the MAVLab is the DelFly, a very light flapping wings UAV.Work on the Delfly started in 2005. The last version, the Delfly Explorer, weights 20 grams. Ithas an onboard stereo vision systems in order to be able to fly autonomously during 9 minutesby avoiding obstacles.

Figure 1.1: The Delfly Explorer

Rapport de PFE Génie Électrique - Option Systèmes 2

The ARDrone 2.0

2 The ARDrone 2.0

The used platform during this internship is the ARDrone 2.0. It is a mass-market quadcopterdevelopped by the french company Parrot controlled from a smartphone through Wifi link.

Figure 2.1: ARDrone 2.0

This UAV has two cameras: one pointing in the flight direction with a 1280x720 pixels res-olution. Pictures taken by this camera are displayed in real-time on the smartphone used tocontrol the drone. The second is looking at the ground with 320 x 240 pixels at 60FPS. It isused for speed measurements. The ARDrone 2.0 has other sensors for attitude estimation: 3axis accelerometer, gyroscope and magnetometer, pressure sensor and ultrasound telemeter foraltitude estimation. The main electronic consists in an Texas Intstruments OMAP processor, achip combining an ARM processor with a DSP. It runs a light version of Linux.The ARDrone 2.0 is a good platform for research: It is light (380g), so not dangerous in case ofcrash, the frame is quite resistent allowing several crashes before it breaks, and the motors trunautomatically off when an object or finger blocks one of the propellers.

2.0.1 Cameras output format

Because the work is done on vision processing, here is presented the output format of the twocameras


The ARDrone 2.0

The cameras output format is UYVY, a derivate of the YUV.In YUV format, each pixel has three components:

• Y: Represents the luminance of the pixel. It corresponds to the gray level.

• U and V: The color of the pixel is determined by the combination of these two components.

Conversion from RGB color space to YUV color space are given here:

Figure 2.2: RGB to YUV conversion

The reverse conversion:

Figure 2.3: YUV to RGB conversion

We will only use the Y (grayscale) value of each pixel for the vision processing in this reportto avoid using too much computing power for color space conversion.The camera output format, UYVY, is a compression of YUV. The picture is encoded two pixel pertwo pixels. For each pixel, the Y component is given. The U component is only given for the firstpixel and the V component is the one of the second pixel. The payload is two bytes per pixelsinstead of three in the raw YUV format.For two pixel, the data stream is the following, with each field of the table as one byte:

U (pixel 0) Y (pixel 0) V (pixel 1) Y (pixel 1)

Picture are retrived in the vision program using Video4Linux library.


Paparazzi

3 Paparazzi

Paparazzi is an open-source auto pilot for UAVs. It was initiated by the ENAC (Ecole Nationalede l’Aviation Civile) in Toulouse. Other Universities have them join its developpement like theTU Delft with the MAVLab.

Figure 3.1: Paparazzi Logo

Paparazzi is compatible with several drones configuration, fixed-wings and multirotors. It offershardware platforms (flight controllers), embedded software for UAV (sensor readings, controlloops, communication...) and a ground station in which flight plans are edited. The flight of theUAV is also controlled from this groundstation. Data can also be recorded during the flight in aLOG file for post-processing and analysis.


Part II

Vision Processing on ARDrone 2.0 DSP

6

The on-board processor of the ARDrone 2 is an OMAP3630 form Texas Instruments. Thischip contains an ARM Cortex A8 processor, which runs Linux 2.6 and a TMS320C64x DSP. Atthis time, only the ARM processor is used in Paparazzi. It is responsible for all tasks, includingattitude estimation, control loops, navigation, and some vision processing. Vision processingneeds a big amount of resources, like memory or computation power. In order to not blockflight control tasks, complex vision processing can’t run on the ARM. But the DSP can be usedfor running these complex vision algorithms. Indeed, a DSP is a processor optimized for signalprocessing algorithm. In this part, I will describe my work about DSP implementation.


The TMS320C64X+ DSP

1 The TMS320C64X+ DSP

The DSP is the heart of the IVA2.2 (Image Video and Audio Accelerator) module. This DSP ispart of the TI’s C6000 DSP family. They have the following internal architecture [8] :

Figure 1.1: C6000 DSP Architecture

The C6000 CPU is made of two identical parts. In each part, there are 4 functional units thatcan execute instructions in parallel. Each unit is specialized in one type of operation :

• .L for logical and arithmetical operations

• .S for shift, branch and compare operations

• .M for multiply operations

• .D for data load and store operations

The DSP can execute up to eight instructions in parallel, one on each unit. Each side of the DSPcore has 32 registers of 32 bits.DSP CPU also supports pipeline. To explain it, we will take a simple example : An instruction isnot proceeded in one step, but requires three :


The TMS320C64X+ DSP

• Read the instruction from the memory (F)

• Decode the instruction (D)

• Execute the decoded instruction (E)

Without pipeline, each step would be executed one after another. But pipeline enables theparallel execution of these three phases : During the execution of one instruction, the followingis decoded and the following of the following is being fetched from memory.

Figure 1.2: Pipeline

In this basic example, we see that without pipeline, the CPU needs 9 clock cycles to completethree instructions. But with pipeline enables, the clock cycles count needed is only 5. With acontinious flow of instruction, the pipelined CPU execute one instruction on each clock cycle,that means three time more efficient than the unpipelined CPU.On the C6000, each of the three previous steps are divided into sub-steps :

• 4-stage fetch

– PG: Program address generate (update program counter register)

– PS: Program address send (to memory)

– PW: Program (memory) access ready wait

– PR: Program fetch packet receive (fetch packet = eight 32-bit instructions)

• 2-stage decode

– DP: Instruction dispatch (or assign, to the functional units)

– DC: Instruction decode

• Up to 10-stage, variable duration execute

– E1 – E10, where E1 is the first sub stage in the execute stage

– Different instructions might require different number of stages to complete its execu-tion

The pipeline is 11 levels deep :


The TMS320C64X+ DSP

Figure 1.3: C6000 Pipeline


DSPBridge

2 DSPBridge

DSPBridge [1] is a driver to communicate with the DSP and also to control it. On the DSP side,each program is called a node. The ARM (MPU side) application will enable the node it needs,exchanging messages with it and then disable the node when it is not needed anymore by theapplication. There are three different ways to communicate with a node:

• Messaging: fixed size messages exchange

• Streaming: This functionality of nodes allows data streaming between MPU and DSP. Notethat data are copied from MPU memory space into DSP memory space

• Memory mapping: MPU memory side can be map to DSP address space. This allowsexchange of big amount of data without any data copy.

DSPBridge also features dynamic loading of nodes: When the DSP is already running, the MPUcan load a new node into it when needed and then delete it without stopping other nodes onthe DSP. The low level DSP part are TI closed source binaries. But on the MPU side, the code of

Figure 2.1: DSP Bridge Architecture

the different parts is Open Source.Building for the MPU requires nothing more than a cross-compiler, like the one used to build


DSPBridge

Paparazzi. But, DSP build requires specific TI Tools. To build DSP code running with DSPbridge,some specifications must be respected: On DSP, nodes must implements three functions:

• nodename_create: This function is called by the DSP-side of DSPBridge driver when anode is set up by the MPU application. It performs the user initialization of the node, likememory allocation.

• nodename_execute: This function is called by DSPbridge driver on the DSP when the nodeis started. This function must contains a loop which is only exited when the MPU sends anend message.

• nodename_delete: This function is called on node’s destruction. It frees for instance theallocated memory.

These three functions are defined in separated files. A nodename.tci file groups the definitionof these functions with other node’s parameters like the name of the node, it’s identificationnumber, the priority, the stacksize... This tci file will be used on the compilation to create theDSP executable.On the ARDrone2, the DSPBridge driven was already installed. It corresponds to the Kernelspace files. The userspace files provides a way to communicate with the kernel layer of DSP-Bridge. First, the userspace files need to be built, because we will then use their functions in theapplications.


Userspace files build

3 Userspace files build

The source code is Open Source and available on Github. With these files are provided somesample applications to verify the system and also to provide examples about message exchange,data streaming or sharing memory.To build the ARM-side files, the cross compiler used to build Paparazzi is adapted. We don’tneed more tools. However, in order to build the DSP’s sample applications, we need specifictoolchains both from Texas Instruments:

• DSP/BIOS version 5.33.04

• Code Generation Tools version 6.0.3

The version number is important, I was blocked during one week because I installed a morerecent version of them. These two tools needs to be installed in the same directory which iscalled DEPOT. For me, DEPOT is ˜/tidsp. In this directory, we download the source files fromGithub:

git clone git://dev.omapzoom.org/pub/scm/tidspbridge/userspace-dspbridge.git

cd userspace-dspbridge

git checkout -b my_userspace --track origin/master

Some downloaded files have to be modified to get a successful build:

• The library path of the ARM cross-compiler has to be added in the Makefiles located inDEPOT/userspace−dspbridge/source/mpuapi/src/bridge/ and inDEPOT/userspace−dspbridge/source/mpu_api/src/qos/

• The library include order inDEPOT/userspace−dspbridge/source/sample/mpu/src/qostest/Makefilehas to be inverted

• The toolchain version numbers in file product.mak have to be updated to match the versionof the installed toolchains. Note that we don’t need to define CGT55_V ER, because theDSP of the ARDrone is a C64+ type.

• Then, in the Makefile located in the source directory, we need to add the following at it’sbeginning:

DEPOT ?= ~/tidsp

CROSS_COMPILE=arm-linux-gnueabi-

All the files, DSPBridge interface and sample for MPU and DSP, can now be built. Output filesfor the ARM have the .out extension. There are two types of output files for the DSP:

• .dof64P: This extension is for base image. It needs to be loaded first in the DSP. It also con-tains all the samples programs for the DSP. The .dof64P program contains also ressourcesfor dynamic loading.


Userspace files build

• .dll64P: This extension stands for dynamic loadable programs. Before dynamically loadinga program, the DSP must be started by having a .dof64P program loaded.

With the sample program are also built two important programs to load programs into the DSP.DSP programs are stored on the Linux filesystem which is only accessible by the ARM. Thecexec program loads a .dof64P program into the DSP. The dynreg program is used for dynamicloading. So, to test the system, we need the DSP base image, the cexec utility and the pingsample program. The built binaries are sent to the ARDrone with a Python script using ftpprotocol. The files are uploaded in the /data/video/dsp directory on the ARDrone. ThroughTelnet, we have access to the ARDrone’s console. Following commands load and execute theping demo:

./cexec.out ddspbase_tiomap3430.dof64P #to load the base image (program) into the DSP

./ping.out #to start ping program on ARM and DSP side.

The result is:

DSP device detected

DSPNodeCreate succeeded

DSPNode_registerNotify succeeded

DSPNode_run succeeded

Ping: Id 1.000000 Msg 0.000000 Mem 15408.000000

Ping: Id 1.000000 Msg 1.000000 Mem 15408.000000

In this sample code, the DSP and ARM regulary exchange message to check the communication.


Standalone DSP Project

4 Standalone DSP Project

For further developments, a new project will be created. The goal is not to build each timeall the sample files with the application. In this way, we can also modify some DSP con-figurations specifically for each program. The structure of the sample project will be kept.In the project directory, called VisionTest in the source folder of userspace-dspbridge, withthree subdirectories: MPU, DSP and Target. In the MPU folder, copy the content of the sam-ples/mpu directory. Then, in the copied src folder, we will keep only cexec, dmmcopy andping folers in order to check if our standalone project is correct. The Makefile must be upa-dated by removing the name of the deleted directories. The DSP code structure is more com-plex. It contains not only the source code, but also files to define the Platform characteris-tics like clock frequency, memory mapping,... First, in the DSP directory, create a new foldernamed ardrone (the name of the platform). To define the platform parameters, Texas In-struments provides templates based on the DSP type to use. These templates are locatedin DEPOT/bios_5_33_04/packages/mycompany/platforms/myboard/. Because the ARDronehas a C64x+ DSP, the good template is Platforam_64P_Group1.tci. This file has to be copiedin the ardrone folder and renamed to Platform.tci. Then, its content has also to be changed:First, update the Platform name to ardrone line 16. Then, the device name line 73 should be setto 3430. In the DSP directory, create a file named config.tcf with following content:

utils.importFile("bridgeutils.tci");

utils.loadPlatform("ardrone");

if (config.hasReportedError == false)

prog.gen();

This file will be interpreted by the TI tconf utility in order to set up all parameters needed tobuild code for the DSP. Tconf is a software included in DSP/BIOS distribution. Then, we willreuse some files from the dsp sample directory:

• config,bld: script file used to configure the build parameters. To add or remove a compileroption, this file has to be edited.

• ddspbase,tcf and ddspbase,tci: These two files are used to configure the build of a staticprogram image for the DSP. Especially, the modules to build are defined in ddspbase,tci byincluding the tci file of the module: We will keep only ping.tci and dmmcopy.tci.

• dmmcopy.tci and ping.tci to build these two nodes as explained above.

• The source code related to ping and dmmcopy nodes (.c and .h)

• package.bld: This file is used to configure the target to build: static or dynamic loadableimages, which source files for which target. In this file, programs64 defines the target tobuild. To build only a static image with ping and dmmcopy, it should be set as follow:


Standalone DSP Project

var programs64 = [

name: "monitor",

sources: [ "main", "dynref" ]

,

name: "ddspbase",

sources: [ "main", "dmmcopyCreate", "dmmcopyExecute", "dmmcopyDelete",

"pingCreate", "pingExecute", "pingDelete", "pingdata", "pinglib"]

];

• dsptoolsymbols,cmd and inst2cmd,xdt and linkerinst.cmd which are linker commandfiles. They don’t need to be edited.

The framework components have also to be copied. They are located in the source/dsp folder.Copy the dsp folder into the VisionTest/DSP fodler. Some of these files can now be changedwithout influencing the build of the samples or other project which will be created later. Now themakefiles have to be updated to our new project structure. Copy Makefile and samplemakefileform the source directory of userspace-dspbridge into VisionTest folder. Makefile is the mainmakefile. It is invocated when the make command is entered into a terminal. Samplemakefilecontains the build procedures for the DSP part. These procedures are called by Makefile. InMakefile:

• API_DIR must be MPU/src (directory where the source code for the MPU is located)

• TDIR is target (the directory where built files will be exported)

In samplemakefile:

• Each path to the software framework, $(CURDIR)/dsp for example, must be changed totarget to the copied software framework and not to the original one. So, DSP/ must beadded before each occurrence of dsp:$(CURDIR)/DSP/dsp/...

• SAMPLE_PACKAGES corresponds to the DSP source code path, it has to be set to $(CURDIR)/DSP

We have now a standalone DSPBridge project we can modify without damaging the samples orthe MPU API.


Shared Memory between ARM and DSP

5 Shared Memory between ARM and DSP

The images to process are captured by either the front camera or the bottom camera. Both ofthem are connected to the ARM. To process their images on the DSP, they need to be transferredfrom the ARM to the DSP. Copy of images from an ARM buffer to a DSP buffer is unappropri-ated, because of a loss of time and memory usage. The pictures taken by the front camera are1,8Mo big. The solution is the shared memory: A block of RAM memory is allocated as sharedmemory. That means that this buffer can be accessed from different ARM threads, but also fromthe DSP.The DSP has it own memory space. Addresses range from 0x0000 0000 to 0x1100 0000 corre-sponds to internal memories (program and data) and internal registers. They are not accessiblefrom the MPU. Addresses form 0x1100 0000 to 0xFFFF FFFF are used by the DSP to accessperipherals and memories outside the IVA subsystem. These external peripherals and memoriesare mapped as virtual addresses for the DSP through the IVA2.2 iMMU address translator, whichenables continual access even if the physical memory is fragmented. To efficiently transfer im-

Figure 5.1: DSP Memory Managment

ages between ARM and DSP, the image buffer allocated in the physical memory by the ARM willbe mapped as virtual address on the DSP side, so that this one can access directly to the buffer.The same process will be reuse to send a processed image from the DSP to the ARM.

The DSPBridge driver already configure the virtual memory as follow: DSPBridge alreadydefines some shared memory segments available for the user. But their size is to smalll forimage transfert. We can’t modify the size of these segment. The L4 peripherals enables the DSPdirectly access to ARM peripherals. The size of the three last segment can be modified by theprogrammer by editing files of the DSP framework (that’s why we copied them previously!).Since we don’t use dynamic loading at this moment, DYNEXTMEM segment won’t be changed.Then, EXTMEM segment is used for dynamic data allocation (malloc function). It’s size can bechanged if more data must be dynamically allocated. Then, the DMM segment is the segmentwhere the images buffer from ARM will be mapped by the DSPBridge driver. In order to allowboth ARM and DSP to access the same part of the memory, this memory parts has to be allocatedas shared memory. In Linux systems, a part of the RAM is reserved for shared memory allocation.



Figure 5.2: DSP Memory Map

Two parameters in the kernel control the amount of available shared memory:

• shmmax: Maximum size of one shared memory segment. To get its size on the ARDrone2 Linux system, type the following command in an ARDrone terminal: sysctl -a | grepshmmax. The output is kernel.shmmax = 33554432, which means that the size of theshared memory is 32 MB.

• shmall: Specify maximum size of all the shared memory segments allocated in PAGE_SIZE.PAGE_SIZE is 4096 bytes. Shmall value on the ARDrone2 is 2097152 pages, which corre-sponds to 8GB. But the RAM on the ARDrone2 is only 1GB.

These default settings are enough for the vision processing application. The sample programdmmcopy is an example how to share buffers via shared memory. But it didn’t work when Itried to run it on the drone. To solve this, the allocation of the shared buffer has been forcedinto the shared memory. Allocation in the shared memory requires two steps: First, space in theshared memory is reserved with the shmget function:

int shmget(key_t key, size_t size, int shmflg);

key is the identifier of the segment we want to create. Size specifies the size in bytes of thespace to reserve and shmflg are flags to specifies access permission for the segment to create. It



returns the system identifier of the new segment created. The, with the function shmat, we geta pointer to the shared memory segment:

void *shmat(int shmid, const void *shmaddr, int shmflg);

shmid is the system identifier of the segment we want the address. This identifier is the onereturned by the shmget function. The user can specify the segment address with the shmaddrparameter. If it is NULL, the system will choose the address. Shmflg specifies if the segmentis attached for read only or also for write operations. The returned value is the pointer to thebuffer in the shared memory space. The steps to share data buffers between the ARM and theDSP with DSPbridge are the followings:

• Allocate a buffer in the shared memory on ARM side (shmget and shmat functions).

• Reserve memory on the DSP with the same size as the buffer previously created (DSPPro-cessor_ReserveMemory)

• Map the ARM buffer with the reserved space on DSP (DSPProcessor_Map)

• Notify the application running on the DSP with the adjusted buffer address so that it canuse it

At the end of the program, the memory has to be released:

• Unmap ARM and DSP buffers (DSPProcessor_UnMap)

• Free memory on the DSP. (DSPProcessor_UnReserveMemory)

• Detach the segment in the shared memory using shmdt

To notify the DSP node with the buffer address in DSP memory space, a message is sent to thenode. Message feature is provided by the DSPBridge API. A message consists of one commandand two optional arguments. All these fields are 32bits long. For buffer address notification,the command is user-defined to DMM_SETUPBUFFERS and the address of the shared buffer isgiven in the first argument. The dmmcopy copy the content of an input-given file to another filethrough the DSP with following steps:

• Two ARM-DSP shared buffer are created and mapped together: One for ARM to DSPcommunication (BufferSend), the other for DSP to ARM communication (BufferRecv).

• The ARM fills BufferSend with content from the input file

• The ARM notifies the DSP that BufferSend is full

• The DSP copies the content of BufferSend into BufferRecv and notify the ARM when thecopy is finished

• The ARM compares the content of BufferSend and BufferRecv to check if no data havebeen lost. If the content matches, the ARM writes the content of BufferRecv to the outputfile. If not, it displays an error.

• The three last steps are repeated until the end of the input file is reached.


Image exchange between ARM and DSP

6 Image exchange between ARM and DSP

In Paparazzi, there is a program to stream video from the front or bottom camera to VLC. Itgrabs a frame from the selected camera, resizes it four times smaller, encodes it in JPEG formatend then send it to the computer over Wifi. This program will be adapted with parts of thedmmcopy sample to have a first vision program with DSP:

• The ARM grab a frame from a camera

• The ARM sends the grabbed frame to the DSP

• The DSP copy the image in the buffer to ARM

• The ARM resizes, encodes and sends the image to VLC

For this purpose, a dsp driver for vision has been created. The initialization function groupsDSP detection, attachement and node initialization. Then, buffers are created and mapped tothe DSP. In this driver, a end function is also written to release memory and ends properly theprogram on DSP side. The prototypes of these functions are:

int DSPVisionOpen(struct VISION_TASK *task, struct img_struct* img2Dsp,

struct img_struct* img2Mpu)

• task is a structure which contains a handle to DSP processor, a handle to the node (neededby DSPBridge initialization functions called inside this open function), buffer addressesand shared memory identifiers.

• img2Dsp is an image structure already initialized with the image dimensions (height andwidth). The buffer containing the image data will be initialized in this function as DSPshared buffer. This structure is used to send image to the DSP.

• img2Mpu is initialized in the same way as img2Dsp. Img2Mpu is used to transfer processedimages from the DSP to the ARM

void DSPVisionClose(struct VISION_TASK *task)

Since all the buffer addresses are savec in the VISION_TASK structure, all the unallocations canbe done without passing additional parameters. The complete program for both ARM and DSPis given in appendix.


First vision processing on the DSP

7 First vision processing on the DSP

The purpose of this part is to move the resizing and JPEG-encoding process from the ARM sideto the DSP side. Since the DSP is also programmed in C, this task is not so complex. The Cfiles containing the code for resizing and encoding must be included to the DSP build process.The data transfert between ARM and DSP is the same as previous. The size of the DSP-to-ARMbuffer is smaller because it contains the resized encoded image. The ARM only runs the framegrabbing, dataexchange with the DSP and the Wifi image sending.The DSP will be later used to run Optic Flow computation.


DSP Optimzations

8 DSP Optimzations

Because of its internal structure, the DSP is able to develop an important computing power byexecuting instructions in parallel. Loops can also be parallelized. The C compiler from TexasInstruments produces already highly optimized binaries. But this compiler can be guided toimprove its efficienty. Texas Instruments gives five techniques to optimized the built code [8]:

• Choosing the right compiler options: The optimization level should be set to 3 with the-o3 command. The -mt option is also to use. The purpose of this option is to reducethe loop carried dependency bound. In a loop, the loop carried dependency bound is thedependency between the actual computed results and the previous computed:

for(i=0;i<5;i++)

A(i) = B(i) + C(i);

In this case, there is no loop carried dependency bound because A(i) doesn’t depend onprevious values. This loop can be parallelized without any problem by computing A(i) andA(i+1) at the same time and incrementing i by 2 on each iteration.

for(i=0;i<5;i++)

A(i) = B(i) + A(i-1);

In this case, A(i) content depends on the previous iteration. A(i) and A(i+1) can’t becomputed in paralell because A(i+1) needs A(i) value to process.Without any knowladge, the compiler is prepared to the worst case, meaning consideringloop carried dependency bound where there isn’t and then reducing code performance.The option -mt tells the compiler that the pointers in function parameters never point atthe same address.

• The restrict keyword: This keyword has to be placed in the pointer declaration to informthe compiler that the pointer will never point to the same memory address. It acts like the-mt option, but is a local way of reducing the loop carried dependency bound.

• The MUST_ITERATE pragma directive: This directive, placed before a loop, allows theprogrammer to pass information about the loop to the compiler, like the minimum andmaximum number of iterations and a factor. The total number of iteration is N*factor.With these information, the compiler can optimized the pipeline and also unroll the loopif it finds it usefull.

• The UNROLL pragma directive: This directive force the compiler to unroll the loop by afactor given by the programmer. Loop unrolling consists in dividing the loop for paralleloperation:


DSP Optimzations

for(i=0;i<N;i++)

out[i] = in1[i] + in2[i];

Unrolling the loop can be done manually:

for(i=0;i<N;i+=2)

out[i] = in1[i] + in2[i]; //runs on one DSP core

out[i+1] = in1[i+1] + in2[i+1]; //runs on the second DSP core

This is equivalent as writing

#pragma UNROLL(2)

for(i=0;i<N;i++)

out[i] = in1[i] + in2[i];

• SIMD instructions for Single Instruction Multiple Data. The DSP works with 64bits widedata. But, if the programmer uses only 16 bits data wide, instead of loosing 48 bits, theDSP can process 4 words of 16 bits in the same instruction, for example it can add 4 16bitswords with 4 other 16bits words and store the 4 results in the memory. These instructionare very useful by loop unrolling to speed-up the computation. The choice of data type is inthis case very important. Based on this, the compiler decides the use of SIMD instructionsor not.


Part III

State estimation on the ARDrone 2.0

24

In Paparazzi autopilot on the ARDrone 2.0, no visual information are used in the state es-timation of the drone. However, with two cameras, vision can provide several information toimprove the state estimation, like the ground speed measured with the downpointing camera orpitch and roll angles estimated with the horizontal camera filming the horizon line. This cameracan also be used to detect obstacles in the flight direction and avoid them.In this part, optic flow will be used to add a groundspeed estimation to the Paparazzi stateestimation.


Existing state estimation for the ARDrone 2.0

1 Existing state estimation for the ARDrone2.0

1.1 Parrot’s state estimation

Parrot describes its state-estimation and control loops in [10] The embedded sensors are:

• One three-axis accelerometer

• One two-axis gyroscope for pitch and roll axis

• One one-axis precision gyroscope for yaw

• Two ultrasonic sensors for altitude estimation

• One barometer

• One 1280x720 front camera used to transmit pictures to the smartphone controlling thedrone

• One 320x240 camera pointing to the ground used to estimate the groundspeed of theARDrone2 through optical flow computation

Accelerometers and Gyroscopes measurements are combined by a complementary filter to esti-mate the attitude of the drone, meaning the roll, pitch and yaw angles and also the velocities.The complementary filter estimates also the bias of the gyroscopes.The ultrasonic sensor and the barometer estimate the altitude of the ARDrone2 over the ground.From this altitude estimation, the vertical speed is computed. Accelerometers are used to in-crease the estimation accuracy.The vertical camera pointing to the ground is used to estimate the speed over ground of thedrone. This camera outputs pictures at 60Hz. Optical flow is computed from these pictures.Two different algorithms are used, depending on the observed scene. The first is based on theLucas-Kanade approach, which consists in calculating the optical flow on the whole picture.The optical flow is computed locally on a fixed-size window centred on the pixel for which theoptical flow is being computed. This approach has been combined with the one of Horn andSchunck with the use of a kernel to smooth spatial and temporal derivates. The assumption isthat the optical flow doesn’t change a lot between one pixel next to another pixel. The secondalgorithm is the “corner tracking” algorithm. It first identifies points of interest in the picturewith a FAST corner detector method. In the next picture from the camera, the position of thepreviously points found is searched. IRLS estimation is the run to find out the speed. The firstalgorithm is used by default on the ARDrone2. When the scene becomes textured enough forcorner detection and at low speed, the second algorithm is used. Back switching to the firstoccurs when the speed goes over a threshold or when not enough corners are detected in thepicture.Output values from optical flow computation are compensated for the vertical and angular ve-locities. This allows getting the two components of the velocity in the horizontal plane. Vision



velocity estimation is improved through data fusion with speed estimation from an aerodynam-ical model.All these technique are combined to improve the accuracy of the state estimation and compen-sate defects of sensors (drift over time): Gyroscope measurements are the primary source forroll, pitch and yaw angles by integration. Gyroscopes bias is estimated thanks to the accelerom-eter’s values through a complementary filter which combines gyroscopes and accelerometersoutputs. The unbiased gyroscopes values are used in the vision processing to compensate thepicture for rotations. Altitude estimation is also used by the vision processing. The computedvelocities from computer vision are used to unbias the accelerometers and improve the attitudeestimation. Accelerometers values can be combined with the vision velocities to improve itsestimation and to increase the rate.

Figure 1.1: ARDrone 2.0 state estimation and control loops

1.2 Paparazzi’s state estimation

Roll and pitch angles are estimated through a quaternion-based complementary filter. Angularrates from gyroscopes are integrated to get the angles. Accelerometers are used to estimatethe drift of the gyroscopes. The estimated drift is substracted to the gyroscopes measurementsbefore integration. Accelerometers measurements are for centrifugal force compensated.The yaw angle (heading) is estimated by the gyroscopes and magnetometers. The expectedvalue for magnetometer measurements are calculated based on the yaw angle measured by thegyroscopes. Then, the expected values are compared to the output values of the magnetometer.Gyroscopes rates correction and bias is computed. For long-term estimation, the GPS is alsoused.Altitude is estimated with an extended filter which computes the altitude, the vertical speed andthe accelerometers bias from acceleration, sonar measures and GPS information.However, in Paparazzi, no visual information is used in the state estimation.



1.3 Complementary filter for state estimation

In the previous sections, complementary filter has often been mentioned.Complementary filter combines information from 2 sources by applying on one a low-pass filterand a high-pass filter on the other. Complementary filters are often used to get pitch and rollangles from gyroscopes and accelerometers measurements [3]. Gyroscopes give the angularspeed. By integration, the angles can be estimated. But gyroscopes drift slowly with time andthe integration will give wrong angular values. Gyroscopes are accurate on short term. Theirmeasurements are filtered by a high-pass filter before integration to eliminate drift. Becauseof the gravity, orientation can be estimated by accelerometers measurements. But these valuesare perturbated by noise (vibration or movements). On long term, accelerometers can be usedto determine the orientation. Theirs output values are filtered by a low-pass filter to eliminatenoise. The complete filter (high-pass filter for gyroscope and low-pass filter for accelerometer)forms a complementary filter. The short-term estimation is done by gyroscopes values, butcorrected on long term by accelerometers values.Complementary filters can be implemented to obtain a good estimation one one value fromseveral sensors when some are reliable on short time (high pass filter) and when others arereliable on long time (low pass filter). Complementary filters are an alternative to the Kalmanfilter for data fusion. The Kalman filter is a filter which estimates the optimal value of noisyinput measurements. It is based on statistical theory. Comparisons between Kalman filter andcomplementary filters for attitude estimation have been made in the case of one degree offreedom angle estimation. The equations are very close with the one of the complementaryfilter. Moreover, the computation power required by the Kalman filter is much more than the onerequired by the complementary filter. But complementary filter cannot be used in all situations.It needs complementary information sources whereas Kalman filter is more flexible.

Figure 1.2: Comparaison between Complementary Filter (Green) and Kalman Filter (Violet)from [2]

Results given by the complementary filter are very close to the one given by the Kalmanfilter.


The Optic Flow

2 The Optic Flow

Optic flow is a video analysis method which compute the pixel displacement between to follow-ing frames from a camera. This displacement depends on the horizontal and vertical movementsof the drone. Knowing the pixel displacement, the drone’s speed can be computed.The optical flow is mainly used on UAVs to measure their ground speed. An other use of theoptical flow is the time to contact. It evaluates the time before the UAV gets in contact with anobstacle. Time to contact is used for obstacle avoidance and automatic landing.Optical flow is modelled by an equation: I2 is the new image grabbed by the camera, I1 is theprevious image.

∂I2(x, y)

∂xu+

∂I2(x, y)

∂yv = −∂I

∂t

with:

• ∂I2(x,y)∂x : Spatial derivate of pixel (x,y) in the X direction in the second image

• u: Optic flow pixel(x,y) in the X direction

• ∂I2(x,y)∂y : Spatial derivate of pixel (x,y) in the Y direction in the second image

• v: Optic flow of pixel(x,y) in the Y direction

• −∂I∂t : Time derivate of the intensity of the pixel (x,y) between image 1 and 2.

Note that this equation can be witten in a more compact form: ∇I2

(uv

)= −∂I

∂t This equation

cannot be solved directly, because of two unknown parameters (u and v) for one equation. Tosolve it, at least one constraint must be added. The choice of this constraint depends on theresolution method.

• Horn-Schunk method [9]:The constraint added is a smoothing constraint based on theassumption that the optical flow doesn’t change a lot between one pixel and its neighbours.This resolution method is also a global method. The computation of the optical flowconsists in finding the minimum of the following expression:∫∫ (

∇I(uv

)+∂I

∂t

)+ α2

((∂u

∂x

)2

+

(∂u

∂y

)2

+

(∂v

∂x

)2

+

(∂v

∂y

)2)dxdy

α adjusts the weight of the regularity factor.

• Lucas-Kanade method [4]:This method is a local method. The optical flow is computedon a small window surrounding the pixel of interest. The optical flow is supposed tobe the same for all the pixels included in the window. Each pixel of the window givesone equation. Then, to solve for u and v on a domain Ω, following expression has to beminimized: ∑

p∈Ω

(∇I(p)

(uv

)+∂I(p)

∂t

)It is solved by pseudoinverse.


The Optic Flow

These two method are the historical methods. Further work on optic flow brought other meth-ods:

• Block matching [5]: A block of pixels is selected in the current picture. Its position isthen searched in the new picture from the camera within a defined search area. Theposition which minimizes the SAD (Sum of Absolute Differences) is selected as being thenew psition of the block. The flow vector is then deduced by position difference.

• Farneback method [7]: The neighborhood of the considered pixel is approximated by aquadratical polynom:f(x) = xTAx + bTx + c. A point in the first picture is approximatedby f1(p) and by f2(p) in the second image, finding the optical flow consists in finding dwhich complete f2(p) = f1(p− d).

• Features tracking method: Points of interest (like corners) are identified in the picture.Then, in the next picture, the position of these previously identified points is identified.Difference between the two position gives the optical flow.

• Pyramidal Lucas-Kanade: This improvement of the Lucas-Kanade method consists in cal-culating the optical flow at different scales. The gaussain pyramid of the picture is built.Then, optical flow is computed for each level of the pyramid with the Lucas-Kanademethod. The higher level (lowest resolution) gives the global mouvement whereas thelowest level (picture without any modification) gives local displacements.

2.1 Getting metric speed from optical flow

Optical flow computes pixels displacements, which depend on the 6 possible mouvements:

• 3 rotations (ωx, ωy, ωz)

• vertical translation Tz

• horizontal translations (Tx, Ty)

The relation between these mouvements and the optical flow (vx, vy) on point (x, y) is following[6]:

vx =Tzx− Txf

Z− ωyf − ωzx+

ωxy2 − ωyxy

f

vy =Tzy − Tyf

Z+ ωxf + ωzy +

ωxxy − ωyx2

f

where f stands for the focal length of the camera(known) and Z for the distance between thecamera and the filmed plane. Because some rotational terms of these equation are non-linear,direct resolution of the six unknown mouvement terms would be a high computation task notsuitable for embedded system. But, in UAVs application, angular rates are given by the on-boardgyroscope. Altimeter gives the altitude Z and the vertical speed Tz. By first compensating thecomputed optical flow (vx, vy) for vertical translation and rotations, the remaining unknownare Tx and Ty. Considering the compensated optical flow for all pixels of the picture, a linearequation system is formed. It can be solved by pseudoinverse (because we have more equationsthan unknowns).


Experiment conditions

3 Experiment conditions

In the following parts, I will present results of experiences featuring optic flow on the ARDrone.Before, the experiment conditions will be described here.The MAVLab has a flight arena, a room in which drones can fly safely. It is equiped with avideo tracking system consisting of 24 cameras tracking infrared markers (special markers orIR-LEDs). The position of the flying UAV can be computed with a mm resolution. This positionis transmetted to the UAV as GPS coordinates in the ECEF frame.For optic flow tests, the GPS speed (given by the vision tracking system) in cm/s is recorded.Optic flow results, time between two frames and altitude above ground (taken from visiontracking for more accuracy) are also logged. These data are post-processed into MATLAB.For first results, the vertical speed is not taken into account in the optic flow computation.The logging is done when the drone flies at constant altitude using the Z_HOLD mode ofthe Paparazzi autopilot. Horizontal mouvements, i.e. the one which are measured by opticflow, are controlled by a joystick. Because rotation compensation is not fully implemented, themouvements are soft, no hard acceleration, to avoid high pitch and roll angles.On the ground, features are added in order to have features to track. They consist of printedpapers with color squares on it. The papers are dispached and glued to the ground of the flightarena.The following picture is the grayscale picture of the ground taken by the bottom camera of theARDrone in flight:

Figure 3.1: View of the ground of the test room from the drone

This picture is used for optic flow computation. It size is 320x240 pixels. The Optic flow is


Experiment conditions

computed in the drone’s frame, which is rotated from the GPS frame. To be able to compareboth optic flow speed and GPS speed, the norm of both will be computed. GPS speed is in cm/s,the following formule is used to get the norm of the optic flow speed in cm/s:

Vof = 0.1 ∗ altitude ∗ FPS ∗

√(TxFx

)2

+

(TyFy

)2

where:

• altitude: altitude above ground given by the Vicon system in millimeters

• FPS: Frame Per Seconds

• Fx: Focal length and scale factor for camera X-axis

• Fy: Focal length and scale factor for camera Y-axis


Current optic flow estimation on the ARDrone 2.0

4 Current optic flow estimation on the ARDrone2.0

Optical flow computation has been implemented on Paparazzi. The used algorithm is the fea-tures tracking. First, features like corners are extracted from the picture. Features are detectedusing the FAST(Features from Accelerated Segment Test) detector. With this method, to test ifthe pixel p is a corner, 16 neighbour pixels are tested. If at least 12 contigous of them are eitherbrighter or darker than the pixel p plus a threshold, the pixel p is classified as corner. The high-speed version of this detector consists in testing 4 surrounding cross pixels (up, down, right andleft). Because at least 12 on 16 pixels must be brighter or darker to classify the tested pixel, on4 pixel, 3 should validate or not the classification. If the pixel is classified as corner, it is addedto a list. A verification accurs to avoid to track the same corner several times.Detected points are then tracked with the Lucas-Kanade optic flow algorythm. Displacement ofeach point is determined in pixels. The average displacement is computed by computing theaverage optic flow from each tracked point. The average flow is then translated to metric opticflow to have the metric ground speed of the drone.Test have been made in the flight area. The speed determined by the optic flow is compared tothe speed measured by the Vicon system. Because the optic flow is based on features tracking,we must add features on the ground in order to have something to track. Using Paparazzi, thespeed values from the GPS(emulated by the Vicon system) and from the optic flow are loggedinto a file. Values are then post-processed by a MATLAB script to compute the norm of the speedmeasured by the GPS and by the optic flow. To compute the metric speed, only the altitude istaken into account in a first time. Vertical speed doesn’t need to be corrected, because it has noeffect when computing the average. Rotation compensation has to be added in a final version.Results are shown in the following graph.Ported on the DSP, the execution time of this method depends on the number of corner detectedin the picture. Above 5 corners, it takes more computation time than the available time betweentwo pictures from the bottom camera.


Current optic flow estimation on the ARDrone 2.0

Figure 4.1: Comparaison between GPS(red) and Optic Flow(blue) speed


New optic flow estimation on the ARDrone 2.0

5 New optic flow estimation on the ARDrone2.0

An other way of computing the optic flow has been developped, which can run in real-time onthe DSP of the ARDrone. It consists first of detecting edges in the picture witha Sobel filter, thenconstructing two histograms of the detected pixels: one in the horizontal direction, the secondin the vertical direction.The Sobel filter combines two features: It detects edges in one direction in the picture(eitherhorizontal or vertical) and aplies a low-pass filter in the other direction. Edge detection is im-plemented by computing the gradient of the tested pixel. The kernel convoled with the picture

for gradient computation is[1 0 −1

]for horizontal gradient and

10−1

for vertical gradient.

The low-pass filter consists in an Gaussian approximation:[1 2 1

]for horizontal direction

and

121

for vertical direction.

The Sobel filter is constructed as follow:

[1 0 −1

]∗

121

=

1 0 −12 0 −21 0 −1

for horizontal edge detection and 1

0−1

∗ [1 2 1]

=

1 2 10 0 0−1 −2 −1

for vertical direction, where ∗ is the convolution operator.The gradient of each pixel in the picture is computed by applying the Sobel filter in both horizon-tal and vertical direction. Then, the global gradient of the pixel is determined: G =

√G2

x +G2y

with Gx the gradient in the horizontal direction and Gy the gradient in the vertical direction.G is then compared to a threshold. If G has a higher value, the pixel is considered as an edgeand is set to 255. Otherwise, it is set to 0. The threshold is automatically adjusted in order todetect between 10% and 20% of the picture as edges.To construct the horizontal histogram, the detected pixels in each column are counted. Thevalue of the conrresponding field of the histogram is set to this number of detected pixels. Thevertical histogram is built by counting the pixels on each lines of the pictures.A displacement of the drone in the horizontal direction of the picture will shift the horizontalhistogram. The same accurs for vertical displacements:

Hxt = Hxt−1(u− Tx)

Hyt = Hyt−1(v − Ty)



By finding the best matching between the current(Ht) and previous histogram(Ht−1) in bothpicture’s direction (x and y), the optic flow (Tx, Ty) is determined.

5.1 Off-board implementation

In order to be tested, this method has been implemented on a PC.The testing procedure is the following:

• A big image is loaded into the memory, resized to 640x480 and converted to YUYV

• A subimage with size 320x240 is extracted from the center of the big picture

• The algorithm runs on the extracted image

• A second subimage with size 320x240 is extracted from the center of the big picture,but shifted of known values in the horizontal and vertical directions. These shift valuessimulate optic flow and are the reference.

• The algorithm runs on the shifted extracted image

• Optic flow computed results are compared to the reference optic flow.

OpenCV is used for image loading, converting, resizing and disping. Conversion to YUYV, subim-age extraction, Sobel filter and histogram matching is an own implementation.Histogram matching is made by first identifying the zones where the gradient is high, i.e greatherthan the average gradient of the histogram. Shifting is determined by testing translating the his-togram of the previous camera picture in range of [−10, 10] with 1 as step. The translationfactor which gives the smallest error is then considered as the optic flow. When all parts of thehistogram have been processed, the global optic flow is retained by taking the median of all theoptic flow values computed on the different parts of the histogram.

Figure 5.1: Two simulated following pictures with displacement in both directions



Figure 5.2: Result of Sobel filter application and thresholding

Figure 5.3: X and Y histograms of the left simulated picture

Figure 5.4: X and Y histograms of the right simulated picture

5.2 On-board implementation

This method has been tested in the flight arena.A compensation has been added for rotational mouvement: When the drone rotates around ei-ther its roll or pitch axis, it induces an optic flow. We considered the effect on the center point ofth picture with the assumption it is located at the intersection of pitch and roll rotation axis. Fora dθ angle variation, the ground displacement is d = altitude ∗ dθ. This displacement expressedin pixels, the compensation term is p ∗ Fx for the X-axis, where p represents the variation of theangle around the Y axis between two frame. For the Y-axis, this term becomes q ∗ Fy. They are



substracted to the computed optic flow.This algorithm runs on the DSP. Optic flow computation on a 320x240 YUYV image runs in 6ms,including gradient determination, histogram build, histogram matching and optic flow determi-nation. Only the Y component of the picture is used. The rotation compensation run on theARM, because the DSP hasn’t access to the rotation rates.The following graph shows the comparaison between the optic flow speed(blue) and the GPSspeed(red):



Figure 5.5: Optic flow speed vs GPS speed in cm/s



The optic flow speed is noisy. To remove the noise, a test has been made with the collecteddata and a lowpass filter on the optic flow speed. Using an optimization algorithm, the cuttingfrequency has been determined to minimize the sum of errors between the red and blue curves.This frequency is 1.8Hz. The result is shown in the next graphic.



Figure 5.6: Lowpass filtered optic flow speed vs GPS speed in cm/s


Extention to vertical speed

6 Extention to vertical speed

The previous described method measures only the mouvements parallel to the groundplane.But, the drone also moves vertically. Vertical speed measurements on UAV is important forautomatic landing. Optic flow for vertical translation mouvements must also be computed.The effect of such a mouvement on the histogram is either a dillatation or a contraction. If thedrone nears the ground, the picture from the camera becomes more detailed and is a part of theprevious picture taken when the drone was higher. This results in the histogram dillatation. Onthe opposite, when the drone flies up, the new picture from the camera contains the previousone in its center. This leads to histogram contraction.Let Tz be the optic flow vertical speed. Because the drone’s Z-axis points to the ground, wewill define Tz > 0 for descending mouvement (histogram dillatation) and Tz < 0 for ascendingmouvement (histogram contraction) The relationship between a point (X,Y) on the ground andits corresponding horizontal position in the picture (u) is:

u = FxX

Z

If we assume only vertical translation:

u = −FxX

Z

Z

Z

u = −uZZ

A limitation concerning the term ZZ appears: In the case Z

Z = 1, all pixels of the picture aremoved to the center of the picture. This makes tracking with histograms not possible due to alack of common parts. This mouvement is an ascending mouvement, so Tz = − Z

Z and:

u = uTz

with Tz > −1. For symmetry reasons, we can assume Tz < 1.Naming H2(u) a histogram of the current picture and H1(u) the one of the previous picture, therelation between both is

H2(u) = H1(u+ u)

with (u) correspondig to the histogram moves (shifting and scaling) due to the mouvements ofthe UAV.Finally, we have:

Hx2(u) = Hx1[(1 + Tz)u+ Tx]

Hy2(u) = Hy1[(1 + Tz)v + Ty]

Determining the 3D-optic flow consists in finding the values of (Tx, Ty, Tz) which makes betterhistograms matching in X and Y direction.



6.1 Off-board implementation

This way of vertcal optic flow computation has also been tested off-board on a computer inthe same manner as in the previous part. First, only the vertical speed measurement has beentested.Vertical mouvement either has a zoom or unzoom effect. To generate such pictures, the follow-ing formule is applied:

picture_moved(u, v) = picture_ref [(1 + Tz)u, (1 + Tz)v]

Then, once histograms of picture_ref and picture_moved are generated, the program tests Tzvalues in range [0.5; 0.5] with 0.01 step. For each Tz value, a temporary histogram based on theguess of the Tz value. Error between the computed temporary histogram and the histogram ofpicture_moved is computed. Value of Tz which gives the minimum error is considered as thetrue value.

Figure 6.1: Two simulated following pictures with displacement to the ground

Figure 6.2: X and Y histograms of the left simulated picture



Figure 6.3: X and Y histograms of the right simulated picture

Then, horizontal speed estimation has been added to this approach to be able to determinethe 3D velocity of the drone. Horizontal optic flow (Tx, Ty) are also included in the temporaryhistpgram construction.The 3D optic flow is mainly right computed, but for values of some values of Tz above 0.2, theestimation becomes wrong.An other problem is the computation time: testing all the possible combinations of Tx, Ty andTz takes time (around 100ms on the computer). For on-board implementation, this time hasto be reduced. One possibility is to suppose that the acceleration is small, so only the valuesin a limited range around the last measured speed can be tested. Another would be the useof optimisation algorithms. Gradient descend can’t be implemented because of the presence oflocal minimas.

Figure 6.4: Error depending on the tested Tz value

6.2 On-board implementation

In order to run on real-time, the range of tested values for horizontal and vertical flow have tobe limited. In order not to loose in resolution, the previous computed values for Tx, Ty and Tzare used as initial guess. Then, small variation are applied to these values in order to determinethe new optic flow values.Real tests have been done in the arena, the results are not matching the reference given by the



cameras.In order to investigate this problem, a sequence of pictures from the ARDrone down pointingcamera have been recorded while the ARDrone is either ascending or descending. One sequenceconsists of 180 pictures (3 seconds length), which are recorded in raw format on the ARDronefilesystem. After the flight, these pictures are transfered on the PC through Wifi link and con-verted to JPEG pictures. On the PC, a program has been written to read the 180 pictures oneafter another and computing the optic flow between two images.It appears that the sampling frequency of the camera is too high compared to the resolutionof the vertical flow determination algorithm. Implementation has been made by subsamplig at15Hz instead of 60Hz.The following figure shows the vertical speed computed by the optic flow(green) and the refer-ence vertical speed from the camera system(red):

Figure 6.5: Result for vertical optic flow computation



The main remaining problem is the inversion: By positive vertical speed, the optic flow com-putes a negative speed. During take-off, because the drone is very close to the ground, theinduced flow is high and thus can lead to wrong optic flow determination.Because this computation method is not fully completed, the method for only determining hori-zontal optic flow will be used to control the drone with optic flow.


Stabilisation with optic flow

7 Stabilisation with optic flow

On UAVs, optic flow is used for hovering. The goal now is to implement a control loop using theoptic flow speed measurements to make the ARDrone hovering.Optic flow gives two speed estimations: One in each ground-plane direction X and Y. Axes aredefined as follow for the optic flow:

Figure 7.1: ARDrone 2.0 optic flow axis

Rotation around the X-axis is labeled as theta in Paparazzi. Rotation around the Y axis corre-sponds to phi-angle in Paparazzi.Controlling the mouvements of the ARDrone is done by giving angular setpoints to theta andphi. The drone will move in the banking direction. For instance, in order to move the ARDronein the positive X direction, a negative angular setpoint has to be given to phi. In the same way,moving the ARDrone in the positive Y direction is done by setting theta to a negative value.To accomplish optic flow control, the vision loop must interact with the Paparazzi autopilot.A new autopilot mode has been defined. An autopilot mode enables horizontal and verti-cal guidance modes. The vertical guidance will be GUIDANCE_V_HOVER which makes thedrone flying at a constant altitude. The horizontal guidance mode will be a new mode: GUID-ANCE_H_MODE_OPTIC_FLOW. In this mode, the setpoints for theta and phi are the anglesgiven by the vision loop.The vision code is composed of two parts: The first, which runs as a separate thread of Paparazzi,grabs a frame from the down-pointing camera, sends it to the DSP, gets the optic flow resultsand filters it. The second loop is a module of Paparazzi which runs at 60Hz. The control loop isimplemented in this part. If the current autopilot mode matches the AP_MODE_OPTIC_FLOW,a PI controller computes for each axis the angular setpoint in order to make the optic flow mea-surements 0, and then stabilizing the drone in the horizontal plane.Because some variables are shared between two different threads, like the speed computed from



optic flow, a security has to be implemented in order to avoid conflicts. This security consists ina semaphore. The thread who wants to access the variables has first to wait for the semaphorebeing free. Then, it takes the semaphore, processes the values and gives the semaphore back.The semaphore can only be taken by one thread at the same time.The coefficient of th PI controller have been tuned manually in the flight arena. First, the dronetakes off under joystick control until it reaches between 3 and 4 meters height. Then, it entersin the Z_HOLD mode in order to stabilize the flying altitude. In this mode, when no control isapplied to the joystick(theta and phi setpoints are 0), the drone doesn’t hover, but drifts. Then,once the fly altitude is stable, the autopilot mode is manually switched to optic flow control. Pand I are tuning in order the cancel the drift of the drone.In Paparazzi, tuning values such PI coefficients from the ground station, avoiding recompilingand reuploading the code to the drone each time, is done by defining the name of the variablesto change in an xml file describing the minimal and maximal value and also the step. Then,after compilation, P and I can be tuned directly from the ground station.

Figure 7.2: P and I tuning from groundstation

The following coefficients have been determined: P=12 and I=6, assuming angle_setpoint =P ∗ error + I ∗ error_integralResults of the stabilization are shown in the following graph:



Figure 7.3: Hovering results



• Manual take-off and Manual landing:In these modes, the drone is fully manual controlledby hand through a joystick

• Z_HOLD: In this mode, the drone flies at a constant altitude, controlled by Paparazzi loop.Horizontal displacements are still controlled by the joystick. In the second Z_HOLD mode,no control is applied on the joystick.

• OPTIC FLOW SPEED CONTROL: The optic flow control loop gives setpoints for phi andtheta angles in order to have a measured speed equals to 0, and thus stabilize the positionof the drone.

For this hovering implementation, the vertical flow is not taken into account in the optical flowcomputation. The remaining altitude variations induce flow interpreted as horizontal flow. Thisis responsible for a part of the observable position drift in the optic flow mode. Compared to theposition drift of the second Z_HOLD flying step (no control is applied to the drone), the opticflow stabilization reduces strongly the drift.


Conclusion

8 Conclusion

This internship combined both technical and scientifical work.The technical part consisted in making the ARDrone’s DSP usable in an Open Source projet,Paparazzi, enabling the execution of complex vision processing directly on the ARDrone.The scientifical part consisted in adding an autonomous velocity estimation. For this, a optic flowcomputing method has been developped. This method is accurate for horizontal displacements,but needs more research for a 3D implementation. It has been shown that the computed opticalflow can be used to stabilize the position of the drone.I thank all the MAVLab staff, espacially my supervisor, Guido DE CROON, Eric VAN DER HORSTfor his help and Hann Woei HO, PhD student I worked with.I thank also my school supervisor, Renaud KIEFER.


Bibliography

[1] http://omappedia.org/wiki/dspbridge_project.

[2] http://solenerotech1.wordpress.com/2013/10/10/how-to-combine-data-from-gyro-and-accel/.

[3] http://www.olliw.eu/2013/imu-data-fusing/.

[4] Takeo Kanade Bruce D. Lucas. An iterative image registration technique with an applica-tion to stereo vision. Proceedings of Imaging Understanding Workshop, pages 121 – 130,1981.

[5] Zoher Z. Karu C. Quentin David and Dennis M. Freeman. Equivalence of subpixel mo-tion estimators based on optical flow and block matching. Proceedings of InternationalSymposium on Computer Vision, pages 7 – 12, 1995.

[6] Petri Tanskanen Dominik Honegger, Lorenz Meier and Marc Pollefeys. An open sourceand open hardware embedded metric optical flow cmos camera for indoor and outdoorapplications. 2013 IEEE International Conference on Robotics and Automation (ICRA), pages1736 – 1741, 2013.

[7] Gunnar Farneback. Two-frame motion estimation based on polynomial expansion.

[8] Paul Yin from Texas Instruments. Introduction to tms320c6000 dsp optimization. 2011.

[9] Berthold K.P. Horn and Brian G. Rhunck. Determining optical flow. 1981.

[10] David Vissière Pierre-Jean Bristeau, François Callou and Nicolas Petit. The navigation andcontrol technology inside the ar.drone micro uav. Preprints of the 18th IFAC World Congress,pages 1477 – 1484, 2011.

52

LIST OF FIGURES

List of Figures

1.1 The Delfly Explorer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

2.1 ARDrone 2.0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.2 RGB to YUV conversion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42.3 YUV to RGB conversion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

3.1 Paparazzi Logo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

1.1 C6000 DSP Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81.2 Pipeline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91.3 C6000 Pipeline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.1 DSP Bridge Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

5.1 DSP Memory Managment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 175.2 DSP Memory Map . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

1.1 ARDrone 2.0 state estimation and control loops . . . . . . . . . . . . . . . . . . . 271.2 Comparaison between Complementary Filter (Green) and Kalman Filter (Violet)

from [2] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

3.1 View of the ground of the test room from the drone . . . . . . . . . . . . . . . . . 31

4.1 Comparaison between GPS(red) and Optic Flow(blue) speed . . . . . . . . . . . . 34

5.1 Two simulated following pictures with displacement in both directions . . . . . . 365.2 Result of Sobel filter application and thresholding . . . . . . . . . . . . . . . . . . 375.3 X and Y histograms of the left simulated picture . . . . . . . . . . . . . . . . . . . 375.4 X and Y histograms of the right simulated picture . . . . . . . . . . . . . . . . . . 375.5 Optic flow speed vs GPS speed in cm/s . . . . . . . . . . . . . . . . . . . . . . . . 395.6 Lowpass filtered optic flow speed vs GPS speed in cm/s . . . . . . . . . . . . . . . 41

6.1 Two simulated following pictures with displacement to the ground . . . . . . . . 436.2 X and Y histograms of the left simulated picture . . . . . . . . . . . . . . . . . . . 436.3 X and Y histograms of the right simulated picture . . . . . . . . . . . . . . . . . . 446.4 Error depending on the tested Tz value . . . . . . . . . . . . . . . . . . . . . . . . 446.5 Result for vertical optic flow computation . . . . . . . . . . . . . . . . . . . . . . 45

7.1 ARDrone 2.0 optic flow axis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 477.2 P and I tuning from groundstation . . . . . . . . . . . . . . . . . . . . . . . . . . 487.3 Hovering results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49


Rapport de PFE -...

Documents

Transcript of Rapport de PFE -...