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

标题: MPU6050姿态解算方法分析与Arduino连接串口6050模块的代码分享 [打印本页]

作者: haoren 时间: 2015-9-15 01:32
标题: MPU6050姿态解算方法分析与Arduino连接串口6050模块的代码分享
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做出了平衡小车，用料简单，效果很好。借用几张图：
平衡车静态展示：

工作状况展示，弱光拍摄，曝光时间长，得益于稳定的控制效果，小车纹丝不动。

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

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

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

数据编号	数据内容	含义
0	0x55	包头
1	0x51	标识这个包是加速度包
2	AxL	X轴加速度低字节
3	AxH	X轴加速度高字节
4	AyL	y轴加速度低字节
5	AyH	y轴加速度高字节
6	AzL	z轴加速度低字节
7	AzH	z轴加速度高字节
8	TL	温度低字节
9	TH	温度高字节
10	Sum	校验和

角速度输出：

数据编号	数据内容	含义
0	0x55	包头
1	0x52	标识这个包是角速度包
2	wxL	X轴角速度低字节
3	wxH	X轴加速度高字节
4	wyL	y轴加速度低字节
5	wyH	y轴加速度高字节
6	wzL	z轴加速度低字节
7	wzH	z轴加速度高字节
8	TL	温度低字节
9	TH	温度高字节
10	Sum	校验和

角度输出：

数据编号	数据内容	含义
0	0x55	包头
1	0x53	标识这个包是角度包
2	RollL	X轴角度低字节
3	RollH	X轴角度高字节
4	PitchL	y轴角度低字节
5	PitchH	y轴角度高字节
6	YawL	z轴角度低字节
7	YawH	z轴角度高字节
8	TL	温度低字节
9	TH	温度高字节
10	Sum	校验和

现在开始处理串口数据：
ARDUINO 代码复制打印下载

/*
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[11,counter=0;
unsigned char sign=0;
float a[3,w[3,angle[3,T;
void setup() {
// initialize serial:
Serial.begin(115200);
}
void loop() {
if(sign)
{
sign=0;
if(Re_buf[0==0x55) //检查帧头
{
switch(Re_buf [1)
{
case 0x51:
a[0 = (short(Re_buf [3<<8| Re_buf [2))/32768.0*16;
a[1 = (short(Re_buf [5<<8| Re_buf [4))/32768.0*16;
a[2 = (short(Re_buf [7<<8| Re_buf [6))/32768.0*16;
T = (short(Re_buf [9<<8| Re_buf [8))/340.0+36.25;
break;
case 0x52:
w[0 = (short(Re_buf [3<<8| Re_buf [2))/32768.0*2000;
w[1 = (short(Re_buf [5<<8| Re_buf [4))/32768.0*2000;
w[2 = (short(Re_buf [7<<8| Re_buf [6))/32768.0*2000;
T = (short(Re_buf [9<<8| Re_buf [8))/340.0+36.25;
break;
case 0x53:
angle[0 = (short(Re_buf [3<<8| Re_buf [2))/32768.0*180;
angle[1 = (short(Re_buf [5<<8| Re_buf [4))/32768.0*180;
angle[2 = (short(Re_buf [7<<8| Re_buf [6))/32768.0*180;
T = (short(Re_buf [9<<8| Re_buf [8))/340.0+36.25;
Serial.print("a:");
Serial.print(a[0);Serial.print(" ");
Serial.print(a[1);Serial.print(" ");
Serial.print(a[2);Serial.print(" ");
Serial.print("w:");
Serial.print(w[0);Serial.print(" ");
Serial.print(w[1);Serial.print(" ");
Serial.print(a[2);Serial.print(" ");
Serial.print("angle:");
Serial.print(angle[0);Serial.print(" ");
Serial.print(angle[1);Serial.print(" ");
Serial.print(angle[2);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[counter=(unsigned char)Serial.read();
if(counter==0&&Re_buf[0!=0x55) return; //第0号数据不是帧头
counter++;
if(counter==11) //接收到11个数据
{
counter=0; //重新赋值，准备下一帧数据的接收
sign=1;
}
}
}

说了这么半天，无图无真相，斗胆上一个软件效果图：

串口打印效果：

Arduino-MPU6050.zip

1.23 KB, 下载次数: 45, 下载积分: 黑币 -5

MPU6050资料V4.zip

4.28 MB, 下载次数: 35, 下载积分: 黑币 -5

作者: xingxupo 时间: 2020-8-2 10:01
为什么我arduino的显示上传出错

欢迎光临 (http://www.51hei.com/bbs/)