06gr1037b Student Report

7/31/2019 06gr1037b Student Report

1/233

AALBORG UNIVERSITYDepartment of Control Engineering

Fredrik Bajers Vej 7 - 9220 Aalborg st - Telephone 96 35 80 80

TITLE:

Development and Navigation of an

Autonomous UAV

PROJECT PERIOD:

September 5th - June 1st

PROJECT GROUP:

1034B

GROUP MEMBERS:

Jacob T. HolmgaardChristian S. JensenStefan Lnner

SUPERVISORS:

Anders la Cour-Harbo

NUMBERS PRINTED: 8

REPORT PAGE NUMBER: 135

TOTAL PAGE NUMBER: 225

SYNOPSIS:

This thesis concerns develop-ment and navigation of an au-tonomous UAV platform, ableto complete the levels 1 and2 of IARC06. After a de-sign of the UAV hardware and

software platform, the applica-tions needed to complete thelevels are defined. To guidethe UAV through the levels 1and 2, a navigation system wasdeveloped. A map over thearea in which the missions areto be executed has been de-rived. From this map the GPScoordinates for the flight pathhas been extracted, and throughoptimization algorithms a nearoptimal flight path has been

found. Through grounded teststhe navigation system quali-tively proved to be able to com-plete levels.


2/233


3/233

Preface

This thesis is made by group 1034B at the Department of Control En-gineering at Aalborg University in the period between September 5th2005 and June 1st 2006. The thesis has been done as a part of thedevelopment of an autonomous helicopter based UAV in-progress atthe control department at Aalborg University. The report consists offour parts: "Mission Description and Problem Formulation", "PlatformDesign", "Mission Applications", and "Conclusion and Perspective". Thefour parts all begin with an introduction and consist of several chap-ters each containing a description of the content and, if necessary, ashort summary. Supplementary subjects are placed in appendices in

the back of the report and are numbered A, B and so forth. Enclosed onthe inside of the back page is the project CD, containing MATLAB code,SIMULINK models, C code, and a pdf copy of this report.

Reading Instructions

To help guide the reader through the report, a reading guide is pre-sented.

throughout the thesis different text syntaxes are used. When writ-ing the name of a variable, the text is emphasized and when writingthe name of an interprocess communication message, the text is capi-talized. An example of a variable is variable and an example of a mes-sage is MESSAGE.

Figures and tables are numbered consecutively starting over fromeach chapter, for example the 4rd Figure in the 3th chapter would beFigure 3.4. When referring to an equation, the number is simply inparenthesis, such that if a reference is made to the 7th equation in the2th chapter it would say: "as seen in (2.7)".

References to literature will throughout the thesis be written insquare brackets containing the authors surname and the year of the

III


4/233

publishing. This refer to the bibliography, where further informationabout the source can be found. The reference to a source might alsocontain a page number if necessary, e.g. [Fossen 2002, p. 5].

Jacob T. Holmgaard Christian S. Jensen

Stefan Lnner

IV


5/233

Contents

Introduction 1

I Mission Description and Problem Statement 5

1 Mission Overview 7

1.1 Description of Levels . . . . . . . . . . . . . . . . . . . . 71.2 Mission Approach . . . . . . . . . . . . . . . . . . . . . . 8

2 Problem Formulation 11

II Platform Design 13

3 Hardware Platform 15

3.1 Platform Description . . . . . . . . . . . . . . . . . . . . 153.2 Hardware Implementation . . . . . . . . . . . . . . . . . 19

4 Software Platform 234.1 Software Environment . . . . . . . . . . . . . . . . . . . 234.2 Software Architecture . . . . . . . . . . . . . . . . . . . . 23

III Mission Applications 31

5 Navigator 33

5.1 Analysis of Navigator . . . . . . . . . . . . . . . . . . . 335.2 Design of Navigator . . . . . . . . . . . . . . . . . . . . 405.3 Simulation of Navigator . . . . . . . . . . . . . . . . . . 525.4 Test of Navigator . . . . . . . . . . . . . . . . . . . . . . 595.5 Tuning of Navigator . . . . . . . . . . . . . . . . . . . . 65

6 Vision 69

V


6/233

CONTENTS

6.1 Analysis of Vision . . . . . . . . . . . . . . . . . . . . . . 69

6.2 Design of Vision . . . . . . . . . . . . . . . . . . . . . . . 70

6.3 Test of Vision . . . . . . . . . . . . . . . . . . . . . . . . . 72

7 Mission Control 75

7.1 Design of Mission Control . . . . . . . . . . . . . . . . . 75

7.2 Test of Mission Control . . . . . . . . . . . . . . . . . . . 78

8 Flight Data Logger 818.1 Analysis of Flight Data Logger . . . . . . . . . . . . . . 81

8.2 Design of Flight Data Logger . . . . . . . . . . . . . . . 82

9 Ground Station 83

9.1 Analysis of Ground Station . . . . . . . . . . . . . . . . 83

9.2 Design of Ground Station . . . . . . . . . . . . . . . . . 85

9.3 Test of Ground Station . . . . . . . . . . . . . . . . . . . 88

10 Map Generation for UAV Navigation 93

10.1 Analysis of Map Generation . . . . . . . . . . . . . . . . 93

10.2 House Map . . . . . . . . . . . . . . . . . . . . . . . . . . 9410.3 Waypoint Placement . . . . . . . . . . . . . . . . . . . . 95

10.4 Coordinate Transformation . . . . . . . . . . . . . . . . 96

11 Generation and Optimization of Level 2 Path 109

11.1 Basic Graph Theory . . . . . . . . . . . . . . . . . . . . . 110

11.2 Description of Path Generation Problem . . . . . . . . . 111

11.3 Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . 112

11.4 Comparison of Heuristics . . . . . . . . . . . . . . . . . 117

11.5 Path Generation for Level 2 . . . . . . . . . . . . . . . . 118

11.6 Evaluation of Generated Path . . . . . . . . . . . . . . . 123

IV Conclusion and Perspectives 127

12 Conclusion 129

13 Perspectives 133

Bibliography 134

VI


7/233

CONTENTS

Appendix 135

A RTAI Linux Platform 137

B Comparison of Route Generation Heuristics 139

C Helicopter code 143

C.1 Mission Control Code . . . . . . . . . . . . . . . . . . . 143C.2 Navigator Code . . . . . . . . . . . . . . . . . . . . . . . 148

C.3 Controller Code . . . . . . . . . . . . . . . . . . . . . . . 165C.4 Ground Station Code . . . . . . . . . . . . . . . . . . . . 169C.5 Logger Code . . . . . . . . . . . . . . . . . . . . . . . . . 189C.6 Vision Code . . . . . . . . . . . . . . . . . . . . . . . . . 196C.7 Platform definitions Header File . . . . . . . . . . . . . 205

D Route Generation Code 213

D.1 Optimal Route Algorithm . . . . . . . . . . . . . . . . . 213D.2 Nearest Neighbor Heuristic . . . . . . . . . . . . . . . . 213D.3 2-OPT Optimization Heuristic . . . . . . . . . . . . . . . 214

E Map Generation Code 217

E.1 Map Transformation Algorithm . . . . . . . . . . . . . . 217E.2 Relative House Coordinates . . . . . . . . . . . . . . . . 219E.3 Relative Waypoint Coordinates . . . . . . . . . . . . . . 219

F Listing of Interprocess Communication Messages 225

VII


8/233


9/233

Introduction

The motivation for this thesis is to participate in the InternationalAerial Robotics Competition 2006 (IARC06) at Fort Benning, Geor-gia, USA. It is a competition organized by the Association for Un-manned Vehicle Systems International (AUVSI), which is the worlds

largest non-profit organization devoted exclusively to advancing theunmanned systems community. The purpose of the competition is tomake an Unmanned Aerial Vehicle (UAV) perform a defined mission.AUVSI have defined three mission examples, which all require thesame behavior and all would be dangerous for people to carry out.

One mission example is a hostage situation, where a group of peo-ple have been detained by a terrorist group at an embassy. 3 km fromthe embassy a submarine is situated, in which a rescue team is readyto launch a UAV which is intended to determine how many terroristsare guarding the hostages. The embassy can be identified by the na-tional seal over the main entrance, and can be entered through some

open windows. The mission is to make the UAV navigate the 3 km tothe embassy, and then recognize the building visually by identifyingthe national seal on the wall. Furthermore the UAV has to investigatethe building further to locate an entrance large enough for deployinga vehicle into the building. This vehicle must provide a visual link tothe hostage situation, enabling the rescue team to identify the numberof terrorists. The mission has to be carried out in less than 15 minutesin order for the rescue team not to loose the advantage of the elementof surprise. A full description of the missions can be found on thewebsite [AUVSI 2006]. The content of the UAV missions in the otherscenarios are similar to the mission described above and forms four

levels to complete in the IARC06.The McKenna MOUT Site at Fort Benning in Georgia, USA, at

which the IARC06 is held, is a military training facility used by theU.S. Army Battle Lab. The facility is used in the conduct of trainingexercises, experimentation with aerial and ground unmanned plat-forms, and testing of new equipment and concepts for the U.S. Armyand other Armed Forces. A view of the area is shown in Figure 1.The facility forms a realistic urban environment which features 15 Eu-ropean style buildings forming a small city. This makes the site wellsuited for experimental UAV missions and therefore also for hosting

1


10/233

INTRODUCTION

Figure 1: Elevated view of the facility at theMcKenna MOUT Site.

the IARC06.In this thesis the project group have developed a fully functional

autonomous UAV platform. The majority of the components havebeen designed and implemented from the ground up, and all of thethe following topics have been considered by the project group in thisproject:

Development of hard real-time multi thread software environ-

ment.

Real-time constrains in relation to control, navigation, and com-munication.

Flexible navigation strategies and methods.

Physical modifications of model helicopter.

Flight path generation and optimization.

Software/hardware interfacing.

Automated building of maps for navigation.

Thorough ground test of hardware, real-time software, and al-gorithms.

Visual recognition and categorization.

Development of on-board electronics.

In-flight error handling of unforseen events.

Aerial test of hardware, real-time software, and algorithms.

2


11/233

Due to the extensive scope of the development of the UAV other peo-ple of the local UAV group has also been involved in the developmentprocess. Since several simultaneous projects uses the UAV platform,the following tasks has not been handled by this group:

Development of helicopter simulation math model, sensor-fusion,observer, and controller.

Parts of the on-board electronics.

Wire harness and power supply.

Flight approval test.

Vision recognition algorithms.

Sensor server software.

This briefly describes the scope of this project. In the next chapter,the mission description and problem statement will be presented.

3


12/233


13/233

Part I

Mission Description andProblem Statement

This first part of the project report contains two chapters concerning therules and content of the IARC06, and the groups preliminary consid-erations of this project. The first chapter gives an introduction to theIARC06 and defines the tasks of the competition, followed by a descrip-tion of the approach intended by the group. The second chapter forms theoverall problem formulation for this project.

5


14/233


15/233

Mission Overview 1

The IARC06 level 1, 2, 3, and 4 requirements, and mission approach pre-sented in this chapter, forms the basis for the problem formulation in chapter

2.

1.1 Description of Levels

Based on the mission mentioned in the introduction on page 1, AUVSIhas defined 4 levels for the autonomous UAV mission of the IARC06.The four levels of the competition each consists of autonomous tasksthat has to be completed by the UAV within specific rules. In eachlevel the autonomy required to complete the level increases makingeach level more difficult to complete.

In level 1 the scope is to autonomously fly a route of approximately3 km in less than 15 minutes. This route will consist of a set of GPScoordinates (waypoints) placed in the landscape surrounding the city.The coordinates are provided immediately prior to the flight.

In level 2 the UAV must fly over a city in search of a marker inform of the sign shown in Figure 1.1. This marker is placed on a wallon one of the buildings in the city. The UAV must first locate themarker and then proceed with a further investigation of the house,on which the marker is found, to locate entrances larger than 1x1 m.These entrances could be either windows or doors, the marker is notnecessarily placed on the same side of the building as the best suited

entrance. For level 2 to be successfully completed, the mission mustbe carried out in under 15 minutes. Once the entrance is located level2 is complete.

In level 3 the UAV must deploy another autonomous vehicle thro-ugh the located entrance. This vehicle must be autonomous, but notnecessarily aerial. When inside the building, the vehicle must pho-tograph all the walls and transmit the pictures to the helicopter, thismust be done in under 15 minutes.

In level 4 the level is completed successfully if level 1, 2, and 3 arecarried out in less than 15 minutes.

7


16/233

CHAPTER 1. MISSION OVERVIEW

Figure 1.1: Illustration the marker the UAV mustrecognize in level 2.

As the framework of the IARC06 now has been presented, the mis-sion approach will be described in the next section.

1.2 Mission Approach

This project focuses on completing level 1 and 2 of the IARC06. Thefirst step is to define the strategy for completing these levels. Numer-

ous considerations are involved in this. However, only the result ofthese, the so-called mission approach, is shown here. The mission ap-proach describes the steps needed to complete the levels 1 and 2.

In Figure 1.2 an overview of the mission approach formed by thegroup is illustrated. Firstly a path must be derived on-board and inreal-time on the UAV based on the waypoints given for the level 1flight. It is necessary to be able to fly as fast as possible due to themission time constraints and to have as much time as possible for in-vestigation of the city in level 2. When the path has been flown, level1 is complete.

When level 1 is completed, the UAV will be placed near the city,

hence, level 2 is initiated immediately after level 1 completion. In thelevel 2 flight, the marker needs to be identified, and an entrance largerthan 1 1 m must be located. It has been chosen to investigate thecity by a number of predefined waypoints, placed throughout the city,in which the helicopter will hover and take the pictures. The pre-defined picture waypoints are located in positions such that all thewalls at which the marker or a house entrance could be placed is pho-tographed by the UAV. Two of the advantages of waypoint-based in-vestigation of the city are: 1) knowledge of which house is being pho-tographed and 2) hereby knowledge of on which house the marker is

8


17/233

1.2. MISSION APPROACH

Determine path fromwaypoints

Follow path

Level 1

Fly to picture waypoint

Take and analyze pictures

Marker found?

Identify house

Photograph walls

Identify entrances

yes

no

Level 2

Figure 1.2: Overview of the mission approach

found. In this project, it is assumed that hover forms the best condi-tions for taking pictures, i.e. it minimizes picture distortion and mo-tion blur.

An alternative approach for investigating the city could be contin-

uous trajectory flight where the houses are photographed when theUAV is in motion. However, as the navigation strategy in waypoint-based investigation complies well with navigation strategy needed forlevel 1, it is possible to use the same navigation system for both level1 and level 2. Therefore the waypoint-based approach is found to bethe best suited approach for completing level 2 in this project.

All of the picture waypoints will be derived from a map of the citygenerated off-line before the competition.

The helicopter then flies from waypoint to waypoint taking pic-tures of the houses and analyzing them in real-time in search of the

9


18/233

CHAPTER 1. MISSION OVERVIEW

marker. When the marker is identified on a house the next step isto photograph each wall of this house in search of an entrances largerthan 11 m. This search is performed in the same way as when search-ing for the marker, using the picture waypoints related to this specifichouse. From these pictures a suitable entrance is chosen, and this con-cludes level 2.

The scope of this thesis is to make the UAV complete level 1 and 2.This choice has been made since it can be achieved with the use of onlyone autonomous vehicle. Level 3 is out of scope due to the limitedamount of payload of the helicopter platform and completion of level3 will also include development of a second autonomous vehicle thathas to be deployed from the UAV.

After having defined the approach to each level of the competitiona problem formulation can be formed to specifically define the tasksinvolved in this project.

10


19/233

Problem Formulation 2

The scope of this thesis is to develop a fully functional UAV platformcapable of completing level 1 and level 2 of the IARC06.

Based on the mission approach described in the previous sectionthe tasks to be completed by the group in this project can now be de-fined. Each task and its content are described in the list below. Thefirst 5 tasks are based directly on the mission approach and are cru-cial to the completion of level 1 and level 2 of the IARC06. The last 2tasks in the list are required in the implementation and test of the first5 tasks and in the general use of the UAV platform.

Real-time software environment for scheduling of in-flight softwaretasks. To solve this task a hard real-time multi thread softwareenvironment will be fully implemented on an on-board com-puter configuration, able to handle all real-time and non real-time software subsystems, running during flight.

Navigation system for path planning and navigation during level 1and level 2. The navigation system will be fully designed andimplemented on the UAV, and capable of real-time planning ofa path for level 1 based on any given waypoints, and perform awaypoint based level 2 flight.

Map and Path Generation for optimal navigation during level 2 flight.An algorithm for generation of GPS coordinates for the way-points of level 2 will be designed and implemented based on arelative map extracted from an aerial photo. Furthermore routeoptimization algorithms will be investigated and applied to en-sure a near optimal flight path in level 2.

Vision system for recognition of the marker and house entrances. Forthis purpose an on-board camera will be implemented on theUAV and a software subsystem will be designed and implemen-ted able to communicate with the camera hardware and performreal-time analysis of the visual input during flight.

UAV hardware platform for full autonomous flight. The hardware

11


20/233

CHAPTER 2. PROBLEM FORMULATION

platform includes implementation of an on-board computer, sen-sors and actuators for navigation and control, hardware inter-face for interfacing of sensors and actuators to the on-board com-puter.

Ground station for mission control and visual presentation of heli-copter data in real-time. A software system will be designed andimplemented on a computer on the ground capable of wirelessin-flight communication with the UAV, visual real-time presen-

tation of mission status and helicopter data, and data logging forlater off-line processing flight data.

Helicopter modifications to accommodate the needed space for theon-board electronics on the stock model helicopter. The modi-fications includes airframe modifications and rearrangement ofcomponents.

The completion of level 1 and level 2 of the IARC06 is done in co-operation with group 1034A who are responsible for modelling andcontrol in relation to the UAV development. Due to this all develop-ment in this thesis will be done under the assumption of a stable UAV

platform able to fly according to any given reference input.The UAV developed in this project functions as a multiple purpose

UAV platform at the Control Department at Aalborg University and itwill be developed in cooperation with group 1034A, Associate Profes-sor Anders la Cour-Harbo, and Ph.D. Student Morten Bisgaard.

12


21/233

Part II

Platform Design

This part contains two chapters and concerns the structure of the hard-ware and software platform for this project. The first chapter describes theon-board electronics of the UAV and its implementation on the model he-licopter. The second chapter defines all the necessary software subsystemsand the communication between them.

13


22/233


23/233

Hardware Platform 3

For this project the hardware platform has partly been given in advance. ABergen Industrial Twin helicopter has been acquired as the base of a multiple

purpose UAV platform developed at the Control Department at Aalborg Uni-versity. Due to this, the project is bounded by the capabilities and limitationsof this specific helicopter.

The intention is to mechanically modify the stock helicopter in order tofit sensors enabling the determination of position, attitude, rotational veloc-ity, acceleration, and vision from the helicopter. Furthermore an OBC, anda hardware input-output interface for sensors and actuators, will be fitted toprocess the sensor data and control the helicopter.

3.1 Platform DescriptionIn this section the constructed hardware platform will be described.The platform has been developed in cooperation with Associate Pro-fessor Anders la Cour-Harbo and Ph.D. Student Morten Bisgaard. Thissection provides an overall description of the developed hardware de-sign.

The helicopter airframe used as the basic mechanical platform is aBergen Industrial Twin which has the following specifications

Length: 1663.0 mm

Height: 558.8 mm

Weight: 8.165 kg

Main Rotor Diameter: 1940 mm

Tail Rotor Diameter: 340 mm

Payload: 11 kg approx.

Engine: Twin Cylinder Bergen/Zenoah - 52 cc

15


24/233

CHAPTER 3. HARDWARE PLATFORM

Figure 3.1: Stock Bergen Industrial Twin withoutmodifications.

An overview of the different hardware parts and their interaction areshown in Figure 3.2. Each part will be described in the following sec-tion.

Servos are the actuators of the system. The collective pitch servo, thepitch servo and the roll servo are of the type JR NES 4131 andthe throttle servo is a JR NES 4721. These are all controlled by a50 Hz PWM signal from Servo Board. The tail rotor servo con-trolling the yaw motion of the helicopter is a Futaba S9256 whichis a digital high-speed servo controlled by Gyro Controller. Thiscontrol signal is a 330 Hz PWM signal. All the servos are sup-

plied with 6 V from Radio Receiver.When flying manually all the servo control-signals run directlyfrom Radio Receiver through Servo Board to each servo withthe exception of the signal to the throttle servo which runs di-rectly from the governor, and the tail servo which runs to GyroController. When flying autonomous the servo signals are allgenerated by Servo Board based on the servo positions receivedfrom Onboard Computer.

Gyro Sensor and Gyro Controller are the GY611 from Futaba consist-

16


25/233

3.1. PLATFORM DESCRIPTION

OnboardComputer

SerialInterface

Servo Board

Servo

Pitch

Servo

Roll

Servo

Col.

Servo

Throttle

ServoYaw

Radio Receiver

Governor

Tachometer

Wireless

Network

Camera

GPS

AntennaGPS

Gyro /

Accelerometer

Compas

GyroController

GyroSensor

Figure 3.2: Overview of the hardware interaction.

ing of a yaw angular-velocity sensor and a controller that sta-bilizes the yaw angular velocity to 0 rad/s but still enables thepilot to give a yaw angle input to maneuver the helicopter. TheGY611 is supplied with 6 V from Radio Receiver

Radio Receiver is a Graupner JR smc 20DS which is a 10 channel PCMreceiver. The receiver is controlled by a Graupner MX-22 trans-

mitter and supplied with 6 V from a 4200 mAh NiMH accumu-lator.

Onboard Computer is a DELL Latitude D400 which is a 1.8 GHz Pen-tium M with 512 MB DDR RAM. Onboard Computer is con-nected to Servo Board via a RS232 connection which is vital forcontrol of the helicopter since this connection has some real-timedemands in order to control the helicopter at a fixed frequency.The two blocks Serial Interface and Camera are USB and USB2.0 connections respectively. WLAN is the Wireless Network

17


26/233


connection, the build-in wireless adapter is used in this case.

Serial Interface is an Edgeport/8r from Digi International which is aUSB-to-serial converter converting one USB port to eight serialcom ports which are necessary for interfacing GPS, Compassand Gyro/Accelerometer to Onboard Computer

Gyro/Accelerometer is a Falcon MX consisting of three gyroscopesand accelerometers measuring angular rates and accelerations

in three dimensions. Gyro/Accelerometer communicates withSerial Interface through a RS232 connection and is being sup-plied with 5 V from the PSU.

Compass is a Honeywell HMR2300 which is a three-axis digital mag-netometer measuring the helicopters attitude in three dimen-sions in relation to the earths magnetic field. Compass is sup-plied with 11.1 V from the PSU and connected to Serial Interfacevia a RS232 connection.

GPS and GPS antenna is a Novatel OEM4-G2L GPS card with an ac-

tive antenna. It has a 20 Hz refresh rate. The GPS is suppliedwith 3.3 V from the PSU and connected to Serial Interface thro-ugh a RS232 connection.

Governor is a Futaba GV-1 which controls the rotor RPM to a fixedvalue regardless of the drag of the rotor blades. Governor isconnected to Tachometer which provides the rpm-feedback tothe governor. Governor is also connected to Receiver in orderfor the pilot to be able to give an input.

Camera is a UI-1410-M from Ueye which is a CMOS-based mono-chrome VGA-camera. Camera is connected to Onboard Com-

puter via a USB2.0 connection.

Servo Board is the interface between the onboard computer and theactuators of the system. Servo Board is based on a PIC18F458from Microchip which is used to generate PWM signals to theservos based on the servo positions supplied via RS232 by theonboard computer. Servo Board is also used for sampling of theservo positions when flying manual. During manual flight thePWM signals from Radio Receiver passes through Servo Boardto each servo.

18


27/233

3.2. HARDWARE IMPLEMENTATION

Tachometer is a GGS-23 from Stator Gator which is a governor sensorthat sends a puls to Governor and Servo Board each time thesparkplugs gives a spark. This is necessary in order to calculatethe main-rotor RPM.

As the on-board electronics has now been described, the followingsection will elaborate on the modifications made to the helicopter inorder to fit the on-board electronics.

3.2 Hardware Implementation

In order to implement the necessary electronics described in the previ-ous section some modifications had to be made to the stock helicopter.In this section the modifications will be described and supported bypictures.

A logical place for mounting the needed flight electronics is asclose as possible to center of mass in order to affect the flying capa-bilities as little as possible. Therefore the flight electronics is oftenplaced right under the rotor shaft between the landing gear. How-

ever, this was not possible on this UAV since this space was reservedfor equipment in relation to another project where the UAV also hasto be used.

It was decided to mount the flight electronics in front of the rotorshaft instead. To make this possible some modification and rearrange-ment was necessary.

As is shown in Figure 3.3 the gas tank and accumulator have beenmoved to the tail section in order to make space for the aluminumbox and the laptop placed in the nose of the helicopter. The equip-ment placed in the tale also counter weights the extra mass added inthe the nose to maintain the center of mass close to the rotor shaft,

as intended on the stock Bergen helicopter. Also a cut-away in thehelicopter carbon-fiber frame was made to make the necessary space.

The aluminum box is mounted on two aluminum brackets whichis mounted direct to the motor block. The aluminum box and thebrackets have no direct contact but are connected through a set of rub-ber mounts to reduce vibrations on the electronics inside the box. Therubber mounts are shown in Figure 3.4 together with the mounting ofthe camera.

In Figure 3.5 the on-board computer (OBC) is shown in the left sideof the picture and the flight electronics inside the aluminum box in the

19


28/233


Figure 3.3: Modifications and rearrangement ofstock helicopter. Stock helicopter in top of pic-ture and modified helicopter in bottom of thepicture.

Figure 3.4: Close up of aluminum brackets andrubber mountings for the aluminum box.

20


29/233

3.2. HARDWARE IMPLEMENTATION

right side. The OBC is held in place by two velcro straps mounted onthe sides of the aluminum box.

Figure 3.5: Top view of OBC and flight electronicsinside the aluminum box

As shown in Figure 3.6 the compass and GPS antenna are bothmounted on the tail and not together with the other electronics insidethe aluminum box. This is done to avoid the main rotor to shield theGPS antenna causing limited satellite tracking ability, since the mainrotor is made of carbon fiber which is a conducting material. The com-

pass has to be placed as far away from other electronics as possible toavoid noise and bias on the sensor data caused by electromagnetic ra-diation.

Figure 3.6: Mounting of GPS antenna and com-pass on the tail

This concludes the description of the implementation of hardwareand modifications made to the helicopter. The UAV is consideredcomplete as a development-UAV hardware platform at this point. Dueto involvement of the UAV in other projects, ongoing changes in the

21


30/233


configuration of the helicopter will be made to improve the currentimplementation and in order to adapt the platform to other specificapplications. Further changes to the set-up will not be covered inthis thesis. During test flights the described set-up has proven toperform satisfying and has gained flight approval from RC-Unionen,Denmark.

22


31/233

Software Platform 4

In this chapter the software environment, under which the execution of thesoftware subsystems takes place, will be chosen. Furthermore the purpose of

the software subsystem and their real-time demands for them will be stated.Afterwards a description of the communication design between the softwaresubsystems is elaborated and lastly the dataflow between the subsystems willbe presented.

4.1 Software Environment

As stated in the problem formulation on page 11 a real-time operatingsystem is needed, under which real-time tasks can be executed. In thissection the choice of the real-time operating system is made, and theinstallation is described.

A Real-Time Application Interface, better know as RTAI, which isan extension for the common Linux Kernel, has been chosen as thereal-time operating system. It is based on a regular Linux distribu-tion with a modified kernel. The modified kernel enables real-timetask execution, and is a regular Linux kernel with a RTAI patch ap-plied enabling this execution. The advantage of using RTAI is that ahard real-time software environment is provided on a normal Linuxdevelopment platform, which gives the flexibility of Linux during de-velopment.

A Debian Linux 3.1 distribution has been chosen, and patched

with RTAI version 3.2. In order to patch the kernel with RTAI a newkernel must be compiled, which is a complicated process and thus willnot be mentioned here, but can be found in Appendix A.

4.2 Software Architecture

In the previous section, it was chosen to implement the software un-der RTAI Linux. In this section, the software subsystems will be pre-sented, and will be categorized as being either real-time or non real-

23


32/233

CHAPTER 4. SOFTWARE PLATFORM

time tasks. Afterwards the communication design and a descriptionof the dataflow between the software subsystems will be elaborated.

4.2.1 Software Subsytems and Real-Time Demands

It has been chosen to divide the system into two layers. A softwarelayer and a hardware layer. An overview of this division showingthe software subsystems is given in Figure 4.1, where the hardware

Mission Control

NavigatorMap Information

ControllerSensor Server

Flight Data

Logger

Groundstation

serverVision

Sensors Servoboard Servos Camera

Software

Hardware

Figure 4.1: Overview of the subsystems, illustrat-ing the high/low-level division.

layer corresponds to the description in chapter 3. In the following, thetasks of the software subsystems are briefly described and it is selectedwhether the tasks are defined to be real-time tasks or non real-timetasks.

Sensor Server has the task of collecting data from the sensors, usedby some of the other subsystems. This subsystem is developed

solely by Morten Bisgaard and the development will not be dealtwith in this thesis, but still it is taken into account when design-ing the system architecture, as it has to be placed in the overallarchitecture. It is not necessary to implement the sensor serverin real-time even though it acquires the sensor data, since thesensor data is not requested by the OBC, but is send with a peri-odic frequency from the respective sensor to the OBC. Since thesensors does not operate with the same frequencies the result isa asynchronously sensor data flow into the OBC. For a non real-time task the ability to read from the sensors using a software

24


33/233

4.2. SOFTWARE ARCHITECTURE

interrupt is available, therefore it has been chosen to define thesensor server as a non real-time subsystem.

Controller provides a stabilizing control of the helicopter. It is a hovercontroller which also can be used for forward flight up to 3 m/s.Furthermore, the Controller provides an interface for the navi-gation carried out by this group and it feeds the flight data tothe flight data logger. The environment in which the Controlleris implemented, together with the interfaces, are developed by

this group, but the development and implementation of the con-trol algorithm, the sensor fusion, and the state observer are car-ried out by group 1034A. The controller needs to be able to ex-ecute the control algorithm with a fixed frequency in order toguarantee stability, which is why the controller is defined to be areal-time subsystem. Since only the controller environment willbe developed by this group it has been chosen not to deal withthe design in this thesis. Still it has been implemented during thedevelopment of the other subsystems as it is needed in the over-all architecture. The implementation of the controller is shownin Appendix C.3.

Map Information is a subsystem with the task of providing informa-tion about level 1 and level 2. It contains the waypoints used forlevel 1 flight, and for level 2 it contains the waypoints where thehelicopter must stop to search for the marker and for a houseentrance. Furthermore it contains the coordinates of the build-ings in the city. Since the only task of the map information is toprovide the map data for the other subsystems it is defined to bea non real-time subsystem.

Navigator consists of tasks divided into two levels. In level 1 the nav-igator must navigate the helicopter through the waypoints de-

fined by the level 1 waypoints in the map information. In level 2the navigator must navigate the helicopter to a predefined set ofwaypoints defined by the level 2 waypoints in the map informa-tion to search for the marker. When the marker has been found,the house is identified and the navigator investigates the housefurther in search for the best suited entrance. The navigator usesthe controller to move the helicopter by sending periodic refer-ences. To ensure that the execution frequency is constant for thenavigator it has been chosen to define the navigator as a real-time subsystem.

25


34/233


Flight Data Logger has the task of logging the flight data during flight.This is necessary in order to be able to analyze the flight dataoff-line. In order to log the flight data it must be stored to a filewhich preferable was chosen to be readable by M ATLAB, andwith a structure that corresponds to the sensor data was chosen.As it is not possible to write data to files from real-time tasks,since a data write to a file is a non real-time operation, it is re-quired that the flight data logger is a non real-time task.

Ground-station Server has the tasks of transmitting the flight data toa ground-station PC, which displays the flight data during flightusing a ground-station client. The flight data received on theground-station PC is also logged in order to have the data in caseof a crash resulting in the OBC is destroyed. The ground-stationserver does not require real-time execution since the display ofdata in real-time on the ground-station client is not required, andthus the ground-station server is defined to be a non real-timesubsystem.

Vision has the task is to search the pictures acquired by the camerato determine the likelihood of the marker being present within

the picture. Furthermore it is also able to detect if a house en-trance is present within the picture. When the marker or houseentrance are detected, the vision subsystem notifies the missioncontrol which decides the action to be taken. The vision sub-system is developed by this group but the detection algorithmsare developed by Anders la Cour-Harbo. The driver providedalong with the camera used for acquiring images is a precom-piled driver which limits the usage of the driver to non real-timeonly, and hence the vision subsystem is defined to be a non real-time subsystem.

Mission Control manages the execution of the mission parts and isresponsible for accomplishing the central helicopter missions.It is expandable, if other systems, such as a collision-avoidancesystem, is needed. If such systems are implemented the missioncontrol needs to be implemented in real-time, and hence the mis-sion control is defined as being a real-time subsystem.

In summary, this leads to having defined Controller, Navigatorand Mission Control as the three real-time tasks that are present withinthe system, while the other subsystems are defined as non real-timetasks.

26


35/233

4.2. SOFTWARE ARCHITECTURE

4.2.2 Software Structure and Communication Design

After defining which tasks require real-time execution and which doesnot, each of the subsystems can be placed in an overall structure. Thestructural diagram has been modified during the development pro-cess of the different subsystems and resulted in the diagram shownin Figure 4.2, which also shows the communication between the dif-ferent tasks and the interaction with the hardware layer. One of the

Sensors

SensorServer

WLAN

Flight

DataLogger

Camera

SharedMemory

Hardware HardwareNon Real-Time Real-Time

Actuators

MapInformation

FIFO

GoundStation

Server

Actuatorpositions

Vision Mission Control

Navigator

Controller

M.Box

M.Box

M.Box

Figure 4.2: Software structural and communica-tion diagram.

most central blocks within the structural diagram shown in Figure 4.2is the shared-memory block. The shared-memory block enables thesoftware subsystems to acquire or store data to be exchanged with theother subsystems. This memory space is unbuffered which means thatonly the latest value stored are available. The structure of the memoryspace has been chosen as shown in Table 4.1. In Table 4.1, the variablesLocked, Sensorsvalid and Samplenumber are variables while the elementsGyro, Compass, GPS, Servo and Tacho are data structures containinga set of variables corresponding to those the sensors read. The datastructure Navigator contains information used to follow the status of

27


36/233


Element Description

locked Semaphore used for signaling a write to theshared memory

SensorsValid Flag that signals that the sensor data is validsamplenumber Samplenumber for the current dataGyro Gyroscope dataCompass Compass dataGPS GPS dataServo Servo positionsTacho Engine tachometerSystem System information data (WLAN strength,

battery level)Estimator Estimated state valuesController Controller output dataNavigator Information about the state of the navigatorMap Information Struct containing map information data

Table 4.1: Shared-memory structure.

the navigator when sent back to the ground station, and is also usedfor writing information for debugging during development. The datastruct Map Information contains all the information known about themap of the city and is used for level 2 flight. The elements of thementioned data structures are shown in more detail in Appendix C.7.The appendix contains the implementation of a platform definitionsheader file which, among others, contains the definitions of the datastructures.

For enabling interprocess communication (IPC) between the soft-ware subsystems in Figure 4.2 a commonly known IPC type has beenused called mailboxes. The mailboxes are indicated by M.Box in Fig-

ure 4.2, and are basically a shared memory area as the one previouslydescribed. In the box M.Box in Figure 4.2 a mailbox exists for eachdirection the communication travels, but they are shown as one boxfor the sake of simplicity. Through the mailboxes the tasks are ableto communicate by synchronizing through a variable and exchang-ing one or more variables located within the mailbox. In Table 4.2an example is shown of the structure the mailboxes use. Besides themailboxes, a FIFO buffer has been used between the controller andflight data logger as a buffer for the data passed continuously by thecontroller to the flight data logger, since the flight data logger is a non

28


37/233


38/233


server reads the data available within the shared memory if locked isnot set, and transmits this data back to the ground-station client run-ning on the ground-station PC.

Besides the continuous loop just explained, the navigator, missioncontrol, vision and map information runs in parallel, but these subsys-tems are decoupled from the sensor data passing process. There areno data flow between these subsystems, only communication betweenthe subsystems is passed. However, the navigator uses the GPS posi-tions and map information data located in the shared memory, andthus it must be connected to this. The purpose of the map informationsubsystem is to load data, stored within a file, into the shared memorywhen the Map Information is initialized.

After having defined the software platform, the real-time demandsfor the software subsystems, and the communication between them,can be developed separately.

30


39/233


40/233


41/233

Navigator 5

To make the helicopter fly through a set of coordinates, a navigator is needed.In this chapter, the analysis and design of the navigator is described. After

the design process, the navigator is implemented in ANSI C in such way thatthe same C file is used in simulation in MATLAB and on the final implemen-tation on the helicopter. This way, if a change is made in simulation it is alsoincluded in the navigator implemented on the helicopter. The simulation andtest of the navigator is also documented in this chapter. Lastly, the navigatorwill be tuned to minimize the flight time.

5.1 Analysis of Navigator

As stated in Chapter 1, the purpose of this project is to carry out level

1 and 2 of the IARC06. In level 1, the purpose is to fly through all thewaypoints as fast as possible. In level 2 the helicopter is supposed tostop and hover in each waypoint, facing a predetermined direction, totake pictures of a specific wall to find the marker. The house on whichthe marker is found is then examined in the same way for an entrance.To accomplish this, it is necessary to be able to navigate the helicopterbetween waypoints. Furthermore, for level 2, it is necessary to be ableto set a specific heading of the helicopter, to control what the on-boardcamera is taking pictures of. Hence, a navigator is designed to calcu-late velocity and yaw references and send them to the controller tomake the helicopter behave as desired.

Firstly some variables, which are used in the following sections,are defined. Figure 5.1 illustrates some of the variables, associatedwith the helicopter navigation, which is described as

WPN The waypoint the helicopter is flying towards, WPN+1 is thewaypoint the helicopter will fly towards when having reachedWPN.

VC The vector describing the desired course, that is the vector be-tween W PN1 and W PN

33


42/233

CHAPTER 5. NAVIGATOR

WP WP

Helicopter

N-1 N

V

V

p

c

VnVo

Figure 5.1: Illustration of the helicopter betweentwo waypoints and the variables associatedwith this.

VN The vector from the helicopter to W PN

VP The vector from W PN1 to the helicopter

VO The velocity reference vector, that is the output from the naviga-tor to the controller

Vcross The cross-product vector ofVC and VN

5.1.1 Path Computation

The input to the navigator is a set of waypoints which consists of a po-sition given in absolute coordinates. Hence, an earth fixed referenceframe (EF) is defined as having the y-axis pointing in northern direc-tion and the x-axis pointing in eastern direction. In the EF, angles aredefined from the y-axis and is positive counter clockwise as illustratedin figure 5.2.

N

E

Figure 5.2: Definition of the EF and angles in thisframe.

34


43/233

5.1. ANALYSIS OF NAVIGATOR

The navigator needs to be generic in the way that it has to workwith an undefined number of waypoints and with any given con-troller. Hence, the navigator must be prepared to handle differentvelocities, different accuracy etc.. This requires a variable hysteresisaround each waypoint to support the varying accuracies. Therefore, ahysteresis circle, called waypoint circle, is placed around every way-point, and when the helicopter enters this waypoint circle it is consid-ered as having reached the waypoint. The waypoint circle might nothave the same radius in level 1 and 2 as the two levels sets differentdemands.

Before navigation is possible a path must be computed. This pathmust combine all the waypoints, such that the helicopter will entereach waypoint circle. This could for instance be done by deriving amathematical function which runs through all of the waypoints. How-ever as this approach could become very complex as the order of thefunction is most likely to increase with the number of waypoints, amore simple approach is considered. Another approach could be tosimply combine the waypoints with vectors between them. This ap-proach has the advantage of being quite simple to implement, butlacks the ability to avoid a known obstacle without placing an extra

waypoint. This is not a problem in this project as the helicopter canbe set to fly at higher altitude than the altitude of the tallest obstacle.This approach is chosen as it is easy to implement and is adequate forcompleting level 1 and 2.

The chosen approach for deriving a path between the waypointscan be combined with different ways of flying through the waypoints,and some of these ways are illustrated in Figures 5.3(a)-5.3(c).

(a) Example 1 (b) Example 2 (c) Example 3

Figure 5.3: Examples of path connections insidethe waypoint circle.

Figure 5.3(a) shows the least complicated way of connecting thewaypoints. Here the helicopter flies towards WPN and comes to ahalt. When the helicopter has stopped, it yaws until the nose of the he-

35


44/233


licopter points in the direction ofWPN+1, then it begin to fly towardsthis waypoint. This reduces the path-following complexity to simpleforward flight. This solution is, obviously, not the fastest way to fly aroute, but it can be extended to be more efficient. Figure 5.3(b) showsa scenario where the helicopter flies towards WPN and when it entersthe waypoint circle it immediately receives a new reference pointingtowards WPN+1 and flies towards this waypoint without stopping.This will cause the flight path of the helicopter to become smooth in-side the waypoint circle. It could also be a possibility not to steer di-rectly towards the waypoint, but towards a point on the associatedwaypoint circle and then follow a specific curve through the circle,such that the helicopter would overfly the waypoint more accurately.This is illustrated in Figure 5.3(c). As the helicopter is considered asbeing inside the waypoint when entering the waypoint circle, the way-point connection method illustrated in figure 5.3(b) is chosen.

As the helicopter will get a new reference when entering the way-point circle, it will most likely have to make a turn. To perform thisturn it might be necessary to lower the helicopter speed. This means,that the helicopter velocity reference must be decreased before thehelicopter enters the waypoint circle. As the navigator has informa-

tion of the path, whereas the controller has not, the navigator shouldconsider this problem. Hence, a circle with a larger radius than thewaypoint circle is placed around all the waypoints to define when thehelicopter speed must be decreased, this circle is called the velocity-reference circle.

This means, that when the helicopter approaches a waypoint, itwill fly as illustrated in Figure 5.4. The figure illustrates that the speedof the helicopter is decreased when it enters the velocity reference cir-cle. When the helicopter enters the waypoint circle, it is considered asbeing in the waypoint, and a new reference pointing towards the nextwaypoint is used. The helicopter will then turn while flying with the

slow speed, and when it leaves the velocity reference circle the speedwill be increased again to make the helicopter reach the next waypointas fast as possible.

5.1.2 Deviations

Another consideration which needs to be made regarding the follow-ing of a path is how to handle a potential deviation from the course.This deviation could be caused by many things, wind, model error,controller error etc..

36


45/233


Decrease speed

New reference

Increase speed

Velocity Reference Circle

Waypoint Circle

Figure 5.4: Illustration of how the helicopter actswhen approaching a waypoint.

If it does not matter whether the helicopter deviates or not, thenavigator could simply use the vector from the current position of thehelicopter to WPN, which is the vectorVn, as reference for the velocityand heading. If this method is used, the path between the waypointsis completely neglected and the only thing the navigator considers isthe waypoints. This could potentially cause the helicopter to deviatefrom Vc, but the helicopter will eventually reach the waypoint. In

some applications such a deviation does not matter, but in the case ofcontrolling a helicopter, it is very important to minimize deviationsfrom the planned path. If the helicopter deviates to much from Vc inthe vertical or horizontal directions, it might collide with the groundor any obstacles present and crash. Hence, it is necessary to steer thehelicopter not just towards WPN, but also back on the initial courseVc, if the helicopter deviates from it. This gives the ability to followa course between two waypoints instead of just flying randomly be-tween them.

5.1.3 Navigation Method

To accomplish the ability of following a course, a navigation methodmust be chosen. The common methods used for navigation are basedon control of marine vehicles, as autopilots were first developed torelieve the pilots of smaller boats on routine cruising [Fossen 2002,p. 5]. The desire regarding the deviation, when autonomously navi-gating a marine vehicle, is to make the vehicle approach the originalcourse in the same way as the helmsman of the vehicle would havedone. That is, if the deviation, which is called the cross-track error,ect, gets larger, the navigation has to steer more after the course than

37


46/233


the waypoint. This should ensure that the vehicle will have a smoothpath back towards the course. This is illustrated in Figure 5.5. The

Vehicle

Course

Deviation

Desired approach

Figure 5.5: Illustration of how a deviating vehicleshould approach the course.

navigation method which is commonly used to ensure the approachillustrated in Figure 5.5, is line-of-sight (LOS) navigation [Fossen 2002,p. 168]. Basically the LOS navigation method works by placing a circlewith a suitable radius around the vehicle. The vector from the vehi-cle to the intersection between the course and the circle around thevehicle is then used to calculate the heading and velocity reference ofthe vehicle, as illustrated in Figure 5.6. The LOS navigation method

Vc

Vo

dlos

Figure 5.6: Illustration of how the line-of-sightnavigation method works.

is developed and intended for marine vehicles, but for navigating amodel helicopter it has some disadvantages. Ifect gets larger than theLOS-distance, then the navigation does not produce an output as nointersection of the circle and Vc is present, and if the circle is madelarger the helicopter is navigated slower towards Vc. Furthermore itis not possible to change how the angle between Vc and Vo developsas a function ofect. In LOS navigation, the angle develops as

=

2 arccos

ect

dlos

(5.1)

38


47/233


which is illustrated in Figure 5.7. It would be preferable to be able to

ect

Figure 5.7: Illustration of how the angle betweenVc and Vo evolves as a function of ect whenusing line-of-sight navigation.

adjust how the angle develops as a function ofect, to make the navi-gator as independent of the underlying controller as possible. Whereone controller might be able to use the rate of angle change LOS pro-vides, this rate might make another controller become unstable.

Instead of using a circle to determine the direction ofVo, it couldbe advantageous to calculate Vo by rotating VN. By using VN as basis

for calculating Vo, the actual heading towards WPN is always takeninto account. In this way of navigating, VN is rotated by an angle,helm, which is a function of ect, to produce Vo. By designing thisfunction properly the vehicle will follow the desired approach illus-trated in Figure 5.5. The actual function used in this project will bedesigned in section 5.2. As this is the approach a helmsman woulduse, this navigation method will be called helmsman navigation.

If the vehicle does not deviate from the course, the helmsman navi-gation will simply useVN as heading. If a deviation occurs, the helms-man navigation will rotate VN about the cross product ofVc and VN,Vcross. The reason why VN is rotated around the cross product, is

that the correction needs to be done in all three dimensions, as thehelicopter can deviate in three dimensions. If ect gets larger, helmincreases and VN is rotated further. This is illustrated in Figure 5.8.

To conclude, it is chosen to create a path from the waypoints byplacing vectors between them. It is also chosen to use the connectionshown in figure 5.3(b) where the helicopter flies towards WPN until itenters the waypoint circle, then it gets WPN+1 as reference. To makesure that the helicopter speed is slow enough for it to be able to turnwithin the waypoint circle, it is chosen to place a velocity-referencecircle outside the waypoint circle. Lastly it is chosen that the heli-

39


48/233


Course

Flight direction

Helicopter

ect

Figure 5.8: Illustration of how the algorithms steerthe helicopter towards the desired course, as afunction of the deviation from it ect.

copter must minimize the deviation by use of helmsman navigationand thereby follow the vector between two waypoints instead of justflying randomly between them. The design of a navigator able to per-form these choices is described in the following.

5.2 Design of Navigator

As the helicopter motion is different in level 1 and 2, the navigatormust be able to steer the helicopter in both ways. The difference be-tween the two ways is the action taken when the helicopter enters awaypoint.

In level 1, it is only necessary to define the waypoints as a set of co-ordinates in three dimensions. But as a heading and a house number isneeded in level 2, these two entries are added to the list of waypoints.Hence, they do not only consist of coordinates but a yaw angle and ahouse number are also attached. The yaw angle defines which way thehelicopter must point when taking pictures of the houses, and whenthe marker is found, the house number is returned to the mission con-

40


49/233

5.2. DESIGN OF NAVIGATOR

trol. The list of waypoints is therefore constructed as

x1 y1 z1 yaw1 house_number1x2 y2 z2 yaw2 house_number2...

xn yn zn yawn house_numbern

. (5.2)

In Figure 5.9 an example of a path made from three waypoints is illus-trated. Making this path is one of the tasks for the navigator, and ad-ditionally the navigator must also steer the helicopter along the path,and make sure it does not deviate from it. As illustrated in Figure 5.9,

Course

Waypoint circleVelocity-reference circle

Figure 5.9: Example of a path with threewaypoints.

two circles are placed around each of the waypoints, the waypoint cir-cle and the velocity-reference circle. When the helicopter enters thevelocity-reference circle, the velocity reference will decrease. Then,when it enters the waypoint circle, it is considered as having reachedthe waypoint, and the next waypoint is given as reference. The circlesmight not have the same radius in level 1 and level 2.

To simplify the design of the navigator, it is divided into two parts,a path planner and a path follower. The purpose of the path planner

is to receive a message from the mission control, described in chapter7, with a set of waypoints and activate the path follower with the twowaypoints W PN1 and W PN. From these two waypoints, the pathfollower calculates a velocity-reference vector for the helicopter, anddepending on the deviation of the helicopter, this velocity reference isaltered to make sure the helicopter flies towards the desired course.The communication in the navigator is illustrated in Figure 5.10.

The messages from the mission control consists of a command tothe navigator and a vision result. Before the navigator receives the firstmessage from the mission control, it is set to hover. In Figure 5.11, the

41


50/233


Navigator Path Planner Path Follower

Waypoint list

Loop

Velocity reference

Previous and next waypoint

Velocity and yaw references and house number

Figure 5.10: Sequence diagram of the navigator.

flow chart of the navigator is presented. And as it shows, the firstthing the navigator does, is to check if a new message has been re-ceived from the mission control. If no new message is present, thenavigator uses the previous command. This command could be eitherINIT 1, INIT 2, LEVEL 1 or LEVEL 2. An init command is sent to ready

the navigator for flying level 1 or level 2. The level commands is thensend to execute a flight of level 1 or level 2.

If a new message is present, the navigator reads the contents ofthe message and stores the command and vision result, which are in-cluded in the message. If the command stored is an init command,the navigator checks if the initialization has already been done. Thisis necessary as the navigator is a loop and the command might not beupdated between two loops. If the initialization has already been per-formed once, the navigator stops and waits until the scheduler startsit again. If the initialization has not been performed, then the naviga-tor adds the current position of the helicopter as the first coordinate in

the waypoint list. This is to make the navigator generic in the sensethat it can be started in a random position and seek the first waypointfrom this location. When this is done, the navigator informs the mis-sion control that the initialization has been performed, and that thenavigator is ready to fly the level.

If the command stored is a level command instead of an init com-mand, the navigator calls the path planner, which will be describedlater. The path planner returns several values, velocity and yaw refer-ences, a house number and a path planner state. The returned refer-ences are sent to the controller, and then the navigator checks the path

42


51/233


Read message

from MissionControl

New message

Add current

position in level xwaypoint list

Command

Initializationexecuted once

Initialize level x

Send init x ok to

Mission Control

Call Path Planner

Send calculatedreferences to

Controller

Path Plannerstate

Delay

Ongoing

No

Yes

Level x

Init x

No

Yes

InsidewaypointReset sent-

variable

sent-variableset

Set sent-variableNo

Send state ofNavigator to

Mission Control

Reset sent-variable

Path Planner

state

Level_1_doneLevel_2_done_not_found

Level_2_done_found

Send state ofNavigator to

Mission Control

Level_1_doneLevel_2_done_not_found

Send state ofNavigator and

House number toMission Control

Level_2_done_found

(Command, Vision Result)

Yes

Figure 5.11: Flow chart of the entire navigator.

43


52/233


planner state. It is based on this state that the navigator determines ifand what to tell the mission control. The different path planner statesare shown in table 5.1

Path Planner State Description

ONGOING Helicopter between waypointsLEVEL_1_DONE Level 1 is flownLEVEL_2_INSIDE_WAY-POINT Helicopter inside level 2 waypointLEVEL_2_DONE_FOUND Level 2 done - return house number

LEVEL_2_DONE_NOT_FOUND Level 2 flown

Table 5.1: Description of the path planner states.

When the helicopter enters a waypoint circle in level 2, the mis-sion control must be informed of it. Hence, if the path planner stateis LE VEL_2_INSIDE _WAYPOINT, then the navigator sends a mes-sage informing the mission control that the helicopter has entered awaypoint unless it has already been sent.

If the path planner state is either LEVEL _1_DONE or LEVEL_2-_DONE_NOT_FOUND, the navigator sends a message to the mis-

sion control that the level is flown and no more waypoints are present.The same is done if the path planner state is LEVEL_2_DONE_FOUND,then a house number is attached to the message telling the missioncontrol on which house the marker has been found.

In the following sections, the path planner and path follower willbe designed. The design of the path planner is divided into two parts,one for level 1 and one for level 2. This is not the case for the pathfollower as it can be designed generic such that it can be used in bothlevel 1 and 2.

5.2.1 Path Planner

Depending on which level the helicopter is attempting to fly, thereis a difference in the procedure of the path. Hence, in the followingsections, two instances of the path planner are described, one for eachlevel.

Path Planner During Level 1 Flight

In Figure 5.12 the flow chart describing the procedure of the path plan-ner during level 1 flight is presented. Firstly, the path planner state isset to ONGOING. This state is only changed if the last waypoint in

44


53/233


Start path-follower with

waypoints

V_out from path-

follower

Helicopterinside waypoint

circle

Yes

No

Last waypoint?Yes

No

Next waypoint

Hover

Calculate yaw angle fromV_out

Path planner state =

PP_ONGOING


PP_LEVEL_1_DONE

Figure 5.12: Flow chart of the path planner duringlevel 1 flight.

level 1 is reached. Then the path-follower algorithm is called with twowaypoints as inputs, WPN1 and WPN. The path follower returns avelocity-reference vector Vo to the path planner which uses it to cal-culate a yaw-angle reference. The yaw angle reference is defined asbeing 0 rad when pointing in northern direction and rising counter-clockwise.

The length of the vector, consisting of the two first elements ofVo,

that is the velocity reference in the lateral and longitudinal direction, iscalculated. This vector is illustrated in Figure 5.13. The yaw referencecan now be calculated by use of Pythagoras as

yaw = arccos

x

h

2(5.3)

There is a potential problem with this way of calculating the yaw ref-erence. It does not matter ify is positive or negative, (5.3) will yieldthe same result in both cases as it does not depend on y. Hence, if

45


54/233


V (2)

V (1)o

o

x

yh

yaw

Figure 5.13: Illustration used in calculation of theyaw reference.

y is negative, the yaw reference is calculated as

yaw = arccos

x

h

2(5.4)

The yaw references calculated in (5.3) and (5.4) is an absolute valuebetween 3

2rad and

2rad. This value needs to be an unbounded

value between rad and rad as the controller does not supportthe fact that 3

2rad and

2rad are the same angle. If for instance the

previous yaw reference were 350 and the next is 5, this would makethe controller try to force the large number down to the smaller one,hence rotating clockwise 345 instead of rotating counter-clockwise15. That is, if the yaw reference change becomes larger than rad,the controller will rotate the helicopter the wrong way. Hence, a func-tion is made to make the yaw reference unbounded.

The path planner checks the distance from the helicopter to thenext waypoint to examine whether or not the helicopter has reacheda waypoint circle. If it has not reached the waypoint circle, the algo-rithm will call the path follower with the same two waypoints again.If it has reached the waypoint circle, the helicopter is considered as

being in the waypoint and the path follower is called with the next setof waypoints. This loop continues until the last waypoint is reached.When it has been reached, the path planner will return a message tothe mission control telling it that the waypoints has been flown.

Path Planner During Level 2 Flight

The first part of the path planner is identical in level 1 and 2. This isthe case untilVo is returned by the path follower, whereafter there aresome differences. In Figure 5.14 a flow diagram of the path planner

46


55/233


Path planner state =PP_ONGOING

Start path-follower withwaypoints

V_outfrom path-

follower

Helicopterinside waypoint

circle?

Yes

No

Is messagefrom missioncontrol valid?

Hover=1

No

Yes

Fetch yaw from waypointlist

Path planner state =PP_LEVEL_2_INSIDE_WP

Distance between

waypoints

Long

ShortYes

NoCalculate yaw from V_out

Set variableheli_inside_wp=1

Yes

NoLast waypoint?

Check value of

message

Found

Not found


PP_LEVEL_2_DONE_FOUND

Path planner state =PP_LEVEL_2_DONE_NOT_FOUND

Set variableheli_inside_wp=0

Fetch yaw from waypointlist

Variableheli_inside_wp

= 1?

Hover=0

Hover=0

Path planner state =PP_ONGOING

Next waypoint

Figure 5.14: Flow chart of the path planner duringlevel 2 flight.

47


56/233


during level 2 flight is given. In level 1, the yaw reference is alwayspointing in the flight direction, during level 2 flight the decision re-garding the yaw angle is somewhat more complex.

It will probably be necessary for the helicopter to photograph along wall and hence necessary for it to take several picture while point-ing in almost the same direction. In this case it would make little senseto fly after the nose for a short distance and then rotate the helicopterback in the same direction again. Even so, it would still be preferable

to be able to fly after the nose if the helicopter has to fly from oneside of the town to the other. Hence, if the distance between two way-points is larger than 2 times the radius of the velocity-reference circle,the helicopter will fly after the nose. If the distance between WPN1and WPN is shorter, then the yaw-angle value attached to WPN willbe used while flying. This way the helicopter is ready to take picturesthe instance it enters the waypoint circle, and thus it does not have toyaw first.

If the helicopter is outside a waypoint, it can either be because itis flying between two waypoints or it could be because it has driftedoutside the waypoint it was supposed to be located in. It is very im-

portant for the helicopter to be located inside the waypoint while thepictures are being taken, else the algorithm does not know what thehelicopter is taking pictures of. Hence, a variable HELI_IN SIDE_ WPis set whenever the helicopter enters a waypoint and reset when it issupposed to leave the waypoint again. That is why, in the top of Fig-ure 5.14, when asking for the distance between the waypoints, it ischecked if the helicopter is supposed to be inside a waypoint. If it is,then the yaw reference must be set to the yaw value attached to thewaypoint the helicopter should be in, if it is not, then the helicopter isflying between two waypoints which are far apart, and the yaw mustbe calculated by use of (5.3) and (5.4).

Then it is checked whether or not the helicopter is inside a way-point circle, and if not, the hover variable is reset. This is because ofthe drifting problem. In case the helicopter drifts outside the way-point circle, it must fly back inside it. If the helicopter is inside thewaypoint circle, the yaw reference is set to the value attached to thewaypoint, and the hover variable is set. The helicopter must hoveruntil the mission control tells the navigator that the marker is foundor not found. Hence, the path planner examines if a message fromthe mission control is valid. If it is not, the variable HELI_INSIDE_WPis set to indicate that the helicopter is supposed to be inside a way-

48


57/233


point. If a valid message is present, it is examined whether the mes-sage is FOUND or NOT_FOUND. If the message equals FOUND,then the path planner sets the state to LEVEL_2_DONE_FOUND.When the path planner enters this state, the navigator will send thehouse number of the house the helicopter was photographing to themission control. If the message equals NOT_FOUND, it is checkedif the current waypoint is the last. If it is not the last, then the heli-copter will fly towards the next waypoint and hence set the state toONGOING. If it is the last waypoint, The path planner state is setto LEV EL_2_DONE _NOT _FOUND. When this state is entered, thenavigator tells the mission control that all the waypoints have beenflown.

5.2.2 Path Follower

The purpose of the path follower is to receive input from the pathplanner in form of two waypoints, and then calculate a helicopter ve-locity reference. This reference is of course dependent on the coursebetween the two waypoints, but it is also dependent on how muchthe helicopter deviates from this course. In the following, the path

follower will be described based on the flow chart in Figure 5.15.The first task for the path follower is to check if the hover variable

is set. It can be set from the mission control or it can be set by thenavigator either if the last waypoint is reached in level 1 or if anywaypoint is reached in level 2. If this variable is set, the algorithmsets the velocity reference, Vo, to zeros and then return it to the pathplanner.

If the hover variable is not set, the path follower calculates thecross-track error ect. ect is defined as being the perpendicular dis-tance from the helicopter to the course vector Vc, as illustrated in Fig-ure 5.16. As both Vc and Vp are known, ect can be calculated by use

of the cross product of the two vectors as

ect =|Vc Vp|

|Vc|(5.5)

Next, the vector from the current position of the helicopter to the nextwaypoint, Vn, is calculated. To be able to control the speed of thehelicopter, Vn, is transformed into being a unit vector Vnunit .

If the helicopter deviates from Vc, Vn must be rotated, to deter-mine vo, in such way that the helicopter will fly towards the desired

49


58/233


Waypoints from

path planner

Calculate cross-track error(e_ct)

Calculate V_n

Calculate V_cross(V_nx V_c)

Using e_ct, rotate V_naround V_cross tocalculate V_out_unit

Helicopter insidevelocity-reference

circle

YesMultiply V_out_unit with

turn-velocity factor tocalculate V_out

No

Multiply V_out_unit withcruise-velocity factor to

calculate V_out

ReturnV_out to pathplanner

Is hover=1

Yes

No

V_out=[0 0 0]

Figure 5.15: Flow chart of the path follower.

50


59/233


ect

WP WP

Helicopter

L N

V

V

p

c

Figure 5.16: Illustration to be used in calculatingect.

course. This is done using helmsman navigation. As described in sec-tion 5.1, a function describing the angle helm must be chosen, andit would be preferable if it is possible to alter the chosen function inboth amplitude and evolution. The function must increase helm thelarger the deviation is, but it needs to have a definable limitation. Thisis to secure that the angle which Vn is rotated, is not to large. As anexample it makes no difference ifVn is rotated with 2 rad. A func-tion which has these features are the hyperbolic tangent function. Byinserting two parameters, X and Y, it is possible to control both howlarge the maximum amplitude can get as well as how fast the function

evolves to this maximum. Hence, helm is calculated as

helm = X tanh(Y ect) . (5.6)

A plot of the function in (5.6) is illustrated in Figure 5.17. To be sure

ect

F( )ecthelm

Figure 5.17: Illustration of the function tanh(ect).

the rotation makes the helicopter fly towards the course in all threedimensions, the rotation vector should be perpendicular to both Vnand Vc. Hence, the cross product of these is calculated, this crossproduct is called Vcross.

To rotate Vn about Vcross, Eulers eigenaxis rotation is used. Thisfunction can rotate a vector or reference frame around another vector.

51


60/233


The eigenaxis rotation matrix, C, is described by [Bak 2002, p. 19] as

C(Vcross, helm) = cos(helm)I + (1 cos(helm))VcrossVTcross

sin(helm)E, (5.7)

whereE is described as

E =

0 Vcross(3) Vcross(2)

Vcross(3) 0 Vcross(1)

Vcross(2) Vcross(1) 0

. (5.8)

Vn is now rotated helm radians around Vcross, and the result is theunit vector Voutunit . The rotation is performed by

Voutunit = C(Vcross, helm) Vn. (5.9)

When this is done, it is checked whether or not the helicopter is in-side a velocity-reference circle. This information is used to determinethe size of the velocity reference Vo. If it is inside a velocity-referencecircle, Voutunit is multiplied with a factor describing the speed duringturns, and if it is outside the velocity-reference circles, Voutunit is mul-

tiplied with a factor describing the speed during cruise flight.The velocity-reference vector,Vo, is now returned to the path plan-ner and the path follower terminates.

5.3 Simulation of Navigator

To verify that the navigator works, it has been simulated using M AT-LAB and SIMULINK. These simulations are made to show that the nav-igator qualitatively works as intended. Simulations of level 1 and 2flights will be presented to show that the navigator is able to fly theselevels. Also, a simulation showing whether or not the yaw angle refer-

ence is calculated properly and a simulation showing if the helicopterspeed is changed when entering and leaving a velocity-reference cir-cle, are presented.

All of the simulations are performed on a full helicopter modelcontrolled by an optimal controller which is implemented in SIMULINK ,this is elaborated in [Hald et al. 2006]. To minimize the possibility forerrors, the navigator has been implemented in ANSI C in such a waythat the same C file is used in simulation and on the final implemen-tation on the helicopter. This way, if a change is made in simulation itis also included in the navigator implemented on the helicopter.

52


61/233

5.3. SIMULATION OF NAVIGATOR

5.3.1 Simulation of Level 1

In level 1, the navigator is supposed to fly a route defined by a setof waypoints. When the helicopter reaches a waypoint circle it issupposed to fly towards the next waypoint. To simulate this, thehelicopter is set to fly the set of waypoints listed in Table 5.2. A 2-

Waypoint number Coordinate [m]

1 (0, 0, 0, N/A, N/A)

2 (15, 8, 0, N/A, N/A)3 (30, 0, 0, N/A, N/A)

Table 5.2: Waypoints for the level 1 exampleflight.

dimensional plot of the simulation of this flight is shown in Figure 5.18.As Figure 5.18 shows, the helicopter flies from waypoint number 1-3

5 0 5 10 15 20 25 30 355

0

5

10

Eastern Distance [m]

NorthernDistance[m]

Level 1 example flight simulation

Figure 5.18: 2-dimensional plot of a level 1 exam-ple flight simulation.

without noticeable deviations between the waypoints. The only de-viation present, is the deviation at the moment the helicopter entersthe waypoint circle of waypoint 2. This deviation is expected as thehelicopter flies towards waypoint 3 the instance it enters the waypointcircle of waypoint 2.

The distance flown in this simulation is much shorter than the ac-tual level 1 distance, Hence, a new simulation is performed of a morerealistic level 1 flight. In this simulation, the waypoints flown arelisted in Table 5.3 The route made from these waypoints are 3 kmlong, and are situated realistically with reference to how the way-

53


62/233



1 (0, 0, 0, N/A, N/A)2 (10, 0, 0, N/A, N/A)3 (410, 920, 0, N/A, N/A)4 (1410, 920, 0, N/A, N/A)5 (1018, 0, 0, N/A, N/A)

Table 5.3: Waypoints for the realistic level 1 flight.

points would be situated in the competition. A 2-dimensional plotof the simulation of this flight is shown in Figure 5.19. This simulation

0 200 400 600 800 1000 1200 1400

0

100

200

300

400

500

600

700

800

900

1000


NorthernDistance[m]

Level 1 realistic flight simulation

Figure 5.19: 2-dimensional plot of a level 1 realistic

flight simulation.

showed that the navigator is able to navigate the helicopter along a3 km long route in the desired way.

5.3.2 Simulation of Level 2

In level 2, the navigator is supposed to make the helicopter fly to awaypoint and rotate the helicopter to a heading defined by the way-point list. To simulate this, the helicopter is set to fly the waypoints

54


63/233

5.3. SIMULATION OF NAVIGATOR

presented in Table 5.4. When the helicopter enters waypoint 2, a mes-


1 (0, 0, 0, 0, 0)2 (15, 8, 0,

2, 1)

3 (30, 0, 0, 0, 2)

Table 5.4: Waypoints for the level 2 exampleflight.

sage from the mission control will be send to the navigator telling itthat the the marker is not found. This should make the helicopterfly towards waypoint 3, and when it enters this waypoint, the navi-gator will receive a message telling it that the marker is found. Thisshould make the navigator return the house number attached. A 2-dimensional plot of the simulation of this flight is shown in Figure 5.20.The plot in Figure 5.20 shows that the helicopter flies through the way-

5 0 5 10 15 20 25 30 355

0

5

10


NorthernDistance

[m]

Level 2 example flight simulation

Figure 5.20: 2-dimensional plot of a level 2 exam-ple flight simulation. The arrows are the head-ing of the helicopter when inside the way-points

points 1-3. The interesting things during the level 2 flight is how theyaw angle acts. Hence, a plot of the evolution of the yaw angle isshown in Figure 5.21 The first part of the plot in Figure 5.21 has avalue of approximately 1 rad. This fits the heading of the helicopterduring the flight between waypoint 1 and 2. When the helicopter en-ters the waypoint circle of waypoint 2, the yaw reference is set to thevalue listed in the waypoint list which is

2rad. When the message

telling the helicopter that the marker was not found is received, thehelicopter flies towards waypoint 3 and the yaw reference rises to ap-

55


64/233


0 40 80 120 160 200 240 280 320 3602

1

0

1

2

3

4

5

6

7

Time [s]

Angle[rad]

Yaw angle during level 2 example flight simulation

Figure 5.21: Plot of the evolution of the yaw angleduring the level 2 example flight simulation.

proximately 4 rad, this is the angle of the flight between waypoint 2and 3. When the helicopter enters the waypoint circle of waypoint 3,it changes the heading to the yaw reference attached to this waypoint,which is 0 rad or 2 rad as the plot shows. This yaw reference is helduntil the navigator receives message that the marker is found, thenthe navigator returns the house number 2, which was attached to thewaypoint.

5.3.3 Simulation of Unbounded Yaw

This simulation serves the purpose of showing that the yaw reference

given to the controller is not bound by some period of2 rad. To simu-late this the helicopter is set to fly a level 1 route which is constructedin such way that the yaw angle always will rise. The helicopter isset to fly the waypoints listed in Table 5.5. A 2-dimensional plot ofthe flight using the waypoints listed in Table 5.5 is illustra

Documents

06gr1037b Student Report