Pitch Roll Yaw с использованием кватернионов - PullRequest
Новая
       70

Pitch Roll Yaw с использованием кватернионов

0 голосов
/ 11 марта 2019

Я пытаюсь получить Pitch, Roll и Yaw из кватернионных значений благодаря приведенному ниже коду Arduino. Когда я печатаю эти значения на мониторе последовательного порта или в режиме «Обработка», устройство слишком «чувствительно», а значение рыскания не является точным. Это может произойти из-за моей инициализации вектора Q или из-за усиления Kp и Ki. Я безуспешно пытался настроиться на прибыль.

Кто-нибудь может мне помочь ?? Это было бы очень приятно!

Всем хорошего дня!

Код: 

#include <Wire.h>
#include <SPI.h>
#include <Kalman.h>
#include <math.h>
#include <Adafruit_LSM9DS1.h>
#include <Adafruit_Sensor.h>

#define LSM9DS1_SCK A5
#define LSM9DS1_MISO 12
#define LSM9DS1_MOSI A4
#define LSM9DS1_XGCS 6
#define LSM9DS1_MCS 5
#define CONVERSIONG 3.9

// i2c
Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1();

float q[4]={1.0f,0.0f,0.0f,0.0f};
float Ki=0.005;
float Kp=2;
float eInt[3];
uint32_t timer;
double accX,accY,accZ;
double gyroX,gyroY,gyroZ;
double magX,magY,magZ;
float pitch;
float roll;
float yaw;

void setupSensor()
{
// 1.) Set the accelerometer range
lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G);

// 2.) Set the magnetometer sensitivity

// 3.) Setup the gyroscope
lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_245DPS);

}

void setup() {

Serial.begin(115200);
while (!Serial) {
  delay(1); 
}
if (!lsm.begin())
{
  Serial.println("Oops ... unable to initialize the LSM9DS1. Check your wiring!");
  while (1);
}
Serial.println("Found LSM9DS1 9DOF"); 
setupSensor();
}

void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
  float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local 
  variable for readability
  float norm;
  float hx, hy, bx, bz;
  float vx, vy, vz, wx, wy, wz;
  float ex, ey, ez;
  float pa, pb, pc;

  float q1q1 = q1 * q1;
  float q1q2 = q1 * q2;
  float q1q3 = q1 * q3;
  float q1q4 = q1 * q4;
  float q2q2 = q2 * q2;
  float q2q3 = q2 * q3;
  float q2q4 = q2 * q4;
  float q3q3 = q3 * q3;
  float q3q4 = q3 * q4;
  float q4q4 = q4 * q4;   

  // Normalise accelerometer measurement
  norm = sqrtf(ax * ax + ay * ay + az * az);
  if (norm == 0.0f) return; // handle NaN
  norm = 1.0f / norm;        // use reciprocal for division
  //Serial.println(norm);
  ax *= norm;
  ay *= norm;
  az *= norm;

  // Normalise magnetometer measurement
  norm = sqrtf(mx * mx + my * my + mz * mz);
  if (norm == 0.0f) return; // handle NaN
  norm = 1.0f / norm;        // use reciprocal for division
  //Serial.println(norm);
  mx *= norm;
  my *= norm;
  mz *= norm;

  // Reference direction of Earth's magnetic field
  hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
  hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
  bx = sqrtf((hx * hx) + (hy * hy));
  bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);

  // Estimated direction of gravity and magnetic field
  vx = 2.0f * (q2q4 - q1q3);
  vy = 2.0f * (q1q2 + q3q4);
  vz = q1q1 - q2q2 - q3q3 + q4q4;
  wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
  wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
  wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);  

  // Error is cross product between estimated direction and measured direction of gravity
  ex = (ay * vz - az * vy) + (my * wz - mz * wy);
  ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
  ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
  if (Ki > 0.0f)
  {
      eInt[0] += ex;      // accumulate integral error
      eInt[1] += ey;
      eInt[2] += ez;
  }
  else
  {
      eInt[0] = 0.0f;     // prevent integral wind up
      eInt[1] = 0.0f;
      eInt[2] = 0.0f;
  }

  // Apply feedback terms
  gx = gx + Kp * ex + Ki * eInt[0];
  gy = gy + Kp * ey + Ki * eInt[1];
  gz = gz + Kp * ez + Ki * eInt[2];

  // Integrate rate of change of quaternion
  pa = q2;
  pb = q3;
  pc = q4;
  double dt = (double)(micros()-timer)/1000000;
  timer=micros();
  q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * dt);
  q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * dt);
  q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * dt);
  q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * dt);

  // Normalise quaternion
  norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
  norm = 1.0f / norm;
  q[0] = q1 * norm;
  q[1] = q2 * norm;
  q[2] = q3 * norm;
  q[3] = q4 * norm;
}

void loop() {
// put your main code here, to run repeatedly:
lsm.read();  /* ask it to read in the data */ 

/* Get a new sensor event */ 
sensors_event_t a, m, g, temp;

lsm.getEvent(&a, &m, &g, &temp);

gyroX=g.gyro.x;
gyroY=g.gyro.y;
gyroZ=g.gyro.z;

accX=a.acceleration.x;
accY=a.acceleration.y;
accZ=a.acceleration.z;

magX=m.magnetic.x;
magY=m.magnetic.y;
magZ=m.magnetic.z;

MahonyQuaternionUpdate(accX, accY, accZ, gyroX, gyroY, gyroZ, magX, magY, magZ);

pitch=atan2(2*(q[0]*q[1]+q[2]*q[3]),1-2*(q[1]*q[1]+q[2]*q[2]))*RAD_TO_DEG;
roll=asin(2*(q[0]*q[2]-q[3]*q[1]))*RAD_TO_DEG;
yaw=atan2(2*(q[0]*q[3]+q[1]*q[2]),1-2*(q[2]*q[2]+q[3]*q[3]))*RAD_TO_DEG;
Serial.print(pitch);
Serial.print(',');
Serial.print(roll);
Serial.print(',');
Serial.println(yaw);

}

...