scyishuying 发表于 2013-11-8 22:22:05

【送模块】MPU6050姿态解算方法分析与Arduino连接串口6050模块的代码分享

本帖最后由 scyishuying 于 2013-11-15 16:47 编辑

极客工坊的跳蚤市场中,有抢楼活动,有机会免费获得串口6050模块,已送出4个免费码,3个半价码,活动持续进行中,路过的朋友请帮顶一下:
http://www.geek-workshop.com/thread-7666-1-1.html

MPU6050模块是InvenSense公司推出的一款低成本的6轴传感器模块,包括三轴加速度,三轴角速度。其体积小巧,用途非常广。做平衡小车,四轴飞行器,飞行鼠标等等,都是必不可少而且是最优的传感器解决方案。本人根据自己的一些实际工作经验和使用体会来谈谈MPU6050的相关问题吧,抛砖引玉,如有不当之处,欢迎大家批评指正。

不论是做平衡还是四轴飞行器,关键的问题在于两方面,一是模块姿态的确定,通常需要用到积分运算与卡尔曼滤波算法,需要较强的数学功底与编程能力,二是稳定控制,方法比较单一,就是经典的PID控制算法,难点在于需要根据实际情况调整PID的参数,需要做实验确定,不难,只是费时间。因此以下主要分析姿态确定问题。

虽然6050模块能够输出三轴加速度和三轴角速度的数据,但实际应用的时候,直接使用的确不是这些量,而是需要根据这些数据解算出三轴的角度数据。比如平衡小车,需要算出
模块的俯仰角,然后控制算法根据角度大小控制小车轮子的移动。四轴飞行器需要根据俯仰角度、滚转角度,和飞行指令来调节四个电机的转速。

从6轴的原始数据得到三轴的角度计算是一个比较复杂的运动学解算过程,有的童鞋可能会说,不就是三轴角速度积分不就行了吗?这就是没有实践,想当然的说法。有三点需要注意的问题:

1.三轴姿态的解算不能直接积分。因为三轴是有耦合的,只有在三轴角度为小角度的时候可以这么算,角度大了以后,比如60度了,这么算的误差就很大。标准的做法是用四元数的方法做姿态解算,积分的方法可以用4阶龙格-库塔法,或者4阶Gill法。详情请参考:航空航天器运动的建模——飞行动力学的理论基础 肖业伦著 北京航空航天大学出版社。

2.积分运算的累积误差。角速度积分运算是有累积误差的,累积误差在短时间内表现不明显,只要零点漂移处理得好了,1分钟以内的漂移都不大,但时间长了,就会有累积误差,5分钟就漂到不知道哪里去了。

3.角速率零点漂移。所谓零点漂移就是模块静止的时候,我们认为正常的输出应该是0,或者均值为0的数据,但是实际上6050的输出不是,可能在2°/s或者其他,而且每次都不一样,如果不校准,别说1分钟了,10秒钟误差就有20度。

根据上面的分析,似乎要获得角度非常困难呀。又有些聪明的朋友会想,用角速率积分这么麻烦,我不怎么算好了,条条大路通罗马,为啥非得用这个方法。6050不是能输出加速度吗?我用重力在3轴的分量的反正切值,作为滚转角和俯仰角不久行了。

用加速度计算的确也是一种方法,但使用加速度也有三方面的问题:

1.无法在动态情况下使用,使用重力的来解算姿态的前提条件是模块本身没有加速度,因此模块输出的三轴加速度值,正好是重力在模块本体坐标系下的分量,从而能够求出俯仰和滚转的姿态角度。一旦模块运动起来,这种方法就傻了,因为模块无法分辨出哪些是重力的分量,那些是模块本身的加速度引起的。目前市面上很多倾角仪就是这种思路,但问题就是没法在动态情况下使用,最简单的测试方法就是把模块水平放置桌面上,缓慢运动,发现X,Y轴的角度基本不变化,都在0度左右,一旦快速运动起来,X,Y轴就显示有很大的角度了。而实际上模块一直水平,没有变化。

2.精度差。6050模块的加速度本身的精度不高,就算是在静态情况下,角度测量的精度也只能到1°左右。

3.三轴耦合问题。利用加速度求解姿态的时候,也会有三轴耦合的问题,因为姿态表示与坐标旋转顺序有关,这样只有一种一个轴能用反正切值计算,另一个轴不能用反正切值计算。

那么怎么才能得到高精度不漂移的三轴角度呢?陀螺仪精度高,但时间长了会有漂移,加速度动态精度差,但没有长期漂移。能否综合利用陀螺仪和加速度计的特点,优势互补获得准确的姿态角度呢?答案是肯定的,方法就是用卡尔曼滤波做数据融合。大致的思路是将模块的姿态用四元素表示,作为系统的状态量,模块的姿态运动学方程作为滤波的状态转移方程,加速度信息作为滤波的观察量信息,然后利用卡尔曼滤波的计算方法迭代计算更新,详细的过程可以参考惯性导航方面的书籍。不过卡尔曼滤波算法比较复杂,需要用到矩阵运算等等,数学功底和编程基础要求都较高,不是初学者能够快速掌握的。而且MPU6050模块是IIC接口输出的,也给初学者带来了不少障碍与困难。

难道不会卡尔曼滤波,就不能做出高精度的东西了吗?好在万能的淘宝上面,已经有专业的团队已经为我们处理好了这些问题,已经有成熟的高精度串口6050模块面世了。模块不仅能够输出6050原始的三轴加速度,三轴角速度信息,在模块内部还集成了高精度的卡尔曼滤波算法,输出滤波融合以后的角度数据,而且是串口接口的,能够直接与计算机对接,查看数据。目前该串口6050模块已经量产,大家可以以非常低廉的价格获得此款模块了,不用把时间和精力花在姿态的求解与计算上面了。大家可以集中精力解决产品的其他调试问题。地址:
http://item.taobao.com/item.htm?id=19785706431
目前以及有网友用这款模块配合arduino uno R3做出了平衡小车,用料简单,效果很好。借用几张图:
平衡车静态展示:
http://image.geek-workshop.com/forum/201311/15/154424t6628du2m80d6y7m.jpg
工作状况展示,弱光拍摄,曝光时间长,得益于稳定的控制效果,小车纹丝不动。
http://image.geek-workshop.com/forum/201311/15/154427oxa4dje7iakpiltd.jpg

以下再来说是串口6050模块如何连接arduino。

代码分为两个部分,一是中断接收,当收到6050串口发送过来的数据以后,找到数据头,将数据放入数据缓冲区内。二是数据解析与处理,将数据缓冲区的数据取出,解析成对应的角速度,角速度和角度数据。

先看数据协议:
模块发送每帧数据分为3个数据包,分别为加速度包,角速度包和角度包,3个数据包顺序输出。波特率115200时每隔10ms输出1帧数据,波特率9600时每隔50ms输出一帧数据。
加速度输出:

数据编号数据内容含义
00x55包头
10x51标识这个包是加速度包
2AxLX轴加速度低字节
3AxHX轴加速度高字节
4AyLy轴加速度低字节
5AyHy轴加速度高字节
6AzLz轴加速度低字节
7AzHz轴加速度高字节
8TL温度低字节
9TH温度高字节
10Sum校验和
角速度输出:
数据编号数据内容含义
00x55包头
10x52标识这个包是角速度包
2wxLX轴角速度低字节
3wxHX轴加速度高字节
4wyLy轴加速度低字节
5wyHy轴加速度高字节
6wzLz轴加速度低字节
7wzHz轴加速度高字节
8TL温度低字节
9TH温度高字节
10Sum校验和
角度输出:
数据编号数据内容含义
00x55包头
10x53标识这个包是角度包
2RollLX轴角度低字节
3RollHX轴角度高字节
4PitchLy轴角度低字节
5PitchHy轴角度高字节
6YawLz轴角度低字节
7YawHz轴角度高字节
8TL温度低字节
9TH温度高字节
10Sum校验和

现在开始处理串口数据:

/*
This code is used for connecting arduino to serial mpu6050 module, and test in arduino uno R3 board.
connect map:
arduino   mpu6050 module
VCC    5v/3.3v
TX   RX<-0
TX   TX->1
GND    GND
note:
because arduino download and mpu6050 are using the same serial port, you need to un-connect 6050 module when you want to download program to arduino.
Created 14 Nov 2013
by Zhaowen

serial mpu6050 module can be found in the link below:
http://item.taobao.com/item.htm?id=19785706431
*/

unsigned char Re_buf,counter=0;
unsigned char sign=0;
float a,w,angle,T;
void setup() {
// initialize serial:
Serial.begin(115200);
}

void loop() {
if(sign)
{
   sign=0;
   if(Re_buf==0x55)      //检查帧头
   {
      switch(Re_buf )
      {
      case 0x51:
                a = (short(Re_buf <<8| Re_buf ))/32768.0*16;
                a = (short(Re_buf <<8| Re_buf ))/32768.0*16;
                a = (short(Re_buf <<8| Re_buf ))/32768.0*16;
                T = (short(Re_buf <<8| Re_buf ))/340.0+36.25;
                break;
      case 0x52:
                w = (short(Re_buf <<8| Re_buf ))/32768.0*2000;
                w = (short(Re_buf <<8| Re_buf ))/32768.0*2000;
                w = (short(Re_buf <<8| Re_buf ))/32768.0*2000;
                T = (short(Re_buf <<8| Re_buf ))/340.0+36.25;
                break;
      case 0x53:
                angle = (short(Re_buf <<8| Re_buf ))/32768.0*180;
                angle = (short(Re_buf <<8| Re_buf ))/32768.0*180;
                angle = (short(Re_buf <<8| Re_buf ))/32768.0*180;
                T = (short(Re_buf <<8| Re_buf ))/340.0+36.25;
                Serial.print("a:");
                Serial.print(a);Serial.print(" ");
                Serial.print(a);Serial.print(" ");
                Serial.print(a);Serial.print(" ");
                Serial.print("w:");
                Serial.print(w);Serial.print(" ");
                Serial.print(w);Serial.print(" ");
                Serial.print(a);Serial.print(" ");
                Serial.print("angle:");
                Serial.print(angle);Serial.print(" ");
                Serial.print(angle);Serial.print(" ");
                Serial.print(angle);Serial.print(" ");
                Serial.print("T:");
                Serial.println(T);
                break;
      }
    }
}
}

void serialEvent() {
while (Serial.available()) {
   
    //char inChar = (char)Serial.read(); Serial.print(inChar); //Output Original Data, use this code

    Re_buf=(unsigned char)Serial.read();
    if(counter==0&&Re_buf!=0x55) return;      //第0号数据不是帧头            
    counter++;      
    if(counter==11)             //接收到11个数据
    {   
       counter=0;               //重新赋值,准备下一帧数据的接收
       sign=1;
    }
      
}
}   

说了这么半天,无图无真相,斗胆上一个软件效果图:
http://image.geek-workshop.com/forum/201309/29/021013gpvppe39p2c9k0nc.png

串口打印效果:

scyishuying 发表于 2013-11-8 22:52:26

自己顶一下!

SuperAnt 发表于 2013-11-9 00:35:43

分析得很有道理,膜拜!

邵林寺 发表于 2013-11-9 06:08:49

码字辛苦…!!了

邵林寺 发表于 2013-11-9 07:16:39

这是个好东西,真想弄一个玩玩,用在我的平衡车上

zjhyhky 发表于 2013-11-9 07:49:43

楼主,能否给出一个Arduino范例?上面的意思没看懂,没有setup()、没有loop(),不知道在Arduino中怎么用

scyishuying 发表于 2013-11-9 09:42:56

zjhyhky 发表于 2013-11-9 07:49 static/image/common/back.gif
楼主,能否给出一个Arduino范例?上面的意思没看懂,没有setup()、没有loop(),不知道在Arduino中怎么用

Arduino的板子今天刚到,调好了马上发布

scyishuying 发表于 2013-11-9 09:44:00

邵林寺 发表于 2013-11-9 07:16 static/image/common/back.gif
这是个好东西,真想弄一个玩玩,用在我的平衡车上

恭喜,抢到串口6050模块的免费码了。马上发放给您~

邵林寺 发表于 2013-11-9 09:50:21

scyishuying 发表于 2013-11-9 09:44 static/image/common/back.gif
恭喜,抢到串口6050模块的免费码了。马上发放给您~

好的:'(:D哈哈,正好放在我的小车上做对比

scyishuying 发表于 2013-11-9 10:10:14

邵林寺 发表于 2013-11-9 09:50 static/image/common/back.gif
好的哈哈,正好放在我的小车上做对比

嗯嗯,收到货以后跟大家分享一下使用感受哈~

SuperAnt 发表于 2013-11-9 23:08:38

已经有3个免费码和2个半价码放出了,还有机会,我去刷屏去了!

et-mac 发表于 2013-11-9 23:27:37

看了平衡小车很好玩.也想搞个玩玩.
谢谢分享!

邵林寺 发表于 2013-11-10 13:16:47

赠的模块已经在路上了,明天收到后又有的玩了

zhaowenwin 发表于 2013-11-10 20:13:17

邵林寺 发表于 2013-11-10 13:16 static/image/common/back.gif
赠的模块已经在路上了,明天收到后又有的玩了

真的假的,运气真好,收到了跟大家分享一下效果~

zhaowenwin 发表于 2013-11-10 20:14:36

et-mac 发表于 2013-11-9 23:27 static/image/common/back.gif
看了平衡小车很好玩.也想搞个玩玩.
谢谢分享!

我也是正在制作中。。。
页: [1] 2 3 4 5
查看完整版本: 【送模块】MPU6050姿态解算方法分析与Arduino连接串口6050模块的代码分享