【導(dǎo)讀】西華大學(xué)畢業(yè)設(shè)計(jì)說(shuō)明書(shū)

　　

【正文】 軸上，瞬時(shí)針轉(zhuǎn)動(dòng)角速度為正，逆時(shí)針轉(zhuǎn)動(dòng)角速度為負(fù)。而加速度因?yàn)檗D(zhuǎn)動(dòng)的原因，Y 軸上的加速度絕對(duì)值在減小，而 X 軸上的加速度絕對(duì)值在增大， 而水平上的 Y 坐標(biāo)軸加速度幾乎為 0g。當(dāng) 傳感器 繞 X 軸轉(zhuǎn)動(dòng) 時(shí) 情況和繞 Y 軸轉(zhuǎn)動(dòng)類似。 當(dāng) 傳感器 繞 Z 軸轉(zhuǎn)動(dòng) 時(shí) , X、 Y 軸上的角速度因?yàn)殪o止幾乎為 0176。 /sec。 Z 軸上，瞬時(shí)針轉(zhuǎn)動(dòng)角速度為正，逆時(shí)針轉(zhuǎn)動(dòng)角速度為 負(fù)。因?yàn)?X,Y 軸都在水平方向上轉(zhuǎn)動(dòng)， Z 軸一直豎直向上，所以 X,Y,Z軸上的加速度都幾乎不變，并且 X,Y 軸上的加速度幾乎為 0。 姿態(tài)解算 在 ， 四旋翼飛行器的飛行方向與速度都是由飛行器的傾角決定的，并且飛行器傾斜的角度越大，飛行速度也就越快。所以我們期望得到的是姿態(tài)數(shù)據(jù)，也就是歐拉角：航向角（ yaw）、橫滾角（ roll）和俯仰角（ pitch）。有了這三個(gè)角，我們就可以得到當(dāng)前四軸的姿態(tài)，這才是我們想要的結(jié)果。而不是 X,Y,Z 三個(gè)軸的加速 第 24 頁(yè) 西華大學(xué)畢業(yè)設(shè)計(jì)說(shuō)明書(shū) 度或者角速度。 那能否使用加速度來(lái)計(jì)算姿態(tài) 角呢？加速度坐標(biāo)模型如圖 所示。 圖 加速度坐標(biāo)模型 圖 要知道當(dāng)飛行器只受重力場(chǎng)時(shí)， x^2+y^2+z^2 =1g, 向量 R 是加速度計(jì)測(cè)量（它可以是引力場(chǎng)或慣性力）的力矢量。 Rx， Ry， Rz， 分別 R 在 X， Y， Z 軸上的矢量投影。有下面的關(guān)系式： R^2 = RX^2 + RY^2 + RZ^2。 我們感興趣的是向量 R 和 X、 Y、 Z 軸之間的角度，將它們定義為 Axr、 Ayr、 Azr。模型圖如圖 所示。 可以看到由 R 和 Rx 組成的直角三角形。 圖 重力分量坐標(biāo)模型 圖 這里我們可以很容 易推導(dǎo)出下面的公式，得到重力向量與各個(gè)軸間的夾角，即姿態(tài)角。 第 25 頁(yè) 西華大學(xué)畢業(yè)設(shè)計(jì)說(shuō)明書(shū) COS（ Axr） = RX / R 依此類推： COS（ Ayr） = RY / R COS（ Azr） = RZ / R 當(dāng)使用 arccos()反余弦 : Axr = arccos(RX / R) Ayr = arccos(RY / R) Azr = arccos(RZ / R) 具體而言，就是： cosX = cos(Axr) = Rx / R cosY = cos(Ayr) = Ry / R cosZ = cos(Azr) = Rz / R 這 三個(gè)公式通常被稱為方向余弦?？梢院苋菀椎刈C明： SQRT（ cosX ^ 2 + COSY ^ 2 + cosZ ^ 2） = 1 這個(gè)特點(diǎn)可以避免記錄 R 矢量的模（長(zhǎng)度）。很多時(shí)候，只有慣性矢量方向才是真正有用的，對(duì)矢量進(jìn)行規(guī)范化對(duì)簡(jiǎn)化程序運(yùn)算很有意義。 比如， 當(dāng)水平放置 MPU6050,只有 Z 軸感受到重力向量，它將輸出 1g。對(duì)應(yīng)的 ADC值就是 16384 (2g 的量程 )。此時(shí) ,R 就是重力向量 ,Rx=0. Ry=0. Rz = R =1g. 滿足 R ^ 2 = RX ^ 2 + RY ^ 2 + RZ ^ 2 得 到重力向量與各個(gè)軸的夾角。 Axr = arccos(RX / R) = 90 度。 Ayr = arccos(RY / R) = 90 度。 Azr = arccos(RZ / R) = 0 度。 這里似乎只需要加速度就可以獲得飛行器的姿態(tài)角，但飛行器在實(shí)際飛行的過(guò)程中，由于飛行器本身的擺動(dòng)、振動(dòng)或者加速運(yùn)動(dòng)時(shí)所產(chǎn)生的加速度會(huì)對(duì)重力加速度產(chǎn)生很大的干擾信號(hào)（ MPU6050 并不能區(qū)分哪個(gè)是真正需要的加速度），干擾它疊加在應(yīng)該測(cè)量的重力加速度信號(hào)上使得輸出的數(shù)據(jù)無(wú)法準(zhǔn)確反映飛行器的傾角，如圖 所示。 圖 單軸加速度測(cè)量值曲線圖 圖中實(shí)線表示 MPU6050 單軸測(cè)得加速度變化量，而虛線表示重力加速度在該軸上 第 26 頁(yè) 西華大學(xué)畢業(yè)設(shè)計(jì)說(shuō)明書(shū) 的分量，可以看出，是有干擾疊加在重力加速度分量信號(hào)上的。這些波動(dòng)噪聲可以通過(guò)數(shù)據(jù)平滑濾波將其濾除。但是平滑濾波一方面會(huì)使得信號(hào)無(wú)法實(shí)時(shí)反映飛行器的傾角變化，進(jìn)而減緩了程序?qū)τ陲w行器控制。另一方面也會(huì)將飛行器角速度的變化信息濾掉。上述兩方面的濾波效果使得飛行器保持平衡的難度加大。 對(duì)于角速度而言，它并不會(huì)受到飛行器運(yùn)動(dòng)的影響，所以該數(shù)據(jù)中的噪聲很小，可以把轉(zhuǎn)動(dòng)角速度進(jìn)行積分得到姿態(tài)角，這樣又可以進(jìn)一步平滑 信號(hào)，從而使得到的角度信號(hào)更加穩(wěn)定。但是這存在一個(gè)問(wèn)題，由于從陀螺儀角速度獲得角度信息，需要經(jīng)過(guò)積分運(yùn)算。如果角速度信號(hào)存在微小的偏差和漂移，經(jīng)過(guò)積分運(yùn)算之后，變化形成積累誤差。這個(gè)誤差會(huì)隨著時(shí)間延長(zhǎng)逐步增加，最終導(dǎo)致無(wú)法形成正確的角度信號(hào)。如圖 所示。 圖 角速度積分漂移現(xiàn)象曲線圖 圖中實(shí)線表示由角速度積分后的角度值，虛線表示真實(shí)的角度值 ?？梢缘玫浇Y(jié)論，單單依靠角速度的到的角度無(wú)法精確反應(yīng)飛行器的姿態(tài)。如何消除這個(gè)累積誤差呢？ 一種方法就是通過(guò)上面的加速度獲得的角度信息對(duì)此進(jìn)行校正。通過(guò)對(duì) 比積分所得到的角度與重力加速度所得到的角度，使用它們之間的偏差改變陀螺儀的輸出，從而積分的角度逐步跟蹤到加速度傳感器所得到的角度。 還有一種方法，利用 MPU6050 的數(shù)字運(yùn)動(dòng)處理器，即 DMP。首先我們需要得到MPU6050 的原始數(shù)據(jù)（線加速度和轉(zhuǎn)動(dòng)角速度），然后調(diào)用數(shù)字運(yùn)動(dòng)處理器 DMP，將數(shù)據(jù)進(jìn)行簡(jiǎn)單處理后導(dǎo)入 DMP，經(jīng)過(guò) DMP 內(nèi)部的姿態(tài)算法處理，會(huì)輸出一組四元數(shù)。四元數(shù)其實(shí)是旋轉(zhuǎn)另一種的表示方式，這里不做詳細(xì)介紹。 DMP 輸出的四元數(shù)是 q30格式的，也就是浮點(diǎn)數(shù)放大了 2 的 30 次方倍。必須先將其轉(zhuǎn)換為浮 點(diǎn)數(shù)，再除以 2 的30 次方，計(jì)算公式為： q0 = quat[0] / q30。 // q30= q1 = quat[1] / q30。 q2 = quat[2] / q30。 q3 = quat[3] / q30。 在 得到四元數(shù)之后，還需要根據(jù)四元數(shù)到歐拉角的轉(zhuǎn)換公式計(jì)算出歐拉角，即得到y(tǒng)aw、 roll 和 pitch。公式如下： 第 27 頁(yè) 西華大學(xué)畢業(yè)設(shè)計(jì)說(shuō)明書(shū) pitch = asin(2 * q1 * q3 + 2 * q0* q2)* 。 roll = atan2(2 * q2 * q3 + 2 * q0 * q1, 2 * q1 * q1 2 * q2* q2 + 1)* 。 yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1q2*q2q3*q3) * 。 同樣的，我們 用杜邦線 把 GY86 模塊（模塊上載有 MPU6050 芯片）接入單片機(jī)，先讓單片機(jī)接收 MPU6050 原始數(shù)據(jù)，并把數(shù)據(jù)做一系列的處理并送入 DMP，再根據(jù)以上兩個(gè)公式對(duì) DMP 的數(shù)據(jù)做進(jìn)一步的處理，最后將計(jì)算出的歐拉角 yaw、 roll 和 pitch顯示在 TFT 屏幕上，方便觀察 和分析。模塊連接圖如圖 所示。 圖 模塊連接圖 將 GY86 模塊按不同傾角擺放，具體如下表 所示。 表 DMP 測(cè)試 說(shuō)明 GY86 模塊狀態(tài) TFT 顯示 X 坐標(biāo) 軸 roll繞動(dòng) Y 坐標(biāo) 軸 pitch繞動(dòng) Z 坐標(biāo) 軸 yaw繞動(dòng) GY86 依靠三角板的 30 角，X 軸平行于斜邊。即繞 Y 軸逆時(shí)針轉(zhuǎn)動(dòng)，所以 pitch 為30 度。 第 28 頁(yè) 西華大學(xué)畢業(yè)設(shè)計(jì)說(shuō)明書(shū) X 坐標(biāo) 軸 roll繞動(dòng) Y 坐標(biāo) 軸 pitch繞動(dòng) Z 坐標(biāo) 軸 yaw繞動(dòng) GY86 依靠三角板的 60 角，Y 軸平行于斜邊。即繞 X 軸順時(shí)針轉(zhuǎn)動(dòng)，所以 roll 為 60度。 X 坐標(biāo) 軸 roll繞動(dòng) Y 坐標(biāo) 軸 pitch繞動(dòng) Z 坐標(biāo) 軸 yaw繞動(dòng) GY86 依靠三角板的 30 角，Z 軸垂直于斜邊。即繞 Z 軸順時(shí)針轉(zhuǎn)動(dòng)，所以 yaw 為 30度。 從表 DMP 測(cè)試?yán)锟梢钥闯?，利?DMP 進(jìn)行姿態(tài)解算，可以準(zhǔn)確地得出飛行器的歐拉角 。 數(shù)據(jù)中斷 雖然我們可以通過(guò) DMP 模式獲取正確的歐拉角，但是應(yīng)該在什么時(shí)候去讀取 DMP的四元數(shù)呢？因?yàn)?DMP 模式下這要的數(shù)據(jù)都是存儲(chǔ)在 MPU6050 里面的 FIFO， FIFO 里面存的包括 GYRO， ACC，四元數(shù)等參數(shù)。并 且 DMP 里面的數(shù)據(jù)只能從 FIFO 讀取的， DMP 庫(kù)里的做法是： FIFO 里數(shù)據(jù)過(guò)半滿時(shí)，會(huì)發(fā)出沖洗 FIFO 指令。當(dāng) FIFO 數(shù)據(jù)不足時(shí)，會(huì)延時(shí)等待。如果我們讀取早了，可能獲取的是上一次的數(shù)據(jù)，也可能 DMP 正在 第 29 頁(yè) 西華大學(xué)畢業(yè)設(shè)計(jì)說(shuō)明書(shū) 處理，需要延時(shí)等待。如果讀取晚了，最直觀的影響就是飛行器姿態(tài)調(diào)節(jié)緩慢，無(wú)法正常飛行。所以隨意的去讀取數(shù)據(jù)或者讀取的時(shí)機(jī)選擇得不好時(shí)（太快或太慢）就會(huì)出現(xiàn)數(shù)據(jù)不連續(xù)。 為了解決這個(gè)問(wèn)題 ， 使用了 MPU6050 芯片的數(shù)據(jù)就緒中斷模式，再利用單片機(jī)的外部中斷 ， 我們就可以在 MPU6050 數(shù)據(jù)就緒時(shí)立刻獲得飛行器的姿態(tài) 。 使用 MPU6050 數(shù)據(jù)就緒中斷模式需要配置以下的寄存器 ，如表 所示。 表 寄存器 58中斷標(biāo)志寄存器 Register (Hex) Register (Decimal) Bit7 Bit6 Bit5 Bit4 Bit3 Bit2 Bit1 Bit0 3A 58 MOT_INT FIFO _OFLOW _INT I2C _MST _INT DATA _RDY _INT 這個(gè)寄存器展示各個(gè)中斷源的中斷標(biāo)志。每個(gè)位在讀取后被清除。這個(gè)寄存器我們需要注意 DATA_RDY_INT 位。當(dāng)數(shù)據(jù)就緒中斷（ Data Ready interrupt）產(chǎn)生，該位自動(dòng)置 1。讀取寄存器該位清 0。 表 寄存器 55中斷配置寄存器 Register (Hex) Register (Decimal) Bit7 Bit6 Bit5 Bit4 Bit3 Bit2 Bit1 Bit0 37 55 INT_ LEVEL INT_ OPEN LATCH _INT _EN INT_RD _CLEAR FSYNC _INT _LEVEL FSYNC _INT _EN I2C _BYPASS _EN 這個(gè)寄存器配置 INT 引腳中斷信號(hào)的產(chǎn)生。 INT_LEVEL 該位等于 0， INT 引腳的邏輯電平為高電平。該位等于 1， INT 引腳的邏輯電平為低電平。 LATCH_INT_EN 該位等于 0， INT 引腳產(chǎn)生 50us 脈沖。該位等于 1， INT 引腳保持有效電平直到中斷被清除。 INT_RD_CLEAR 該位等于 0，只有讀取 INT_STATUS (寄存器 58) 會(huì)導(dǎo)致中斷狀態(tài)位清除。 INT_RD_CLEAR 該位等于 1，任何讀取動(dòng)作會(huì)導(dǎo)致中斷狀態(tài)位清除。 經(jīng)過(guò)分析后，程序里是這樣配置的： INT_LEVEL 等于 1， INT 引腳的邏輯電平為低電平有效。 INT_OPEN 該位等于 0， the INTpin is configure das pushpull。 LATCH_INT_EN 等于 1， INT 引腳保持有效電平直到中斷被清除。 INT_RD_CLEAR 該位等于 1，任何讀取動(dòng)作會(huì)導(dǎo)致中斷狀態(tài)位清除。 第 30 頁(yè) 西華大學(xué)