DIY三軸穩(wěn)定云臺(tái)理論基礎(chǔ)

三軸穩(wěn)定器就是可以在運(yùn)動(dòng)的過(guò)程中保持相機(jī)的平穩(wěn)，正常走路 相機(jī)是穩(wěn)定的 不會(huì)晃

在

自動(dòng)控制的故事

ac4626307

中我們提到了反饋控制的原理

反饋是一個(gè)過(guò)程：

1、設(shè)定目標(biāo)，對(duì)小朋友走路的例子來(lái)說(shuō)，就是前進(jìn)的路線。

2、測(cè)量狀態(tài)，小朋友的眼睛看著路，就是在測(cè)量自己的前進(jìn)方向。

3、將測(cè)量到的狀態(tài)和設(shè)定的目標(biāo)比較，把眼睛看到的前進(jìn)方向和心里想的前進(jìn)方向作比較，判斷前進(jìn)方向是否正確；如果不正確，相差有多少。

4、調(diào)整行動(dòng)，在心里根據(jù)實(shí)際前進(jìn)方向和設(shè)定目標(biāo)的偏差，決定調(diào)整的量。

5、實(shí)際執(zhí)行，也就是實(shí)際挪動(dòng)腳步，重回正確的前進(jìn)方向。

在整個(gè)走路的過(guò)程中，這個(gè)反饋過(guò)程周而復(fù)始，不斷進(jìn)行，這樣，小朋友就不會(huì)走得東倒西歪了。

而在之前的很多電賽過(guò)往作品中我們也見(jiàn)到了PID的實(shí)際用于反饋控制的效果，卡爾曼濾波強(qiáng)無(wú)敵

【PID】風(fēng)力擺

ac4192287?

【PID】滾球機(jī)器人2

ac4240704?

【PID】三角擺

ac4273942?

電賽的控制類就是讓你會(huì)調(diào)PID參數(shù)，沒(méi)有別的

1）電源類：簡(jiǎn)易數(shù)控直流電源、直流穩(wěn)壓電源；

2）信號(hào)源類：實(shí)用信號(hào)源的設(shè)計(jì)和制作、波形發(fā)生器、電壓控制LC振蕩器等；

3）高頻無(wú)線電類：簡(jiǎn)易無(wú)線電遙控系統(tǒng)、調(diào)幅廣播收音機(jī)、短波調(diào)頻接收機(jī)、調(diào)頻收音機(jī)等；

4）放大器類：實(shí)用低頻功率放大器、高效率音頻功率放大器、寬帶放大器等；

5）儀器儀表類：簡(jiǎn)易電阻、電容和電感測(cè)試儀、簡(jiǎn)易數(shù)字頻率計(jì)、頻率特性測(cè)試儀、數(shù)字式工頻有效值多用表、簡(jiǎn)易數(shù)字存儲(chǔ)示波器、低頻數(shù)字式相位測(cè)量?jī)x、簡(jiǎn)易邏輯分析儀；

6）數(shù)據(jù)采集與處理類：多路數(shù)據(jù)采集系統(tǒng)、數(shù)字化語(yǔ)音存儲(chǔ)與回放系統(tǒng)、數(shù)據(jù)采集與傳輸系統(tǒng)；

7）控制類：水溫控制系統(tǒng)、自動(dòng)往返電動(dòng)小汽車、簡(jiǎn)易智能電動(dòng)車、液體點(diǎn)滴速度監(jiān)控裝置。

可以看得出來(lái)，整個(gè)系統(tǒng)由雙軸伺服機(jī)構(gòu)和傳感器與控制器組成，能夠?qū)崿F(xiàn)自動(dòng)將小球穩(wěn)定在某一指定點(diǎn)上

比例-積分-微分控制

比例-積分-微分控制規(guī)律是工業(yè)上最常用的控制規(guī)律。人們一般根據(jù)比例-積分-微分的英文縮寫(xiě)，將其簡(jiǎn)稱為PID控制。即使在更為先進(jìn)的控制規(guī)律廣泛應(yīng)用的今天，各種形式的PID控制仍然在所有控制回路中占85%以上。

在PID控制中，積分控制的特點(diǎn)是：只要還有余差（即殘余的控制偏差）存在，積分控制就按部就班地逐漸增加控制作用，直到余差消失。所以積分的效果比較緩慢，除特殊情況外，作為基本控制作用，緩不救急。微分控制的特點(diǎn)是：盡管實(shí)際測(cè)量值還比設(shè)定值低，但其快速上揚(yáng)的沖勢(shì)需要及早加以抑制，否則，等到實(shí)際值超過(guò)設(shè)定值再作反應(yīng)就晚了，這就是微分控制施展身手的地方了。作為基本控制使用，微分控制只看趨勢(shì)，不看具體數(shù)值所在，所以最理想的情況也就是把實(shí)際值穩(wěn)定下來(lái)，但穩(wěn)定在什么地方就要看你的運(yùn)氣了，所以微分控制也不能作為基本控制作用。比例控制沒(méi)有這些問(wèn)題，比例控制的反應(yīng)快，穩(wěn)定性好，是最基本的控制作用，是“皮”，積分、微分控制是對(duì)比例控制起增強(qiáng)作用的，極少單獨(dú)使用，所以是“毛”。在實(shí)際使用中比例和積分一般一起使用，比例承擔(dān)主要的控制作用，積分幫助消除余差。微分只有在被控對(duì)象反應(yīng)遲緩，需要在開(kāi)始有所反應(yīng)時(shí)，及早補(bǔ)償，才予以采用。只用比例和微分的情況很少見(jiàn)。

MPU6050是一種非常流行的空間運(yùn)動(dòng)傳感器芯片，可以獲取器件當(dāng)前的三個(gè)加速度分量和三個(gè)旋轉(zhuǎn)角速度。由于其體積小巧，功能強(qiáng)大，精度較高，不僅被廣泛應(yīng)用于工業(yè)，同時(shí)也是航模愛(ài)好者的神器，被安裝在各類飛行器上馳騁藍(lán)天。

MPU-6050產(chǎn)品為全球首例六軸運(yùn)動(dòng)感測(cè)追蹤(MotionTracking)組件，專為須要低功耗、低成本、高性能的消費(fèi)性電子產(chǎn)品設(shè)計(jì)。
MPU-6000系列產(chǎn)品包含InvenSense的運(yùn)動(dòng)感測(cè)融合演算(MotionFusion)與運(yùn)行校正韌件(run-time calibration firmware)，可讓采用運(yùn)動(dòng)感測(cè)功能之產(chǎn)品的制造商，省去挑選、檢測(cè)、在系統(tǒng)階段整合各別組件時(shí)所須的費(fèi)用與麻煩，并保證其感測(cè)組件融合算法(sensor fusion algorithms)與校正程序(calibration procedures)，能提供消費(fèi)者最佳性能(optimal performance)。

隨著Arduino開(kāi)發(fā)板的普及，許多朋友希望能夠自己制作基于MPU6050的控制系統(tǒng)，但由于缺乏專業(yè)知識(shí)而難以上手。此外，MPU6050的數(shù)據(jù)是有較大噪音的，若不進(jìn)行濾波會(huì)對(duì)整個(gè)控制系統(tǒng)的精準(zhǔn)確帶來(lái)嚴(yán)重影響。

MPU6050芯片內(nèi)自帶了一個(gè)數(shù)據(jù)處理子模塊DMP，已經(jīng)內(nèi)置了濾波算法，在許多應(yīng)用中使用DMP輸出的數(shù)據(jù)已經(jīng)能夠很好的滿足要求。

微分控制的重點(diǎn)不在實(shí)際測(cè)量值的具體數(shù)值，而在其變化方向和變化速度。微分控制在理論上和實(shí)用中有很多優(yōu)越性，但局限也是明顯的。如果測(cè)量信號(hào)不是很“干凈”，時(shí)不時(shí)有那么一點(diǎn)不大不小的“毛刺”或擾動(dòng)，微分控制就會(huì)被這些風(fēng)吹草動(dòng)搞得方寸大亂，產(chǎn)生很多不必要甚至錯(cuò)誤的控制信號(hào)。所以工業(yè)上對(duì)微分控制的使用是很謹(jǐn)慎的。

本文將直接面對(duì)原始測(cè)量數(shù)據(jù)，從連線、芯片通信開(kāi)始一步一步教你如何利用Arduino獲取MPU6050的數(shù)據(jù)并進(jìn)行卡爾曼濾波，最終獲得穩(wěn)定的系統(tǒng)運(yùn)動(dòng)狀態(tài)。

一、Arduino與MPU-6050的通信

為避免糾纏于電路細(xì)節(jié)，我們直接使用集成的MPU6050模塊。MPU6050的數(shù)據(jù)接口用的是I2C總線協(xié)議，因此我們需要Wire程序庫(kù)的幫助來(lái)實(shí)現(xiàn)Arduino與MPU6050之間的通信。請(qǐng)先確認(rèn)你的Arduino編程環(huán)境中已安裝Wire庫(kù)。

Wire庫(kù)的官方文檔（http://www.arduino.cc/en/Reference/Wire）中指出：在UNO板子上，SDA接口對(duì)應(yīng)的是A4引腳，SCL對(duì)應(yīng)的是A5引腳。MPU6050需要5V的電源，可由UNO板直接供電。按照下圖連線。?

（紫色線是中斷線，這里用不到，可以不接）?

MPU6050的數(shù)據(jù)寫(xiě)入和讀出均通過(guò)其芯片內(nèi)部的寄存器實(shí)現(xiàn)，這些寄存器的地址都是1個(gè)字節(jié)，也就是8位的尋址空間，其寄存器的詳細(xì)列表說(shuō)明書(shū)請(qǐng)點(diǎn)擊下載：https://www.olimex.com/Products/Modules/Sensors/MOD-MPU6050/resources/RM-MPU-60xxA_rev_4.pdf

1.1 將數(shù)據(jù)寫(xiě)入MPU-6050

在每次向器件寫(xiě)入數(shù)據(jù)前要先打開(kāi)Wire的傳輸模式，并指定器件的總線地址，MPU6050的總線地址是0x68（AD0引腳為高電平時(shí)地址為0x69）。然后寫(xiě)入一個(gè)字節(jié)的寄存器起始地址，再寫(xiě)入任意長(zhǎng)度的數(shù)據(jù)。這些數(shù)據(jù)將被連續(xù)地寫(xiě)入到指定的起始地址中，超過(guò)當(dāng)前寄存器長(zhǎng)度的將寫(xiě)入到后面地址的寄存器中。寫(xiě)入完成后關(guān)閉Wire的傳輸模式。下面的示例代碼是向MPU6050的0x6B寄存器寫(xiě)入一個(gè)字節(jié)0。

Wire.beginTransmission(0x68); //開(kāi)啟MPU6050的傳輸 Wire.write(0x6B); //指定寄存器地址 Wire.write(0); //寫(xiě)入一個(gè)字節(jié)的數(shù)據(jù) Wire.endTransmission(true); //結(jié)束傳輸，true表示釋放總線

1.2 從MPU-6050讀出數(shù)據(jù)

讀出和寫(xiě)入一樣，要先打開(kāi)Wire的傳輸模式，然后寫(xiě)一個(gè)字節(jié)的寄存器起始地址。接下來(lái)將指定地址的數(shù)據(jù)讀到Wire庫(kù)的緩存中，并關(guān)閉傳輸模式。最后從緩存中讀取數(shù)據(jù)。下面的示例代碼是從MPU6050的0x3B寄存器開(kāi)始讀取2個(gè)字節(jié)的數(shù)據(jù)：

Wire.beginTransmission(0x68); //開(kāi)啟MPU6050的傳輸 Wire.write(0x3B); //指定寄存器地址 Wire.requestFrom(0x68, 2, true); //將輸據(jù)讀出到緩存 Wire.endTransmission(true); //關(guān)閉傳輸模式 int val = Wire.read() << 8 | Wire.read(); //兩個(gè)字節(jié)組成一個(gè)16位整數(shù)

1.3 具體實(shí)現(xiàn)?

通常應(yīng)當(dāng)在setup函數(shù)中對(duì)Wire庫(kù)進(jìn)行初始化：

Wire.begin();

在對(duì)MPU6050進(jìn)行各項(xiàng)操作前，必須啟動(dòng)該器件，向它的0x6B寫(xiě)入一個(gè)字節(jié)0即可啟動(dòng)。通常也是在setup函數(shù)完成，代碼見(jiàn)1.1節(jié)。?

二、 MPU6050的數(shù)據(jù)格式

我們感興趣的數(shù)據(jù)位于0x3B到0x48這14個(gè)字節(jié)的寄存器中。這些數(shù)據(jù)會(huì)被動(dòng)態(tài)更新，更新頻率最高可達(dá)1000HZ。下面列出相關(guān)寄存器的地址，數(shù)據(jù)的名稱。注意，每個(gè)數(shù)據(jù)都是2個(gè)字節(jié)。

• 0x3B，加速度計(jì)的X軸分量ACC_X

• 0x3D，加速度計(jì)的Y軸分量ACC_Y

• 0x3F，加速度計(jì)的Z軸分量ACC_Z

• 0x41，當(dāng)前溫度TEMP

• 0x43，繞X軸旋轉(zhuǎn)的角速度GYR_X

• 0x45，繞Y軸旋轉(zhuǎn)的角速度GYR_Y

• 0x47，繞Z軸旋轉(zhuǎn)的角速度GYR_Z?
  

MPU6050芯片的座標(biāo)系是這樣定義的：令芯片表面朝向自己，將其表面文字轉(zhuǎn)至正確角度，此時(shí)，以芯片內(nèi)部中心為原點(diǎn)，水平向右的為X軸，豎直向上的為Y軸，指向自己的為Z軸。見(jiàn)下圖：

我們只關(guān)心加速度計(jì)和角速度計(jì)數(shù)據(jù)的含義，下面分別介紹。?

2.1 加速度計(jì)?

加速度計(jì)的三軸分量ACC_X、ACC_Y和ACC_Z均為16位有符號(hào)整數(shù)，分別表示器件在三個(gè)軸向上的加速度，取負(fù)值時(shí)加速度沿座標(biāo)軸負(fù)向，取正值時(shí)沿正向。

三個(gè)加速度分量均以重力加速度g的倍數(shù)為單位，能夠表示的加速度范圍，即倍率可以統(tǒng)一設(shè)定，有4個(gè)可選倍率：2g、4g、8g、16g。以ACC_X為例，若倍率設(shè)定為2g（默認(rèn)），則意味著ACC_X取最小值-32768時(shí)，當(dāng)前加速度為沿X軸正方向2倍的重力加速度；若設(shè)定為4g，取-32768時(shí)表示沿X軸正方向4倍的重力加速度，以此類推。顯然，倍率越低精度越好，倍率越高表示的范圍越大，這要根據(jù)具體的應(yīng)用來(lái)設(shè)定。

我們用f表示倍率，f=0為2g，f=3為16g，設(shè)定加速度倍率的代碼如下：

Wire.beginTransmission(0x68); //開(kāi)啟MPU-6050的傳輸 Wire.write(0x1C); //加速度倍率寄存器的地址 Wire.requestFrom(0x68, 1, true); //先讀出原配置 unsigned char acc_conf = Wire.read(); acc_conf = ((acc_conf & 0xE7) | (f << 3)); Wire.write(acc_conf); Wire.endTransmission(true); //結(jié)束傳輸，true表示釋放總線

再以ACC_X為例，若當(dāng)前設(shè)定的加速度倍率為4g，那么將ACC_X讀數(shù)換算為加速度的公式為：

，g可取當(dāng)?shù)刂亓铀俣取?/p>

2.2 角速度計(jì)

繞X、Y和Z三個(gè)座標(biāo)軸旋轉(zhuǎn)的角速度分量GYR_X、GYR_Y和GYR_Z均為16位有符號(hào)整數(shù)。從原點(diǎn)向旋轉(zhuǎn)軸方向看去，取正值時(shí)為順時(shí)針旋轉(zhuǎn)，取負(fù)值時(shí)為逆時(shí)針旋轉(zhuǎn)。

三個(gè)角速度分量均以“度/秒”為單位，能夠表示的角速度范圍，即倍率可統(tǒng)一設(shè)定，有4個(gè)可選倍率：250度/秒、500度/秒、1000度/秒、2000度/秒。以GYR_X為例，若倍率設(shè)定為250度/秒，則意味著GYR取正最大值32768時(shí)，當(dāng)前角速度為順時(shí)針250度/秒；若設(shè)定為500度/秒，取32768時(shí)表示當(dāng)前角速度為順時(shí)針500度/秒。顯然，倍率越低精度越好，倍率越高表示的范圍越大。

我們用f表示倍率，f=0為250度/秒，f=3為2000度/秒，除角速度倍率寄存器的地址為0x1B之外，設(shè)定加速度倍率的代碼與2.1節(jié)代碼一致。

以GYR_X為例，若當(dāng)前設(shè)定的角速度倍率為1000度/秒，那么將GRY_X讀數(shù)換算為角速度（順時(shí)針）的公式為：

。

三、運(yùn)動(dòng)數(shù)據(jù)

在讀取加速度計(jì)和角速度計(jì)的數(shù)據(jù)并換算為物理值后，根據(jù)不同的應(yīng)用，數(shù)據(jù)有不同的解譯方式。本章將以飛行器運(yùn)動(dòng)模型為例，根據(jù)加速度和角速度來(lái)算出當(dāng)前的飛行姿態(tài)。

3.1 加速度計(jì)模型

我們可以把加速度計(jì)想象為一個(gè)正立方體盒子里放著一個(gè)球，這個(gè)球被彈簧固定在立方體的中心。當(dāng)盒子運(yùn)動(dòng)時(shí)，根據(jù)假想球的位置即可算出當(dāng)前加速度的值。想象如果在太空中，盒子沒(méi)有任何受力時(shí)，假想球?qū)⑻幱谡行牡奈恢茫齻€(gè)軸的加速度均為0。見(jiàn)下圖：

如果我們給盒子施加一個(gè)水平向左的力，那么顯然盒子就會(huì)有一個(gè)向左的加速度，此時(shí)盒內(nèi)的假想球會(huì)因?yàn)閼T性作用貼向盒內(nèi)的右側(cè)面。如下圖所示：

為了保證數(shù)據(jù)的物理意義，MPU6050的加速度計(jì)是以假想球在三軸上座標(biāo)值的相反數(shù)作為三個(gè)軸的加速度值。當(dāng)假想球的位置偏向一個(gè)軸的正向時(shí)，該軸的加速度讀數(shù)為負(fù)值，當(dāng)假想球的位置偏向一個(gè)軸的負(fù)向時(shí)，該軸的加速度讀數(shù)為正值。

根據(jù)以上分析，當(dāng)我們把MPU6050芯片水平放于地方，芯片表面朝向天空，此時(shí)由于受到地球重力的作用， 假想球的位置偏向Z軸的負(fù)向，因此Z軸的加速度讀數(shù)應(yīng)為正，且在理想情況下應(yīng)為g。注意，此加速度的物理意義并不是重力加速度，而是自身運(yùn)動(dòng)的加速度，可以這樣理解：正因?yàn)槠渥陨磉\(yùn)動(dòng)的加速度與重力加速度大小相等方向相反，芯片才能保持靜止。

3.2 Roll-pitch-yaw模型與姿態(tài)計(jì)算

表示飛行器當(dāng)前飛行姿態(tài)的一個(gè)通用模型就是建立下圖所示坐標(biāo)系，并用Roll表示繞X軸的旋轉(zhuǎn)，Pitch表示繞Y軸的旋轉(zhuǎn)，Yaw表示繞Z軸的旋轉(zhuǎn)。

由于MPU6050可以獲取三個(gè)軸向上的加速度，而地球重力則是長(zhǎng)期存在且永遠(yuǎn)豎直向下，因此我們可以根據(jù)重力加速度相對(duì)于芯片的指向?yàn)閰⒖妓愕卯?dāng)前姿態(tài)。

注意，因?yàn)閍rccos函數(shù)只能返回正值角度，因此還需要根據(jù)不同情況來(lái)取角度的正負(fù)值。當(dāng)y值為正時(shí)，Roll角要取負(fù)值，當(dāng)x軸為負(fù)時(shí)，Pitch角要取負(fù)值。

3.4 Yaw角的問(wèn)題?

因?yàn)闆](méi)有參考量，所以無(wú)法求出當(dāng)前的Yaw角的絕對(duì)角度，只能得到Y(jié)aw的變化量，也就是角速度GYR_Z。當(dāng)然，我們可以通過(guò)對(duì)GYR_Z積分的方法來(lái)推算當(dāng)前Yaw角（以初始值為準(zhǔn)），但由于測(cè)量精度的問(wèn)題，推算值會(huì)發(fā)生漂移，一段時(shí)間后就完全失去意義了。然而在大多數(shù)應(yīng)用中，比如無(wú)人機(jī)，只需要獲得GRY_Z就可以了。

如果必須要獲得絕對(duì)的Yaw角，那么應(yīng)當(dāng)選用MPU9250這款九軸運(yùn)動(dòng)跟蹤芯片，它可以提供額外的三軸羅盤數(shù)據(jù)，這樣我們就可以根據(jù)地球磁場(chǎng)方向來(lái)計(jì)算Yaw角了，具體方法此處不再贅述。

四、數(shù)據(jù)處理與實(shí)現(xiàn)

MPU6050芯片提供的數(shù)據(jù)夾雜有較嚴(yán)重的噪音，在芯片處理靜止?fàn)顟B(tài)時(shí)數(shù)據(jù)擺動(dòng)都可能超過(guò)2%。除了噪音，各項(xiàng)數(shù)據(jù)還會(huì)有偏移的現(xiàn)象，也就是說(shuō)數(shù)據(jù)并不是圍繞靜止工作點(diǎn)擺動(dòng)，因此要先對(duì)數(shù)據(jù)偏移進(jìn)行校準(zhǔn) ，再通過(guò)濾波算法消除噪音。

4.1 校準(zhǔn)

校準(zhǔn)是比較簡(jiǎn)單的工作，我們只需要找出擺動(dòng)的數(shù)據(jù)圍繞的中心點(diǎn)即可。我們以GRY_X為例，在芯片處理靜止?fàn)顟B(tài)時(shí)，這個(gè)讀數(shù)理論上講應(yīng)當(dāng)為0，但它往往會(huì)存在偏移量，比如我們以10ms的間隔讀取了10個(gè)值如下：

-158.4, -172.9, -134.2, -155.1, -131.2, -146.8, -173.1, -188.6, -142.7, -179.5

這10個(gè)值的均值，也就是這個(gè)讀數(shù)的偏移量為-158.25。在獲取偏移量后，每次的讀數(shù)都減去偏移量就可以得到校準(zhǔn)后的讀數(shù)了。當(dāng)然這個(gè)偏移量只是估計(jì)值，比較準(zhǔn)確的偏移量要對(duì)大量的數(shù)據(jù)進(jìn)行統(tǒng)計(jì)才能獲知，數(shù)據(jù)量越大越準(zhǔn)，但統(tǒng)計(jì)的時(shí)間也就越慢。一般校準(zhǔn)可以在每次啟動(dòng)系統(tǒng)時(shí)進(jìn)行，那么你應(yīng)當(dāng)在準(zhǔn)確度和啟動(dòng)時(shí)間之間做一個(gè)權(quán)衡。?

三個(gè)角速度讀數(shù)GYR_X、GYR_Y和GYR_Z均可通過(guò)統(tǒng)計(jì)求平均的方法來(lái)獲得，但三個(gè)加速度分量就不能這樣簡(jiǎn)單的完成了，因?yàn)樾酒o止時(shí)的加速度并不為0。

加速度值的偏移來(lái)自兩個(gè)方面，一是由于芯片的測(cè)量精度，導(dǎo)至它測(cè)得的加速度向量并不垂直于大地；二是芯片在整個(gè)系統(tǒng)（如無(wú)人機(jī)）上安裝的精度是有限的，系統(tǒng)與芯片的座標(biāo)系很難達(dá)到完美重合。前者我們稱為讀數(shù)偏移，后者我們稱為角度偏移。因?yàn)樽x數(shù)和角度之間是非線性關(guān)系，所以要想以高精度進(jìn)行校準(zhǔn)必須先單獨(dú)校準(zhǔn)讀數(shù)偏移，再把芯片固定在系統(tǒng)中后校準(zhǔn)角度偏移。然而，由于校準(zhǔn)角度偏移需要專業(yè)設(shè)備，且對(duì)于一般應(yīng)用來(lái)說(shuō)，兩步校準(zhǔn)帶來(lái)的精度提升并不大，因此通常只進(jìn)行讀數(shù)校準(zhǔn)即可。下面介紹讀數(shù)校準(zhǔn)的方法。我們還3.2節(jié)的飛機(jī)為例，分以下幾個(gè)步驟：

1. 首先要確定飛機(jī)的坐標(biāo)系，對(duì)于多軸飛行器來(lái)說(shuō)這非常重要。如果坐標(biāo)系原點(diǎn)的位置或坐標(biāo)軸的方向存在較大偏差，將會(huì)給后面的飛控造成不良影響。

2. 在確定了飛機(jī)的坐標(biāo)系后，為了盡量避免讀數(shù)偏移帶來(lái)的影響，首先將MPU6050牢牢地固定在飛機(jī)上，并使二者座標(biāo)系盡可能的重合。當(dāng)然把Z軸反過(guò)來(lái)裝也是可以的，就是需要重新推算一套角度換算公式。

3. 將飛機(jī)置于水平、堅(jiān)固的平面上，并充分預(yù)熱。對(duì)于多軸無(wú)人機(jī)而言，空中懸停時(shí)的XY平面應(yīng)當(dāng)平行于校準(zhǔn)時(shí)的XY平面。此時(shí)，我們認(rèn)為芯片的加速度方向應(yīng)當(dāng)與Z軸負(fù)方向重合，且加速度向量的模長(zhǎng)為g，因此ACC_X和ACC_Y的理論值應(yīng)為0，ACC_Z的理論值應(yīng)為-16384（假設(shè)我們?cè)O(shè)定2g的倍率，1g的加速度的讀數(shù)應(yīng)為最大值-32768的一半）。

4. 由于ACC_X和ACC_Y的理論值應(yīng)為0，與角速度量的校準(zhǔn)類似，這兩個(gè)讀數(shù)偏移量可用統(tǒng)計(jì)均值的方式校準(zhǔn)。ACC_Z則需要多一步處理，即在統(tǒng)計(jì)偏移量的過(guò)程中，每次讀數(shù)都要加上16384，再進(jìn)行統(tǒng)計(jì)均值校準(zhǔn)。

4.2 卡爾曼濾波

對(duì)于夾雜了大量噪音的數(shù)據(jù)，卡爾曼濾波器的效果無(wú)疑是最好的。如果不想考慮算法細(xì)節(jié)，可以直接使用Arduino的Klaman Filter庫(kù)完成。在我們的模型中，一個(gè)卡爾曼濾波器接受一個(gè)軸上的角度值、角速度值以及時(shí)間增量，估計(jì)出一個(gè)消除噪音的角度值。跟據(jù)當(dāng)前的角度值和上一輪估計(jì)的角度值，以及這兩輪估計(jì)的間隔時(shí)間，我們還可以反推出消除噪音的角速度。

實(shí)現(xiàn)代碼見(jiàn)4.3節(jié)。下面介紹卡爾曼濾波算法細(xì)節(jié)，不感興趣的可跳過(guò)。

（想看的人多了再寫(xiě)）

4.3 實(shí)現(xiàn)代碼

以下代碼在Arduino軟件1.65版本中編譯、燒寫(xiě)以及測(cè)試通過(guò)。?

// 本代碼版權(quán)歸Devymex所有，以GNU GENERAL PUBLIC LICENSE V3.0發(fā)布// http://www.gnu.org/licenses/gpl-3.0.en.html// 相關(guān)文檔參見(jiàn)作者于知乎專欄發(fā)表的原創(chuàng)文章：// http://zhuanlan.zhihu.com/devymex/20082486//連線方法//MPU-UNO//VCC-VCC//GND-GND//SCL-A5//SDA-A4//INT-2 (Optional)#include <Kalman.h>#include <Wire.h>#include <Math.h>float fRad2Deg = 57.295779513f; //將弧度轉(zhuǎn)為角度的乘數(shù)const int MPU = 0x68; //MPU-6050的I2C地址const int nValCnt = 7; //一次讀取寄存器的數(shù)量const int nCalibTimes = 1000; //校準(zhǔn)時(shí)讀數(shù)的次數(shù)int calibData[nValCnt]; //校準(zhǔn)數(shù)據(jù)unsigned long nLastTime = 0; //上一次讀數(shù)的時(shí)間float fLastRoll = 0.0f; //上一次濾波得到的Roll角float fLastPitch = 0.0f; //上一次濾波得到的Pitch角Kalman kalmanRoll; //Roll角濾波器Kalman kalmanPitch; //Pitch角濾波器void setup() { ?Serial.begin(9600); //初始化串口，指定波特率 ?Wire.begin(); //初始化Wire庫(kù) ?WriteMPUReg(0x6B, 0); //啟動(dòng)MPU6050設(shè)備 ?Calibration(); //執(zhí)行校準(zhǔn) ?nLastTime = micros(); //記錄當(dāng)前時(shí)間}void loop() { ?int readouts[nValCnt]; ?ReadAccGyr(readouts); //讀出測(cè)量值 ? ?float realVals[7]; ?Rectify(readouts, realVals); //根據(jù)校準(zhǔn)的偏移量進(jìn)行糾正 ?//計(jì)算加速度向量的模長(zhǎng)，均以g為單位 ?float fNorm = sqrt(realVals[0] * realVals[0] + realVals[1] * realVals[1] + realVals[2] * realVals[2]); ?float fRoll = GetRoll(realVals, fNorm); //計(jì)算Roll角 ?if (realVals[1] > 0) { ? ?fRoll = -fRoll; ?} ?float fPitch = GetPitch(realVals, fNorm); //計(jì)算Pitch角 ?if (realVals[0] < 0) { ? ?fPitch = -fPitch; ?} ?//計(jì)算兩次測(cè)量的時(shí)間間隔dt，以秒為單位 ?unsigned long nCurTime = micros(); ?float dt = (double)(nCurTime - nLastTime) / 1000000.0; ?//對(duì)Roll角和Pitch角進(jìn)行卡爾曼濾波 ?float fNewRoll = kalmanRoll.getAngle(fRoll, realVals[4], dt); ?float fNewPitch = kalmanPitch.getAngle(fPitch, realVals[5], dt); ?//跟據(jù)濾波值計(jì)算角度速 ?float fRollRate = (fNewRoll - fLastRoll) / dt; ?float fPitchRate = (fNewPitch - fLastPitch) / dt; //更新Roll角和Pitch角 ?fLastRoll = fNewRoll; ?fLastPitch = fNewPitch; ?//更新本次測(cè)的時(shí)間 ?nLastTime = nCurTime; ?//向串口打印輸出Roll角和Pitch角，運(yùn)行時(shí)在Arduino的串口監(jiān)視器中查看 ?Serial.print("Roll:"); ?Serial.print(fNewRoll); Serial.print('('); ?Serial.print(fRollRate); Serial.print("),\tPitch:"); ?Serial.print(fNewPitch); Serial.print('('); ?Serial.print(fPitchRate); Serial.print(")\n"); ?delay(10);}//向MPU6050寫(xiě)入一個(gè)字節(jié)的數(shù)據(jù)//指定寄存器地址與一個(gè)字節(jié)的值void WriteMPUReg(int nReg, unsigned char nVal) { ?Wire.beginTransmission(MPU); ?Wire.write(nReg); ?Wire.write(nVal); ?Wire.endTransmission(true);}//從MPU6050讀出一個(gè)字節(jié)的數(shù)據(jù)//指定寄存器地址，返回讀出的值unsigned char ReadMPUReg(int nReg) { ?Wire.beginTransmission(MPU); ?Wire.write(nReg); ?Wire.requestFrom(MPU, 1, true); ?Wire.endTransmission(true); ?return Wire.read();}//從MPU6050讀出加速度計(jì)三個(gè)分量、溫度和三個(gè)角速度計(jì)//保存在指定的數(shù)組中void ReadAccGyr(int *pVals) { ?Wire.beginTransmission(MPU); ?Wire.write(0x3B); ?Wire.requestFrom(MPU, nValCnt * 2, true); ?Wire.endTransmission(true); ?for (long i = 0; i < nValCnt; ++i) { ? ?pVals[i] = Wire.read() << 8 | Wire.read(); ?}}//對(duì)大量讀數(shù)進(jìn)行統(tǒng)計(jì)，校準(zhǔn)平均偏移量void Calibration(){ ?float valSums[7] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0}; ?//先求和 ?for (int i = 0; i < nCalibTimes; ++i) { ? ?int mpuVals[nValCnt]; ? ?ReadAccGyr(mpuVals); ? ?for (int j = 0; j < nValCnt; ++j) { ? ? ?valSums[j] += mpuVals[j]; ? ?} ?} ?//再求平均 ?for (int i = 0; i < nValCnt; ++i) { ? ?calibData[i] = int(valSums[i] / nCalibTimes); ?} ?calibData[2] += 16384; //設(shè)芯片Z軸豎直向下，設(shè)定靜態(tài)工作點(diǎn)。}//算得Roll角。算法見(jiàn)文檔。float GetRoll(float *pRealVals, float fNorm) { ?float fNormXZ = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[2] * pRealVals[2]); ?float fCos = fNormXZ / fNorm; ?return acos(fCos) * fRad2Deg;}//算得Pitch角。算法見(jiàn)文檔。float GetPitch(float *pRealVals, float fNorm) { ?float fNormYZ = sqrt(pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]); ?float fCos = fNormYZ / fNorm; ?return acos(fCos) * fRad2Deg;}//對(duì)讀數(shù)進(jìn)行糾正，消除偏移，并轉(zhuǎn)換為物理量。公式見(jiàn)文檔。void Rectify(int *pReadout, float *pRealVals) { ?for (int i = 0; i < 3; ++i) { ? ?pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 16384.0f; ?} ?pRealVals[3] = pReadout[3] / 340.0f + 36.53; ?for (int i = 4; i < 7; ++i) { ? ?pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 131.0f; ?}}

最后，MPU6050數(shù)據(jù)表：https://store.invensense.com/datasheets/invensense/MPU-6050_DataSheet_V3%204.pdf

如果條件允許的話，可以考慮上一個(gè)9250

但是還是那句話，萬(wàn)變不離其宗，云臺(tái)也好溫控也好說(shuō)破大天還是一個(gè)自動(dòng)控制原理的事，關(guān)于《自動(dòng)控制原理》我會(huì)傳視頻教程的