Go Down

Topic: [parte 4]PID e quaternioni ovvero DCM e IMU (Read 2966 times) previous topic - next topic

lesto

Partiamo con le parti di codice NON scritte da me, ma solo modificate per i miei scopi.
Quello che propongo ora è un algoritmo che permette di stimare i 3 angoli attuali (roll pitch e yaw) interpolando i valori di accelerometri e giroscopi.
l'errore accumulato sugli assi roll e pitch verrà annullato dall'accelerometro, rimane però un'errore cumulativo sullo yaw, risolvibile utilizzando un magnetometro.
Il codice originale è preso da: http://code.google.com/p/imumargalgorithm30042010sohm/ e riadattato da me :-)
in particolare elimino l'eventuale lettura 0,0,0 da parte degli accelerometri che impalla tutto l'algoritmo  8-)
In pratica si tratta di un PID P+I utilizzando però i quaternioni.
Il risultato di tutta questa matematica è un quaternione di rotazione che può essere ritrasformato in angoli di inclinazione con la formula:
float roll=atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2));
float pitch=asin( 2*(q0*q2-q1*q3) );
float yaw=atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3));

IMU.h
Code: [Select]

//=====================================================================================================
// IMU.h
// S.O.H. Madgwick
// 25th September 2010
//
// Modified as class for arduino by Lestofante
// 27th December 2010
//
//=====================================================================================================
//
// See IMU.c file for description.
//
//=====================================================================================================
#ifndef IMU_h
#define IMU_h

#include <WProgram.h>
//----------------------------------------------------------------------------------------------------
// Variable declaration
class IMU{
 
 public:
   float q0, q1, q2, q3;      // quaternion elements representing the estimated orientation

   //---------------------------------------------------------------------------------------------------
   // Function declaration
   IMU(); //constructor, set exInt, eyInt and ezInt to 0
   //void IMUupdateVerbose(float gx, float gy, float gz, float ax, float ay, float az);
   void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az);
 private:
   float exInt, eyInt, ezInt;      // scaled integral error
 
};
#endif
//=====================================================================================================
// End of file
//=====================================================================================================


IMU.cpp
Code: [Select]

//=====================================================================================================
// IMU.c
// S.O.H. Madgwick
// 25th September 2010
//
// IMU.cpp
// Modified as class for arduino by Lestofante
// 27th December 2010
//=====================================================================================================
// Description:
//
// Quaternion implementation of the 'DCM filter' [Mayhony et al].
//
// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
//
// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
// orientation.  See my report for an overview of the use of quaternions in this application.
//
// User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz')
// and accelerometer ('ax', 'ay', 'ay') data.  Gyroscope units are radians/second, accelerometer
// units are irrelevant as the vector is normalised.
//
//=====================================================================================================

//----------------------------------------------------------------------------------------------------
// Header files

#include "IMU.h"
//#include <math.h>

//----------------------------------------------------------------------------------------------------
// Definitions

#define Kp 2.0f                  // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.005f            // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.00115f            // half the sample period

//---------------------------------------------------------------------------------------------------
// Variable definitions

//float q0 = 1, q1 = 0, q2 = 0, q3 = 0;      // quaternion elements representing the estimated orientation
//float exInt = 0, eyInt = 0, ezInt = 0;      // scaled integral error

//====================================================================================================
// Function
//====================================================================================================

IMU::IMU(){
 exInt=0;
 eyInt=0;
 ezInt=0;
 q0=1;
 q1=0;
 q2=0;
 q3=0;
}

void IMU::IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
     float norm;
     float vx, vy, vz;
     float ex, ey, ez;        
     
       //eliminate bad data:
       if (ax==0 && ay==0 && az==0){
         Serial.println("Accelerometer bad data");
         return;
       }

     // normalise the measurements
     norm = sqrt(ax*ax + ay*ay + az*az);
     ax = ax / norm;
     ay = ay / norm;
     az = az / norm;            

     // estimated direction of gravity
     vx = 2*(q1*q3 - q0*q2);
     vy = 2*(q0*q1 + q2*q3);
     vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
       
     // error is sum of cross product between reference direction of field and direction measured by sensor
     ex = (ay*vz - az*vy);
     ey = (az*vx - ax*vz);
     ez = (ax*vy - ay*vx);

     // integral error scaled integral gain
     exInt = exInt + ex*Ki;
     eyInt = eyInt + ey*Ki;
     ezInt = ezInt + ez*Ki;

     // adjusted gyroscope measurements
     gx = gx + Kp*ex + exInt;
     gy = gy + Kp*ey + eyInt;
     gz = gz + Kp*ez + ezInt;
       
     // integrate quaternion rate and normalise
     q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
     q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
     q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
     q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
     
     // normalise quaternion
     norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
     q0 = q0 / norm;
     q1 = q1 / norm;
     q2 = q2 / norm;
     q3 = q3 / norm;
}
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

MauroTec

Bravo lesto, già i quaternioni li ho mangiati oggi a pranzo ma mi son rimasti sullo stomaco. ;D

Quindi a che punto sei?
L'hai già sperimentato sul campo?

Ciao.

AvrDudeQui front end per avrdude https://gitorious.org/avrdudequi/pages/Home

lesto

graficamente l'algoritmo funziona più che bene. Però manca la prova sul campo e vedere se un PID solo proporzionale sui motori basta e avanza
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

camba192

ma cosa dovresti far volare esattamente? un quadricottero? hai foto?  :)
comunque complimenti per il progetto lo sfruttero anche io  ;)

c

MauroTec

Un consiglio butta tutto sul playground italiano così comè, perchè tra un pò i post andranno a finire troppo indietro, anche se non è completo c'è sempre da prendere spunto, sia per gli interrupt, che per l'algoritmo.

Io penso che P e I siano sufficienti, D è difficile metterlo a punto, cioè appenda introduci D e facile che devi modificare anche P e I per evitare instabilità, quindi andrei verso la sperimentazione sul campo senza D.

Ciao.
AvrDudeQui front end per avrdude https://gitorious.org/avrdudequi/pages/Home

Go Up
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy