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

ID:90084 · 發(fā)表于 2015-9-15 01:32

MPU6050模塊是InvenSense公司推出的一款低成本的6軸傳感器模塊，包括三軸加速度，三軸角速度。其體積小巧，用途非常廣。做平衡小車，四軸飛行器，飛行鼠標等等，都是必不可少而且是最優(yōu)的傳感器解決方案。本人根據(jù)自己的一些實際工作經(jīng)驗和使用體會來談談MPU6050的相關問題吧，拋磚引玉，如有不當之處，歡迎大家批評指正。

不論是做平衡還是四軸飛行器，關鍵的問題在于兩方面，一是模塊姿態(tài)的確定，通常需要用到積分運算與卡爾曼濾波算法，需要較強的數(shù)學功底與編程能力，二是穩(wěn)定控制，方法比較單一，就是經(jīng)典的PID控制算法，難點在于需要根據(jù)實際情況調(diào)整PID的參數(shù)，需要做實驗確定，不難，只是費時間。因此以下主要分析姿態(tài)確定問題。

雖然6050模塊能夠輸出三軸加速度和三軸角速度的數(shù)據(jù)，但實際應用的時候，直接使用的確不是這些量，而是需要根據(jù)這些數(shù)據(jù)解算出三軸的角度數(shù)據(jù)。比如平衡小車，需要算出
模塊的俯仰角，然后控制算法根據(jù)角度大小控制小車輪子的移動。四軸飛行器需要根據(jù)俯仰角度、滾轉(zhuǎn)角度，和飛行指令來調(diào)節(jié)四個電機的轉(zhuǎn)速。

從6軸的原始數(shù)據(jù)得到三軸的角度計算是一個比較復雜的運動學解算過程，有的童鞋可能會說，不就是三軸角速度積分不就行了嗎？這就是沒有實踐，想當然的說法。有三點需要注意的問題：

1.三軸姿態(tài)的解算不能直接積分。因為三軸是有耦合的，只有在三軸角度為小角度的時候可以這么算，角度大了以后，比如60度了，這么算的誤差就很大。標準的做法是用四元數(shù)的方法做姿態(tài)解算，積分的方法可以用4階龍格-庫塔法，或者4階Gill法。詳情請參考：航空航天器運動的建模——飛行動力學的理論基礎肖業(yè)倫著北京航空航天大學出版社。

2.積分運算的累積誤差。角速度積分運算是有累積誤差的，累積誤差在短時間內(nèi)表現(xiàn)不明顯，只要零點漂移處理得好了，1分鐘以內(nèi)的漂移都不大，但時間長了，就會有累積誤差，5分鐘就漂到不知道哪里去了。

3.角速率零點漂移。所謂零點漂移就是模塊靜止的時候，我們認為正常的輸出應該是0，或者均值為0的數(shù)據(jù)，但是實際上6050的輸出不是，可能在2°/s或者其他，而且每次都不一樣，如果不校準，別說1分鐘了，10秒鐘誤差就有20度。

根據(jù)上面的分析，似乎要獲得角度非常困難呀。又有些聰明的朋友會想，用角速率積分這么麻煩，我不怎么算好了，條條大路通羅馬，為啥非得用這個方法。6050不是能輸出加速度嗎？我用重力在3軸的分量的反正切值，作為滾轉(zhuǎn)角和俯仰角不久行了。

用加速度計算的確也是一種方法，但使用加速度也有三方面的問題：

1.無法在動態(tài)情況下使用，使用重力的來解算姿態(tài)的前提條件是模塊本身沒有加速度，因此模塊輸出的三軸加速度值，正好是重力在模塊本體坐標系下的分量，從而能夠求出俯仰和滾轉(zhuǎn)的姿態(tài)角度。一旦模塊運動起來，這種方法就傻了，因為模塊無法分辨出哪些是重力的分量，那些是模塊本身的加速度引起的。目前市面上很多傾角儀就是這種思路，但問題就是沒法在動態(tài)情況下使用，最簡單的測試方法就是把模塊水平放置桌面上，緩慢運動，發(fā)現(xiàn)X，Y軸的角度基本不變化，都在0度左右，一旦快速運動起來，X，Y軸就顯示有很大的角度了。而實際上模塊一直水平，沒有變化。

2.精度差。6050模塊的加速度本身的精度不高，就算是在靜態(tài)情況下，角度測量的精度也只能到1°左右。

3.三軸耦合問題。利用加速度求解姿態(tài)的時候，也會有三軸耦合的問題，因為姿態(tài)表示與坐標旋轉(zhuǎn)順序有關，這樣只有一種一個軸能用反正切值計算，另一個軸不能用反正切值計算。

那么怎么才能得到高精度不漂移的三軸角度呢？陀螺儀精度高，但時間長了會有漂移，加速度動態(tài)精度差，但沒有長期漂移。能否綜合利用陀螺儀和加速度計的特點，優(yōu)勢互補獲得準確的姿態(tài)角度呢？答案是肯定的，方法就是用卡爾曼濾波做數(shù)據(jù)融合。大致的思路是將模塊的姿態(tài)用四元素表示，作為系統(tǒng)的狀態(tài)量，模塊的姿態(tài)運動學方程作為濾波的狀態(tài)轉(zhuǎn)移方程，加速度信息作為濾波的觀察量信息，然后利用卡爾曼濾波的計算方法迭代計算更新，詳細的過程可以參考慣性導航方面的書籍。不過卡爾曼濾波算法比較復雜，需要用到矩陣運算等等，數(shù)學功底和編程基礎要求都較高，不是初學者能夠快速掌握的。而且MPU6050模塊是IIC接口輸出的，也給初學者帶來了不少障礙與困難。

難道不會卡爾曼濾波，就不能做出高精度的東西了嗎？好在萬能的淘寶上面，已經(jīng)有專業(yè)的團隊已經(jīng)為我們處理好了這些問題，已經(jīng)有成熟的高精度串口6050模塊面世了。模塊不僅能夠輸出6050原始的三軸加速度，三軸角速度信息，在模塊內(nèi)部還集成了高精度的卡爾曼濾波算法，輸出濾波融合以后的角度數(shù)據(jù)，而且是串口接口的，能夠直接與計算機對接，查看數(shù)據(jù)。目前該串口6050模塊已經(jīng)量產(chǎn)，大家可以以非常低廉的價格獲得此款模塊了，不用把時間和精力花在姿態(tài)的求解與計算上面了。大家可以集中精力解決產(chǎn)品的其他調(diào)試問題。地址：
http://item點taobao點com/item.htm?id=19785706431
目前以及有網(wǎng)友用這款模塊配合arduino uno R3做出了平衡小車，用料簡單，效果很好。借用幾張圖：
平衡車靜態(tài)展示：

工作狀況展示，弱光拍攝，曝光時間長，得益于穩(wěn)定的控制效果，小車紋絲不動。

以下再來說是串口6050模塊如何連接arduino。

代碼分為兩個部分，一是中斷接收，當收到6050串口發(fā)送過來的數(shù)據(jù)以后，找到數(shù)據(jù)頭，將數(shù)據(jù)放入數(shù)據(jù)緩沖區(qū)內(nèi)。二是數(shù)據(jù)解析與處理，將數(shù)據(jù)緩沖區(qū)的數(shù)據(jù)取出，解析成對應的角速度，角速度和角度數(shù)據(jù)。

先看數(shù)據(jù)協(xié)議：
模塊發(fā)送每幀數(shù)據(jù)分為3個數(shù)據(jù)包，分別為加速度包，角速度包和角度包，3個數(shù)據(jù)包順序輸出。波特率115200時每隔10ms輸出1幀數(shù)據(jù)，波特率9600時每隔50ms輸出一幀數(shù)據(jù)。
加速度輸出：

數(shù)據(jù)編號	數(shù)據(jù)內(nèi)容	含義
0	0x55	包頭
1	0x51	標識這個包是加速度包
2	AxL	X軸加速度低字節(jié)
3	AxH	X軸加速度高字節(jié)
4	AyL	y軸加速度低字節(jié)
5	AyH	y軸加速度高字節(jié)
6	AzL	z軸加速度低字節(jié)
7	AzH	z軸加速度高字節(jié)
8	TL	溫度低字節(jié)
9	TH	溫度高字節(jié)
10	Sum	校驗和

角速度輸出：

數(shù)據(jù)編號	數(shù)據(jù)內(nèi)容	含義
0	0x55	包頭
1	0x52	標識這個包是角速度包
2	wxL	X軸角速度低字節(jié)
3	wxH	X軸加速度高字節(jié)
4	wyL	y軸加速度低字節(jié)
5	wyH	y軸加速度高字節(jié)
6	wzL	z軸加速度低字節(jié)
7	wzH	z軸加速度高字節(jié)
8	TL	溫度低字節(jié)
9	TH	溫度高字節(jié)
10	Sum	校驗和

角度輸出：

數(shù)據(jù)編號	數(shù)據(jù)內(nèi)容	含義
0	0x55	包頭
1	0x53	標識這個包是角度包
2	RollL	X軸角度低字節(jié)
3	RollH	X軸角度高字節(jié)
4	PitchL	y軸角度低字節(jié)
5	PitchH	y軸角度高字節(jié)
6	YawL	z軸角度低字節(jié)
7	YawH	z軸角度高字節(jié)
8	TL	溫度低字節(jié)
9	TH	溫度高字節(jié)
10	Sum	校驗和

現(xiàn)在開始處理串口數(shù)據(jù)：
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號數(shù)據(jù)不是幀頭
counter++;
if(counter==11) //接收到11個數(shù)據(jù)
{
counter=0; //重新賦值，準備下一幀數(shù)據(jù)的接收
sign=1;
}
}
}

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

串口打印效果：

ID:810428 · 發(fā)表于 2020-8-2 10:01

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

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

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

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