Based On AR - Weeblymzfrank.weebly.com/uploads/1/3/9/6/13967377/quadrotor...Quadrotor Control Project Final Year Project of GAO Mingze (No.2011955028) Control of the quadrotor motion

Quadrotor Control Project Based On AR.Drone

Supervisor: Dr M.ZQ.Chen

U.ID : 2011955028

GAO Mingze, Frank

Mar 13， 2013

Quadrotor Control Project

Final Year Project of GAO Mingze (No.2011955028)

1, Background Introduction 2, Theoretical basis 2.1 Quadrotor Dynamics 2.2 Controller Design & Simulation 3, Platform Construction 3.1 OS transplantation 3.2 Sensing-Tech 4, On-Going Focus 5, Future Plan 6, Q&A 7, Reference Slides for Q&A

Outline:

Background Introduction Quadrotor Control Project


Quadrotor: Classified as rotorcraft which is lifted and propelled by four rotors.



Control of the quadrotor motion is achieved by altering rotation rate of one or more rotor propellers, thereby changing its torque load and thrust/lift characteristics. In this FYP, my concentration is mainly placed on position control

Background Introduction

To achieve the goal of a precision flight attitude control on quadrotor, we need to build the dynamic model.



Theoretical Basis

Quadrotor Dynamic Model

Basic theory of the optimization control on quadrotor vehicle

Assumptions: Quadrotor body is rigid

Propellers are rigid

Design is symmetrical



Quadrotor Dynamics

Frame of reference:

: an body fixed frame with mutally orthogonal axes x,y,z with unit vector and i,j and k.

E: earth fixed frame



Motor Position:

Quadrotor Dynamics

Pitch, Roll and Yaw Angle



Euler angles of the body axes are {e1, e2, e3} with respect to the body-Frame axes, respectively, and are referred to as pitch, roll and yaw. { φ, θ, ψ }

Quadrotor Dynamics

Forces acting on Quadrotor



The center of mass is located at the origin of the -Frame. The imbalance of the force F results in moment, along the direction perpendicular to the plane formed by force F and the vector L

Quadrotor Dynamics

Inertia Moment

In derivation of moment of inertia along x (and y) axis, the following assumptions are made:

1, Motors M1 and M3 are cylindrical in shape with radius ρ, height h and mass m.

2, Central hub or body of quadrotor is also cylindrical with radius R, height H and mass 𝑚0.



Quadrotor Dynamics

After a simple calculation, we may get the inertia moment in x,y,and z direction respectively



Quadrotor Dynamics

𝑰𝒙𝒙 =𝟏

𝟐𝒎𝝆𝟐 +

𝟏

𝟔𝒎𝒉𝟐 + 𝟐𝒎𝒍𝟐 +

𝒎𝟎𝑹𝟐

𝟒+𝒎𝟎𝑯

𝟐

𝟏𝟐

𝑰𝒚𝒚 =𝟏

𝟐𝒎𝝆𝟐 +

𝟏

𝟔𝒎𝒉𝟐 + 𝟐𝒎𝒍𝟐 +

𝒎𝟎𝑹𝟐

𝟒+𝒎𝟎𝑯

𝟐

𝟏𝟐

𝑰𝒛𝒛 =𝟏

𝟐𝒎𝟎𝑹

𝟐 + 𝟒𝒎𝒍𝟐



Under the gravity and actuator action without other external effects, the machine experiences following motions: 1, Rolling along x-axis 2, Pitching along y-axis 3, Yaw motion along z-axis:

Dynamics of Quadrotor:

As the focus is on the hover control of quadrotor, and the fact that horizontal motion is a consequence of rotational motion, the following relations are developed.

Quadrotor Dynamics



Rolling motion is about x-axis. (Rolling Dynamic) Rolling angular displacement as measured with respect to E-Frame using right hand rule is θ. There θ” expresses angular acceleration about x-axis. Based on the moment of inertia of machine about x-axis, the rolling torque on machine is:

Also due to the design of machine, the rolling torque is due to the thrust difference of motors M2 and M4 and the thrust force of each motor is assumed to be perpendicular to the plane of propeller and therefore the plane of quadrotor frame. Therefore the rolling torque is :

Quadrotor Dynamics

𝑰𝒙𝒙𝜽 × 𝒊 = 𝒍(𝑭𝟐 − 𝑭𝟒) × 𝒊

𝝉𝒙𝒙 = 𝑰𝒙𝒙𝜽 𝒊



Similarly, the pitching motion is along the y-axis and the relationship can be easily found as following:

It can be easily found that both the rolling and the pitching motion have a closed relationship with the force Fi ( i=1,2,3,4 ). And according to the property of the DC motors, the force is controlled by the rotation rate of the propeller. Mainly the Yaw Dynamic happens on the X-Y coordinate plane. Compared to rolling and pitching control, a more precise control of the rotating speed should be achieved in raw control, which is not applicable if AR.Drone is used as the platform in this project.

(Pitching Dynamic)

Quadrotor Dynamics

𝑰𝒚𝒚𝝓 = 𝒍(𝑭𝟏 − 𝑭𝟑)

(Yaw Dynamic) *discussed in the reference slides (Ignore)



In this FYP, more efforts were placed on pitching and rolling control. According to the symmetrical assumption we made, for the symmetrical rolling and pitching motion, no net torque(theoretically) will be generated along the z axis.

Hover Yaw Pitch & Roll

Quadrotor Dynamics

Influence of ignoring the Yaw Motion

All Done!!! Let`s start to design the controller!!!



Pitching & Rolling Control:

Controller Design & Simulation

Quadrotor rolling dynamics:

𝑰𝒙𝒙𝜽 × 𝒊 = 𝒍(𝑭𝟐 − 𝑭𝟒) × 𝒊

𝟏

𝟐𝒎𝝆𝟐 +

𝟏

𝟔𝒎𝒉𝟐 + 𝟐𝒎𝒍𝟐 +

𝒎𝟎𝑹𝟐

𝟒+𝒎𝟎𝑯

𝟐

𝟏𝟐× 𝜽 = 𝒍 𝑭𝟐 − 𝑭𝟒

According to propeller aerodynamics:

𝑭 = 𝑲𝒎 𝒓 × 𝒏𝟐 (n is the rotating speed of the propeller) Initial stable condition: Not a Big Deal, Calculate Just For Fun

𝟒 × 𝑲𝒎 × 𝒏𝟎𝟐 = (𝒎𝟎 + 𝟒𝒎)𝒈 𝒏𝒐 =

𝟏

𝟐

(𝒎𝟎+𝟒𝒎)𝒈

𝑲𝒎




Pulse signal with width of U (lasts for 1000-2000 s)

Propeller rotating speed N (lasts from 0-180 r/s)

Controller DC Motor

First order object

𝐍(𝐬)

𝐔(𝐬)=

𝑻𝒇

𝐬+𝟏 in which Tf = 0.18

(Time constant is set to be 1s for the simulation process)

Transfer function between the rotating speed and the pulse signal width

Commands Action




When there is a command sent to the controller: (∆𝐧)

𝐅 = 𝟒 × 𝑲𝒎 𝒏𝟎 + ∆𝐧 𝟐 = 𝟒 ×𝑲𝒎 𝒏𝟎𝟐 + ∆𝒏𝟐 + 𝟐𝒏𝟎∆𝒏

≅ 𝟒𝑲𝒎𝒏𝟎𝟐 + 𝟖𝑲𝒎𝒏𝟎∆𝒏

𝟏

𝟐𝐦𝛒𝟐 +

𝟏

𝟔𝐦𝐡𝟐 + 𝟐𝐦𝐥𝟐 +

𝐦𝟎𝐑𝟐

𝟒+𝐦𝟎𝐇

𝟐

𝟏𝟐× 𝛉 𝐬 × 𝐬𝟐 = 𝟖𝐊𝐦𝒍𝐧𝟎𝐍 𝐬

𝑰𝒙𝒙𝜽 × 𝒊 = 𝒍(𝑭𝟐 − 𝑭𝟒) × 𝒊

𝛉(𝐬)

𝐍(𝐬)=𝟖𝐊𝐦𝒍𝐧𝟎𝐈𝐱𝐱𝐬

𝟐

𝐍(𝐬)

𝐔(𝐬)=

𝑻𝒇

𝐬 + 𝟏

𝜽(𝒔)

𝑼(𝒔)=

𝑪𝟎𝑻𝒇

𝒔𝟐(𝒔 + 𝟏) (𝒄𝟎 =

𝟖𝑲𝒎𝒍𝒏𝟎𝑰𝒙𝒙

)

Also we have :

Then we finally get:




In last equation, 𝐶0 should be tested through the experiment. For the following simulation part, we simply set 𝐶0𝑇𝑓 equals to 20 (a

rough prediction, a more precise value will be given in the report),

the unit of 𝜃 is degree (180º) and the unit of U is s. For the system, we have: Gp(s) =

𝐶0𝑇𝑓

𝑠2(𝑠+1) =

20

𝑠2(𝑠+1)




PD Control:

Open-loop transfer function:

H(s) = Gc(s)*Gp(s) = 𝑲𝒑 𝟏 + 𝑻𝒅𝒔 × 𝑪𝟎𝑻𝒇

𝒔𝟐(𝒔+𝟏)

First we need to find the range of variation, we simply set 𝐾𝑝

equals to 1, the characteristic function of the closed loop function indicates that: 𝒔𝟑 + 𝒔𝟐 + 𝟐𝟎𝑻𝒅𝒔 + 𝟐𝟎 = 𝟎




To plot the root locus diagram for the system, we know that we should plot the roots distribution for the function:

𝟏 + 𝐊 × 𝐇 𝒔 = 𝟎 (for K varies from 0~)

* When we backstep the characteristic function, we may form a new open-loop transfer function. Treat the character 𝑇𝑑 as the K in the root locus diagram, by root locus method, we may find the critical 𝑇𝑑 which makes the system stable.

𝒔𝟑 + 𝒔𝟐 + 𝟐𝟎𝑻𝒅𝒔 + 𝟐𝟎 = 𝟎

New open-loop transfer function:

With the same characteristic function:

G s = 𝑇𝑑 ×𝑁∗ 𝑠

𝐷∗ 𝑠 𝑇𝑑𝑣𝑎𝑟𝑖𝑒𝑠 𝑓𝑟𝑜𝑚 0~

𝑁∗= 20𝑠 ; 𝐷∗ = 𝑠3 + 𝑠2 + 20




Then the critical 𝑇𝑑 we got from the root locus is exactly 𝑇𝑑 which will makes the system stable.

Root locus diagram: >> num=[21.3,0]; >> den=[1 1 0 21.3]; >> rlocus(num,den);

Which means that the gain (𝑇𝑑) must be greater than 1 to make the whole system stable.




The nyquist diagram of the open loop transfer function is used to test such results:

>> num=[19.4,20]; >> den=[1,1,0,0]; >> nyquist(num,den); >> axis([-2,1,-0.05,0.05]); >> hold on; >> num1=[20.6,20]; >> nyquist(num1,den);

In this diagram, the blue line represents for the situation in which td=0.97 and the green line represents for the situation in which td=1.03. The change of the direction shows the change of the system stability.




Disturbance:

>> num=[20]; >> den=[1,1,100,50]; >> step(num,den);

We treat the disturbance in form of the change of the rotating speed of the propeller.

A step disturbance response Kp = 2, Td=2.5




PD controller simulation:

>> kp=2; >> td=2.5; >> s=tf('s'); >> gc=kp*(1+td*s); >> gp=20/s/s/(s+1); >> t=0:0.01:30; >> u=linspace(1,1,length(t)); >> y=lsim(gc*gp/(1+gc*gp),u,t); >> n=linspace(5,5,length(t))+5*rand(1,length(t)); >> yn=lsim(gp/(1+gc*gp),n,t); >> plot(t,y+yn); Not a good control!!!

Random disturbance

A step disturbance response Kp = 2, Td=2.5




PID control:

The open loop transfer function:

𝑮(𝒔) =𝑪𝟎𝑻𝒇𝑲𝒑(𝑻𝒊𝑻𝒅𝒔

𝟐 + 𝑻𝒊𝒔 + 𝟏)

𝑻𝒊𝒔𝟑(𝒔 + 𝟏)

In which set 𝐾𝑝 = 2, 𝑇𝑑 = 2.5, and also set 𝐶0𝑇𝑓 = 20




The characteristic function indicates that:

𝑇𝑖𝑠4 + 𝑇𝑖𝑠

3 + 100𝑇𝑖𝑠2 + 40𝑇𝑖𝑠 + 40 = 0

With the same characteristic function, we could also find the new transfer function which satisfies the form of root locus. Also by plotting root locus diagram we may find the critical 𝑇𝑖 which will stabilize the system.

G s = 𝑇𝑖 ×𝑁∗ 𝑠

𝐷∗ 𝑠 𝑇𝑖 𝑣𝑎𝑟𝑖𝑒𝑠 𝑓𝑟𝑜𝑚 0~

𝑁∗= 𝑠4 + 𝑠3 + 100𝑠2 + 40𝑠; 𝐷∗= 40




Root locus diagram: >> num=[1,1, 100,40,0]; >> den=[40]; >> sys=tf(num,den); >> rlocus(sys); >> axis([-2,2,-10,10]);

From the diagram we could see that to make the system stable, 𝑇𝑖 should be larger than 0.0157




Random select some values for the controller:

Set Kp=2, Ti=10, Td=2.5, and get the step response as the following:

(Without disturbance just to make sure the estimation of 𝑇𝑖 is correct)




Still the same 𝐾𝑝, 𝑇𝑑 and 𝑇𝑖, with a 5 random disturbance, the step

response is shown:

>> kp=2; ti=10; td=2.5; s=tf('s'); gc=kp*(1+1/ti/s+td*s); gp=20/s/s/(s+1); g=gp*gc/(1+gc*gp); t=0:0.01:60; r=linspace(1,1,length(t)); y=lsim(gc*gp/(1+gc*gp),r,t); n=linspace(1,1,length(t))+rand(1,length(t)); yn=lsim(gp/(1+gc*gp),5*n,t); plot(t,y+yn);




Optimized PID: Set Kp=0.00056, Ti=281, Td=47,

AR.Drone Platform



Platform Construction



OS transplantation

With the Apps downloaded from the App store one can control the AR.Drone to do some simple movements.

Apps flying!!!

IMG_0148 00_02_28-00_02_53.avi



OS transplantation

An example of AT.Command :

My little control panel:



OS transplantation

Flying on Control Panel

At the latter part of the video, an open-loop path following command was sent to the Drone. We could find the drone follow the particular path. It just shows such panel has a good control of the drone.

IMG_0161.avi

Some limits:

• Control delay

Unfortunately, this cannot be solved because of the limits of AR.Drone. The frequency of its sending and receiving data has already been set to 30Hz. Only if to modify the hardware can we get a better result.



OS transplantation

• Position float

As a mature product, AR.Drone has alerady been programmed. The design of AR.Drone is mainly for entertainment and for such reason, the flight position control part of AR.Drone is not that stable. In order to design and build a quadrotor control platform we need to make some modifications to AR.Drone.



OS transplantation

1, Angular velocity : gyroscope, compass

2, Angular acceleration: Digital Accelerometer



Sensor:

Sensing-Tech

ADXL345 MPU 6050 MiniIMU



Sensing-Tech

Arduino and X-Bee module:

Programmed in Arduino:

Arduino is an open-source electronics platform based on flexible, easy-to-use hardware and software.

Arduino UNO: X-Bee:



Through the I2C digital interface and based on IEEE 802.15.4 protocol, we could get the data flow of the accelerometer.

Sensing-Tech

Wire control_MiniIMU Wireless Sensor ADXL345

IMG_0172.avi

IMG_0172.avi

IMG_0172.avi

IMG_0173.avi



It seems that we have already had a closed control loop, while there is still one more thing to do, the data flow transfer

controller



Compatibility of the whole system.

( I2C TDMC(LabView) C# )

( I2C .txt C# )

My now working focus and problem:

Too Slow

Quadrotor Control Panel : VC2010, C# input or hardware input Microcontroller of sensors: Arduino, I2C digital interface

On-going Focus

Future Plan:

• Seeking for ways to solve the problem of compatibility and data processing.

• Hardware integration simulation test



Future Plan

Q&A



Q&A

Thanks !!!





The moment of inertia of two identical cylinders connected together by a horizontal arm , and rotating about a vertical axis, which is passing through the center of the arm and is perpendicular to it, as specified is:

For part a, the moment of inertia due to motors M2 and M4 rotating about x-axis is approximated by

For part b, the moment of inertia due to M1, M3 and the central hub about x-axis is approximated by

Quadrotor Dynamics



Therefore the total moment of inertia along x-axis is:

The above is the inertia about x-axis. By exactly similar procedure moment of inertia about y-axis can be found that:

Quadrotor Dynamics

𝑰𝒙𝒙 =𝟏

𝟐𝒎𝝆𝟐 +

𝟏

𝟔𝒎𝒉𝟐 + 𝟐𝒎𝒍𝟐 +

𝒎𝟎𝑹𝟐

𝟒+𝒎𝟎𝑯

𝟐

𝟏𝟐

𝑰𝒚𝒚 =𝟏

𝟐𝒎𝝆𝟐 +

𝟏

𝟔𝒎𝒉𝟐 + 𝟐𝒎𝒍𝟐 +

𝒎𝟎𝑹𝟐

𝟒+𝒎𝟎𝑯

𝟐

𝟏𝟐



Moment of inertia about z-axis, can also be calculated in two parts: 1, Moment of inertia due to central hub. 2, Moment of inertia due to motors M1, M3, M2 and M4.

The total inertia about x,y,z-axis are:

Quadrotor Dynamics

𝑰𝒙𝒙 =𝟏

𝟐𝒎𝝆𝟐 +

𝟏

𝟔𝒎𝒉𝟐 + 𝟐𝒎𝒍𝟐 +

𝒎𝟎𝑹𝟐

𝟒+𝒎𝟎𝑯

𝟐

𝟏𝟐

𝑰𝒚𝒚 =𝟏

𝟐𝒎𝝆𝟐 +

𝟏

𝟔𝒎𝒉𝟐 + 𝟐𝒎𝒍𝟐 +

𝒎𝟎𝑹𝟐

𝟒+𝒎𝟎𝑯

𝟐

𝟏𝟐

𝑰𝒛𝒛 =𝟏

𝟐𝒎𝟎𝑹

𝟐 + 𝟒𝒎𝒍𝟐



Reference Slides

Ziegler method

function [num,den,Kp,Ti,Td,H] = Ziegler_std( key,vars ) %Ziegler method to determine the parameter of PID controller % Detailed explanation goes here Ti=[]; Td=[]; H=[]; K=vars(1); L=vars(2); T=vars(3); a=K*L/T; if key==1,num=1/a; else if key==2,Kp=0.9/a;Ti=3.33*L; else if key==3,Kp=1.2/a;Ti=2*L;Td=L/2; end end end

switch key case 1, num=Kp; den=1; case 2, num=Kp*[Ti,1];den=[Ti,0]; case 3, p0=[Ti*Td,0,0]; p1=[0,Ti,1]; p2=[0,0,1]; p3=p0+p1+p2; p4=Kp*p3; num=p4/Ti; den=[1,0]; end end



Yaw Dynamic * (less important in position control) Yaw motion is due to torque imbalance. A net torque about z-axis is responsible for yaw motion in accordance with right hand rule. Each motor supplies machine torque and drag torque. The net torque on propeller is:

J is the rotational inertia of the rotor and propeller of motor. If all the clockwise and counter-clockwise torques are added, the sum torques and the yaw torque can be expressed as:

Reference Slides

Not a toy. But a powerful working platform

• With the user guide published, more things could be done to AR.Drone.

• We can control the drone by

sending an AT.Command to Drone

through UDP port 5554

• The language of AR.Drone:

AT.Command



Reference Slides



Demonstration test of MiniIMU

MiniIMU = STM32F103 a popular low-power microcontroller + MPU-6050 accelerometer + HMC5883L compass + BMP085 barometer

Integration chips

Reference Slides

The connection between computer and MiniIMU



to

Reference Slides

A wonderful user interface



Reference Slides

But …..

• Even though such sensor bench works, while it has a “tail”. We cannot fix a USB-TTL line on the quadrotor to transmit the data. We need to build a wireless sensor bench to get the flight attitude.



Reference Slides



New sensor, less powerful but can work in wireless way

ADXL 345: A 3-axle accelerometer with a measurement up to 16g. Digital output data is formatted as 16-bit twos complement and is accessible through I2C digital interface.

How to make it a wireless one?

Reference Slides

• About the chip : Arduino

Arduino is an open-source electronics platform based on flexible, easy-to-use hardware and software.

Arduino Mega 2560: http://www.arduino.cc/en/Main/ArduinoBoardMega2560

The language of Arduino: http://arduino.cc/en/Reference/HomePage



Reference Slides

http://www.arduino.cc/en/Main/ArduinoBoardMega2560

http://arduino.cc/en/Reference/HomePage

http://arduino.cc/en/Reference/HomePage



* X-CTU is the compiler (a software to read and send the data to X-bee) of Digi Company. We can read the data sent beck from the X-bee and then such data will be shown inside the software. 64-bit system × 32-bit system √

As a commercial software in which the data cannot be picked, the X-CTU is not applicable in this project and we need to read the data flow directly from the port with Arduino-UNO. Work about the data flow picking has already been solved. And Now we can read the data in Arduino Development environment.

Reference Slides



Vertical Acceleration

When the roll or pitch angle is nor zero then a component of F*cosθcosφ acts along the z-axis

Then we will get that:

As we discussed before, Fi will be influenced by the speed of the propeller. Unfortunately for AR.Drone, the speed of the propeller has been well controlled. We could only choose some particular speed gear. (10 gears)

Vertical Position Control

Reference Slides



A proportional derivative controller is going to be designed. We can control the motion along x-axis with a PD controller given by

The desired pitch angle can be written as

Derivative of pitch angle, gives the desired pitch angle velocity

= = 0 ( )

Pitching and rolling control

U-pitch input = (F3 – F1)*d

(Geometrical Pitching)

Reference Slides



Likewise, the system rolling input could also be set as(same Kp and Kd)

For the Yaw angle, considering the angle velocity and acceleration, the system input could be set as

Simulation and experiment are under designed…

* Yaw Control

Reference Slides



2, Feasibility of the controller design

I can use LabView or MATLAB Simulink to conduct a simulation of the controller design, while it may not has a significant influence on the flight attitude or even cause a chaos in the flight attitude control: AR.Drone is a mature product, and my control panel is based on the user guide, which means each of my commend has already been compiled by AR.Drone and each commend represents a series of movement. The controller nearly has no effect.

Ongoing Focus



Reference Slides



Reference Slides

http://en.wikipedia.org/wiki/Quadrotor

Based On AR - Weeblymzfrank.weebly.com/uploads/1/3/9/6/13967377/quadrotor...Quadrotor Control Project Final Year Project of GAO Mingze (No.2011955028) Control of the quadrotor motion

Documents