Implementation of a Real-Time Attitude System for the Stabilization

Control of a Quad-rotor RobotSérgio Ronaldo Barros dos Santos (ITA-Brazil)

Cairo Lúcio Nascimento Júnior (ITA-Brazil)Sidney Nascimento Givigi Júnior (RMC-Canada)Tiago Henrique Medeiros Mercante (ITA-Brazil)

Neusa Maria Franco de Oliveira (ITA-Brazil)

Introduction The Attitude and Heading Reference System (AHRS) is a device used to determine a vehicle’s attitude and heading.

The AHRS consists of three-axes solid-state or MEMS gyroscopes, accelerometers and magnetometers and also an on-board processing system.

This device is designed to replace traditional mechanical gyroscopic and provide superior reliability and accuracy relative to the data measured.

2/26

Introduction The key difference between an inertial measurement

unit (IMU) and an AHRS is the addition of an on-board processing system. In an AHRS the attitude and heading angles are directly provided, while an IMU just delivers sensor data to an additional device that computes the attitude angles.

The AHRS can be embedded on several types of autonomous vehicles, such as terrestrial, maritime or aerial, as the primary navigation system.

3/26

4/26

Quad-rotor Robot Quad-rotor robots have become popular due to their relatively simple model and the low-cost involved in operating experimental platforms.

5/26

Problem Statement In spite of the four actuators, the quad-rotor is a dynamically unstable system that has to be stabilized through a suitable attitude and heading measurements and also a control system.

6/26

Problem Statement Our goal is to implement an AHRS using low-cost inertial and magnetic sensors that is able to provide accurate attitude information to stabilize the quad-rotor robot during flight.

The proposed method are computed through the angular rates obtained from the gyroscope.

Due to the fast update rate of the measured gyro data, drift and also integration errors are expected.

Then, the fusion with others sensors such as a magnetometer and an accelerometer is necessary.

7/26

Rotation Matrix The rotation matrix is obtained using the rotation rate [p, q, r] measured around x, y and z axis, respectively.

The simplified Direction Cosine Matrix (DCM) can be rewrite as:

1

1

1

nbR

nb

where initially ф, Ѳ, ψ are equal to zero.

8/26

Rotation Matrix In this approach the attitude is constantly updated through the body rotation rate [p, q, r]T. Using the DCM, these values can be expressed as:

0

0

0

pq

pr

qrnb

The rotation matrix is computed at discrete-time, every 20ms using :

)()1()( knbknbknbR

)()1()( knbRknbknb

9/26

Orthogonality Procedure Numerical errors due to the integration process gradually reduce the orthogonality between the components of the rotation matrix around the x, y and z axes.

The correction of this error can be made through the process of enforcing the orthogonality conditions.

Consider the rotation matrix as:

zzRzyRzxRyzRyyRyxRxzRxyRxxR

k

10/26

Orthogonality Procedure

T

yzRyyRyxR

xzRxyRxxRortE

][

T

yzRyyRyxR

ortE

T

xzRxyRxxR

T

ortxzRxyRxxR

2

T

xzRxyRxxR

ortE

T

yzRyyRyxR

T

ortyzRyyRyxR

2

T

ortyzRyyRyxR

T

ortxzRxyRxxR

T

ortzzRzyRzxR

The obtained orthogonality error among the x, y and z vectors of the rotation matrix can be corrected using:

11/26

Orthogonality Procedure After the correction, it should be assured that the x, y and z vectors are of unitary magnitude.

T

ortxzRxyRxxR

T

ortxzRxyRxxR

T

ortxzRxyRxxR

T

ndxzRxyRxxR

32

1

T

ortyzRyyRyxR

T

ortyzRyyRyxR

T

ortyzRyyRyxR

T

ndyzRyyRyxR

32

1

T

ortzzRzyRzxR

T

ortzzRzyRzxR

T

ortzzRzyRzxR

T

ndzzRzyRzxR

32

1

12/26

Orientation The Euler angles is estimated using the corrected rotation matrix.

The attitude of the quad-rotor robot is calculated every 20ms, using:

21arctan

zx

zx

R

R zzzy RR ,2arctan

xxyx RR ,2arctan

13/26

The integrated signal from the gyroscope is combined with the measured magnetic force and acceleration through a fusion algorithm, to detect the gyro offset and provide a negative feedback signal.

Fusion Algorithm

14/26

Fusion Algorithm Using the measured gravity vector, the components of rotation matrix related to roll and pitch angles are verified. The error vector, Ea is calculated by:

The yaw orientation can be computed from the measured magnetic force [mx my mz]T and also the roll and pitch angles obtained previously.

)()cos()()()cos(1 senmsensenmmM zyx

)()cos(2 senmmM zy 12 ,2arctan MMm

zayaxa

T

ndzzRzyRzxR

azEayEaxE

15/26

Fusion Algorithm Using the computed yaw orientation and also the components of the rotation matrix related to roll, pitch and yaw angles, the error vector, Em is calculated.

yxRmxxRmsen

T

ndzzRzyRzxR

mzEmyEmxE

)cos()(

The estimate of the drift defined by a total error vector can be obtained using:

mzEmyEmxE

azEayEaxE

zEyExE

16/26

Finally, the adjust vector applied for gyro drift correction is given by:

dt

zEyExE

Ki

zEyExE

Kpkc

02.0

0.)(

Although the gyros have a regular performance with an uncorrected offset of few degrees per second, the accuracy of the measured attitude can be improved through drift correction.

Fusion Algorithm

17/26

Implementation Firstly, the microcontroller receives all of the sensors data. The three axis gyroscope sends its measurements in

analog format. The three axis accelerometer and magnetometer

send their measurements using I2C protocol.

18/26

Implementation The accelerometers and gyros are sampled in 50Hz and filtered using IIR Butterworth 2 order low pass filter. The cut off frequency chosen is 5Hz. The magnetometer is read at a 10 Hz update rate.

The algorithm uses a defined execution rate of 50Hz.

19/26

Implementation The computed attitude is sent to embedded controller board and also for a Ground Control Station (GCS) through the serial channel.

The attitude is used to feedback the control loops implemented in the embedded microcomputer board.

20/26

Experimental Setup The experimental setup consists of a Hummingbird quad-rotor and a Ground Control Station (GCS).

The GCS allows the user to define the input command to the attitude control loop.

21/26

Experimental Setup The reference signal can be generated by the standard RC Transmitter or through a position control loop implemented in Simulink.

In this work experiments have been conducted using the RC transmitter signal.

The RC transmitter is attached to the USB port of the laptop, used to transmit and to receive the command signal of the Ground Control Station.

The quad-rotor height is controlled manually from the RC transmitter.

22/26

Experimental Setup

23/26

Results The stabilization control of the quad-rotor during hovering is verified through the experimental results

24/26

Results The experimental results show that the method used to obtain the attitude is effective for the stabilization control of a quad-rotor during hovering flight, even while executing high speed maneuvers.

See the validation movie.

25/26

Conclusion The use of AHRS proved to be very valuable for the

stabilization control of a quad-rotor.

It enables the developer to evaluate many aspects of the aircraft autopilot hardware, to refine the control algorithm, to test and tune control parameters, and many others.

This platform can be easily adapted for the implementation of other control methods and also used in different types of unmanned aerial vehicles in several applications.

26/26

Thank you

Questions?