Quadcopter Project- Part 3 – The Math!

Prelude

I started to build a quadcopter from scratch and in the previous posts, I covered making the frame from scrap wood and bought the motors and ESC. I also tried to power the thing up with a basic output but that did not work.

In this post, I explain the theory behind the quadcopter brain.

The Motion of the Quadcopter

We have all seen videos of these machines flying through air and doing all kinds of stuff. We need to understand how the quad does all that because in the case of cars, we can make the vehicle move by rotating the wheels and make it stop by cutting the voltage off. Friction does the job for us in the case of the wheel but in the case of an aerial vehicle, how do you stop the quad? Lets understand the basic motion of the quad.

The quadcopter has four motors and propellers which generate thrust which can make it go up or down. A combination of variable power to these will make the movement possible.

f69b833919c9c3b7b31c6f739fe530a0.jpg

The above diagram shows the lingo associated with aerial vehicles. The Roll, pitch and yaw are the three possible motions of any quad etc and we will need sensors to accurately tell us these three parameters of our quad in air. If we can keep the Yaw, Pitch and Roll fixed, then that means the quad is stable in air right? Well yes but thats just half the story. Imagine a quadcopter which is perfectly oriented in mid air and a gust of wind blows it away in the same orientation, then we need to know about the motion of the quadcopter in the air. A GPS cannot track that accurately but we can use a little math and some sensor data to estimate the movement.

We understand that if all the four rotors generate thrust, the quad goes up and if thrust decreases, gravity does its job and brings it down.

d2f3114899699dfb78a2d0f474db848e.jpg fe6816ee3356a156acd633a55fb5b9b6.jpg

If two rotors on the same side, increase thrust(or decrease if), the quad can roll and if the rotors on the back or front do that, then it can pitch forward or backward as shown in the image above. This is simple physics. In addition to this rolling and pitching movement, the quad will also drift like for example, in the case of the roll, when rolling right, the quad will drift or side strafe right because the thrust on when the quad is tilted, there is a little thrust going sideways was well(see vector math for more details). Similarly, when pitching forward or backward, the quad will move forward or backward. Neat! so we get some basic control. Unfortunately, there is friction and in order to stop, we don’t have a break. So when we want the quad to stop drifting, we need to tilt it in the OPPOSITE directions for a SMALL duration till it comes to a halt. So how long is small? Well depends on the weight of the quad(inertia) and speed(momentum) and this can vary. Instead of doing this manually, the first thing we want to get done on the quad is to make it stop! Sounds counter productive but the truth is that if you can make a quad hover in a fixed location, that means it’s a good machine. We need a brain to do all that for us and you can prolly buy one but since this series is “building a quad from scratch, we will make a brain of our own. Later on we will add more and more functionality as we see fit. Awesomeness here we come!

Reverse Engineering the Brain!

Before we start buying sensors and controllers, we need to understand the requirement of our quadcopter. We understand that the propellers will be responsible for the movement and we can control them using the ESCs that we bought out but the brain is what makes everything ticks and it will fire commands to the propellers. Since we can only program the controller using a programming language, we need a way of representing our entire system mathematically in the form of equations which can be later converted into statements of a programming language. The language will be C and our initial work will be done independent of a particular controller.

Lets dive into a bit of rigid body math…

Reference Systems reverse engineered

Our quadcopter is a fixed(almost) body which is expected to float in mid air. This is termed as a rigid body in free space and the Yaw, Pitch and Roll needs to be quantified. Thanks to mathematicians, we already have a number of ways we can describe our quad in air and they are discussed briefly in the proceeding sections. There are two reference systems that need to be discussed.

One is the fixed body frame where the rotor’s body is taken as the reference and all things happen according to it’s orgientation in space and the other is the North East Down where our earth’s geographics are taken as the reference. Lets try to understand this a little more.

For the quadcopter that is flying in air or any other object, the force of gravity is the only external force that is constant. Hence we can use it to tell which way is up and which way is down. For someone standing on the ground, the quadcopter is a moving and the ground is fixed. Alternatively, for the quadcopter, it may consider the ground to be moving and itself to be fixed. i.e. The motion of a body can only be described relative to something else… Complicated? To simplify this, we use the inertial frame of reference which takes the Acceleration Due to Gravity as a fixed reference. Simply put gravity tells us which way is down. Now where is right left? This is not absolute since there is no gravity on any side and we normally do not need it to orient the quad in a particular North facing directions. If we do, then well the simple answer is use a compass! Ships use a compass to navigate so why not our quad and hence we use an electronic compass to tell us north south east and west. We do need to find out if the quad is drifting and we can estimate that later. Hence we have our three dimensional space all figured out. But we need to find a way to represent this in code…

A bit of math

1. Euler Angles:

From Wikipedia,”The Euler angles are three angles introduced by Leonhard Euler to describe the orientation of a rigid body.[1] To describe such an orientation in 3-dimensional Euclidean space three parameters are required. They can be given in several ways, Euler angles being one of them; see charts on SO(3) for others. Euler angles are also used to describe the orientation of a frame of reference (typically, a coordinate system or basis) relative to another. They are typically denoted as α, β, γ, or φ, θ, ψ.”

fc3f4bad31a4fe1705b795bb190c67a2.jpg

fb8af270ca546d83c9bdd6d8ef9b22c8.jpg

Simply put if we put the quad is at rest and at the center of our assumed origin, then if there is any change in Roll, Pitch or Yaw, then it can be signified using one of the three angles. As seen in the image above, if there is any roll, then phi will change. If there is any pitching, then the angle theta will change and for yaw, there is a change in xi. The sensors can be used to fill in these values. They should measure these angles and we can have a good idea of what the situation of the quad in air. Then if we want to move forward, then we speed up the back rotors such that the theta changes to the appropriate value and so on and so forth.

e2a9196f917e1a75386dcb65ce11a5ca.jpg

The same way, we can describe euler’s angles for a the quadcotper in free space taking any point on the ground as the reference.

13e9c75886c65d16520e2ff4f0eab377.jpg

2. Orientation vector

From Wikipedia,”Euler also realized that the composition of two rotations is equivalent to a single rotation about a different fixed axis (Euler’s rotation theorem). Therefore the composition of the former three angles has to be equal to only one rotation, whose axis was complicated to calculate until matrices were developed.

Based on this fact he introduced a vectorial way to describe any rotation, with a vector on the rotation axis and module equal to the value of the angle. Therefore any orientation can be represented by a rotation vector (also called Euler vector) that leads to it from the reference frame. When used to represent an orientation, the rotation vector is commonly called orientation vector, or attitude vector.”

3. Orientation Matrix

In vector math, its easy to divide a single vector into two orthogonal vector and vice verse and if you are reading this you prolly know that. When we have to deal with rotating bodies, we need a way to decompose the motion of a body in freespace in terms of angles.(In our case, the roll pitch and yaw). Just like vectors, an arbitrary rotation in 3D Space can be described by two angles theta and phi and the position and motion in 3D space can be described by these two angles and a magnitude. The simplest video explaining this concept and rotational matrices, that I could find is given below.

3. Quaternions or Versors

From wikipedia,”In mathematics, the quaternions are a number system that extends the complex numbers. They were first described by Irish mathematician William Rowan Hamilton in 1843[1][2] and applied to mechanics in three-dimensional space. A feature of quaternions is that multiplication of two quaternions is noncommutative. Hamilton defined a quaternion as the quotient of two directed lines in a three-dimensional space[3] or equivalently as the quotient of two vectors.[4]”

I could not isolate a single video to explain quaternions to the masses so I just might do one in the future. The basic principle is to represent orientation and rotation of any object in 3D Space. According to Euler’s rotational theorm, any rotation(around a fixed point, can be represented by a given angle theta and the axis of rotation given by a unit vector. The image below shows the same.

bf42c9b931842c09217287430b8d6297.jpg

Quaternions give a way to ‘encode’ this information in four number. Say I have a point in 3D Space given by (ax,by,cz). A vector from the origin to this point can be written (inEulicidian Space) as p = (ax)i + (by)j + (cz)k where i,j,k are unit vectors orthogonal to each other and representing the Cartesian axes. Say we rotate this through an angle theta around a vector u’ = (ux) i + (uy)j + (yz)k. Then this can be represented by the quaternion as

q = cos(theta/2) + (u’)sin(theta/2) or

q = cos(theta/2) + ()sin(theta/2)(ux) i + (uy)j + (yz)k

This means we can plug in the angle or rotation and the unit vector into the above equation to get a quaternion for rotation. The rotation itself happens if p is the original vector(as described above), q is the quaternion describing the rotation and the vector after rotation is

p’ = qp(q^-1) using Hamilton product

OK. This is getting thick so I will leave it here for now.

The point I am trying to make with the above math is that we need a system of how to represent the orientation of our quad in space with respect to itself and the earth.

We will come to this again once we have a better idea of what exactly we can use from all this.

The AHRS what?

We need our electronic brain to manage the quadcopter and we just went through the mathematical part. In the real world, we can now start talking about the AHRS which stands for attitude and heading reference system.

Attitude? Don’t be scared, Attitude control is controlling the orientation of an object with respect to an inertial frame of reference. Its the orientation with respect to say the earth. Heading is the direction where the quadcopter is going and the AHRS manages to use sensors to calculate the required information.

It’s time!

So the first task we are going to do is to select a microcontroller platform and the sensors. There are lots of projects out there who have used the Atmega328P and arduino as the core and it seems sufficient. However, since my experiments are from scratch and I want to learn to make this thing from scratch I will write my code almost platform Independent. Easier said than done but you will see me do that in the upcoming segments.

Since I am going to be experimenting, I will start mine with both and Arduino Uno as well as the Freescale Freedom K64F board from the IBM IoT Kit. The reason I have chosen these two is because of their differences. The arduino is 8-bit and has less power BUT is more community supported. On the other side, the K64F is more powerful, has an FPU which means I can do floating point math very easily while doing a lot of stuff in parallel. Both are “Arduino Compatible and I will be doing some shield PCBs in the future so their interchangeability will help.

The Freedom to do

The Freedom K64F is an (almost) arduino compatible board with a lot more power and a LOT more IOs and capabilities. It comes with ethernet, USB host and SD Card slot along with the FXOS8700CQ which is a 3 axis accelerometer and a 3 axis magnetometer. I will be doing some demos on this sensor but will prolly move to a different one for the final system.

xk64f_page2.jpg.pagespeed.ic.-N9pOX1f7f.jpg

The image above shows the pinout and there is no need to download any software since we will be using the online compiler! Yes! There is an online compiler which allows you to write the code online, compile it online and downloads the binary to your PC. You just copy the file into your MBED device which shows up as a pendrive and it does the rest! Brilliant. There is an offline option as well but I won’t go into that in the near future.

The Kit I received has two boards-

1. The K64F Board and

2. A Board with an LCD, Speaker, RGB LED, POTS, Joystick and an XBEE Socket.

I plan to use the shield for my ground station where the joystick is used to control the motion and the pots the altitude. Fingers crossed.

FRDM K64F

FRDM K64F

Conclusion

In this post I have tried to dive into the mathematical part and gotten an overview so that when I start coding, I know what all is out there.

In the next post, I will finally start the AHRS with an IMU and demonstrate how we can obtain the orientation of our quad in air. Till then

Cheers,

IP

Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s