Pages: [1]   Go Down
Author Topic: [parte 4]PID e quaternioni ovvero DCM e IMU  (Read 2295 times)
0 Members and 1 Guest are viewing this topic.
0
Offline Offline
Shannon Member
****
Karma: 117
Posts: 10113
:(){:|:&};: TOX id: fcb8e918bef08581e23f6ddf9d4dba77697c25b217bf372736ed959a95fde36df5b8c5b90fbb
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
//=====================================================================================================
// 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:
//=====================================================================================================
// 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;
}
Logged

my Arduino code: https://github.com/lestofante/arduinoSketch
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

0
Offline Offline
Faraday Member
**
Karma: 24
Posts: 2825
Arduino rocks
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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.

Logged

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

0
Offline Offline
Shannon Member
****
Karma: 117
Posts: 10113
:(){:|:&};: TOX id: fcb8e918bef08581e23f6ddf9d4dba77697c25b217bf372736ed959a95fde36df5b8c5b90fbb
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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
Logged

my Arduino code: https://github.com/lestofante/arduinoSketch
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

milano, italy
Offline Offline
Sr. Member
****
Karma: 0
Posts: 493
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

ma cosa dovresti far volare esattamente? un quadricottero? hai foto?  smiley
comunque complimenti per il progetto lo sfruttero anche io  smiley-wink

c
Logged

0
Offline Offline
Faraday Member
**
Karma: 24
Posts: 2825
Arduino rocks
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

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

Pages: [1]   Go Up
Jump to: