极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 125016|回复: 101

MPU6050姿态识别【抛砖】,引'弘毅''Randy'等人【抛玉】

  [复制链接]
发表于 2012-9-17 15:16:54 | 显示全部楼层 |阅读模式
本帖最后由 Ansifa 于 2013-3-4 01:51 编辑

Ansifa注:下面的程序我打包了一个压缩包,已经验证编译。大家可以直接下载



2012年10月26日更新:

把转换原始数据地方改成:
  1.       RwAcc[0] = float(inputStringArr[0]) / 16384.0;
  2.       RwAcc[1] = float(inputStringArr[1])/ 16384.0;
  3.       RwAcc[2] = float(inputStringArr[2])/ 16384.0;

  4.       // 把原始数据转换为"度/秒"
  5.       Gyro[0] = float(inputStringArr[3]) / 131.0;
  6.       Gyro[1] = float(inputStringArr[4]) / 131.0;
  7.       Gyro[2] = float(inputStringArr[5]) /131.0;
复制代码


其实Randy的帖子(获取陀螺仪与加速度相关数据的Arduino示例代码 )里面demo程序已经是这个参数了,但因为没使用浮点形式(即必需使用16384.0,而非16384)导致最后ax/ay/az的值都为0了,所以也就反映不出加速度的微小变化了。

在更新之前,当把模块树立时,processing里显示的3D方块会慢慢恢复到水平状态;而通过上述更新之后,再把模块树立,processing里显示的3D方块也会保持树立状态,这才是正确的!


======================================================

从 Randy 他们公司购买的 MPU6050 模块,集成陀螺仪与加速度传感器。虽然 Randy 已经提供了获取陀螺仪与加速度相关数据的Arduino示例代码 ,但怎么用这些数据来展现当前的姿态却也还是个问题;后来看到弘毅关于ITG3205与ADXL345的姿态识别帖子,然后抄过来一试,效果还真的出来了。

Arduino代码:
  1. #include "Wire.h"
  2. #include "I2Cdev.h"
  3. #include "MPU6050.h"

  4. MPU6050 accelgyro;

  5. int16_t ax, ay, az;
  6. int16_t gx, gy, gz;

  7. char str[512];

  8. void setup() {
  9.     Wire.begin();
  10.     Serial.begin(9600);
  11.     accelgyro.initialize();
  12. }

  13. void loop() {
  14.     accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  15.     sprintf(str, "%d,%d,%d,%d,%d,%d,%d", ax, ay, az, gx, gy, gz);  
  16.     Serial.print(str);
  17.     Serial.write(byte(10));
  18.     delay(50);
  19. }
复制代码


Processing代码:
  1. import processing.serial.*;

  2. Serial myPort;  // 创建串口对象myPort

  3. boolean firstSample = true;

  4. float [] RwAcc = new float[3];         // 通过加速度传感器把重力加速度投影在x/y/z三轴上
  5. float [] Gyro = new float[3];          // 陀螺仪读取
  6. float [] RwGyro = new float[3];        // 重新读取陀螺仪
  7. float [] Awz = new float[2];           // XZ/ YZ平面和Z轴(度)R的投影之间的角度
  8. float [] RwEst = new float[3];


  9. int lastTime = 0;
  10. int interval = 0;
  11. float wGyro = 10.0;

  12. int lf = 10; // 10在ASCII表中表示'\n'
  13. byte[] inBuffer = new byte[100];

  14. PFont font;
  15. final int VIEW_SIZE_X = 600, VIEW_SIZE_Y = 600;


  16. void setup()
  17. {
  18.   size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
  19.   
  20.   //myPort = new Serial(this, Serial.list()[2], 9600); // 设置电脑第三个COM口为连接端口,这个要根据你电脑情况进行设置。
  21.   myPort = new Serial(this, "/dev/ttyUSB0", 9600);  

  22.   // 加载字体,字体必须在代码文件同目录下的data文件夹中
  23.   font = loadFont("/home/zwang/processing/processing-1.5.1/modes/android/examples/Basics/Typography/Letters/data/CourierNew36.vlw");
  24. }


  25. void readSensors() {
  26.   if (myPort.available() > 0) {
  27.     if (myPort.readBytesUntil(lf, inBuffer) > 0) {
  28.       String inputString = new String(inBuffer);
  29.       String [] inputStringArr = split(inputString, ',');

  30.       // 把原始数据转换为G
  31.       RwAcc[0] = float(inputStringArr[0]) / 16384.0;
  32.       RwAcc[1] = float(inputStringArr[1])/ 16384.0;
  33.       RwAcc[2] = float(inputStringArr[2])/ 16384.0;

  34.       // 把原始数据转换为"度/秒"
  35.       Gyro[0] = float(inputStringArr[3]) / 131.0;
  36.       Gyro[1] = float(inputStringArr[4]) / 131.0;
  37.       Gyro[2] = float(inputStringArr[5]) / 131.0;
  38.     }
  39.   }
  40. }


  41. void normalize3DVec(float [] vector) {
  42.   float R;
  43.   R = sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
  44.   vector[0] /= R;
  45.   vector[1] /= R;  
  46.   vector[2] /= R;
  47. }


  48. float squared(float x) {
  49.   return x*x;
  50. }


  51. void buildBoxShape() {
  52.   //box(60, 10, 40);
  53.   noStroke();
  54.   beginShape(QUADS);

  55.   //Z+ (绘图区域)
  56.   fill(#00ff00);
  57.   vertex(-30, -5, 20);
  58.   vertex(30, -5, 20);
  59.   vertex(30, 5, 20);
  60.   vertex(-30, 5, 20);

  61.   //Z-
  62.   fill(#0000ff);
  63.   vertex(-30, -5, -20);
  64.   vertex(30, -5, -20);
  65.   vertex(30, 5, -20);
  66.   vertex(-30, 5, -20);

  67.   //X-
  68.   fill(#ff0000);
  69.   vertex(-30, -5, -20);
  70.   vertex(-30, -5, 20);
  71.   vertex(-30, 5, 20);
  72.   vertex(-30, 5, -20);

  73.   //X+
  74.   fill(#ffff00);
  75.   vertex(30, -5, -20);
  76.   vertex(30, -5, 20);
  77.   vertex(30, 5, 20);
  78.   vertex(30, 5, -20);

  79.   //Y-
  80.   fill(#ff00ff);
  81.   vertex(-30, -5, -20);
  82.   vertex(30, -5, -20);
  83.   vertex(30, -5, 20);
  84.   vertex(-30, -5, 20);

  85.   //Y+
  86.   fill(#00ffff);
  87.   vertex(-30, 5, -20);
  88.   vertex(30, 5, -20);
  89.   vertex(30, 5, 20);
  90.   vertex(-30, 5, 20);

  91.   endShape();
  92. }


  93. void drawCube() {  
  94.   pushMatrix();
  95.   translate(300, 450, 0);
  96.   scale(4, 4, 4);

  97.   rotateX(HALF_PI * -RwEst[0]);
  98.   rotateZ(HALF_PI * RwEst[1]);

  99.   buildBoxShape();

  100.   popMatrix();
  101. }


  102. void getInclination() {
  103.   int w = 0;
  104.   float tmpf = 0.0;
  105.   int currentTime, signRzGyro;


  106.   readSensors();
  107.   normalize3DVec(RwAcc);

  108.   currentTime = millis();
  109.   interval = currentTime - lastTime;
  110.   lastTime = currentTime;

  111.   if (firstSample || Float.isNaN(RwEst[0])) { // NaN用来等待检查从arduino过来的数据
  112.     for (w=0;w<=2;w++) {
  113.       RwEst[w] = RwAcc[w];    // 初始化加速度传感器读数
  114.     }
  115.   }
  116.   else {
  117.     // 对RwGyro进行评估
  118.     if (abs(RwEst[2]) < 0.1) {
  119.       // Rz值非常的小,它的作用是作为Axz与Ayz的计算参照值,防止放大的波动产生错误的结果。
  120.       // 这种情况下就跳过当前的陀螺仪数据,使用以前的。
  121.       for (w=0;w<=2;w++) {
  122.         RwGyro[w] = RwEst[w];
  123.       }
  124.     }
  125.     else {
  126.       // ZX/ZY平面和Z轴R的投影之间的角度,基于最近一次的RwEst值
  127.       for (w=0;w<=1;w++) {
  128.         tmpf = Gyro[w];                        // 获取当前陀螺仪的deg/s
  129.         tmpf *= interval / 1000.0f;                     // 得到角度变化值
  130.         Awz[w] = atan2(RwEst[w], RwEst[2]) * 180 / PI;   // 得到角度并转换为度
  131.         Awz[w] += tmpf;             // 根据陀螺仪的运动得到更新后的角度
  132.       }

  133.       // 判断RzGyro是多少,主要看Axz的弧度是多少
  134.       // 当Axz在-90 ..90 => cos(Awz) >= 0这个范围内的时候RzGyro是准确的
  135.       signRzGyro = ( cos(Awz[0] * PI / 180) >=0 ) ? 1 : -1;

  136.       // 从Awz的角度值反向计算RwGyro的公式请查看网页 [url]http://starlino.com/imu_guide.html[/url]
  137.       for (w=0;w<=1;w++) {
  138.         RwGyro[0] = sin(Awz[0] * PI / 180);
  139.         RwGyro[0] /= sqrt( 1 + squared(cos(Awz[0] * PI / 180)) * squared(tan(Awz[1] * PI / 180)) );
  140.         RwGyro[1] = sin(Awz[1] * PI / 180);
  141.         RwGyro[1] /= sqrt( 1 + squared(cos(Awz[1] * PI / 180)) * squared(tan(Awz[0] * PI / 180)) );
  142.       }
  143.       RwGyro[2] = signRzGyro * sqrt(1 - squared(RwGyro[0]) - squared(RwGyro[1]));
  144.     }

  145.     // 把陀螺仪与加速度传感器的值进行结合
  146.     for (w=0;w<=2;w++) RwEst[w] = (RwAcc[w] + wGyro * RwGyro[w]) / (1 + wGyro);

  147.     normalize3DVec(RwEst);
  148.   }

  149.   firstSample = false;
  150. }


  151. void draw() {  
  152.   getInclination();

  153.   background(#000000);
  154.   fill(#ffffff);

  155.   textFont(font, 20);
  156.   //float temp_decoded = 35.0 + ((float) (temp + 13200)) / 280;
  157.   //text("temp:\n" + temp_decoded + " C", 350, 250);
  158.   text("RwAcc (G):\n" + RwAcc[0] + "\n" + RwAcc[1] + "\n" + RwAcc[2] + "\ninterval: " + interval, 20, 50);
  159.   text("Gyro (°/s):\n" + Gyro[0] + "\n" + Gyro[1] + "\n" + Gyro[2], 220, 50);
  160.   text("Awz (°):\n" + Awz[0] + "\n" + Awz[1], 420, 50);
  161.   text("RwGyro (°/s):\n" + RwGyro[0] + "\n" + RwGyro[1] + "\n" + RwGyro[2], 20, 180);
  162.   text("RwEst :\n" + RwEst[0] + "\n" + RwEst[1] + "\n" + RwEst[2], 220, 180);

  163.   // display axes显示轴
  164.   pushMatrix();
  165.   translate(450, 250, 0);
  166.   stroke(#ffffff);
  167.   scale(100, 100, 100);
  168.   line(0, 0, 0, 1, 0, 0);
  169.   line(0, 0, 0, 0, -1, 0);
  170.   line(0, 0, 0, 0, 0, 1);
  171.   line(0, 0, 0, -RwEst[0], RwEst[1], RwEst[2]);
  172.   popMatrix();

  173.   drawCube();
  174. }
复制代码


注意:需要更改上面的 myPort  和 font 的定义。

然后运行Processing就可以看到效果了。



本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x

评分

参与人数 1 +1 收起 理由
幻生幻灭 + 1 赞一个!

查看全部评分

回复

使用道具 举报

 楼主| 发表于 2012-9-17 15:24:14 | 显示全部楼层
因为没怎么修改过,显示结果貌似还有些误差:如水平放置模块的时候,3D方块稍有点角度;
还有就是 弘毅 版主 这两天可能会放出一个基于MPU6050姿态识别的改进版本,期待之中。。。
回复 支持 反对

使用道具 举报

发表于 2012-9-17 15:59:23 | 显示全部楼层
{:soso_e154:}压力山大。。。我得去努力。。。
回复 支持 反对

使用道具 举报

发表于 2012-9-17 16:29:29 | 显示全部楼层
不错,与processing 完美全体哇,
回复 支持 反对

使用道具 举报

发表于 2012-9-17 16:30:41 | 显示全部楼层
看代码发现,LZ,用的UBUNTU哇,牛鸡哇,再支持一下
回复 支持 反对

使用道具 举报

发表于 2012-9-17 17:48:02 | 显示全部楼层
好东西啊,这个模块很奇怪,一开始用你会觉得和坏的没两样,需要注意的细节也非常多。你的模块能用,恭喜!!!
回复 支持 反对

使用道具 举报

发表于 2012-9-17 19:14:01 | 显示全部楼层
学习了,原来还可以这样玩{:soso_e113:}
回复 支持 反对

使用道具 举报

发表于 2012-9-18 08:56:53 | 显示全部楼层
Processing不了解啊,有LABVIEW的不????
回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-9-18 09:58:44 | 显示全部楼层
本帖最后由 wzc0066 于 2012-9-18 10:00 编辑
太行摄狼 发表于 2012-9-18 08:56
Processing不了解啊,有LABVIEW的不????


我也刚接触Processing,Arduino的IDE就是从Processing基础上修改而来的。所以使用过Arduino的可能会感觉比较熟悉。
至于语法方面,感觉就是将一些Java的操作(如果绘制各种图形等)再进行一次封装(??),所以调用更简洁。
总得来说,入手比较简单,可以一试。
回复 支持 反对

使用道具 举报

发表于 2012-9-18 11:54:49 | 显示全部楼层
这个字体代码错误??哪里下载的?{:soso_e132:}

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-9-18 12:06:55 | 显示全部楼层
菩提明镜 发表于 2012-9-18 11:54
这个字体代码错误??哪里下载的?

就在processing目录下:processing/processing-1.5.1/modes/android/examples/Basics/Typography/Letters/data/CourierNew36.vlw

那是我系统的绝对路径,你要改成你的绝对路径。
回复 支持 反对

使用道具 举报

发表于 2012-9-18 12:13:25 | 显示全部楼层

抱歉,不是说MPU6050出来的已经是数字的了吗? 如下的转换 能解释下是啥意思吗?

// 把原始数据转换为G44.      
RwAcc[0] = float(inputStringArr[0]) / 256.0;45.      
RwAcc[1] = float(inputStringArr[1])/ 256.0;46.      
RwAcc[2] = float(inputStringArr[2])/ 256.0;47. 48.     

// 把原始数据转换为"度/秒"49.      
Gyro[0] = float(inputStringArr[3]) / 14.375;50.      
Gyro[1] = float(inputStringArr[4]) / 14.375;51.      
Gyro[2] = float(inputStringArr[5]) / 14.375;
回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-9-18 14:52:33 | 显示全部楼层
本帖最后由 wzc0066 于 2012-9-18 14:53 编辑

详细参见弘毅版主的帖子:关于ITG3205与ADXL345的姿态识别
Processing程序是抄他的,去他的帖子问下请他给你解释一下。
回复 支持 反对

使用道具 举报

发表于 2012-9-18 16:05:59 | 显示全部楼层
wzc0066 发表于 2012-9-18 12:06
就在processing目录下:processing/processing-1.5.1/modes/android/examples/Basics/Typography/Letters ...

搞定了 谢谢了哦
回复 支持 反对

使用道具 举报

发表于 2012-9-18 23:35:17 | 显示全部楼层
学习了
回复 支持 反对

使用道具 举报

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

本版积分规则 需要先绑定手机号

Archiver|联系我们|极客工坊

GMT+8, 2024-3-29 18:12 , Processed in 0.049910 second(s), 29 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表