Modification of a Quadrotor into a Passively Stable Surveillance

Modification of a Quadrotor into a Passively Stable Surveillance Platform
Engineering Recommendation Report
By:
Abhishek Barnwal
Mitchell Cheong
Steven Stuber
Project Sponsor:
Bernhard Zender
Applied Science 479
Engineering Physics
The University of British Columbia
January 17, 2010
Project Number 952
Executive Summary
The quadrotors (4 rotor vertical take-off helicopters) currently found on the shelves of hobby shops
and various toy stores are a marvelous blend of cheap yet effective design. Long the favorites of
aviation enthusiasts, these “toys” have the potential to be greatly modified to serve other needs for
a very small amount of money. This project attempted to modify an off the shelf quadrotor
helicopter toy into a semi automated, surveillance platform that would receive instructions from a
base laptop. The goal was to try and replicate the performance of some of the more expensive
quadrotors that are currently sold as observation platforms. The modifications included the
attachment of a balloon, a more sophisticated guidance and stabilization package as well as a
camera for taking pictures and video recording. A stabilization algorithm was also designed and
implemented to ensure that the entire platform stays stable while hovering. Preliminary
observations suggest that replicating the performance of significantly more expensive quadrotors
for less money is entirely possible.
ii
Table of Contents
Executive Summary..................................................................................................................................... ii
List of Figures .............................................................................................................................................. v
List of Tables .............................................................................................................................................. vi
1.Background & Motivation ........................................................................................................................ 1
2.Discussion ................................................................................................................................................ 5
2.1 Project Objectives ........................................................................................................................ 5
2.2 Mechanical Aspects...................................................................................................................... 5
2.2.1 Balloon Sizing ...................................................................................................................... 5
2.2.2 Balloon and Electronics Installation .................................................................................... 8
2.2.3 Other Mechanical Considerations .................................................................................... 14
2.3 Hardware and Sensors ............................................................................................................... 15
2.3.1 Magnetometer .................................................................................................................. 16
2.3.2 Ultrasonic Sensor .............................................................................................................. 17
2.3.3 Inertial Measurement Unit ............................................................................................... 18
2.3.4 GPS .................................................................................................................................... 18
2.3.5 Camera .............................................................................................................................. 19
2.3.6 Battery Pack ...................................................................................................................... 19
2.3.7 H-Bridges ........................................................................................................................... 20
2.3.8 XBee Wireless Module ...................................................................................................... 20
2.3.9 Sensor Circuitry ................................................................................................................. 21
2.4 Software Aspects........................................................................................................................ 22
2.4.1 GUI and Interface .............................................................................................................. 22
2.4.2 Movement......................................................................................................................... 22
2.4.3 Stabilization....................................................................................................................... 23
2.4.4 Software Functionality and Notes .................................................................................... 25
2.5 Testing and Tuning Apparatus ................................................................................................... 27
3.Results .................................................................................................................................................... 29
4.Conclusions ............................................................................................................................................ 32
5.Final Recommendations ........................................................................................................................ 33
5.1 Ball Joint ..................................................................................................................................... 33
5.2 Flexible Rod ................................................................................................................................ 33
iii
5.3 Optimizing Ultrasonic Sensor Conditioning ............................................................................... 34
5.4 Dynamic Modeling ..................................................................................................................... 34
5.5 Set-up GPS .................................................................................................................................. 34
5.6 Horizontal Thrusters................................................................................................................... 35
5.7 Better Camera ............................................................................................................................ 35
References ................................................................................................................................................ 36
Appendices................................................................................................................................................ 37
Appendix 1: PID Stabilization and Movement Code ........................................................................ 37
Appendix 2: Hover with Altitude and Yaw Compensation code ...................................................... 46
iv
List of Figures
Figure 1: Quadrotor operation [1] ......................................................................................................... 1
Figure 2: Microdrone MD4-200 [2] ........................................................................................................ 2
Figure 3: Draganflyer X4 [12] ................................................................................................................. 2
Figure 4: Silverlit X-UFO [4] .................................................................................................................... 5
Figure 5: Solidworks render of the finished assembly ........................................................................... 8
Figure 6: Balloon holder ......................................................................................................................... 9
Figure 7: Carbon fibre rod adapter isometric view .............................................................................. 10
Figure 8: Carbon fibre rod adapter dimensions (in mm) ..................................................................... 10
Figure 9: New baseplate (all dimensions in mm) ................................................................................. 11
Figure 10: Base plate with mounted electronics and kite rod ............................................................. 12
Figure 11: Battery holder plate (dimensions in mm) ........................................................................... 12
Figure 12: Battery holder with landing legs ......................................................................................... 13
Figure 13: Camera and sheet metal mount ......................................................................................... 14
Figure 14: Ultrasonic sensor mount ..................................................................................................... 15
Figure 16: Magnetometer [5]............................................................................................................... 16
Figure 15: Electronics flowchart .......................................................................................................... 16
Figure 17: Ultrasonic sensor [6] ........................................................................................................... 17
Figure 18: IMU [7] ................................................................................................................................ 18
Figure 19: GPS receiver chip [8] ........................................................................................................... 19
Figure 20: Quarter h-bridge circuit diagram ........................................................................................ 20
Figure 21: Sensor circuit diagram ........................................................................................................ 21
Figure 22: Quadrotor movement [11] ................................................................................................. 22
Figure 23: Quadrotrotor movement, arrow width is proportional to motor speed [3] ...................... 23
Figure 24: Testing hoist ........................................................................................................................ 27
Figure 25: Test configuration (note tether for preventing any unwanted manoeuvres) .................... 29
Figure 26: Reduced weight of the platform due to the balloon .......................................................... 30
v
List of Tables
Table 1: Component Mass List ............................................................................................................... 6
vi
1. Background & Motivation
Most quadrotors can be put into one of two categories: sophisticated imaging platforms or highend hobbyist toys. Quadrotors in the first category are often developed in either a research setting
or a specialized commercial environment, and require a significant financial investment. The
quadrotors in the second category tend to employ lower quality parts in an attempt to make the
package affordable to hobbyists. As a result, the performance suffers in areas like endurance,
stability, and durability. However, increasing affordability and availability of the electronic sensors
required by quadrotor platforms have made custom modification/upgrades of toy quadrotors
commonplace. These modifications, when added to several advantages inherent to the quadrotor
design, give them immense potential to be used as an affordable tool in a myriad of applications.
Figure 1: Quadrotor operation [1]
Figure 1 above illustrates how a quadrotor works. A quadrotor is composed of four fixed pitch
propellers. During stable flight, two non-adjacent propellers (opposite from each other) spin
clockwise while the other two spin counter-clockwise. This arrangement ensures that the reaction
torques generated by the four propellers cancel each other out, eliminating the need for the tail
rotor that is characteristic of traditional helicopters. Also, the symmetry of the quadrotor design
significantly simplifies the process of balancing and stabilizing the craft.
The quadrotor platform is also an under-actuated system, which means that there are fewer control
axes than the number of degrees of freedom. Quadrotors control only four out of the six degrees of
freedom (three rotation, three translation) allowing for simpler control algorithms (since stable
flight would only require that the same command be sent to all four motors). One final advantage
results from the usage of multiple small rotors in place of one big rotor. Smaller rotors reduce the
overall torque on the system which promotes overall system life. Also, the individual rotors can be
driven to higher velocities which in theory should reduce the amount of vibration while increasing
the efficiency of the motors driving the rotors [1].
1
The above qualities make quadrotors a highly desirable platform for many surveillance operation
modifications. Some interest also has been expressed by police departments in various countries to
procure quadrotor surveillance platforms for use in sporting events and other mass gatherings.
Figure 2: Microdrone MD4-200 [2]
There are many existing companies that design and construct their own in-house quadrotor
platforms (see figures 2 above and 3 below) for video surveillance for short periods of time [2]. The
only problem is that the “professional” quadrotors sold by Draganflyer and Microdrones are
extremely expensive, usually above $1000, are not autonomous and the pilot is usually required to
be trained to fly one properly. The goal of this project was to see if it was possible to modify a
consumer level toy into a semi-autonomous, surveillance capable instrument for far less.
Figure 3: Draganflyer X4 [12]
2
One weakness that most quadrotor systems share is their flight endurance. Quadrotors designed by
Microdrones and Draganfly can remain airborne for no longer than 30 minutes under optimal
conditions, thus limiting their applications. So they cannot be used to gather data or “observe” for
long periods of time [2].
One way to increase the endurance of a quadrotor would be to attach a more powerful battery
pack. However lightweight and high capacity batteries, like lithium polymer batteries, are expensive
and may not be the safest solution due to risks with improper charging. For that reason, an 1800
mAh NiMH battery pack was used (see section 2.2 and onward for a more indepth discussion on the
mechanical components of the modifications).
As for stability and control, algorithms for quadrotors are simpler than ones for a helicopter or
various other rotorcrafts. This is due to the natural stability achieved by operation of the motors at
equal speeds and the quadrotor’s symmetric design. The control used in most systems will
compensate for non-uniform wind turbulence and for instabilities caused by small discrepancies in
performance between the motors. Some quadrotor designs have implemented very complex
control schemes but for the scope of this project simple PID loops were used, since the end goal
was to create a proof of concept. Given enough time however, it is possible to generate a very
powerful and nearly foolproof attitude control system based on the transfer functions of all the
motors and dynamic modeling of the quadrotor itself [1]. Dynamic modeling is a time intensive
process that involves modeling all the gyroscopic and translation motions that the quadrotor would
experience using Newton’s laws.
The control techniques that have been implemented for stabilization of quadrotors in the past are
as follows: PID (Proportional Integral and Derivative), PD2 (Proportional Derivative Squared), and LQ
(Linear Quadratic) [3], [13].
PID is implemented using a difference function between proportional, integral and derivative
difference functions to calculate the change in output (see section 2.4.3 for a more in-depth
discussion on how PID was implemented for the quadrotor and some underlying theory).
PD2 control is similar to PID however there is no integral term and there are two derivative terms in
the equation to find the change. Tayebi and McGlivray proposed a PD2 quadrotor controller where
the two derivative components compensate for Coriolis and gyroscopic torques respectively.
Results showed improvements in transient performance and disturbance rejection over pure PD
control [10].
Linear-Quadratic control optimizes system control by minimizing the cost function in a set of
variables. The cost function is found by summing the deviation from the desired value in each of
the variables. For example, power consumption is an important variable in the quadrotor’s case.
Human input is still required to determine the weighting of each variable by importance, and so
complexity increases as the number of variables gets large [3].
3
Bouabdallah notes that PID works better than LQ due to the simplicity of PID and its tolerance for
model uncertainty [3]. Because of this finding and for time reasons PID was chosen as the control
model.
4
2. Discussion
2.1 Project Objectives
With respect to the discussion in the background section, the primary objective of this project was
to see if was possible to modify a quadrotor toy into a semi-automatic, passively stable surveillance
platform and replicate the performance of more expensive quadrotors.
2.2 Mechanical Aspects
Figure 4: Silverlit X-UFO [4]
The modifications were made to a Silverlit X-UFO (see figure 4 above), an off-the-shelf, consumergrade, remote-controlled quadrotor [4]. A balloon was attached to the center of the quadrotor to
reduce the apparent mass of the overall package, thereby reducing the energy demands of the
motors.
2.2.1
Balloon Sizing
The balloon size can be calculated using elementary hydrostatics:
𝜌𝑎𝑖𝑟 − 𝜌ℎ𝑒𝑙𝑖𝑢𝑚 𝑔
4
3
𝜋𝑟𝑏𝑎𝑙𝑙𝑜𝑜𝑛
3
= 𝑚𝑝𝑎𝑐𝑘𝑎𝑔𝑒 𝑔
(3)
Where 𝜌 is the density of the gas (helium in this case), g is the gravitational acceleration and mpackage
is the mass of the entire package. When equation 3 is solved for r, the dimensions of the balloon
5
will be known. Based on the weight of the out-of-box X-UFO, it was predicted that the total weight
of the new quadrotor platform would be around 800 grams with all the modifications (see Table 1
for breakdown). Taking the density of air to be 1.2 kg/m3 and the density of helium to be 0.1786
kg/m3, the required radius of the balloon was 0.5469 m or 1.794 ft. It was expected, however, that
𝑚𝑝𝑎𝑐𝑘𝑎𝑔𝑒 would be slightly higher due to the variation in the weights of various structural
components that would be added during the course of this project.
Component
X-UFO (without battery)
Battery
Arduino Mega
Shield
GPS
XBee Module
Ultrasonic sensor
Magnetometer
Inertial measurement unit
Aluminum Attachments
Mass (g)
219
348.5
37.0
14.5
21.5
3.0
4.5
~5
~50
~100
Table 1: Component Mass List
The radius of the balloon that equation 3 provides results for a balloon that will be just the right size
to achieve neutral buoyancy in air. That is, it will be able to hover at a constant altitude for an
indefinite amount of time (provided the balloon can maintain the volume of helium inside). Ideally,
the entire package should be slightly negatively buoyant because it needs to be able to descend
very slowly if left to its own devices. The platform’s stabilization system would then be activated
intermittently to maintain height. Therefore when 𝑟𝑏𝑎𝑙𝑙𝑜𝑜𝑛 was solved for, 𝑚𝑝𝑎𝑐𝑘𝑎𝑔𝑒 was reduced
slightly so that the balloon would be appropriately sized for the whole platform to be negatively
buoyant.
The addition of the balloon therefore has three benefits:
 The system would gain some passive stability due to the raised center of mass and inertia of
the balloon
 The energy required to hover and take off would be reduced
 Overall endurance would be increased
To account for various factors and other unknowns, a 4 ft balloon was ordered but for whatever
reason, the suppliers sent a 5 ft balloon instead. According to the suppliers, the 5 ft balloon is
capable of lifting 1.5 kg, well above that of 0.86 kg, which is how much the quadrotor weighs in its
finalized configuration (see next section for more information) [12]. The extra capacity can be used
to attach other heavy items, possibly a better camera or more propellers that allow faster motion
horizontally.
6
As for the balloon, the reason multiple balloons instead of one giant balloon were not considered
was due to the complexity of attaching multiple balloons. Also, using multiple balloons would have
robbed the quadrotor of its horizontal steering capability. The steering for the modified design is
achieved by the quadrotor pivoting around the flexible kite rod. See the section below for more
information.
In terms of flexibility regarding how much lift is required, the weights of all the components were
fairly well known from the beginning. Just in case however, the 5 ft diameter balloon allows for
tuning of the buoyancy. The sections coming up discuss how the balloon was mounted, but as a
preview, the figure below shows the scale of the project in Solidworks. The balloon in the figure is
shown having a diameter of 5ft. To achieve slight negative buoyancy however the balloon only
needs to be around 4ft in diameter.
7
Figure 5: Solidworks render of the finished assembly
2.2.2 Balloon and Electronics Installation
Attaching a balloon to the quadrotor platform adds some difficulty in the craft’s ability to maneuver
horizontally. The quadrotor moves horizontally by generating more thrust on one side of the craft
than the other so that it tilts and some of the thrust is directed horizontally. Initially, string and stiff
rod were the two options considered for attaching the balloon to the quadrotor. However, a string
would allow the balloon to drag behind whenever the quadrotor attempts to maneuver
8
horizontally. The balloon would be a considerable source of drag in any horizontal movements.
The stiff rod also presented a difficulty because, whenever the quadrotor would attempt to tilt, the
balloon would introduce a powerful restoring moment, forcing the craft back into the upright
position. In light, of these considerations, a relatively flexible, hollow carbon fibre kite rod (4 mm
outer diameter) was selected to connect the balloon to the platform. It was anticipated that it
would allow the quadrotor to tilt (due to the rod bending under asymmetric thrust) and generate
horizontal thrust while reducing the restoring moment described above.
The balloon was attached to the platform using the cone-like structure shown in figure 6 below. The
balloon mount was fabricated from cardboard. The kite rod goes up through the hole at the tip of
the cone, then stops right before the hole at the centre of the circular brace to prevent rubbing
against the balloon that will be pulled through the hole in the circular brace. The balloon is pulled
through the opening of the cone, then through the hole in the circular brace and finally through the
hole in the side of the cone. The balloon is then clamped to hold it in place. The exact dimensions of
the balloon holder are not critical. Any cone-like structure of similar shape and dimensions should
be sufficient to hold the balloon securely. The drawing below was made purely for the purpose of
illustration.
Figure 6: Balloon holder
The lower end of the rod is attached to the aluminum shaft shown below in figure 7.
9
Figure 7: Carbon fibre rod adapter isometric view
Figure 8: Carbon fibre rod adapter dimensions (in mm)
The rod slides inside the hole on the top, which is 6 mm deep, and is attached with epoxy. The
bottom end of the adapter goes through the base plate (the base plate was fabricated from 3/16”
10
thick aluminum) shown in figure 9 below. The adapter goes through the centre hole and is secured
with nuts and lock washers on both sides (threads not shown in adapter screen shot). The various
holes in the baseplate are meant for all the new electrical components (refer to section 2.3 for
more information on the new electronics installed on the quadrotor). The large triangular cut is
meant to save weight and allow access to the cables that come from the motors. Figure 10
immediately after figure 9 shows how all the components were mounted. The adapter is shown
assembled in figure 10 as well.
The holes inside the two grey lined rectangles were used to attach standoffs that lead below the
quadrotor to a 1/8” thick aluminum plate shown in figure 11. The NiMH battery pack is secured
below the plate with a zap strap. The holes in the corners of the battery holder plate are used to
attach standoffs that function as the landing legs of the quadrotor to ensure that it has a level
attitude before taking off (a level attitude ensures that all the stability set points are accurate for
stable flight, see sections 2.3.3 and 2.4.2 for more information on stability). Figure 12 shows the
battery holder with the landing legs fully assembled.
Figure 9: New baseplate (all dimensions in mm)
11
Quarter Hbridges
XBee
ArduinoMega
Adapter
IMU
Magnetometer
Figure 10: Base plate with mounted electronics and kite rod
Figure 11: Battery holder plate (dimensions in mm)
12
Figure 12: Battery holder with landing legs
13
2.2.3 Other Mechanical Considerations
The camera mount was made out of a small piece of sheet metal with a drilled hole large enough
for the camera’s lens to see through. Then the sheet metal part was bent so that it could be
fastened to the edge of the battery holder plate with a screw. The figure below shows the
construction of the camera mount along with the camera.
Figure 13: Camera and sheet metal mount
The ultrasonic sensor was mounted to the quadrotor underneath one of the support arms using
tape and string. No rigid or heavy materials were used to avoid making the quadrotor off balance
since the ultrasonic sensor is quite far away from the centrepoint of the quadrotor. The red arrow
in the figure below shows how the ultrasonic sensor was mounted.
14
Figure 14: Ultrasonic sensor mount
2.3 Hardware and Sensors
The hardware consists of the Arduino Mega Prototyping board, IMU (Inertial Measurement Unit),
Magnetometer, Ultrasonic Sensor, Battery, Camera and GPS. The GPS was not implemented due to
time constraints but the reasons for including it are detailed in section 2.3.4. The other sensors are
detailed in the sections that follow.
The electronics are mounted using screws in the holes in the baseplate shown in Figure 9. The
Arduino Mega is mounted vertically using metal screws on L brackets while the sensor board and
power circuit are mounted horizontally using nylon screws as shown in Figure 10.
Originally the relatively small Arduino Duemilanove was considered but was later upgraded to the
Arduino Mega for its increased functionality. The Arduino Mega, a larger version of the standard
Arduino, was chosen to accommodate an increase in the number of sensors which needed a total of
9 analog inputs and the capacity for more hardware UARTS (hardware communication ports). The
Arduino Mega is based on the Atmel Atmega1280 running at 16MHz. Arduino compiler version
0017 was used to upload and edit the Arduino Mega code. The compiler uses C++ code to generate
AVR assembly that is executable on the Atmel microcontroller. The main function of the Arduino
was to gather sensor information and use that in a control algorithm to modify motor output values
and in turn stabilize the quadrotor. The software written for this purpose is detailed in section 2.4.
The figure below shows how all the electrical components communicate with each other.
15
Analog voltage
Data for
debugging
Sensors
Arduino
Laptop
PID Tuning parameters and
kill command
Motors
PWM output to motors
LCD
Viewer
Wireless Camera
On the craft
On the ground
Figure 15: Electronics flowchart
2.3.1 Magnetometer
The magnetometer allows the quadrotor to get a magnetic heading. In other words, it will be
possible for the quadrotor to locate magnetic north. The magnetometer is crucial in detecting and
compensating for unwanted yaw changes during flight.
The sensor chosen was the HMC1052L which works using magnetoresistive sensors [5]. They are
extremely sensitive solid state magnetic sensors that allow the measurement of the direction and
magnitude of the Earth’s magnetic fields. The sensor has a range of sensitivity between 120 microgauss and 6 gauss.
Figure 16: Magnetometer [5]
The sensor outputs a 16 bit precision analog voltage that can be measured and translated into a
magnetic field strength. This analog voltage can be easily measured using an ADC chip or the built in
ADC pins on the Arduino.
16
2.3.2 Ultrasonic Sensor
The ultrasonic sensor allows the quadrotor to detect its altitude which is very important for
maintaining a stable hovering altitude.
The sensor chosen was the Maxbotix LZ-EV0 [6]. It works by sending a cone of ultrasonic frequency
sound waves outward and receiving the resulting sound waves after they are reflected off the
ground or whatever surface the sensor was pointed at. The time delay between the resulting waves
reaching the sensor and the initial waves that were sent out is used to determine the distance.
Figure 17: Ultrasonic sensor [6]
The ultrasonic sensor outputs either an analog voltage, a pulse width modulated output signal or an
RS232 serial output. They can all be converted by the Arduino in software to yield distances that
can be used in PID calculations.
The ultrasonic sensor works well in a range from 10-12 cm to several meters. Ranges outside this
sometimes yield erroneous results for several hundred readings at a time. To combat this issue the
software takes up to several hundred readings if necessary to filter enough good results to
confidently return a value to the PID control. This process utilizes a lot of processor time which can
rob resources from the pitch and roll compensation programs of the quadrotor, thereby affecting
overall stability negatively. A more optimized algorithm, a more expensive sensor or an alternative
interface to the sensor is highly recommended for any future work.
17
2.3.3 Inertial Measurement Unit
The Inertial Measurement Unit (IMU) used was the IMU 6DOF Razor - Ultra-Thin IMU. It consists of
three separate chips: a dual function pitch and roll MEMS gyroscopic sensor, a yaw MEMS
gyroscope and a three axis accelerometer. The IMU has six outputs corresponding to these three
chips and these are read by the Arduino Mega and its 10 bit analog to digital converter pins [7].
Figure 18: IMU [7]
The gyroscope readings were used heavily in the first iteration of the control algorithm before
balloon attachment. The readings were taken from the magnified output Gx4 to allow for a more
accurate measurement using the 10 bit ADC. The original Gx1 readings were around 100 +/- 20
whereas the amplified Gx4 readings were around 360 +/- 60, so Gx4 readings were used. The
readings were used as the derivative values for the yaw, pitch and roll in the PID loops (refer to
section 2.4.4 for a discussion on how this was implemented).
2.3.4 GPS
The GPS chip that was planned to be used was the Lassen iQ [8]. Originally, the proposal outlined
that GPS would be used to let the end user program a set of co-ordinates into the controller laptop
and the quadrotor would fly to that location completely unassisted. It was always planned to be
installed last since it is a high level device and requires that all other systems be functioning
perfectly. The GPS could not be relied upon for accurate positioning data since its best case vertical
and horizontal resolutions (±10 m and ±5 m respectively) are far too small. That means that the GPS
would only really have been useful for hovering over comparatively large areas where the error of
±5 m would not matter.
18
Figure 19: GPS receiver chip [8]
If implemented, the GPS would be read using the serial port on the Arduino Mega.
2.3.5 Camera
The wireless camera was provided by the project lab and its feed can be viewed on a small LCD
screen that can be held on the ground. The camera is battery powered and has a fairly large range
of transmission. In a short test it worked through a wall and several objects at around 5 meters
distance and it is expected that the range would be much higher outside with no obstacles in the
way. A mount was constructed for the camera; however, it was not installed on the quadrotor itself
for testing since the camera’s function does not rely on the function of the quadrotor. In principal
any camera would work as long as there is an appropriate mount for it.
2.3.6 Battery Pack
The X-UFO originally came with a small Li-Polymer battery. However it became immediately
apparent from the size of the battery that it was designed to deliver a massive amount of current in
a small package (since each motor draws roughly 1.5 A at full power). Since the battery was small it
usually only lasted between 5-6 minutes [4]. Therefore, a battery pack composed of 8 rechargeable,
1800 mAh, AA, NiMH batteries weighing 345 g total was used.
19
2.3.7 H-Bridges
An H-Bridge circuit allows a device like a microcontroller to be able to switch on a large current to
operate motors or other similar devices. The H comes from the appearance the circuit takes when
drawn. For this project a Quarter H-bridge was used because unlike an H-bridge that can allow
current in either direction the quadrotor motors only need current in one direction, hence 4 quarter
h-bridges sufficed. Since the quadrotor relies on its own mass to descend, it was unnecessary to
design and build full H-bridges that would allow the motors to be spun backwards.
The circuit (diagram shown below) works using a P channel mosfet, a diode, and a 1 MΩ resistor. Its
operation is facilitated by creating a voltage on the gate of the mosfet from the Arduino and
changing the distribution of charges in the mosfet to allow a current to flow from its drain to
source. This allows current to flow from VCC to ground through the motor and when the device is
switched off the remaining momentum of the motor is dissipated through the diode. The quarter hbridge shown is duplicated four times, one for each motor. Due to the large resistance to ground
from the gate of the mosfet, the Arduino only outputs a very small current and achieves almost
exactly 5 volts at the gate. This is important as a smaller resistor could potentially draw enough
current to cause the Arduino to pass the safe limit of supplied current to any given output pins and
reset during operation due to power spikes.
Figure 20: Quarter h-bridge circuit diagram
2.3.8 XBee Wireless Module
The XBee module uses wireless communication standard 802.15.4 at a frequency of 2.4 GHz. The
wireless modules have firmware installed on them and this can be updated using the X-CTU
software available from the Digikey website [13]. The X-CTU software allows for modification of key
20
parameters to enable communication such as: Source Address, Channel, Destination Address, PAN
ID and Baud Rate. These must be set correctly in order to allow proper communication between
units.
For the purpose of this project the quadrotor had an XBee mounted on it while the laptop used for
testing had a small USB powered XBee that would allow Putty to talk through a serial port, into the
XBee, across the air to the next XBee and finally into the quadrotor’s Arduino microcontroller. This
was previously defined as the laptop->Arduino Interface in figure 15. The range for the wireless
devices is around 100 meters using line of sight and significantly less through buildings. In the case
of the quadrotor, line of sight is always easy to achieve as it is flying out in the open air. Testing was
always done in the same room with the quadrotor and consequently no problems with
communication were found.
2.3.9 Sensor Circuitry
The Xbee module andAll of the sensors except the ultrasonic sensor are mounted on a separate
circuit board that is attached opposite of the Arduino Mega on the baseplate. The sensor circuitry
was previously attached on an Arduino Mega Shield that plugged into the front of the Arduino
Mega and had some work space to mount circuitry. This turned out to be a bad idea as the
quadrotor was a little unbalanced and the sensors are designed to be mounted level with respect to
the ground whereas, in the Arduino Mega Shield case they were mounted perpendicular to the
ground. The finalized sensor circuitry layout is shown in the diagram below. The ultrasonic sensor is
not physically close to the other sensors however, it is included to show the pins used. The XBee
module was also attached to this circuit. Figure 10 a few pages above shows how all the electrical
components were mounted.
Figure 21: Sensor circuit diagram
21
2.4 Software Aspects
2.4.1 GUI and Interface
The interface to the software is mainly through Putty - a free Telnet client. An alternate method to
interface was made in Windows and was programmed in C++ by Steven Stuber using source code
from his previous co-op work term. The Windows interface was initially planned to be used by the
end user of the quadrotor as a more user friendly solution compared to using Putty, but distribution
to others will likely not take place and thus Putty was used. The Putty terminal was set up using the
serial port that corresponds to the wireless Xbee USB device. The serial port was set up using a
baud rate of 9600. Testing commands and PID values were modified wirelessly using this interface
by sending keystrokes through Putty running on the laptop.
2.4.2 Movement
The quadrotor has six degrees of freedom in its motion, three of them are translational and the
remaining three are rotational. Rotation and vertical translation make up the basis of movement for
the quadrotor. Horizontal motion results from combining rotational and vertical translation. The
degrees of freedom of the system will be explained below individually and will refer to the figure
shown below to explain the specifics.
Figure 22: Quadrotor movement [11]
The upward translational motion is the simplest to generate. The four motors simply turn on at the
same speed and the quadrotor generates vertical lift.
Roll refers to the rotation about the axis which is defined as the front of the device. In the diagram
shown the y axis is the front axis and the rotations occur by making motors 2 or 4 have a differential
in speed. If motor 2 has more speed the device will roll to the left and if motor 4 has more speed
the device will roll to the right.
22
Pitch refers to the rotation about the axis defined as the right of the device. In the diagram shown
the x axis is the right axis and the rotation can be generated by changing the speed differential in
motors 1 and 3. Increasing relative power to motor 1 will pitch the front of the device up and
increasing relative power to motor 3 will pitch the front down.
The last rotational motion known as yaw is slightly more complicated than pitch and roll. It refers to
the rotation about the vertical axis of the quadrotor. It requires a differential in the sets of motors
that are opposing each other. To rotate the quadrotor counter clockwise, power is increased to
motors 1 and 3 and decreased to motors 2 and 4. Rotation clockwise is achieved by increasing the
power to 2 and 4 and decreasing power to 1 and 3. This should normally be done by increasing and
decreasing power by the same absolute amount from each motor group to maintain a constant
thrust downwards.
To achieve motion in the horizontal direction, the idea is to apply asymmetric thrust, which means
that the quadrotor would tilt in the direction of travel. As a result, the quadrotor will move up and
in the required direction since there will be some thrust going downwards and some thrust going
opposite to the direction of travel. Figure 23 below shows how horizontal and some other
movements can be generated.
Figure 23: Quadrotrotor movement, arrow width is proportional to motor speed [3]
2.4.3 Stabilization
Stabilization of the quadrotor on its own without any balloon attachments is a formidable task. The
addition of the balloon eliminates the need for roll and pitch control if a rigid enough rod is used for
the attachment (see section 3 for a more in-depth discussion). The paragraphs below will describe
23
the type of control used in order to stabilize the quadrotor without any support. This type of
stabilization will be required if the quadrotor uses a ball joint as described in the recommendations
(section 5.1). The type of stabilization that is required if a rigid rod is used will also be discussed.
Multiple PID loops are needed in a quadrotor to accurately control all three gyroscopic instabilities
that can be introduced by the motion of the platform. The three changes are pitch, yaw, and roll
and they all need to be kept at one specific, pre-set level so that the quadrotor can maintain its
position, regardless of any external influence.
For stabilization without any balloon attachments or a ball joint, the 2 degrees of rotational and
horizontal freedom are retained as compared to the case of a rigid rod attachment to the balloon.
In the case of having all 6 degrees of freedom the control can be split into 4 separate PID loops, one
for each rotational degree of freedom and one for the vertical translation.
PID is a system of control that takes an input i(t) and a desired result r(t) and then the error function
e(t) is defined as shown below.
e t = setpoint − p t
t
𝑐 𝑡 = K P ∗ e t -+ K I ∗
e t dt -+ K D ∗
0
1
de t
dt
(2)
The change c(t) would be converted to various motor speed offsets and the input p(t) would be the
gyroscopic sensor reading for a quadrotor. This equation is evaluated discretely in a microcontroller
and is done in a loop of code. It is called a PID loop for this reason. The constants KP , KI and KD
control the contribution to the final change function. These constants must be tuned in order to
find a balance, where oscillations about the desired result do not occur [1].
To keep the quadrotor level, a setpoint of 0 for both pitch and roll is used (starting from a level
attitude on a flat surface). Yaw can be controlled based on a magnetometer reading to determine
north or any other direction desired. The altitude can also be controlled by implementing a set
point and comparing it against a reading from the ultrasonic sensor. To facilitate horizontal motion,
a simple change in the set point is necessary so that the quadrotor stays in a roll or pitch position
while it moves to a new location. The above control methods account for every degree of freedom
in the system and in total, 4 PID loops are required.
The control for a rigid rod scenario degenerates into a very simple case of the above. Without pitch
and roll the control consists of 2 PID loops, one for yaw and one for altitude. This is the type of
control the quadrotor is currently outfitted with and although it is unable to move horizontally or
compensate for pitch and roll it is a proof of concept that such similar tasks can be completed given
the proper mechanical connection to the balloon and the proper control algorithm.
24
2.4.4 Software Functionality and Notes
The code for the control system was written in C++ and is attached in the appendix. There are two
programs attached, one for control using the quadrotor in a setup where the pitch, roll and yaw
movements are compensated for without altitude compensation and another where the pitch and
roll are fixed (due to the rigid rod, refer to section 3 for an explanation) while altitude and yaw are
controlled. Altitude sensing was a very computationally intensive component of the software and
with pitch and roll implementation the CPU utilization increases greatly and hinders the
performance of the PID loop. That is why altitude sensing is only implemented in the pitch and roll
fixed scenario. The reason for the above is due to the amount of filtering required for the data from
the ultrasonic sensor (see section 2.3.2 for more information).
The altitude was sensed using a large loop that discards values of the ultrasonic sensor that are too
far from the previously read value. The program collects good values until 50 are read and then
they are used to calculate the average. That average is then checked for consistency with the
previously accepted values and then plugged into a PID loop to control the speed of all four motors.
The altitude sensing loop should have an integral component in the PID. To accelerate testing
however, a quickly approximated constant offset was applied to the motors. The correct method
would result in a changing offset that would allow for more stable flight and less oscillation about
the set point.
All the programs contain a PID class that controls the PID loop functions and keeps track of PID
constants, sensor readings and motor actuation. The PID constants are initially set in the
constructor of the PID class but can be changed in real time using the debug section in the main
loop. The debug section reads in keypad presses from the laptop and can change the parameters of
the PID loop while tests are running. The debug section can increase or decrease PID constants in
real time, increase other constants like the set points or offsets of the motors and also has the
ability to kill the motors at any time by pressing “k”. These debug routines are all meant to be
reprogrammed for testing purposes and in the final product they would be removed and replaced
with a more restrictive but user friendly windows GUI.
The loop function is the main program loop in the Arduino and contains four main components for
the purposes of this project: the three PID functions and the debug section. They are constantly
looped in order to keep control active at all times. The PID functions are as follows: a sensor polling
function, a calculation function and a motor update function. The sensor polling function reads the
analog inputs and applies any offsets if necessary. The analog inputs sometimes require offsets as
they often have a nonzero value when the sensor is reading nothing. For example in a 10 bit ADC
that is being used in this project a sensor could produce a value of 350 when it was reading nothing.
Then as the sensor took meaningful readings its value could fluctuate from, from example, 290 to
410 and this would need to be translated and rescaled to a more usable form. The program could
subtract by 350 and then feed that value into a PID loop to generate results that will respond
correctly to external changes. A more concrete example of something that performs like this is an
accelerometer chip. An axis could behave in this manner and negative readings could tend towards
25
290 and positive ones toward 350. This is why it is necessary to have a proper offset for each
separate input. Some of these offsets are set in the beginning of the code by assuming that when
the device is started it will be stationary and level with the ground.
Once the sensor values are all read the program continues to the calculation stage. For pitch and
roll the accelerometer vector is used in order to find the angle for each using a simple trigonometric
relationship. For example pitch = atan(az/ax)*180/3.14159 where az and ax are the z and x
components of the accelerometer vector respectively.
Yaw is calculated using the magnetometer readings; it involves an arctangent as well. The
derivatives of yaw, pitch and roll are all from the same gyroscopic sensor and in this project the Gx4
readings were used. Gx4 refers to the gyroscope readings that are amplified by four times the
normal reading. This is useful because the range of values the gyroscope normally outputs is low
enough that multiplying by four will not overshoot the 10 bit ADC maximum value of 1024. The
sensor values are then plugged into a PID loop after some small offsets are applied to the yaw, pitch
and roll angles and the change values are generated.
The algorithm for yaw control is somewhat complex when compared to the pitch and roll ones. It
first determines the max and min of the four motor values. The algorithm takes the minimum
motor value and the difference between the maximum motor value and the maximum allowed
motor value and finds the smallest value between those two. That value is then allowed to be used
when redistributing motor output values in order to generate yaw motion. For example, if the
maximum allowed motor value is 80 and in some case the min and max values were 20 and 70
respectively the two values that would be compared would be 20 and 10 (min and difference
between max and max allowed) so the smallest is 10. This would then allow even the motor that
was closest to the maximum to be increased by 10 without reaching the maximum allowed value
and similarly for the minimum value. The result is that yaw control is only achievable when the
motors are not very close to the maximum or minimum allowable motor values as motor value
redistribution is necessary.
The last PID function is the motor update function. It is responsible for taking the outputs of the PID
and converts them into individual motor offsets. The methodology for converting these values to
individual motor values is explained in section 2.4.2. The motors are then double checked to make
sure they are not overflowing the maximum specified motor output level or attempting to apply a
negative value to the motors. The motors are then scaled according to their relative ability that was
estimated during testing in order to keep the system as balanced as possible. This was necessary
because during preliminary equipment check at the beginning of the project revealed that some of
the motors were not able to spin as fast as the others.
26
2.5 Testing and Tuning Apparatus
Once the mechanical assembly was completed, it was realised that it would be impractical to fill the
balloon with helium each time stabilization testing was conducted to tune the PID parameters. So, a
testing hoist was fabricated out of 80-20 aluminum, a hollow carbon fibre rod, and some sheet
metal. The figure below illustrates the testing mechanism.
Figure 24: Testing hoist
The arm is free to slide along the bracket on top of the column. Using elementary force and
moment balance equations, it is possible to calculate the mass and distance away from the pivot
required such that the quadrotor’s apparent weight is reduced by the same amount that the
balloon would reduce the mass by. Once the mass is known, the bracket is taped off so that the rod
cannot slide freely. In doing so, a simple lever arm turns into an effective and practical way to test
stabilization and gives ample time to tune PID constants.
Testing was later done by simply holding a string connected to the quadrotor while tuning various
parameters. A laptop was used with the debug section of code outlined in section 2.4.4. The
quadrotor’s motion was observed while testing and the PID constants were qualitatively increased
27
or decreased depending on how unstable it appeared to be. If the device oscillated too much the
proportional gain was decreased slightly and the derivative gain was increased. Testing was carried
out until the quadrotor was able to stay reasonably level hanging from a string and then work
began on attaching the quadrotor to the balloon.
28
3. Results
Before the balloon was attached, the quadrotor was completely capable of stabilizing itself on the
hoist if it encountered any pitch and roll motions. Once the balloon was attached, the entire
package was moved to a larger room with a higher ceiling to test some other manoeuvres that
required some additional space.
Based on testing with the balloon attached (see figures below), it appeared that whole setup was
extremely stable. The inertia of the balloon and stiffness of the carbon fibre rod added a lot of
stability to the platform and ensured that unwanted pitch and roll conditions would never occur.
Figure 25: Test configuration (note tether for preventing any unwanted manoeuvres)
29
The standard ascent and descent manoeuvres worked exactly as predicted, albeit the platform
ascended too quickly. That was occurring because there was too much helium in the balloon, upon
checking it was discovered that the apparent weight of the entire system was 70 g (see figure
below), which was extremely low. To remedy the quick ascension, enough helium was let out to
increase the apparent weight to 160 g which ensured a smooth and controllable ascent rate. Once
the ascension problem was taken care of, horizontal manoeuvrability was tested.
Figure 26: Reduced weight of the platform due to the balloon
Horizontal manoeuvrability proved to be troublesome due to the added inertia of the balloon and
also due to the added stiffness of the kite rod and the way it was mounted. During design it was
assumed that the rod would be flexible enough to bend when asymmetric thrust was applied. The
bent rod would direct some thrust in the horizontal direction, thereby allowing the quadrotor to
move in the opposite direction. Unfortunately, that barely occurred and as a result the quadrotor
was not capable of horizontal movement. Please refer to the final recommendations section for
suggestions that should aid in fixing the horizontal movement problem.
On the bright side, the added inertia of the balloon and the stiffness of the rod made the entire
quadrotor extremely stable. Observations indicated that pitch and roll were non-existent no matter
how fast the entire system accelerated, so it became possible to delete the stabilization algorithms
for pitch and roll, thereby saving valuable processing power for other uses.
30
Without pitch and roll, tuning was only necessary for yaw and altitude control. These were also
tuned in real time via laptop and only a few hours were needed to reach a state of moderate
stability. The altitude control had one small problem with oscillation after attempting to descend to
a lower setpoint. This type of instability was due to the lack of integral gain in the PID. Instead of
integral gain a constant offset was used to perform a quick qualitative test of its performance.
Hover testing worked well in the sense that It was possible to set a hovering altitude on the control
laptop that the quadrotor would ascend or descend to smoothly and maintain the altitude for as
long as necessary. Yaw control was also implemented with hover control and the quadrotor would
try to stay at the pre-set bearing. It also became immediately apparent that battery endurance was
not an issue as there was no decline in thrust output after roughly an hour of testing. The balloon
was indeed saving a lot of battery power. The only problem was that there was no way to control
horizontal movement while hovering or yawing due to the above mentioned reasons.
31
4. Conclusions
With regards to the goals mentioned in the background section, the quadrotor satisfies most of
them. Testing suggests that battery endurance increased significantly, from 5 minutes from the
original li-Poly battery to at least 2 hours of continuous testing on the hoist without a significant
drop in thrust output.
Horizontal movement capability currently eludes the quadrotor however the designers have ideas
to fix that. Please refer to the recommendations section for the ideas.
Hover and yaw capabilities were also tested successfully. The quadrotor is completely capable of
maintaining a set altitude and heading. Testing on the hoist without the balloon also indicated that
it was possible for the quadrotor to quickly stabilize itself after any adverse pitch and roll
movements were encountered.
The quadrotor definitely satisfies the proof of concept goal that the designers set out to prove at
the beginning of the project. It is entirely possible to replicate the capabilities of more expensive
quadrotor surveillance platforms that cost thousands of dollars for significantly less.
32
5. Final Recommendations
Based on evidence and observations, the following suggestions would make any future iteration of
this project more successful. These are all the suggestions that would have been have followed by
the designers given more time. Arranged in decreasing preference, they are listed below followed
by an in-depth discussion about each of them:
1.
2.
3.
4.
Installing a ball joint between the base plate and kite rod
Using a more flexible rod between the balloon and the quadrotor
Optimizing ultrasonic sensor conditioning
Accurate dynamic modeling of the quadrotor system as described by Bouabdallah, Noth
and Siegwart and many others, followed by simulation, which can easily be conducted in
Matlab using simulink [1], [3].
5. Enabling GPS
6. Attaching a set of small rotors that generate thrust in the horizontal direction
7. Better Camera
5.1 Ball Joint
A ball joint mounted between the kite rod and the baseplate would allow the quadrotor to pivot
around the joint. This would allow the quadrotor to direct thrust in the horizontal direction more
effectively. One side effect of this is that it will become necessary to add the pitch and roll
compensation algorithms since the ball joint will let the quadrotor tilt in any direction and those
movements will have to be compensated for. This is one solution that should definitely be
considered for any future work.
5.2 Flexible Rod
A more flexible rod is also a way to allow more horizontal thrust to be transmitted. Since the
designer’s initial design relied on a moderately flexible shaft to bend under applied asymmetric
thrust, this modification goes along with the original design. However, going with this option would
mean fabricating the adapter again because the rod is attached to the adapter with epoxy.
Carbon fibre is known to be a very stiff material and therefore a hollow plastic rod would be better
suited for the purpose. A hollow plastic rod should be flexible enough to bend significantly under
asymmetric thrust thereby allowing the quadrotor to move horizontally.
33
5.3 Optimizing Ultrasonic Sensor Conditioning
In the current software there is either support for yaw, pitch and roll compensation with no altitude
compensation when using a non rigid rod to attach the balloon or there is support for yaw and
altitude compensation when using a rigid rod to attach the balloon. The two schemes should ideally
be merged into one version that can handle yaw, pitch, roll and altitude compensation
simultaneously without over utilizing the processor and still achieving stability. The current problem
with simply merging the lines of code has to do with the effort put into conditioning the ultrasonic
sensor input.
The ultrasonic sensor performs relatively error-free when at heights above 10-12 cm and below
heights of a few meters. When the quadrotor is below 10-12 cm the sensor returns erroneous
results that need to be filtered at the expense of using more processing time. The filtering and PID
loops running simultaneously would probably overwhelm the Arduino and the response time for
the stability corrections to be applied would be negatively affected.
The filtering is a key requirement to solving this problem. A more expensive ultrasonic sensor, an
alternate interface method or a more optimized filtering algorithm could be used to attempt to
reduce these erroneous readings. The remaining combination of code is trivial because pitch and
roll control are relatively simple computationally and would not contribute much toward over
utilizing the processor.
5.4 Dynamic Modeling
This modification would make an excellent tie-in to adding the ball joint on the baseplate and if
future designers want to try implementing a new control scheme. Since the quadrotor would be
free to move about the ball joint, it would be free to pitch and roll. Dynamic modeling of the system
would become necessary to design an accurate stability control system that uses the resources of
the Arduino more efficiently than a simple PID loop. Modeling and simulating the system will
probably take a long time; however, the model should predict all the movements that the
quadrotor could possibly experience and allow the designer to make corrections before ever
fabricating a physical model.
Dynamic modeling should result in a more efficient control algorithm in which it should be possible
to run pitch, yaw and roll compensation simultaneously with altitude control.
5.5 Set-up GPS
The GPS was intended for use in larger scale operations while interacting with services such as
Google Maps. In a simple scenario the current system with its 100 meter line of sight range could be
34
used to, for example, center on top of a small land plot of a realtors choosing and take an aerial
photo that would normally be expensive to acquire otherwise. However, the current wireless
communication module would not be able to function at ranges large enough to do other types of
large scale observation. Either a very powerful system capable of transmitting line of sight signals
several miles or a cellular based system would be ideal in order to facilitate map based movement
for large scale operations. The large scales also will not be affected by the accuracy of the GPS
(within 5 meters horizontally).
5.6 Horizontal Thrusters
A set of rotors could be added to the device that point and generate thrust in the horizontal
direction. This would allow a rigid rod design to exist while still preserving the ability to translate
the device horizontally. Realistically, this modification would be a little messy to implement since
the quadrotor does not really have much more room to install additional rotors. The best place to
install more rotors would perhaps be on the rod itself. 2 rotors attached to the center of the rod
and capable of being driven both forwards and backwards would be very effective in pushing the
entire platform in any horizontal direction.
However, the added rotors would come at the cost of additional drain on the batteries, due to the
additional lift requirements and the power requirements of the new rotors. The additional drain
would lessen the flight endurance of the quadrotor.
5.7 Better Camera
It is also possible to buy an off-the-shelf digital camera with controller ports for use with the
quadrotor. It would allow the user to take pictures remotely and would provide much better image
and video quality than the low quality wireless camera currently used. The type of camera
purchased must have a LANC controller port. The Arduino can fully utilize the LANC protocol and its
functionality which could allow the user to take full control of the camera remotely. The software
for controlling the camera using the LANC protocol is already working and has been tested by
Steven Stuber in a previous work term.
35
References
[1] Michael Fisher. (2007, June) University of Central Queensland - Attitude Stabilisation of a
Quadrotor Aircraft. [Online]. content.cqu.edu.au/FCWViewer/getFile.do?id=18029
[2] Microdrones GmbH. (2009, September) Microdrones - your eye in the sky. [Online].
http://www.microdrones.com/en_md4-200.php
[3] S. Bouabdallah, A. Noth, and R. Siegwart, "PID vs LQ control techniques applied to an indoor
micro quadrotor," IEEE Explore, vol. 3, pp. 2451-2456, October 2004. [Online].
http://asl.epfl.ch/aslInternalWeb/ASL/publications/uploadedFiles/330.pdf
[4] Hobbytron. (2009, September) X UFO by silverlit RC flying machine. [Online].
http://www.hobbytron.com/SilverLit-X-UFO-RC-Flying-Machine-RTR-with-4-Motors.html
[5] sparkfun.com. (2009) Dual Axis Magnetic Sensor Breakout - Honeywell HMC1052L. [Online].
http://www.sparkfun.com/datasheets/IC/HMC105X.pdf
[6] sparkfun.com. (2009, September) Ultrasonic Range Finder - Maxbotix LV-EZ0. [Online].
http://www.sparkfun.com/commerce/product_info.php?products_id=8502
[7] sparkfun.com. (2009, September) 12 Channel Lassen IQ GPS Receiver with DGPS. [Online].
http://www.sparkfun.com/commerce/product_info.php?products_id=163
[8] Sparkfun.com. (2009, October) IMU 6DOF Razor - Ultra-Thin IMU. [Online].
http://www.sparkfun.com/commerce/product_info.php?products_id=9431
[9] (2009, September) Wholesale Balloon Supplies. [Online].
http://wholesaleballoonsupplies.com/Helium_Balloon_Flying_Time.php
[10] S. McGilvray A. Tayebi, "Attitude Statbilization of a Four Rotor Aerial Robot," in December,
Paradise Island, Bahamas, 2004, pp. 14-17, 43rd IEEE conference on Decision and Control.
[11] Gabriel hoffman. (2007, January) Wikipedia Commons. [Online].
http://commons.wikimedia.org/wiki/File:Quadrotor_yaw_torque.png
[12] Draganfly Innovations. (2009, September) Draganflyer X4. [Online].
http://www.draganfly.com/uav-helicopter/draganflyer-x4/index.php
[13] Digikey. (2009, September) Digikey Knowledge Base. [Online].
http://www.digi.com/support/kbase/kbaseresultdetl.jsp?kb=125
36
Appendices
Appendix 1: PID Stabilization and Movement Code
// number of intervals in the integral PID sum
#define TIME_INTS 10
// maximum value allowed to give to motors
int motormax=75;
int min_motor,max_motor;
int in,i;
// stops all power to motors when == 1
int kill=1;
// class to organize PID control
class PID
{
public:
double
kp_roll,kd_roll,kd_pitch,kp_pitch,ax,ay,az,kp_yaw,kd_yaw,yaw_change;
double dpitch,dyaw,droll,pitch,roll,pitch_change,roll_change,yaw;
double M1,M2,M3,M4,dp_c,dy_c,dr_c,mod;
double int_roll[TIME_INTS],int_pitch[TIME_INTS];
double int_pitch_sum,int_roll_sum,ki_roll,ki_pitch;
double mag1,mag2,cx,cy,cz,mod2,calt,alt;
PID(double kpr, double kpp, double kpy, double kdr, double kdp,
double kdy, double kip, double kir)
{
kp_roll = kpr;
kp_pitch = kpp;
kd_roll = kdr;
kd_pitch = kdp;
ki_roll = kir;
ki_pitch = kip;
kd_yaw = kdy;
kp_yaw = kpy;
// setting offsets of analog reads
dp_c=255;
dy_c=255;
dr_c=255;
cx = 340;
cy = 332;
cz = 363;
//2 on the fly variables that can be remotely modified
mod = 0;
mod2 = 0;
// initializing all intervals to 0 for integral gain sum
for(i=0;i<TIME_INTS;i++)
37
{
int_pitch[i] = 0;
int_roll[i] = 0;
}
}
// set all the offsets of the IMU to their value so that
// the resulting value given to the PID is 0.
// usually done in the start of the program when the
// flyer is level. the z axis of the accelerometer is
// not zeroed as it contains the value of gravity on it.
void zero()
{
dp_c = (float)analogRead(5);
dy_c = (float)analogRead(4);
dr_c = (float)analogRead(3);
cx = (float)analogRead(0);
cy = (float)analogRead(1);
}
// polls all the inputs and offsets them so they are meaningful
void poll()
{
// Gx4 gyroscope inputs (magnified input)
dpitch = (float)analogRead(4) - dp_c;
dyaw = (float)analogRead(3) - dy_c;
droll = (float)analogRead(5) - dr_c;
//
ax
ay
az
Accelerometer vector readings
= (float)analogRead(0) - cx;
= (float)analogRead(1) - cy;
= (float)analogRead(2) - cz;
// Magnetometer component readings
mag1 = (float)analogRead(7) - 491;
mag2 = (float)analogRead(6) - 491;
// Ultrasonic sensor reading (altitude)
alt = (float)analogRead(15)/2.0;
}
void calc()
{
// calculating pitch from the acceleration vector
pitch = atan(az/ax)*180.0/3.14159;
// offsetting so 0 is when quadrotor is level
if( pitch < 0 )
{
pitch = pitch+90;
}
else
{
38
pitch = pitch-90;
}
// calculating roll from acceleration vector
roll = atan(az/ay)*180.0/3.14159;
// offsetting so 0 is when quadrotor is level
if( roll < 0 )
{
roll = roll+90;
}
else
{
roll = roll-90;
}
// calculating yaw from magnetometer inputs
yaw = atan2(mag2,mag1)*180.0/3.14159 + 180;
// -90 = north
// setting integral gain interval to the current pitch and roll
int_pitch[i] = pitch;
int_roll[i] = roll;
// zeroing the integral gain sum
int_pitch_sum=0;
int_roll_sum=0;
// summing over all integral gain intervals
for(int j=0;j<TIME_INTS;j++)
{
int_pitch_sum += int_pitch[j];
int_roll_sum += int_roll[j];
}
// increase integral gain interval counter
i++;
// resetting integral gain interval counter if it goes over
if( i >= TIME_INTS )
i=0;
// fix yaw so discontenuity is in rear direction
if( yaw > 270 )
yaw -= 360;
// calculate yaw pitch and roll correction value
pitch_change = kp_pitch*(pitch) + kd_pitch*(dpitch) +
ki_pitch*int_pitch_sum;
roll_change = kp_roll*(roll) + kd_roll*(droll) +
ki_roll*int_roll_sum;
yaw_change = kp_yaw*(yaw-90.0) + kd_yaw*(dyaw);
}
39
void updatemotors()
{
// zero all motors
M1=M2=M3=M4=0;
// apply pitch correction
if( pitch_change > 0 )
{
M1 += pitch_change;
M4 += pitch_change;
}
else if( pitch_change <= 0 )
{
M2 -= pitch_change;
M3 -= pitch_change;
}
// apply roll correction
if( roll_change > 0 )
{
M2 += roll_change;
M4 += roll_change;
}
else if( roll_change <= 0 )
{
M1 -= roll_change;
M3 -= roll_change;
}
// applying yaw correction is slightly more complicated
// finding max and min of all motor values so far
min_motor = min(M1,min(M2,min(M3,M4)));
max_motor = max(M1,max(M2,max(M3,M4)));
if( yaw_change > 0 )
{
// at least make yaw = min motor if it can be
if(yaw_change > min_motor)
{
yaw_change = min_motor;
// if min motor is too small then just give yaw change 5
if(min_motor < 5 && yaw_change > 5)
yaw_change = 5;
}
// if yaw change is too big make it the maximum
if(yaw_change + max_motor > motormax)
yaw_change = motormax - max_motor;
// make yaw correction
M1 += yaw_change;
M2 += yaw_change;
40
M3 -= yaw_change;
M4 -= yaw_change;
}
else
{
// make yaw positive and do the same as before
yaw_change *= -1;
if(yaw_change > min_motor)
{
yaw_change = min_motor;
if(min_motor < 5 && yaw_change > 5)
yaw_change = 5;
}
if(yaw_change + max_motor > motormax)
yaw_change = motormax - max_motor;
// make yaw correction
M1
M2
M3
M4
-=
-=
+=
+=
yaw_change;
yaw_change;
yaw_change;
yaw_change;
}
// one of the real time modifyable variables for testing
M1+=mod2;
M2+=mod2;
M3+=mod2;
M4+=mod2;
// make sure motors do not exceed maximum
if( M1 > motormax
{
M1=motormax;
}
if( M2 > motormax
{
M2=motormax;
}
if( M3 > motormax
{
M3=motormax;
}
if( M4 > motormax
{
)
)
)
)
41
M4=motormax;
}
// make sure we dont get any negative values (nonsense)
if( M1
{
M1=0;
}
if( M2
{
M2=0;
}
if( M3
{
M3=0;
}
if( M4
{
M4=0;
}
< 0 )
< 0 )
< 0 )
< 0 )
// scale motors according to their relative efficiency
// that I guessed through testing
M2 = (int)((float)M2*1.2);
M3 = (int)((float)M3*1.4);
M4 = (int)((float)M4*1.6);
// if we hit the kill key then stop all motors
if( kill == 1 )
{
M1=0;
M2=0;
M3=0;
M4=0;
}
// finally actually output motor values
analogWrite(2,M1);
analogWrite(3,M2);
analogWrite(4,M3);
analogWrite(5,M4);
}
};
// initialize PID instance and set all constants
PID control(2.3,1.3,0.12,1.4,1.4,0.10,0.01,0.01);
void setup()
{
// initilize wireless serial connection
42
Serial3.begin(9600);
delay(500);
// zero sensors while its starting (hopefully level)
control.zero();
}
void loop()
{
// poll sensors
control.poll();
// calculate corrections
control.calc();
// update motors
control.updatemotors();
//real time testing section
if(Serial3.available())
{
in=Serial3.read();
Serial3.write(in);
if( in == 'a' )
{
// control.kp_yaw += 0.1;
//Serial3.println(control.kp_yaw);
control.kp_pitch += 0.1;
control.kp_roll += 0.1;
Serial3.println(control.kp_roll);
}
else if( in == 'z' )
{
// control.kd_yaw += 0.1;
// Serial3.println(control.kd_yaw);
control.kp_pitch -= 0.1;
control.kp_roll -= 0.1;
Serial3.println(control.kp_roll);
}
else if( in == 's' )
{
// control.kd_yaw -= 0.1;
// Serial3.println(control.kd_yaw);
control.kd_pitch += 0.1;
control.kd_roll += 0.1;
Serial3.println(control.kd_roll);
}
43
else if( in == 'x' )
{
// control.kp_yaw -= 0.1;
// Serial3.println(control.kp_yaw);
control.kd_pitch -= 0.1;
control.kd_roll -= 0.1;
Serial3.println(control.kp_roll);
}
else if( in == 'd' )
{
//control.ki_roll += 0.01;
//control.ki_yaw += 0.01;
//Serial3.println(control.ki_yaw);
}
else if( in == 'c' )
{
//control.ki_roll -= 0.01;
//control.ki_yaw -= 0.01;
//Serial3.println(control.ki_yaw);
}
else if( in == 'f' )
{
control.mod += 0.1;
Serial3.println(control.mod);
}
else if( in == 'v' )
{
control.mod -= 0.1;
Serial3.println(control.mod);
}
else if( in == 'g' )
{
control.mod2 += 2;
Serial3.println(control.mod2);
}
else if( in == 'b' )
{
control.mod2 -= 2;
Serial3.println(control.mod2);
}
44
// kill key = 'k'
else if( in == 'k' )
{
if( kill == 1 )
kill = 0;
else
kill = 1;
}
}
}
45
Appendix 2: Hover with Altitude and Yaw Compensation code
// maximum value allowed to give to motors
int motormax=75;
int min_motor,max_motor;
int in;
// stops all power to motors when == 1
int kill=1;
// class to organize PID control
class PID
{
public:
double kp_yaw,kd_yaw,yaw_change;
double dyaw,yaw,alt_change,kp_alt,kd_alt;
double M1,M2,M3,M4,dy_c,mod,alterror;
double mag1,mag2,alt,dalt,lastalt,setalt;
double altset[51];
int alt_disable,bigchange;
PID(double
{
kd_yaw =
kp_yaw =
kp_alt =
kd_alt =
kpy, double kdy, double kpa, double kda)
kdy;
kpy;
kpa;
kda;
// setting offsets of analog reads
dy_c=255;
// number of large changes in altitude measurement
bigchange=0;
// size of a 'large' change in altitude
alterror = 50;
}
// set all the offsets of the IMU to their value so that
// the resulting value given to the PID is 0.
// usually done in the start of the program when the
// flyer is level.
void zero()
{
dy_c = (float)analogRead(4);
// find a non zero and non large value for the altitude reading
// upon program start ( you wont start it 10ft in the air )
do
{
lastalt = (float)analogRead(15);
}while(lastalt > 100 || lastalt == 0);
46
alt = lastalt;
setalt = alt;
}
// polls all the inputs and offsets them so they are meaningful
void poll()
{
// Gx4 gyroscope inputs (magnified input)
dyaw = (float)analogRead(3) - dy_c;
// Magnetometer component readings
mag1 = (float)analogRead(7) - 491;
mag2 = (float)analogRead(6) - 491;
// Ultrasonic sensor reading (altitude)
getAlt();
}
void getAlt()
{
int j=0;
int k=0;
double diff,adiff;
double altsum=0;
// try 500 times to get 50 good readings
while(j<500 && k<50)
{
altset[k] = analogRead(15);
// if its a bad reading then just use old values
if( k == 0 )
diff = altset[k]-lastalt;
else
diff = altset[k]-altset[k-1];
// fix difference so its absolute value
if( diff < 0 )
adiff = diff*-1;
// if difference is small enough then keep reading
if( adiff < alterror )
{
altsum += altset[k];
k++;
}
j++;
}
// if we looped too many times it must be failing to read so
47
// so ignore the next value that is read
if( j > 99)
{
alt_disable=1;
alt = lastalt;
}
// calculate average of good readings
alt = (float)altsum/(float)k;
// check if new result is off by too much since last time
diff = alt - lastalt;
if(diff < 0 )
diff *= -1;
// if its off by too much its a 'big change' and it should
// be ignored and we should use the last good value
if( diff > alterror )
{
alt_disable=1;
alt = lastalt;
bigchange++;
}
// after 50 big changes it must be stuck somewhere so we will
// reset the initial values of the sensor and try again
if( bigchange > 50 )
{
bigchange=0;
do
{
lastalt = (float)analogRead(15);
}while(lastalt > 150 || lastalt == 0);
alt = lastalt;
setalt = alt;
}
}
void calc()
{
// calculating yaw from magnetometer inputs
yaw = atan2(mag2,mag1)*180.0/3.14159 + 180;
// -90 = north
// fix yaw so discontenuity is in rear direction
if( yaw > 270 )
yaw -= 360;
// calculate yaw pitch and roll correction value
48
dalt = alt - lastalt;
lastalt = alt;
yaw_change = kp_yaw*(yaw-90.0) + kd_yaw*(dyaw);
alt_change = kp_alt*(setalt-alt) - kd_alt*(dalt);
// if we chose to ignore the reading we can just set it to zero
if(alt_disable == 1)
{
alt_disable=0;
alt_change=0;
}
else
{
//Serial3.print("Alt: ");
//Serial3.println(alt);
}
}
void updatemotors()
{
// zero all motors
M1=M2=M3=M4=0;
// constant offset helps with stability
M1=40+alt_change;
M2=40+alt_change;
M3=40+alt_change;
M4=40+alt_change;
// applying yaw change
// finding max and min of all motor values so far
min_motor = min(M1,min(M2,min(M3,M4)));
max_motor = max(M1,max(M2,max(M3,M4)));
if( yaw_change > 0 )
{
// at least make yaw = min motor if it can be
if(yaw_change > min_motor)
{
yaw_change = min_motor;
// if min motor is too small then just give yaw change 5
if(min_motor < 5)
yaw_change = 5;
}
// if yaw change is too big make it the maximum
if(yaw_change + max_motor > motormax)
yaw_change = motormax - max_motor;
// make yaw correction
49
M1
M2
M3
M4
+=
+=
-=
-=
yaw_change;
yaw_change;
yaw_change;
yaw_change;
}
else
{
// make yaw positive and do the same as before
yaw_change *= -1;
if(yaw_change > min_motor)
{
yaw_change = min_motor;
if(min_motor < 5)
yaw_change = 5;
}
if(yaw_change + max_motor > motormax)
yaw_change = motormax - max_motor;
// make yaw correction
M1
M2
M3
M4
-=
-=
+=
+=
yaw_change;
yaw_change;
yaw_change;
yaw_change;
}
// one of the real time modifyable variables for testing
//M1+=mod;
//M2+=mod;
//M3+=mod;
//M4+=mod;
// make sure motors do not exceed maximum
if( M1 > motormax )
{
M1=motormax;
}
if( M2 > motormax )
{
M2=motormax;
}
if( M3 > motormax )
{
M3=motormax;
}
50
if( M4 > motormax )
{
M4=motormax;
}
// make sure we dont get any negative values (nonsense)
if( M1
{
M1=0;
}
if( M2
{
M2=0;
}
if( M3
{
M3=0;
}
if( M4
{
M4=0;
}
< 0 )
< 0 )
< 0 )
< 0 )
// scale motors according to their relative efficiency
// that I guessed through testing
M2 = (int)((float)M2*1.2);
M3 = (int)((float)M3*1.4);
M4 = (int)((float)M4*1.6);
// if we hit the kill key then stop all motors
if( kill == 1 )
{
M1=0;
M2=0;
M3=0;
M4=0;
}
// finally actually output motor values
analogWrite(2,M1);
analogWrite(3,M2);
analogWrite(4,M3);
analogWrite(5,M4);
}
};
// initialize PID instance and set all constants
PID control(0.0,0.0,0.0,0.0);
void setup()
51
{
// initilize wireless serial connection
Serial3.begin(9600);
delay(500);
// zero sensors while its starting (hopefully level)
control.zero();
}
void loop()
{
// poll sensors
control.poll();
// calculate corrections
control.calc();
// update motors
control.updatemotors();
//real time testing section
if(Serial3.available())
{
in=Serial3.read();
Serial3.write(in);
if( in == 'a' )
{
control.kp_yaw += 0.01;
Serial3.println(control.kp_yaw);
}
else if( in == 'z' )
{
control.kp_yaw -= 0.01;
Serial3.println(control.kp_yaw);
}
else if( in == 's' )
{
control.kd_yaw += 0.01;
Serial3.println(control.kd_yaw);
}
else if( in == 'x' )
{
control.kd_yaw -= 0.01;
Serial3.println(control.kd_yaw);
}
else if( in == 'd' )
{
control.kp_alt += 0.1;
Serial3.println(control.kp_alt);
}
else if( in == 'c' )
{
control.kp_alt -= 0.1;
52
Serial3.println(control.kp_alt);
}
else if( in == 'f' )
{
control.kd_alt += 0.1;
Serial3.println(control.kd_alt);
}
else if( in == 'v' )
{
control.kd_alt -= 0.1;
Serial3.println(control.kd_alt);
}
else if( in == 'g' )
{
control.mod += 2;
control.setalt = control.mod;
Serial3.print("Alt: ");
Serial3.println(control.alt);
Serial3.print("Set: ");
Serial3.println(control.mod);
}
else if( in == 'b' )
{
control.mod -= 2;
control.setalt = control.mod;
Serial3.print("Alt: ");
Serial3.println(control.alt);
Serial3.print("Set: ");
Serial3.println(control.mod);
}
else if( in == 'h' )
{
control.alterror += 2;
Serial3.println(control.setalt);
}
else if( in == 'n' )
{
control.alterror -= 2;
Serial3.println(control.setalt);
}
// kill key = 'k'
else if( in == 'k' )
{
if( kill == 1 )
kill = 0;
else
53
kill = 1;
}
}
}
54