MPU6050姿態解算方法分析與Arduino連接串口6050模塊的代碼分享

ID:90084 · 發表于 2015-9-15 01:32

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;
}
}
}

說了這么半天，無圖無真相，斗膽上一個軟件效果圖：

串口打印效果：

ID:810428 · 發表于 2020-8-2 10:01

為什么我arduino的顯示上傳出錯

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

MPU6050姿態解算方法分析與Arduino連接串口6050模塊的代碼分享