University of Florida

FlameWheel UAV

On the development of a controller for controlled and autonomous flight.

Matt Thompson

12/4/2012

EEL4665Dr. Arroyo

Dr. SchwartzJosh WeaverTim Martin

Table of ContentsAbstract............................................................................................................................................3Executive Summary.........................................................................................................................3Introduction......................................................................................................................................4Integrated System............................................................................................................................4Sensors and Filtering.......................................................................................................................5Behaviors and Control.....................................................................................................................7Experimental Results.......................................................................................................................8Conclusion.....................................................................................................................................10Appendix A: References................................................................................................................11Appendix B: Code.........................................................................................................................11

2

AbstractThis project’s goal was the creation of a custom, high performance quadcopter controller board capable of autonomous flight using an ARM processor. After only one semesters work, the board demonstrates basic autonomous behavior such as stabilizing the vehicle and maintaining altitude. Although the board is capable of achieving its more ambitious goals, the timeline ultimately did not allow for the requisite software to be fully developed and debugged. Still, a great deal of experience and expertise with quadrotors and their controls was developed, as well as applying indirect Kalman filters to estimate attitude in a computationally constrained and noisy environments. The board and its software can ultimately be seen as ahead of the state of art in terms of potential capabilities, with many open source groups including the ArduPilot looking at using a similar ARM chips for their next generation flight controller, which is still in its preliminary design stages.

Executive SummaryThis robot’s ultimate goal is to serve as a proof of concept for transport and deployment capabilities of autonomous air vehicles. It will ultimately be capable of autonomously taking off, traveling a series of waypoints, deploying a payload at each before finally returning home and autonomously landing. These goals are achieved by creating a custom, high performance flight controller, controlling an inexpensive but robust hobby quadcopter frame, the DJI Flame Wheel.

Ultimately, the UAV did not progress quickly enough to achieve high level autonomous behavior, as too much time had to be spent implementing lower level building blocks. Just stabilizing manual flight of the vehicle requires device drivers for all the sensors, working state estimator, tuned controller, base station logging software, and high level flight initialization and control code. All of these fundamentals were implemented from scratch and tested. Time did allow for the first stage of autonomous behavior, altitude holding, to be implemented in a limited fashion.

The design of the flight controller was the first major component completed during the six weeks of the semester. The main goal of the board was to mate a powerful processor with a sensor suite to enable the quadcopter to fly. The STM32F405 ARM CPU was selected, as it features a high clock rate of 168 MHz while still not requiring external memory, thus keeping part count down. The MPU6000 IMU was selected for the sensor suite, as it contains three accelerometers and three gyroscopes, again keeping part count down. Other parts on the board include an altimeter and numerous IO for external devices on the quad.

The next major development was the state estimator, which was completed around week 10 of the semester. The design chosen was based on literature, and makes use of an Extended Kalman Filter, which requires large amounts of memory and arithmetic to run efficiently. This filter proved to be a good test of the capabilities of the CPU, as well as plenty capable of tracking the orientation of the vehicle with proper tuning. Before this filter could be written, a small linear algebra library was written from scratch with low memory, embedded environment in mind.

The final development in the project was the controller to allow the quadcopter to be stabilized under manual flight. This controller was not fully completed until the day before media day, with adjustments to gains and the overall control scheme still yielding improvements in flight performance until the very last minute. The controller was also given a degree of basic autonomy

3

to hold altitude. In this mode, the quad is autonomously controlling its own throttle, while the pilot still provides directional control.

IntroductionMy interest in creating a quadcopter actually began my senior year of high school, where Will Rolke and I attempted first attempted to design one from the ground up. We got relatively far considering how little we actually knew at the time, creating a wooden frame, purchasing motors and ESCs, and designing a protoboard circuit using SparkFun breakouts. It ultimately never had a successful flight, but taught us a lot about robotics, embedded software, and control.

This project represents my second attempt at a quadrotor. This time, the goal was even more ambitious, a fully autonomous flight of a proof of concept application. The quad would take off with multiple payloads on board, representing valuables such as supplies, intelligence, or personnel. The quad would then travel to a sequence of waypoints, descending to deploy a payload at each before returning to the start location and landing.

Unfortunately, time constraints necessitated that the project simplify to achieving stable flight and altitude holding capabilities. This paper will describe both what was actually achieved, as well as the plans for fully autonomous behavior that weren’t completely realized in time. It will begin by first describing the capabilities of the PCBs that were custom-designed for the project. Then, the sensors and their associated filtering, the largest and most difficult part of the project, will be discussed in detail, followed by the controls and behaviors that enable flight. Finally, it will cover experimental results and the conclusion of the project.

Integrated System

Figure 1: Block diagram of FlameWheel

To achieve its goals, the UAV contains the components shown in Figure 1, namely two PCBs, the flight controller and the Mag/GPS board, supplemented by motor controllers, sonars, and XBee radios, along with a LiPo battery to supply power. As quadcopters require constant in flight adjustments to its motors in order to remain stable, most of the sensors and processing time on the flight controller is spent tracking the attitude of the quad, then making necessary corrections

4

to keep the quad at the desired orientation. For now, the desired orientation comes from an R/C remote, but the ultimate goal is to calculate the desired orientation from the GPS, giving fully autonomous flight.

Figure 2: The Flight Controller (Left) and Mag/GPS Board (Right)

The Flight Controller, seen close up in Figure 2, houses the STM32F405, a Cortex M4 microcontroller from ST. It runs at 168 MHz and features a single-precision FPU, which allows it to performing floating point operations at high speed. It also contains two important sensors, the MPU6000 which provides a three axis gyroscope and accelerometer, as well as the BMP085 altimeter, which gives high resolution pressure measurements which can be converted to an absolute height above sea level. The Flight Controller board features connectors to other boards, including 4 UARTs to communicate with the RC receiver, GPS, XBee, and debugging PC. Finally, the flight controller contains the necessary PWM IO pins to drive up to six ESCs, two servos, and one sonar.

The Mag/GPS board simply houses and powers the magnetometer and GPS. It was separated from the flight controller to allow the magnetometer to be moved farther from the motors if significant magnetic interference problems appeared. Ultimately, this separation ended up not significantly improving magnetic interference, which was mostly solved in software, so later revisions of the Flight Controller will feature on board magnetometer and GPS connections.

Sensors and FilteringThe primary sensor used to stabilize the Quadcopter is the three axis gyroscope contained in the MPU6000. These gyroscopes give a very accurate measurement of the angular rate of the quad, even in the presence of vibrations. These sensors are used to integrate the quaternion differential equation at 1 kHz, yielding the attitude of the quaternion as a rotation from its initial orientation, forming the inertial navigation system (INS).

This approach alone yields three main problems. The first is that all the INS knows is the orientation of the quad relative to its initial orientation, which is different from the orientation of the quad relative to the ground. The second problem is that as an integrator, the INS accumulates noise, scale error, sensor nonlinearity, and gradually drifts away from the correct orientation. Finally, gyroscopes contain a small but substantial offset, causing a stationary gyroscope to

5

report motion on the order of a few tenths of a degree per second. This offset causes the INS to drift rather quickly about a specific axis. Worse yet, over time and especially after changes in temperature, these biases can shift, growing, shrinking, or even changing sign.

The initial orientation problem is fortunately easily solved by using the accelerometer and magnetometer. These two sensors give very accurate measurements of the earth’s magnetic and gravitational fields while the quad is stationary and the motors are off. These vector measurements are averaged for one second before being used in the TRIAD algorithm to give a highly accurate estimate of the quad’s initial orientation relative to the earth. Since the quadcopter is known to be stationary during this process, gyroscope readings are also averaged to give an accurate estimate of the gyro bias.

With an accurate initial orientation and bias estimate, the INS can proceed for upwards of 30 seconds to a minute before accumulating a degree of error. But due to the non-linear nature of orientation, the error compounds itself particularly while the quad is in motion, and the second degree of error appears in less than a minute, with the third coming even sooner. This error growth is made even faster if the gyro biases were to shift after the initial measurement, which is quite common due to the IC’s self-heating.

Figure 3: Overview of Indirect Kalman Filter

Clearly this is unacceptable for stable flight. To keep errors bounded, an extended Kalman filter is used, based on the work described in [1] and shown in Figure 3, which compares the accelerometers and magnetometers against the INS and attempts to track this non-linear error growth at 100Hz. As it tracks this error, it determines if the error is part of the random error added by sensor noise in integration, or a constant error added by a shift in the gyro biases. Every second, the INS’s attitude and gyro biases are corrected using the estimated errors from the Kalman filter. Then the Kalman filter is re-initialized to zero error, so the process can begin anew.

The sonar, altimeter, accelerometer, and GPS sensors each give a way of estimating the height of the quad. If all four sensors were operational, this altitude estimation problem would be solved elegantly by a second linear Kalman filter, tracking the altitude and vertical speed of the quad, as well as the height of the ground below the quad when it is in view of the sonar. Unfortunately

6

due to time constraints the altimeter and GPS weren’t fully debugged and this second Kalman Filter was never fully developed and simulated.

The altitude estimator currently in use on the quadcopter uses only the sonar. It filters the sonar with a median filter to eliminate most noisy and false returns, and then scales this distance to an altitude above the ground. This signal is then differentiated and low-pass filtered to estimate the velocity of the quadcopter. Because the sonar cannot measure the absolute height of the quad and instead only measures the height above the ground to a maximum of 20 feet, it will work best with the quad relatively low to the ground in a flat area with no objects to trigger false returns on the sonars.

Behaviors and ControlThe basic level of control that is always running on the flight controller is attitude control. This tracking controller, shown in Figure 5, attempts to physically move the quad to the desired attitude and angular rate by making minute adjustments to the speeds of the motors. It does so by computing the error quaternion between the desired and actual orientations, then forms the P term of the controller by converting the error quaternion to a rotation about x, y, and z. The I term is computed by integrating this proportional error. Finally, the D term comes from the error between the desired rate and the actual rate of the quad according to the gyros.

Figure 4: Attitude Control Flow

These three terms are summed to produce the final control signal before being passed to the motor mapper, which performs a linear mapping of the control signal to small adjustments to the four rotors. Roll and Pitch are controlled in an intuitive manner, by slightly speeding up and slowing down opposite pairs of rotors to create torque. Yaw, demonstrated in Figure 6, is controlled by first making the front right and rear left motors spin counter clockwise. Yaw torque can then be generated by accelerating one diagonal pair and decelerating the other. The counter torques of these motors will now slightly differ, causing the quad copter to yaw in the opposite direction of the faster pair of motors.

The second and final control behavior that was implemented in the quad copter in the time allotted is an altitude controller. This controller attempts to track a desired altitude and vertical velocity using an additional PID controller. The output of this PID controller is the throttle of the motor mapper, which then

7

Desired Attitude

PID Controller

Motor Mapper

Figure 5: Torques generated by quadcopter

applies the relative thrust offsets from the attitude controller on top of this signal. Tuning this controller to give good results is difficult due to the suboptimal sonar-based altitude controller.

Figure 6: High level control scheme

Time permitting; a fully autonomous flight behavior would’ve been created by using the GPS to implement a position controller shown in Figure 7. The position controller would use the GPS position to determine a desired heading. This heading would be passed to a velocity controller which would roll and pitch the vehicle to manipulate the downward thrust into lateral motion much like a human pilot would. This desired orientation would then be passed to the existing attitude controller. The quad would traverse each waypoint in order, optionally activating payload mechanisms or cameras at each point. Autonomous takeoff and landing would be used to begin and end each mission, consisting of a fixed vertical trajectory fed to the altitude controller.

An additional behavior which always runs in the background of the flight controller is base station communication. At 20 Hz, vital statistics on the quadcopter including Kalman filter states, outputs of every term in the attitude controller, current gains, and battery life are broadcast in a single XBee API mode packet to be received by the base station software running on the PC. This software is written in Python and uses the Gtk3 and Cairo libraries to render a graphical display of the data coming from the quadcopter.

Experimental ResultsThe quadcopter sustains stable flight, and is not prone to significant drifting or leaning, enabling it to be flown precisely in constrained areas or freely in open ones. The vehicle and sensors picked up far more vibrations than initially anticipated, which did ultimately require the filter and controls to be made looser and less precise, but in flight the quadcopter still appears assertive and in control, even when altitude hold is enabled and the pilot stops providing control input.

8

Position GPS

Velocity GPS

Attitude INS

Magnetic disturbances from the motors were surprisingly not a significant contribution, when compared to soft and hard iron effects from the motors and the frame, as well as misalignment in the magnetometer itself. Once soft, hard, and misalignment calibrations were performed using the sphere fitting technique borrowed from SubjuGator, the motors do not contribute enough magnetic field to introduce more than a degree of error, which is ultimately lost in other sources of error.

Another result obtained was that it takes very little changes in commanded velocity to the ESCs to produce large amounts of torque on the quad. It does, however, take a relatively long time for the ESCs to accelerate or decelerate the motor, due to the inertia of the motor and prop system in Figure 8. This means that the ideal PID tuning is actually to use as low of gains as possible, to give a very gentle control response that is still robust to wind and keeps the quad level, as a response that is too strong will oscillate out of control due to the actuator delay.

If state estimation is working properly, having the gains well below their ideal values will still give a flyable platform, it will just require more human intervention to keep in place. Conversely, gains that are even slightly too high will yield a vehicle which may oscillate at the slightest disturbance. Gains that are off by even a factor of 1.5 could at any moment go into out of control swinging motions, which can be startlingly powerful and keep the quad in the air well below the takeoff throttle by simply throwing the quad from left to right but higher and higher in the air. Therefore, it is definitely best to start with very low gains and slowly increase them until the vehicle is easy to control.

The minimum speed that an ESC can spin a motor at is also substantial, particularly with no payload on the vehicle. During moderate to high speed descents, the controller hits these saturation limits and therefor suffers from reduced control effort which manifests as small but persistent oscillations through the duration of the descent. This exact phenomenon was observed on this frame when controlled by an ArduPilot, so it is a tricky but common problem, with the easiest solution probably to simply increase the weight of the quad and thus allow descents to use higher RPMs.

9

Figure 7: Motor with Propeller

Conclusion

Figure 8: The fully assembled FlameWheel

At the end of the semester, the quadcopter, shown fully assembled in Figure 9, has unfortunately not reached its ultimate goal of fully autonomous flight. This came about due to several reasons, the largest one being that I simply could not find enough time to get that much functionality debugged and tested. With a vehicle this expensive that could easily suffer catastrophic failure and be put weeks behind schedule due to even a small software mistake, extensive bench testing and code review was required to double and triple check that nothing would go wrong for an actual flight test. Since once in flight means to debug the vehicle are quite limited, low level components like sensor drivers and filtering received extra attention, as problems here would merely manifest themselves as unreliable or faulty flights. Even with all of this additional effort, there is still at least one bug that sometimes causes the quadcopter to fail its pre-flight checks that I have yet to reproduce on the bench. I have achieved an excellent reliability once in flight however, as the quadcopter has never crashed due to a software failure, only poor choice of gains during the first few flight tests.

The vehicle also serves as a very extensible platform for future work. The quadcopter presently takes off at around 40% throttle and flies for close to 20 minutes, so there is plenty of headroom for additional payloads. In software, the STM32F405 has taken everything the project has thrown at it without batting an eye. Extensive benchmarking was never done because it never had to be done, however the code at the present consumes less than 10% of the CPU which can be measured by performing spot checks on the debugger, which reveals on most checks that the CPU is waiting for the next RTOS task. One exciting possibility this allows would be the implementation of a full position indirect Kalman filter, similar to one used on SubjuGator. This could give vastly improved position holding and trajectory tracking capabilities over similar cost boards of lower computational capability by making use of the accelerometers to track the motion of the vehicle, corrected over time using the GPS.

10

Appendix A: References[1] Young Soo Suh , "Orientation Estimation Using a Quaternion-Based Indirect Kalman Filter With Adaptive Estimation of External Acceleration," Instrumentation and Measurement, IEEE Transactions on , vol.59, no.12, pp.3296-3305, Dec. 2010

Appendix B: CodeAll available on GitHub: http://www.github.com/matthewbot/FlamewheelFC

11