查看: 10859|回复: 4

Arduino/Genuino 101 入门教程三:姿态角

[复制链接]
入门教程二最后说教你用数据来干坏事,想了想,不能干,万一被警察叔叔抓去了就不好了,要做个良好市民。但是数据都读出来了,不用它干点啥多可惜,下面就用它来计算刚体的姿态角,这样,前面下载的库文件就派上用场了。
先热热身,扯一会,姿态角,这个词对玩四轴或者对四轴感兴趣的人一定不陌生,因为比较穷,菜鸟就默默做了一个感兴趣的人,先让我哭一会

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

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

【产品链接】:  Arduino 101

找到库文件MadgwickAHRS,如果你在前面安装了库Madgwick打开Arduino IDE的 文件 -> 首选项
1.jpg

复制上面的文件夹位置,在电脑里面搜索,会在文件夹下找到libraries/Madgwick,里面就有算法的源代码(我的完整目录是:C:\Users\LG\Documents\Arduino\libraries\Madgwick\src):
2.jpg

打开MadgwickAHRS.h文件,在头文件中我们能找到三个成员函数:getPitch(), getRoll(), getYaw() :
3.jpg

修改:
我们可以在原来三个成员函数的返回值各乘以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是这样的,保存一下:
4.jpg

下载测试程序,在源码的基础上,修改了获取欧拉角的三个成员函数:
  1. /*
  2.   ===============================================
  3.   Example sketch for CurieIMU library for Intel(R) Curie(TM) devices.
  4.   Copyright (c) 2015 Intel Corporation.  All rights reserved.

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

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

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

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

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

  26.   Genuino 101 CurieIMU Orientation Visualiser
  27.   Hardware Required:
  28.   * Arduino/Genuino 101

  29.   Modified Nov 2015
  30.   by Helena Bisby <support@arduino.cc>
  31.   This example code is in the public domain
  32.   http://arduino.cc/en/Tutorial/Genuino101CurieIMUOrientationVisualiser
  33. */

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

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

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


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

  48.   // initialize device
  49.   CurieIMU.begin();

  50.   if (calibrateOffsets == 1) {
  51.     // use the code below to calibrate accel/gyro offset values
  52.     Serial.println("Internal sensor offsets BEFORE calibration...");
  53.     Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS)); Serial.print("\t");
  54.     Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS)); Serial.print("\t");
  55.     Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS)); Serial.print("\t");
  56.     Serial.print(CurieIMU.getGyroOffset(X_AXIS)); Serial.print("\t");
  57.     Serial.print(CurieIMU.getGyroOffset(Y_AXIS)); Serial.print("\t");
  58.     Serial.print(CurieIMU.getGyroOffset(Z_AXIS)); Serial.print("\t");
  59.     Serial.println("");

  60.     Serial.print("Starting Gyroscope calibration...");
  61.     CurieIMU.autoCalibrateGyroOffset();
  62.     Serial.println(" Done");
  63.     Serial.print("Starting Acceleration calibration...");
  64.     CurieIMU.autoCalibrateAccelerometerOffset(X_AXIS, 0);
  65.     CurieIMU.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
  66.     CurieIMU.autoCalibrateAccelerometerOffset(Z_AXIS, 1);
  67.     Serial.println(" Done");
  68.   }
  69. }

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

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

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

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

  80.    Serial.print(0);
  81.     Serial.print(","); // print comma so values can be parsed
  82.     Serial.print(pitch);
  83.     Serial.print(","); // print comma so values can be parsed
  84.     Serial.println(roll);


  85. /*  Serial.print(ax); Serial.print("\t");
  86.   Serial.print(ay); Serial.print("\t");
  87.   Serial.print(az); Serial.print("\t");
  88.   Serial.print(gx); Serial.print("\t");
  89.   Serial.print(gy); Serial.print("\t");
  90.   Serial.print(gz); Serial.print("\t");
  91.   Serial.println("");


  92.   if (Serial.available() > 0) {
  93.     int val = Serial.read();
  94.     if (val == 's') { // if incoming serial is "s"
  95.       Serial.print(yaw);
  96.       Serial.print(","); // print comma so values can be parsed
  97.       Serial.print(pitch);
  98.       Serial.print(","); // print comma so values can be parsed
  99.       Serial.println(roll);
  100.     }
  101.   }*/
  102. }
复制代码

串口得到的数据,因为没有电子罗盘对Yaw的数据进行修正,这个数据是会飘的,不准确,所以注释了对他的计算,你可以打开试试,我把板子绕X轴旋转90度得到的数据。
5.jpg



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);
现在你可以玩九轴了,是不是很开心!

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

Arduino/Genuino 101 入门教程】

  * DF创客社区版权所有,
欢迎转载
  转载请务必标注来源: DF创客社区+作者姓名+原文网址。



苦海  初级技匠

发表于 2016-3-20 10:19:14

1秒可以更新多少次姿态数据 官方有这方面的资料么 ??
回复 支持 反对

使用道具 举报

Jason_G  高级技师
 楼主|

发表于 2016-3-20 11:08:07

苦海 发表于 2016-3-20 10:19
1秒可以更新多少次姿态数据 官方有这方面的资料么 ??

没有实测,这个资料我还没看到。
回复 支持 反对

使用道具 举报

iooops  版主

发表于 2016-3-20 21:29:47

我来占个地板
回复 支持 反对

使用道具 举报

nxcosa  中级技师

发表于 2016-4-6 11:59:33

本帖最后由 nxcosa 于 2016-4-6 12:06 编辑

66666666666666
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

为本项目制作心愿单
购买心愿单
心愿单 编辑
wifi气象站

硬件清单

btnicon
我也要做!
点击进入购买页面
上海智位机器人股份有限公司 沪ICP备09038501号-4

© 2013-2019 Comsenz Inc. Powered by Discuz! X3.4 Licensed

mail