从陀螺仪数据获取四元数如何获得身体坐标？

问题描述

我已经将陀螺仪连接到arduino，并且在所有三个轴上都获得了以弧度/秒为单位的角速率。

我希望能够摆脱偏航，俯仰和滚入身体坐标，因此三个旋转轴固定在身体上。我现在遇到的问题是，当我滚动传感器时，我得到的偏航和俯仰被交换了。当我将传感器旋转90度时，偏航和俯仰改变了位置。介于两者之间的任何位置，偏航和俯仰都是两者之间的混合。

相反，我想相对于新的身体旋转而不是初始位置保持俯仰和偏航。

这是我的代码：

void loop() {
  currentTime = millis();
  dt = ((currentTime - prevTime) / 1000.0 );
  
  // Puts gyro data into data[2],data[4],data[5]
  readBMI();

  if(firstPass == false) {
    
    omega[0] = (data[3]);
    omega[1] = (data[4]);
    omega[2] = (data[5]);

    wLength = sqrt(sq(omega[0]) + sq(omega[1]) + sq(omega[2]));
    
    theta = wLength * dt;
    q_new[0] = cos(theta/2);
    q_new[1] = (omega[0] / wLength * sin(theta / 2));
    q_new[2] = (omega[1] / wLength * sin(theta / 2));
    q_new[3] = (omega[2] / wLength * sin(theta / 2));

    q[0] = q[0] * q_new[0] - q[1] * q_new[1] - q[2] * q_new[2] - q[3] * q_new[3];
    q[1] = q[0] * q_new[1] + q[1] * q_new[0] + q[2] * q_new[3] - q[3] * q_new[2];
    q[2] = q[0] * q_new[2] - q[1] * q_new[3] + q[2] * q_new[0] + q[3] * q_new[1];
    q[3] = q[0] * q_new[3] + q[1] * q_new[2] - q[2] * q_new[1] + q[3] * q_new[0];

    float sinr_cosp = 2 * (q[0] * q[1] + q[2] * q[3]);
    float cosr_cosp = 1 - 2 * (sq(q[1]) + sq(q[2]));
    roll = atan2(sinr_cosp,cosr_cosp)  * 180 / PI;

    pitch = asin(2 * (q[0] * q[2] - q[3] * q[1]))  * 180 / PI;

    double siny_cosp = 2 * (q[0] * q[3] + q[1] * q[2]);
    double cosy_cosp = 1 - 2 * (sq(q[2]) + sq(q[3]));
    yaw = atan2(siny_cosp,cosy_cosp)  * 180 / PI;

  }

  Serial.print(roll);
  Serial.print(" ");
  Serial.print(pitch);
  Serial.print(" ");
  Serial.print(yaw);
  Serial.print(" ");
  Serial.println();

  delay(20);
  prevTime = currentTime;
}

我正确地确定了角度，但是我唯一的问题是它滚动时的偏航和俯仰交换。所以我想我需要一种从世界转化为身体的配偶的方法吗？

解决方法

暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！

如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。

小编邮箱:dio#foxmail.com (将#修改为@）

c++gyroscope math quaternions