06gr1037b student report
TRANSCRIPT
-
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.
-
7/31/2019 06gr1037b Student Report
2/233
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
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/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
8/233
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
12/233
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
14/233
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
22/233
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
26/233
CHAPTER 3. HARDWARE PLATFORM
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
28/233
CHAPTER 3. HARDWARE PLATFORM
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
30/233
CHAPTER 3. HARDWARE PLATFORM
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
34/233
CHAPTER 4. SOFTWARE PLATFORM
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
36/233
CHAPTER 4. SOFTWARE PLATFORM
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
-
7/31/2019 06gr1037b Student Report
37/233
-
7/31/2019 06gr1037b Student Report
38/233
CHAPTER 4. SOFTWARE PLATFORM
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
-
7/31/2019 06gr1037b Student Report
39/233
-
7/31/2019 06gr1037b Student Report
40/233
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
44/233
CHAPTER 5. NAVIGATOR
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
-
7/31/2019 06gr1037b Student Report
45/233
5.1. ANALYSIS OF NAVIGATOR
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
-
7/31/2019 06gr1037b Student Report
46/233
CHAPTER 5. NAVIGATOR
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
-
7/31/2019 06gr1037b Student Report
47/233
5.1. ANALYSIS OF NAVIGATOR
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
-
7/31/2019 06gr1037b Student Report
48/233
CHAPTER 5. NAVIGATOR
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
50/233
CHAPTER 5. NAVIGATOR
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
-
7/31/2019 06gr1037b Student Report
51/233
5.2. DESIGN OF NAVIGATOR
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
-
7/31/2019 06gr1037b Student Report
52/233
CHAPTER 5. NAVIGATOR
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
-
7/31/2019 06gr1037b Student Report
53/233
5.2. DESIGN OF NAVIGATOR
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
Path planner state =
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
-
7/31/2019 06gr1037b Student Report
54/233
CHAPTER 5. NAVIGATOR
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
-
7/31/2019 06gr1037b Student Report
55/233
5.2. DESIGN OF NAVIGATOR
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
Path planner state =
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
-
7/31/2019 06gr1037b Student Report
56/233
CHAPTER 5. NAVIGATOR
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
-
7/31/2019 06gr1037b Student Report
57/233
5.2. DESIGN OF NAVIGATOR
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
-
7/31/2019 06gr1037b Student Report
58/233
CHAPTER 5. NAVIGATOR
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
-
7/31/2019 06gr1037b Student Report
59/233
5.2. DESIGN OF NAVIGATOR
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
-
7/31/2019 06gr1037b Student Report
60/233
CHAPTER 5. NAVIGATOR
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
-
7/31/2019 06gr1037b Student Report
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
-
7/31/2019 06gr1037b Student Report
62/233
CHAPTER 5. NAVIGATOR
Waypoint number Coordinate [m]
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
Eastern Distance [m]
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
-
7/31/2019 06gr1037b Student Report
63/233
5.3. SIMULATION OF NAVIGATOR
presented in Table 5.4. When the helicopter enters waypoint 2, a mes-
Waypoint number Coordinate [m]
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
Eastern Distance [m]
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
-
7/31/2019 06gr1037b Student Report
64/233
CHAPTER 5. NAVIGATOR
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