Technical documentation for AR.Drone project

Inkyu, Hu and Van

Cyphy Lab.Queensland University of Technology

January 14, 2013

Contents

1 Introduction 2

2 System Overview 22.1 AR.Drone platform . . . . . . . . . . . . . . . . . . . . . . . . 22.2 Initial ARDrone coordinates . . . . . . . . . . . . . . . . . . . 22.3 Navdata . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22.4 ROS topic name . . . . . . . . . . . . . . . . . . . . . . . . . . 3

3 VICON comparison 4

4 Software building instruction 54.1 Building ardrone autonomy package . . . . . . . . . . . . . . . 54.2 Running ardrone autonomy driver . . . . . . . . . . . . . . . . 64.3 Building ethzasl ptam package . . . . . . . . . . . . . . . . . . 64.4 Running ethzasl ptam package . . . . . . . . . . . . . . . . . . 74.5 Building ardrone pose estimation package . . . . . . . . . . . 104.6 Running ardrone pose estimation package . . . . . . . . . . . 10

5 Kalman Filter for states estimation 125.1 States . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125.2 Process model . . . . . . . . . . . . . . . . . . . . . . . . . . . 125.3 Measurement model . . . . . . . . . . . . . . . . . . . . . . . . 125.4 Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135.5 Update . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

6 Scale Estimation 136.1 Scale calibration . . . . . . . . . . . . . . . . . . . . . . . . . . 136.2 Online scale estimation . . . . . . . . . . . . . . . . . . . . . . 136.3 Experimental validation . . . . . . . . . . . . . . . . . . . . . 13

7 Control 137.1 System Identification . . . . . . . . . . . . . . . . . . . . . . . 137.2 Controller design . . . . . . . . . . . . . . . . . . . . . . . . . 147.3 Performance evaluation . . . . . . . . . . . . . . . . . . . . . . 14

8 Experimental results 14

1

1 Introduction

This document addresses the procedure how to develop an autonomous flysystem based on off-the-shelf AR.Drone.

2 System Overview

2.1 AR.Drone platform

2.2 Initial ARDrone coordinates

This document describes our ardrone coordinate systems: a IMU and a cam-era.

Figure 1: Close look. IMU and camera coordinate definitions. Red, greenand blue denote x,y,z respectively.

2.3 Navdata

navdata is a fundamental data structure which contains Euler angles, alti-tude, accelerations, linear velocity1. This data can be obtained up to 100 Hz.

1ROS Header message definition. http://www.ros.org/doc/api/roslib/html/msg/Header.html

2

Table 1: navdata structure definition with SI unit.

navdata type Description Unit

batteryPercent float32 0 to 100 %rotX float32 left/right tilt

rotY float32 forward/backward tilt

rotZ float32 orientation,yaw

altd float32 estimated altitude mvx float32 linear x velocity m/svy float32 linear y velocity m/svz float32 linear z velocity m/s

accx float32 body x acceleration m/s2

accy float32 body y acceleration m/s2

accz float32 body z acceleration m/s2

gyrox float32 angle rate about x axis /sgyroy float32 angle rate about y axis /sgyroz float32 angle rate about z axis /s

tm float32 Time stamp from ardrone secheader Header ROS header1

2.4 ROS topic name

For ROS, we have to define topic names and namespace. Figure2 shows briefdescription of ROS topic messaging map. More detail also can be found inthe repository named ardrone navi/src/ours.

3

Data

ardrone_brownARDrone

Parrot ardrone_pose_estimation

drone_teleop cyphy_kbd

Cmd

/ardrone/navdata

time stamp (sec)pitch (deg)roll (deg)yaw (deg)vx (m/s)vy (m/s)vz (m/s)

altitude (m)

Image topic

/ardrone/image_raw

/cmd_vel

linear.xlinear.ylinear.z

angular.z

/cmd_vel

/keycode

100Hz

15Hz

rviz/tf

/world to /base_link

Figure 2: Blue boxes denote ROS nodes the orange box is a ardrone platform.Gray boxes present both ROS topic name and data structure. Generally aprefix / stands for ROS topic.

0 10 20 30 40 50 60 700.8

0.6

0.4

0.2

0

0.2

0.4

0.6

0.8

sec

m/s

ardrone vxground truth vx

0 10 20 30 40 50 60 701.5

1

0.5

0

0.5

1

1.5

sec

m/s

ardrone vyground truth vy

Figure 3: Blue denotes ground truth velocity obtained from VICON and redis AR.Drone estimated lateral x and y velocity respectively.

3 VICON comparison

In this section ground truthed comparison is presented. We compared lat-eral body velocity estimation from AR.Drone to velocity obtained from sub-millimetre accuracy VICON system. In addition roll-pitch-yaw angle estima-tion comparison is presented.

4

0 10 20 30 40 50 60 7010

5

0

5

10

15

sec

Deg

ardrone roll (degree)ground truth roll

0 10 20 30 40 50 60 7020

15

10

5

0

5

10

sec

Deg

ardrone pitch (degree)ground truth pitch

0 10 20 30 40 50 60 7010

5

0

5

10

sec

Deg

ardrone yaw (degree)ground truth yaw

Figure 4: Blue denotes ground truth angles obtained from VICON and redis AR.Drone estimated angles, roll, pitch and yaw respectively.

4 Software building instruction

In this section, how to build ardrone brown, ardrone pose estimation andethzasl ptam packages will be described step by step. Before getting started,we define the repository sync local folder as:

1 /home/ enddl22 /Workspace/ a r d r i n e s i d e p r o j e c t

and ROS working space path is

1 /home/ enddl22 /Workspace/ f u e r t e

Note that this path should be different for each machine.

4.1 Building ardrone autonomy package

Compared to previous ardrone brown package, ardrone autonomy packageextended the driver compatible with AR.Drone 2.0 and also increase theimage sampling rates.

In order to build ardrone autonomy package on your local machine, youshould checkout the package from our repository located in https://enddl22@bitbucket.org/enddl22/ardrone_side_project.git. Once you are underthe folder named ardrone autonomy, the driver should be built with followingcommands.

1 . / bu i l d sdk . sh

5

2 mkdir bu i ld3 cd bu i ld4 cmake . .5 make

Now you should see the ardrone driver appearing under subfolder /bin.

4.2 Running ardrone autonomy driver

By running this package we are able to access all data of AR.Drone. Open anew console and type

1 r o s c o r e

Searching SSID ardrone 015526 from your WiFi searching lists and thisSSID should be different for each ardrone. After establishing connection,type

1 i f c o n f i g

You should able to see 192.168.1.x for your WiFi adaptor. Go to ardrone brown/binand run ros ardrone driver node.

1 cd /home/ enddl22 /Workspace/ f u e r t e / ardrone brown / bin2 . / a r d r o n e d r i v e r

Checking all data is being published using ros topic command.

1 r o s t o p i c l i s t vYou should be able to see /ardrone/navdata on the publishing lists.

1 r o s t o p i c echo / ardrone /navdata

You should be able to see data, rotX, rotY, rotZ, accX etc on the console.Further if you want to plot all data from AR.Drone, type

1 ros launch ardrone brown rxp l o t da ta . launch

You should be able to see Fig. 5.

4.3 Building ethzasl ptam package

To avoid corruption the local repository, lets make a working copy to workspace.

1 cp a . / a r d r o n e s i d e p r o j e c t / s r c / ours / ethzas l ptam /home/ enddl22 /Workspace/ f u e r t e /

2 cd /home/ enddl22 /Workspace/ f u e r t e / ethzas l ptam3 rosmake ethzas l ptam

6

Figure 5: Plotting all data using rxplot tool.

You should be able to see the following log message

. . .[ rosmake ] Bu i l t 25 packages with 0 f a i l u r e s .[ rosmake ] Summary output to d i r e c t o r y

Now we are at

1 pwd /home/ enddl22 /Workspace/ f u e r t e / ethzas l ptam

Lets check all executable binaries are generated correctly.

1 cd ptam/ bin

You should be able to see build, bin folders and also can see executable filesfrom bin folder inside.

4.4 Running ethzasl ptam package

We need open 4 terminals for roscore, ardrone brown ros ardrone driver,image proc to covert RGB image into gray scale and ptam. Running thefollowing commands for each terminal window.

7

1 r o s c o r e2 ardrone brone ( d i r path=ardrone brown / bin )3 ROS NAMESPACE=ardrone rosrun image proc image proc ( d i r

path=i ma g e p i p e l i n e / image proc / bin )4 ros launch . / ptam . launch ( d i r path ethzas l ptam /ptam/

launch )

You should be able to see Fig.6.

8

(a) Image overlaying window with tracking features and the reference frame.

(b) 3D Map and 6 degree of freedom camera position estimation visualisationview

Figure 6: PTAM

9

4.5 Building ardrone pose estimation package

As mentioned before, lets make a working copy to workspace.

1 cp a . / a r d r o n e s i d e p r o j e c t / s r c / ours /a rd rone po s e e s t imat i on /home/ enddl22 /Workspace/f u e r t e /

2 cd /home/ enddl22 /Workspace/ f u e r t e /a rd rone po s e e s t imat i on

3 rosmake a rd rone po s e e s t imat i on

You should be able to see the following log message

. . .[ rosmake ] Resu l t s :[ rosmake ] Cleaned 20 packages .[ rosmake ] Bu i l t 20 packages with 0 f a i l u r e s .[ rosmake ] Summary output to d i r e c t o r y

You should be able to see build, bin folders and also can see executable filesfrom bin folder inside.

4.6 Running ardrone pose estimation package

Make sure ardrone brown driver running and publishing navdata topic bytyping

1 r o s t o p i c l i s t vYou should be able to see the following message

Publ ished t o p i c s : / ardrone /navdata [ ardrone brown /Navdata ] 1 pu b l i s h e r / ardrone / image raw [ sensor msgs /Image ] 1 pu b l i s h e r / rosout agg [ rosgraph msgs /Log ] 1 pu b l i s h e r / rosout [ rosgraph msgs /Log ] 1 pu b l i s h e r / c l o ck [ rosgraph msgs /Clock ] 1 pu b l i s h e r

Launching the ardrone pose estimation node.

1 ros launch a rd rone po s e e s t imat i on a r d r o n e n a v i t r j .launch

Note that if you are publishing navdata through rosbag file then you needto modify source code. Since ardrone pose estimation will be started positionestimation 4 second later after receiving taking off command. This 4 secondwas picked experimentally and is the time when AR.Drone goes to hoveringstate.

10

Figure 7: Position estimation visualisation using rviz.

Modify ardrone pose estimation/src/ardrone pose estimation.cpp file linenumber 338

i f ( now . toSec ( ) t a k e o f f s t a r t . toSec ( ) >4 &&p i c k t a k e o f f t i m e )

to

i f ( 1 )

then rebuild the package

1 rosmake a rd rone po s e e s t imat i on

Then you should be able to see Fig. 7. Note that rviz is installed properlyon your system otherwise need to build rviz by typing

1 rosmake r v i z

You should be able to see the following message.

[ rosmake1] F in i shed

[ rosmake ] Summary output to d i r e c t o r y

5 Kalman Filter for states estimation

In this section, we will describe only states, process model and measurementmodel. More detail and well-documented Kalman filter can be found fromwikipedia.2

5.1 States

States are expressed w.r.t world coordinate.

Xk =[xk yk xk yk

]T(1)

5.2 Process model

Linear constant-velocity process model is

F =

1 0 00 1 0 0 0 1 00 0 0 1

(2)where is sample rate of Kalman Filter, 160 Hz.

5.3 Measurement model

H =

1 0 0 00 1 0 00 0 1 00 0 0 1

(3)

zk = HXk (4)

where zk is 4 1 position and velocity measurement vector. Position isobtained from PTAM with scale estimation at 15 Hz and velocity comes fromAR.Drone onboard at 160Hz. We choose Q and R matrix as

Q =diag[0.1 m 0.1 m 0.3 m/s 0.3 m/s

]2R =diag

[0.1 m 0.1 m 0.05 m/s 0.05 m/s

]22wikipedia Kalman Filter. http://en.wikipedia.org/wiki/Kalman_filter

12

5.4 Prediction

States prediction may be represented

Xk = FXk1 (5)

Covariance prediction may be represented

Pk =FPk1FT + Q (6)

Pk is predicted covariance and Pk1 is estimated covariance at k 1.

5.5 Update

Compute Kalman gain.

S =HPk HT + R

K =Pk HS1

Compute innovation matrix.

N =zHXkStates and covariance update

Xk =Xk + KN

Pk =Pk KHPk

6 Scale Estimation

6.1 Scale calibration

6.2 Online scale estimation

6.3 Experimental validation

7 Control

7.1 System Identification

For x,y,z,yaw models.

13

7.2 Controller design

Something about PIDs.

7.3 Performance evaluation

Simulated model output VS real model output.

8 Experimental results

Hovering and way points following.

14