5/24/2015

Proyecto Matemticas Aplicadas

Alumnos: Jess Alberto Garca Cruz, Alejandro Mosqueda Vargas
Dra. Graciela Velasco Herrera
ResumenEn este proyecto se pretende controlar en tiempo real la
rotacin de una figura animada por computadora a partir de los
cuaterniones entregados por el Sensor Acelermetro MPU6050 y el
uso del Arduino Nano ATMega328.

Pitch), los problemas surgen cuando la segunda rotacin en la

secuencia se aproxima a 90 grados (esto trae una alineacin entre
el primer y tercer eje de rotacin causando un bloqueo).

Palabras ClaveCuaterniones, Rotacin, Sensor MPU6050,

Acelermetro, Arduino Nano.

Una manera de evitar los problemas originados por el uso de

ngulos de Euler es utilizar cuaterniones como mtodo
alternativo para representar rotaciones. Los cuaterniones
describen una o varias rotaciones en tres dimensiones mediante
el uso de cuatro valores escalares. Tres de estos escalares definen
un eje, y el cuarto especifica una rotacin alrededor de dicho eje.

I.

INTRODUCCIN

ediante el uso del microcontrolador Arduino y el

Sensor MPU6050 acelermetro-giroscopio, se puede
implementar un sistema que calcule el Pitch, Roll y
Yaw (giro alrededor de los ejes X, Y y Z , respectivamente ,
tambin conocidos como ngulos de Euler ) .

El uso de cuaterniones no presenta el problema de bloqueo de

ejes y se pueden representar rotaciones en rangos de ms de 180
grados sin dificultad. Adems tienen algunas propiedades
matemticas tiles tales como la interpolacin directa entre dos
cuaterniones, Esto puede ser til en la animacin y reduccin de
errores de redondeo cuando mltiples operaciones de rotacin se
realizan en secuencia.

Ejemplo de rotacin de un vector utilizando un cuaternin:

Sea el vector p = [i+2j+0k] y la forma general de un cuaternin
de rotacin q=[cos(/2),n sen(/2)], se desea girar el vector p en
sentido horario un ngulo de =60 .
Figura 1 Representacin del Pitch, Roll y Yaw en una aeronave.

El clculo de Pitch () y Roll () se puede obtener a partir de los

datos adquiridos por el acelermetro Ax, Ay, Az (No se puede
calcular el Yaw usando datos del acelermetro) con las
siguientes formulas:

=arc tan(

=arc tan(

Ax

2
y

+ A z2

Ay

2
x

+ Az

De las formulas anteriores se puede observar que la funcin

tangente est acotada desde -/2 a /2, por lo que est limitado el
rango de movimiento que puede detectar el sensor.
Adems el uso de ngulos de Euler para calcular rotaciones
puede conducir a un problema conocido como Gimbal Lock.
Debido a que las rotaciones angulares de Euler se realizan en
secuencia (por ejemplo primero Yaw, luego Raw y al ltimo

Figura 2 Se desea girar 60 grados sobre el plano YX en sentido

horario usando la regla de la mano derecho.

El vector n=-k usando la regla de la mano derecha.

Por lo que q=[cos(30),-sen(30)k) q=[.87, .5k]
Se pretende rotar el vector p un ngulo =60 utilizando la
siguiente formula:

P' =q p q1

5/24/2015
Recordando que el producto de dos cuaterniones es
[Sb,b]=[SaSb-a*b,Sab+Sba+axb]

[Sa,a]

qpq-1 = (.87 - .5k)(i + 2j)(.87 + .5k) =

(.87i + 1.74j - .5ki k j)(.87 + .5k) =
(1.87i + 1.24j)(.87 + .5k) =
1.63i + .94ik + 1.08j + .62jk =
2.25i + .14j
P=2.25i+.14j
Se cumple que:
P y P tienen la misma magnitud.

|P|= 12 +22= 5
|P '|=2.25 2+.14 2= 5
Expresados en forma de cuaternin, tanto P como P son
cuaterniones puros.
P=[0,i+2j]
P=[0,2.25i+.14j]

Figura 3 Conexiones entre el Arduino Nano y el Sensor

Acelermetro MPU-6050.

Una vez terminada la conexin e instalado las libreras de i2C en

una laptop, se debe cargar el Arduino nano con el cdigo A del
apndice de este trabajo. El cdigo anterior viene de ejemplo en
el interfaz de desarrollo Arduino al instalar las libreras
I2CDevLib y permite que el sensor MPU6050 entregue
aceleraciones en trminos de cuaterniones.
A continuacin se instala el software Processing, las libreras de
Toxiclib y posteriormente se carga el cdigo B del apndice en
un sketch de Processing.
Al conectar el Arduino nano para la recepcin de datos en el
puerto serial y compilar el cdigo B, Se abre una ventana donde
se muestra una animacin de un avin.
Esta animacin va a representar las rotaciones en tiempo real del
sensor MPU-6050.

II. PROCEDIMIENTO
III.
Material:
Arduino Nano ATMega328.
Sensor Acelermetro-Giroscopio MPU6050.
Interfaz de desarrollo Arduino y la librera I2Cdevlib.
Interfaz de Processing y las libreras Toxiclibs.
Conexiones:

5V<->VCC (Alimentar con 5 volts el sensor desde el

Arduino.)

GND<->GND (conexin tierra a tierra)

A5<->SCL (Pin anlogo 5 del Arduino hacia el Serial

Clock Line del MPU6050)

A4<->SDA (Pin anlogo 4 del Arduino hacia el Serial

Data Line del MPU6050)

Interruptor (Pin 2 digital hacia el interruptor del sensor)

Figura 4 La representacin animada de un avin rota gracias a los

cuaterniones entregados por el sensor MPU 6050.

IV.

RESULTADOS

La animacin 3D sigue la rotacin del Sensor, se presentan

ligeros retrasos e interrupciones, pero son despreciables y casi no
se perciben.

5/24/2015

V. CONCLUSIONES
El hecho de que la animacin mantenga una respuesta confiable
y que se puedan hacer giros mayores a 180 grados, se debe a que
la representacin de giros por medio de cuaterniones disminuye
la complejidad de operaciones durante el procesamiento.
En los primeros segundos se puede observar que la figura
animada mantiene un ligero movimiento aun cuando el Sensor no
est en movimiento, pero despus de un cierto tiempo la figura
comienza a establecer su comportamiento.
Figura 5

Esto se debe a que el sensor realiza una calibracin para corregir

los valores transmitidos.
En este proyecto fsico y programable se reflej la gran ventaja
de trabajar con cuaterniones que con ngulos de Euler sera
mucho ms complicado, obteniendo buenos resultados, aplicando
de esta manera lo visto durante nuestras clases de teora.

VI. APNDICE:
CDIGO A:
#include "I2Cdev.h"

Figura 6

Figura 7

#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION ==
I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
#define OUTPUT_TEAPOT
bool blinkState = false;
bool dmpReady = false; // set true if DMP init was
successful
uint8_t mpuIntStatus; // holds actual interrupt
status byte from MPU
uint8_t devStatus;
// return status after each
device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size
(default is 42 bytes)
uint16_t fifoCount;
// count of all bytes currently
in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
Quaternion q;
// [w, x, y, z]
quaternion
container
VectorInt16 aa;
// [x, y, z]
accel sensor
measurements
VectorInt16 aaReal;
// [x, y, z]
gravity-free
accel sensor measurements
VectorInt16 aaWorld; // [x, y, z]
worldframe accel sensor measurements
VectorFloat gravity; // [x, y, z]
gravity
vector
float euler[3];
// [psi, theta, phi] Euler angle
container
float ypr[3];
// [yaw, pitch, roll]
yaw/pitch/roll container and gravity vector

5/24/2015
volatile bool mpuInterrupt = false;
// indicates
whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
void setup() {
#if I2CDEV_IMPLEMENTATION ==
I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if
CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION ==
I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration,
others continue immediately
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ?
F("MPU6050 connection successful") : F("MPU6050
connection failed"));
Serial.println(F("\nSend any character to begin
DMP programming and demo: "));
while (Serial.available() && Serial.read()); //
empty buffer
while (!Serial.available());
// wait for
data
while (Serial.available() && Serial.read()); //
empty buffer again
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory
default for my test chip
if (devStatus == 0) {
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
Serial.println(F("Enabling interrupt detection
(Arduino external interrupt 0)..."));
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
Serial.println(F("DMP ready! Waiting for first
interrupt..."));
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
"));

Serial.print(F("DMP Initialization failed (code

Serial.print(devStatus);
Serial.println(F(")"));

// configure LED for output

pinMode(LED_PIN, OUTPUT);

void loop() {
if (!dmpReady) return;
while (!mpuInterrupt && fifoCount < packetSize)
{
}
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024)
{

mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
} else if (mpuIntStatus & 0x02) {
// wait for correct available data length,
should be a VERY short wait
while (fifoCount < packetSize) fifoCount =
mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
#ifdef OUTPUT_READABLE_QUATERNION
mpu.dmpGetQuaternion(&q, fifoBuffer);
Serial.print("quat\t");
Serial.print(q.w);
Serial.print("\t");
Serial.print(q.x);
Serial.print("\t");
Serial.print(q.y);
Serial.print("\t");
Serial.println(q.z);
#endif
#ifdef OUTPUT_TEAPOT
// display quaternion values in InvenSense
Teapot demo format:
teapotPacket[2] = fifoBuffer[0];
teapotPacket[3] = fifoBuffer[1];
teapotPacket[4] = fifoBuffer[4];
teapotPacket[5] = fifoBuffer[5];
teapotPacket[6] = fifoBuffer[8];
teapotPacket[7] = fifoBuffer[9];
teapotPacket[8] = fifoBuffer[12];
teapotPacket[9] = fifoBuffer[13];
Serial.write(teapotPacket, 14);
teapotPacket[11]++; // packetCount, loops
at 0xFF on purpose
#endif
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
}

5/24/2015
CDIGO B:

import processing.serial.*;
import processing.opengl.*;
import toxi.geom.*;
import toxi.processing.*;
ToxiclibsSupport gfx;
Serial port;
// The serial port
char[] teapotPacket = new char[14]; // InvenSense Teapot
packet
int serialCount = 0;
// current packet byte position
int aligned = 0;
int interval = 0;
float[] q = new float[4];
Quaternion quat = new Quaternion(1, 0, 0, 0);
float[] gravity = new float[3];
float[] euler = new float[3];
float[] ypr = new float[3];
void setup() {
size(300, 300, OPENGL);
gfx = new ToxiclibsSupport(this);
lights();
smooth();
println(Serial.list());
String portName = "COM4";
port = new Serial(this, portName, 115200);
port.write('r');
}
void draw() {
if (millis() - interval > 1000) {
port.write('r');
interval = millis();
}
background(0);
pushMatrix();
translate(width / 2, height / 2);
float[] axis = quat.toAxisAngle();
rotate(axis[0], -axis[1], axis[3], axis[2]);
fill(255, 0, 0, 200);
box(10, 10, 200);
fill(0, 0, 255, 200);
pushMatrix();
translate(0, 0, -120);
rotateX(PI/2);
drawCylinder(0, 20, 20, 8);
popMatrix();
fill(0, 255, 0, 200);
beginShape(TRIANGLES);
vertex(-100, 2, 30); vertex(0, 2, -80); vertex(100, 2, 30); //
wing top layer
vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30); //

wing bottom layer

vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70); // tail
left layer
vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70); // tail
right layer
endShape();
beginShape(QUADS);
vertex(-100, 2, 30); vertex(-100, -2, 30); vertex( 0, -2, -80);
vertex( 0, 2, -80);
vertex( 100, 2, 30); vertex( 100, -2, 30); vertex( 0, -2, -80);
vertex( 0, 2, -80);
vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2, 30);
vertex(100, 2, 30);
vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, -30, 98);
vertex(-2, -30, 98);
vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, 0, 70);
vertex(-2, 0, 70);
vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2, 0, 70);
vertex(-2, 0, 70);
endShape();
popMatrix();
}
void serialEvent(Serial port) {
interval = millis();
while (port.available() > 0) {
int ch = port.read();
print((char)ch);
if (ch == '$') {serialCount = 0;} // this will help with
alignment
if (aligned < 4) {
// make sure we are properly aligned on a 14-byte packet
if (serialCount == 0) {
if (ch == '$') aligned++; else aligned = 0;
} else if (serialCount == 1) {
if (ch == 2) aligned++; else aligned = 0;
} else if (serialCount == 12) {
if (ch == '\r') aligned++; else aligned = 0;
} else if (serialCount == 13) {
if (ch == '\n') aligned++; else aligned = 0;
}
//println(ch + " " + aligned + " " + serialCount);
serialCount++;
if (serialCount == 14) serialCount = 0;
} else {
if (serialCount > 0 || ch == '$') {
teapotPacket[serialCount++] = (char)ch;
if (serialCount == 14) {
serialCount = 0; // restart packet byte position
q[0] = ((teapotPacket[2] << 8) | teapotPacket[3]) /
16384.0f;
q[1] = ((teapotPacket[4] << 8) | teapotPacket[5]) /
16384.0f;
q[2] = ((teapotPacket[6] << 8) | teapotPacket[7]) /
16384.0f;
q[3] = ((teapotPacket[8] << 8) | teapotPacket[9]) /
16384.0f;
for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 +
q[i];
quat.set(q[0], q[1], q[2], q[3]);

5/24/2015
gravity[0] = 2 * (q[1]*q[3] - q[0]*q[2]);
gravity[1] = 2 * (q[0]*q[1] + q[2]*q[3]);
gravity[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] +
q[3]*q[3];
euler[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3],
2*q[0]*q[0] + 2*q[1]*q[1] - 1);
euler[1] = -asin(2*q[1]*q[3] + 2*q[0]*q[2]);
euler[2] = atan2(2*q[2]*q[3] - 2*q[0]*q[1],
2*q[0]*q[0] + 2*q[3]*q[3] - 1);
ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3],
2*q[0]*q[0] + 2*q[1]*q[1] - 1);
ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1]
+ gravity[2]*gravity[2]));
ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0]
+ gravity[2]*gravity[2]));
*/

vertex(0, 0, 0);
for (int i = 0; i < sides + 1; i++) {
vertex(topRadius * cos(angle), 0, topRadius *
sin(angle));
angle += angleIncrement;
}
endShape();
}
// If it is not a cone, draw the circular bottom cap
if (bottomRadius != 0) {
angle = 0;
beginShape(TRIANGLE_FAN);
// Center point
vertex(0, tall, 0);
for (int i = 0; i < sides + 1; i++) {
vertex(bottomRadius * cos(angle), tall, bottomRadius *
sin(angle));
angle += angleIncrement;
}
endShape();
}

}
}
}

}
}
void drawCylinder(float topRadius, float bottomRadius, float
tall, int sides) {
float angle = 0;
float angleIncrement = TWO_PI / sides;
beginShape(QUAD_STRIP);
for (int i = 0; i < sides + 1; ++i) {
vertex(topRadius*cos(angle), 0, topRadius*sin(angle));
vertex(bottomRadius*cos(angle), tall,
bottomRadius*sin(angle));
angle += angleIncrement;
}
endShape();
// If it is not a cone, draw the circular top cap
if (topRadius != 0) {
angle = 0;
beginShape(TRIANGLE_FAN);
// Center point

VII. REFERENCIAS
[1]
[2]
[3]

http://www.geekmomprojects.com/mpu-6050-dmp-data-from-i2cdevlib/
http://3dgep.com/understanding-quaternions/
http://tutis.ca/Rotate/7quaternions.htm