查看: 20487|回复: 4

[Arduino/Genuino 101 入门教程] Arduino/Genuino 101 入门教程三：姿态角

入门教程二最后说教你用数据来干坏事，想了想，不能干，万一被警察叔叔抓去了就不好了

，要做个良好市民。但是数据都读出来了，不用它干点啥多可惜，下面就用它来计算刚体的姿态角，这样，前面下载的库文件就派上用场了。
先热热身，扯一会，姿态角，这个词对玩四轴或者对四轴感兴趣的人一定不陌生，因为比较穷，菜鸟就默默做了一个感兴趣的人，先让我哭一会

。

哭完了，继续，在我刚接触姿态角，并且想把它算出来的时候，是很困难的，先把加速度计，陀螺仪和电子托盘获取的数据折腾出来，再对数据滤波得到更加稳定有效的数据，用到互补滤波，卡尔曼滤波什么的，再对滤波后的数据处理计算出欧拉角，虽然有现成算法，但是弄明白这些算法的原理相当难！你可以试试哦，欢迎入坑！:victory:

咳咳，扯完了，干正事：
这里，如果撇开算法的理论，站在巨人的肩膀上，想得到姿态角还是挺容易的，直接调用库函数：MadgwickAHRS(四元数梯度下降算法),最后我们将得到的四元数转换成欧拉角。但是，要想得到比较直观的欧拉角，这里提供的库函数我们需要修改一下，因为现在的计算公式是为了另一个好玩的示例准备的，下个教程教你用，这回不骗你！:lol：

【产品链接】： Arduino 101

找到库文件MadgwickAHRS，如果你在前面安装了库Madgwick打开Arduino IDE的文件 -> 首选项
Arduino/Genuino 101 入门教程三：姿态角图1

复制上面的文件夹位置，在电脑里面搜索，会在文件夹下找到libraries/Madgwick，里面就有算法的源代码（我的完整目录是：C:\Users\LG\Documents\Arduino\libraries\Madgwick\src）：
Arduino/Genuino 101 入门教程三：姿态角图2

打开MadgwickAHRS.h文件，在头文件中我们能找到三个成员函数：getPitch(), getRoll(), getYaw() ：
Arduino/Genuino 101 入门教程三：姿态角图3

修改：
我们可以在原来三个成员函数的返回值各乘以57.3，也能得到欧拉角，因为后面我们还需要用到这个库，所以我们再添加三个成员函数，计算公式稍作改变，这三个获取欧拉角的计算公式我有点纠结，因为查资料我得到了不同版本的计算公式，我用了下面的，如果这个公式有问题，还望哪位大神及时纠正，非常感谢！：
float mygetPitch(){return atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;};
float mygetRoll(){return asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;};
float mygetYaw(){return atan2f( 2 * (q0 * q1 + q2 * q3), q0*q0 - q1*q1 - q2*q2 + q3*q3 )*57.3;};

最后的有文件MadgwickAHRS.h是这样的，保存一下：
Arduino/Genuino 101 入门教程三：姿态角图4

下载测试程序，在源码的基础上，修改了获取欧拉角的三个成员函数：

/*
  ===============================================
  Example sketch for CurieIMU library for Intel(R) Curie(TM) devices.
  Copyright (c) 2015 Intel Corporation.  All rights reserved.

  Based on I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050
  class by Jeff Rowberg: https://github.com/jrowberg/i2cdevlib

  ===============================================
  I2Cdev device library code is placed under the MIT license
  Copyright (c) 2011 Jeff Rowberg

  Permission is hereby granted, free of charge, to any person obtaining a copy
  of this software and associated documentation files (the "Software"), to deal
  in the Software without restriction, including without limitation the rights
  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  copies of the Software, and to permit persons to whom the Software is
  furnished to do so, subject to the following conditions:

  The above copyright notice and this permission notice shall be included in
  all copies or substantial portions of the Software.

  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  THE SOFTWARE.
  ===============================================

  Genuino 101 CurieIMU Orientation Visualiser
  Hardware Required:
  * Arduino/Genuino 101

  Modified Nov 2015
  by Helena Bisby <support@arduino.cc>
  This example code is in the public domain
  https://arduino.cc/en/Tutorial/Genuino101CurieIMUOrientationVisualiser
*/

#include <CurieIMU.h>
#include <MadgwickAHRS.h>

Madgwick filter; // initialise Madgwick object
int ax, ay, az;
int gx, gy, gz;
float yaw;
float pitch;
float roll;
int factor = 800; // variable by which to divide gyroscope values, used to control sensitivity
// note that an increased baud rate requires an increase in value of factor

int calibrateOffsets = 1; // int to determine whether calibration takes place or not


void setup() {
  // initialize Serial communication
  Serial.begin(9600);

  // initialize device
  CurieIMU.begin();

  if (calibrateOffsets == 1) {
    // use the code below to calibrate accel/gyro offset values
    Serial.println("Internal sensor offsets BEFORE calibration...");
    Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getGyroOffset(X_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getGyroOffset(Y_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getGyroOffset(Z_AXIS)); Serial.print("\t");
    Serial.println("");

    Serial.print("Starting Gyroscope calibration...");
    CurieIMU.autoCalibrateGyroOffset();
    Serial.println(" Done");
    Serial.print("Starting Acceleration calibration...");
    CurieIMU.autoCalibrateAccelerometerOffset(X_AXIS, 0);
    CurieIMU.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
    CurieIMU.autoCalibrateAccelerometerOffset(Z_AXIS, 1);
    Serial.println(" Done");
  }
}

void loop() {
  // read raw accel/gyro measurements from device
  CurieIMU.readMotionSensor(ax, ay, az, gx, gy, gz); 

  // use function from MagdwickAHRS.h to return quaternions
  filter.updateIMU(gx / factor, gy / factor, gz / factor, ax, ay, az);

  // functions to find yaw roll and pitch from quaternions
  //yaw = filter.mygetYaw();
  roll = filter.mygetRoll();
  pitch = filter.mygetPitch();

  // print gyro and accel values for debugging only, comment out when running Processing

   Serial.print(0);
    Serial.print(","); // print comma so values can be parsed
    Serial.print(pitch);
    Serial.print(","); // print comma so values can be parsed
    Serial.println(roll);


 /*  Serial.print(ax); Serial.print("\t");
  Serial.print(ay); Serial.print("\t");
  Serial.print(az); Serial.print("\t");
  Serial.print(gx); Serial.print("\t");
  Serial.print(gy); Serial.print("\t");
  Serial.print(gz); Serial.print("\t");
  Serial.println("");


  if (Serial.available() > 0) {
    int val = Serial.read();
    if (val == 's') { // if incoming serial is "s"
      Serial.print(yaw);
      Serial.print(","); // print comma so values can be parsed
      Serial.print(pitch);
      Serial.print(","); // print comma so values can be parsed
      Serial.println(roll);
    }
  }*/
}
复制代码

串口得到的数据，因为没有电子罗盘对Yaw的数据进行修正，这个数据是会飘的，不准确，所以注释了对他的计算，你可以打开试试，我把板子绕X轴旋转90度得到的数据。
Arduino/Genuino 101 入门教程三：姿态角图5

OK，到这里我们能得到板子的姿态角了，这里有些问题，希望大神能指点一二：

1.在库函数原来的计算公式后面乘以57.3也能得到欧拉角，两个欧拉角的计算公式不同，都对吗，还是都存在问题？
2.在单独获取Pitch数据的时候比较稳定，基本对Roll没有影响，但是最后数据收敛的速度有点慢，不知道为什么？
3.在单独获取Roll数据的时候，Pitch数据也有变动，不应该是这样的，这是为什么？

拓展：
在CurieIMU这个驱动库的examples里面还有很多示例可以玩起来，下面没有写进教程的例子简单说明一下：

AccelerometerOrientation：当你转动开发板的时候，示例会告诉你你现在开发板是处于什么状态，例如：正面并且数字引脚向上，串口会显示：
up
digital pins up"

RawImuDataSerial：读取加速度计和陀螺仪的原始数据，可以读出来看看
ShockDetect：检测来自坐标轴方向的震动，体验不是很好
StepDetect：简单计步器算法，能工作，体验不是很好
TapDetect：检测来自坐标轴方向的敲击，体验不是很好

补充一下：
你可能会发现，在计算四元数的时候用到了下面的成员函数：
void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
代码中是这样调用的：
filter.updateIMU(gx / factor, gy / factor, gz / factor, ax, ay, az);

问题来了，这里的参数只有加速度计和陀螺仪的，那么，如果我们拿到了集成了电子罗盘的DFRobot 版Inter Curie，怎么把电子罗盘的数据融合进去呢？不用担心哦，在算法库里面还有个成员函数能帮你解决问题：
void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
现在你可以玩九轴了，是不是很开心！

看累了没，开心一刻：
朋友们一起去爬山,到山顶,一女生面对秀丽山河高喊:祖国啊!我的母亲!一暗恋她的男生赶紧跟着大喊:祖国啊!我的丈母娘!