加速計(jì)和陀螺儀指南_第1頁
加速計(jì)和陀螺儀指南_第2頁
加速計(jì)和陀螺儀指南_第3頁
加速計(jì)和陀螺儀指南_第4頁
加速計(jì)和陀螺儀指南_第5頁
已閱讀5頁,還剩9頁未讀, 繼續(xù)免費(fèi)閱讀

下載本文檔

版權(quán)說明:本文檔由用戶提供并上傳,收益歸屬內(nèi)容提供方,若內(nèi)容存在侵權(quán),請進(jìn)行舉報(bào)或認(rèn)領(lǐng)

文檔簡介

1、介紹本指南旨在向興趣者介紹慣性MEMS(微機(jī)電系統(tǒng))傳感器,特別是加速度計(jì)和陀螺儀以及其他整合IMU(慣性測量單元)設(shè)備。IMU單元例子:上圖中MCU頂端的ACC Gyro 6DOF,名為USBThumb,支持USB/串口通信在這篇文章中我將概括這么幾個(gè)基本并且重要的話題:- 加速度計(jì)(accelerometer)檢測什么- 陀螺儀(gyroscope,也稱作 gyro)檢測什么- 如何將傳感器ADC讀取的數(shù)據(jù)轉(zhuǎn)換為物理單位(加速度傳感器的單位是g,陀螺儀的單位是 度/秒)- 如何結(jié)合加速度傳感器和陀螺儀的數(shù)據(jù)以得到設(shè)備和地平面之間的傾角的準(zhǔn)確信息在整篇文章中我盡量將數(shù)學(xué)運(yùn)算降低到最少。如果你

2、知道什么是正弦、余弦、正切函數(shù),那無論你的項(xiàng)目使用哪種平臺(tái)你應(yīng)該都會(huì)明白和運(yùn)用這篇文章中的思想,這些平臺(tái)如Arduino、Propeller、Basic Stamp、Ateml芯片、PIC芯片等等。總有些人認(rèn)為使用IMU單元需要復(fù)雜的數(shù)學(xué)運(yùn)算(復(fù)雜的FIR或IIR濾波,如卡爾曼濾波,Parks-McClellan濾波等)。你如果研究這些會(huì)得到很棒且很復(fù)雜的結(jié)果。我解釋事情的方式,只需要基本的數(shù)學(xué)。我非常堅(jiān)信簡單的原則。我認(rèn)為一個(gè)簡單的系統(tǒng)更容易操作和監(jiān)控,另外許多嵌入式設(shè)備并不具備能力和資源去實(shí)現(xiàn)需要進(jìn)行矩陣運(yùn)算的復(fù)雜算法。我會(huì)用我設(shè)計(jì)的一個(gè)新IMU模塊Acc_Gyro Acceleromet

3、er + Gyro IMU作為例子。在下面的例子中我們會(huì)使用這個(gè)設(shè)備的參數(shù)。用這個(gè)模塊作為介紹非常合適,因?yàn)樗?個(gè)設(shè)備組成:- LIS331AL (datasheet) 3軸 2G 模擬加速度計(jì)- LPR550AL (datasheet) 雙軸(俯仰、翻滾) 500/s 加速度傳感器- LY550ALH (datasheet) 單軸(偏航)陀螺儀 最后這個(gè)設(shè)備在這篇介紹中不使用,不過他在DCM Matrix implementation中有重要作用它們一起組成了一個(gè)6自由度的慣性測量單元。這是個(gè)花哨的名字!然而,在花哨的名字后面是個(gè)非常有用的設(shè)備組合,接下來我們會(huì)詳細(xì)介紹之。第一部分 加速度

4、計(jì)要了解這個(gè)模塊我們先從加速度計(jì)開始。當(dāng)我們在想象一個(gè)加速度計(jì)的時(shí)候我們可以把它想作一個(gè)圓球在一個(gè)方盒子中。你可能會(huì)把它想作一個(gè)餅干或者甜圈,但我就把它當(dāng)做一個(gè)球好了:我們假定這個(gè)盒子不在重力場中或者其他任何會(huì)影響球的位置的場中,球處于盒子的正中央。你可以想象盒子在外太空中,遠(yuǎn)離任何天體,如果很難想象,那就當(dāng)做盒子在航天飛機(jī)中,一切東西都處于無重力狀態(tài)。在上面的圖中你可以看到我們給每個(gè)軸分配了一對墻(我們移除了Y+以此來觀察里面的情況)。設(shè)想每面墻都能感測壓力。如果我們突然把盒子向左移動(dòng)(加速度為1g=9.8m/s2),那么球會(huì)撞上X-墻。然后我們檢測球撞擊墻面產(chǎn)生的壓力,X軸輸出值為-1g。

5、請注意加速度計(jì)檢測到得力的方向與它本身加速度的方向是相反的。這種力量通常被稱為慣性力或假想力。在這個(gè)模型中你你應(yīng)該學(xué)到加速度計(jì)是通過間接測量力對一個(gè)墻面的作用來測量加速度的,在實(shí)際應(yīng)用中,可能通過彈簧等裝置來測量力。這個(gè)力可以是加速度引起的,但在下面的例子中,我們會(huì)發(fā)現(xiàn)它不一定是加速度引起的。如果我們把模型放在地球上,球會(huì)落在Z-墻面上并對其施加一個(gè)1g的力,見下圖:在這種情況下盒子沒有移動(dòng)但我們?nèi)稳蛔x取到Z軸有-1g的值。球在墻壁上施加的壓力是由引力造成的。在理論上,它可以是不同類型的力量 - 例如,你可以想象我們的球是鐵質(zhì)的,將一個(gè)磁鐵放在盒子旁邊那球就會(huì)撞上另一面墻。引用這個(gè)例子只是為了

6、說明加速度計(jì)的本質(zhì)是檢測力而非加速度。只是加速度所引起的慣性力正好能被加速度計(jì)的檢測裝置所捕獲。雖然這個(gè)模型并非一個(gè)MEMS傳感器的真實(shí)構(gòu)造,但它用來解決與加速度計(jì)相關(guān)的問題相當(dāng)有效。實(shí)際上有些類似傳感器中有金屬小球,它們稱作傾角開關(guān),但是它們的功能更弱,只能檢測設(shè)備是否在一定程度內(nèi)傾斜,卻不能得到傾斜的程度。到目前為止,我們已經(jīng)分析了單軸的加速度計(jì)輸出,這是使用單軸加速度計(jì)所能得到的。三軸加速度計(jì)的真正價(jià)值在于它們能夠檢測全部三個(gè)軸的慣性力。讓我們回到盒子模型,并將盒子向右旋轉(zhuǎn)45度。現(xiàn)在球會(huì)與兩個(gè)面接觸:Z-和X-,見下圖:0.71g這個(gè)值是不是任意的,它們實(shí)際上是1/2的平方根的近似值。

7、我們介紹加速度計(jì)的下一個(gè)模型時(shí)這一點(diǎn)會(huì)更清楚。在上一個(gè)模型中我們引入了重力并旋轉(zhuǎn)了盒子。在最后的兩個(gè)例子中我們分析了盒子在兩種情況下的輸出值,力矢量保持不變。雖然這有助于理解加速度計(jì)是怎么和外部力相互作用的,但如果我們將坐標(biāo)系換為加速度的三個(gè)軸并想象矢量力在周圍旋轉(zhuǎn),這會(huì)更方便計(jì)算。請看看在上面的模型,我保留了軸的顏色,以便你的思維能更好的從上一個(gè)模型轉(zhuǎn)到新的模型中。想象新模型中每個(gè)軸都分別垂直于原模型中各自的墻面。矢量R是加速度計(jì)所檢測的矢量(它可能是重力或上面例子中慣性力的合成)。RX,RY,RZ是矢量R在X,Y,Z上的投影。請注意下列關(guān)系:,R 2 = RX 2 + RY 2 + RZ

8、2(公式1)此公式等價(jià)于三維空間勾股定理。還記得我剛才說的1/2的平方根0.71不是個(gè)隨機(jī)值吧。如果你把它們代回上式,回顧一下重力加速度是1g,那我們就能驗(yàn)證:1 2 =(SQRT(1/2) 2 + 0 2 +(SQRT(1/2) 2在公式1中簡單的取代: R=1, Rx = -SQRT(1/2), Ry = 0 , Rz = -SQRT(1/2)經(jīng)過一大段的理論序言后,我們和實(shí)際的加速度計(jì)很靠近了。RX,RY,RZ值是實(shí)際中加速度計(jì)輸出的線性相關(guān)值,你可以用它們進(jìn)行各種計(jì)算。在我們運(yùn)用它之前我們先討論一點(diǎn)獲取加速度計(jì)數(shù)據(jù)的方法。大多數(shù)加速度計(jì)可歸為兩類:數(shù)字和模擬。數(shù)字加速度計(jì)可通過I2C,

9、SPI或USART方式獲取信息,而模擬加速度計(jì)的輸出是一個(gè)在預(yù)定范圍內(nèi)的電壓值,你需要用ADC(模擬量轉(zhuǎn)數(shù)字量)模塊將其轉(zhuǎn)換為數(shù)字值。我將不會(huì)詳細(xì)介紹ADC是怎么工作的,部分原因是這是個(gè)很廣的話題,另一個(gè)原因是不同平臺(tái)的ADC都會(huì)有差別。有些MCU具有內(nèi)置ADC模塊,而有些則需要外部電路進(jìn)行ADC轉(zhuǎn)換。不管使用什么類型的ADC模塊,你都會(huì)得到一個(gè)在一定范圍內(nèi)的數(shù)值。例如一個(gè)10位ADC模塊的輸出值范圍在0 . 1023間,請注意,1023 = 2 10 -1。一個(gè)12位ADC模塊的輸出值范圍在0 . 4095內(nèi),注意,4095 = 2 12-1。我們繼續(xù),先考慮下一個(gè)簡單的例子,假設(shè)我們從10

10、位ADC模塊得到了以下的三個(gè)軸的數(shù)據(jù):AdcRx = 586AdcRy = 630AdcRz = 561每個(gè)ADC模塊都有一個(gè)參考電壓,假設(shè)在我們的例子中,它是3.3V。要將一個(gè)10位的ADC值轉(zhuǎn)成電壓值,我們使用下列公式:VoltsRx = AdcRx * VREF / 1023小注:8位ADC的最大值是255 = 2 8 -1,12位ADC最大值是4095 = 2 12 -1。將3個(gè)軸的值代入上式,得到:VoltsRx = 586 * 3.3 / 1023 =1.89V(結(jié)果取兩位小數(shù))VoltsRy = 630 * 3.3 / 1023 =2.03VVoltsRz = 561 * 3.3

11、 / 1023 =1.81V每個(gè)加速度計(jì)都有一個(gè)零加速度的電壓值,你可以在它的說明書中找到,這個(gè)電壓值對應(yīng)于加速度為0g。通過計(jì)算相對0g電壓的偏移量我們可以得到一個(gè)有符號(hào)的電壓值。比方說,0g電壓值 VzeroG= 1.65V,通過下面的方式可以得到相對0g電壓的偏移量:DeltaVoltsRx = 1.89V - 1.65V = 0.24VDeltaVoltsRy = 2.03V - 1.65V = 0.38VDeltaVoltsRz = 1.81V - 1.65V = 0.16V現(xiàn)在我們得到了加速度計(jì)的電壓值,但它的單位還不是g(9.8m/s2),最后的轉(zhuǎn)換,我們還需要引入加速度計(jì)的靈敏

12、度(Sensitivity),單位通常是 mV/g。比方說,加速度計(jì)的靈敏度 Sensitivity= 478.5mV / g = 0.4785V /g。靈敏度值可以在加速度計(jì)說明書中找到。要獲得最后的單位為g的加速度,我們使用下列公式計(jì)算:RX = DeltaVoltsRx /SensitivityRX = 0.24V / 0.4785V / G =0.5gRY = 0.38V / 0.4785V / G =0.79gRZ = 0.16V / 0.4785V / G =0.33g當(dāng)然,我們可以把所有的步驟全部放在一個(gè)式子里,但我想通過介紹每一個(gè)步驟以便讓你了解怎么讀取一個(gè)ADC值并將其轉(zhuǎn)換為

13、單位為g的矢量力的分量。Rx = (AdcRx * Vref / 1023 VzeroG) / Sensitivity(公式2)Ry = (AdcRy * Vref / 1023 VzeroG) / SensitivityRz = (AdcRz * Vref / 1023 VzeroG) / Sensitivity現(xiàn)在我們得到了慣性力矢量的三個(gè)分量,如果設(shè)備除了重力外不受任何外力影響,那我們就可以認(rèn)為這個(gè)方向就是重力矢量的方向。如果你想計(jì)算設(shè)備相對于地面的傾角,可以計(jì)算這個(gè)矢量和Z軸之間的夾角。如果你對每個(gè)軸的傾角都感興趣,你可以把這個(gè)結(jié)果分為兩個(gè)分量:X軸、Y軸傾角,這可以通過計(jì)算重力矢量和

14、X、Y軸的夾角得到。計(jì)算這些角度比你想象的簡單,現(xiàn)在我們已經(jīng)算出了Rx,Ry,Rz的值,讓我們回到我們的上一個(gè)加速度模型,再加一些標(biāo)注上去:我們感興趣的角度是向量R和X,Y,Z軸之間的夾角,那就令這些角度為Axr,Ayr,Azr。觀察由R和Rx組成的直角三角形:cos(Axr) = Rx / R , 類似的:cos(Ayr) = Ry / Rcos(Azr) = Rz / R從公式1我們可以推導(dǎo)出 R = SQRT( Rx2 + Ry2 + Rz2)通過arccos()函數(shù)(cos()的反函數(shù))我們可以計(jì)算出所需的角度:Axr = arccos(Rx/R)Ayr = arccos(Ry/R)A

15、zr = arccos(Rz/R)我們花了大段的篇幅來解釋加速度計(jì)模型,最后所要的只是以上這幾個(gè)公式。根據(jù)你的應(yīng)用場合,你可能會(huì)用到我們推導(dǎo)出來的幾個(gè)過渡公式。我們接下來要介紹陀螺儀模塊,并向大家介紹怎么融合加速度計(jì)和陀螺儀的數(shù)據(jù)以得到更精確的傾角值。但在此之前,我們再介紹幾個(gè)很常用的公式:cosX = cos(Axr) = Rx / RcosY = cos(Ayr) = Ry / RcosZ = cos(Azr) = Rz / R這三個(gè)公式通常稱作方向余弦,它主要表達(dá)了單位向量(長度為1的向量)和R向量具有相同的方向。你可以很容易地驗(yàn)證:SQRT(cosX 2 + COSY 2 + cosZ

16、 2)= 1這是個(gè)很好的性質(zhì),因?yàn)樗苊饬宋覀円恢睓z測R向量的模(長度)。通常如果我們只是對慣性力的方向感興趣,那標(biāo)準(zhǔn)化模長以簡化其他計(jì)算是個(gè)明智的選擇。第二部分陀螺儀對于陀螺儀我們將不會(huì)像加速度計(jì)一樣介紹它的等價(jià)盒子模型,而是直接跳到加速度計(jì)的第二個(gè)模型,通過這個(gè)模型我們會(huì)向大家介紹陀螺儀是怎么工作的。陀螺儀的每個(gè)通道檢測一個(gè)軸的旋轉(zhuǎn)。例如,一個(gè)2軸陀螺儀檢測繞X和Y軸的旋轉(zhuǎn)。為了用數(shù)字來表達(dá)這些旋轉(zhuǎn),我們先引進(jìn)一些符號(hào)。首先我們定義:Rxz 慣性力矢量R在XZ平面上的投影Ryz 慣性力矢量R在YZ平面的上投影在由Rxz和Rz組成的直角三角形中,運(yùn)用勾股定理可得:Rxz2 = Rx2 + R

17、z2 ,同樣:Ryz2 = Ry2 + Rz2同時(shí)注意:R2 = Rxz2 + Ry2 ,這個(gè)公式可以公式1和上面的公式推導(dǎo)出來,也可由R和Ryz所組成的直角三角形推導(dǎo)出來R 2 = Ryz 2 + RX 2在這篇文章中我們不會(huì)用到這些公式,但知道模型中的那些數(shù)值間的關(guān)系有助于理解。相反,我們按如下方法定義Z軸和Rxz、Ryz向量所成的夾角:AXZ - Rxz(矢量R在XZ平面的投影)和Z軸所成的夾角AYZ - Ryz(矢量R在YZ平面的投影)和Z軸所成夾角現(xiàn)在我們離陀螺儀要測量的東西又近了一步。陀螺儀測量上面定義的角度的變化率。換句話說,它會(huì)輸出一個(gè)與上面這些角度變化率線性相關(guān)的值。為了解釋

18、這一點(diǎn),我們先假設(shè)在t0時(shí)刻,我們已測得繞Y軸旋轉(zhuǎn)的角度(也就是Axz),定義為Axz0,之后在t1時(shí)刻我們再次測量這個(gè)角度,得到Axz1。角度變化率按下面方法計(jì)算:RateAxz = (Axz1 Axz0) / (t1 t0).如果用度來表示角度,秒來表示時(shí)間,那這個(gè)值的單位就是 度/秒。這就是陀螺儀檢測的東西。在實(shí)際運(yùn)用中,陀螺儀一般都不會(huì)直接給你一個(gè)單位為度/秒的值(除非它是個(gè)特殊的數(shù)字陀螺儀)。就像加速度計(jì)一樣,你會(huì)得到一個(gè)ADC值并且要用類似公式2的式子將其轉(zhuǎn)換成單位為 度/秒的值。讓我們來介紹陀螺儀輸出值轉(zhuǎn)換中的ADC部分(假設(shè)使用10位ADC模塊,如果是8位ADC,用1023代替

19、255,如果是12為ADC用4095代替1023)。RateAxz = (AdcGyroXZ * Vref / 1023 VzeroRate) / Sensitivity公式3RateAyz = (AdcGyroYZ * Vref / 1023 VzeroRate) / SensitivityAdcGyroXZ,AdcGyroYZ - 這兩個(gè)值由ADC讀取,它們分別代表矢量R的投影在XZ和YZ平面內(nèi)里的轉(zhuǎn)角,也可等價(jià)的說,旋轉(zhuǎn)可分解為單獨(dú)繞Y和X軸的運(yùn)動(dòng)。Vref ADC的參考電壓,上例中我們使用3.3VVzeroRate 是零變化率電壓,換句話說它是陀螺儀不受任何轉(zhuǎn)動(dòng)影響時(shí)的輸出值,對Acc

20、 Gyro板來說,可以認(rèn)為是1.23V(此值通??梢栽谡f明書中找到但千萬別相信這個(gè)值,因?yàn)榇蠖鄶?shù)的陀螺儀在焊接后會(huì)有一定的偏差,所以可以使用電壓計(jì)測量每個(gè)通道的輸出值,通常這個(gè)值在焊接后就不會(huì)改變,如果有跳動(dòng),在設(shè)備使用前寫一個(gè)校準(zhǔn)程序?qū)ζ溥M(jìn)行測量,用戶應(yīng)當(dāng)在設(shè)備啟動(dòng)的時(shí)候保持設(shè)備靜止以進(jìn)行校準(zhǔn))。Sensitivity 陀螺儀的靈敏度,單位mV/(deg/s),通常寫作mV/deg/s,它的意思就是如果旋轉(zhuǎn)速度增加1/s,陀螺儀的輸出就會(huì)增加多少mV。Acc_Gyro板的靈敏度值是2mV/deg/s或0.002V/deg/s讓我們舉個(gè)例子,假設(shè)我們的ADC模塊返回以下值:AdcGyroXZ

21、= 571AdcGyroXZ = 323用上面的公式,在代入Acc Gyro板的參數(shù),可得:RateAxz = (571 * 3.3V / 1023 1.23V) / ( 0.002V/deg/s) = 306 deg/sRateAyz = (323 * 3.3V / 1023 1.23V) / ( 0.002V/deg/s) = -94 deg/s換句話說設(shè)備繞Y軸(也可以說在XZ平面內(nèi))以306/s速度和繞X軸(或者說YZ平面內(nèi))以-94/s的速度旋轉(zhuǎn)。請注意,負(fù)號(hào)表示該設(shè)備朝著反方向旋轉(zhuǎn)。按照慣例,一個(gè)方向的旋轉(zhuǎn)是正值。一份好的陀螺儀說明書會(huì)告訴你哪個(gè)方向是正的,否則你就要自己測試出哪個(gè)

22、旋轉(zhuǎn)方向會(huì)使得輸出腳電壓增加。最好使用示波器進(jìn)行測試,因?yàn)橐坏┠阃V沽诵D(zhuǎn),電壓就會(huì)掉回零速率水平。如果你使用的是萬用表,你得保持一定的旋轉(zhuǎn)速度幾秒鐘并同時(shí)比較電壓值和零速率電壓值。如果值大于零速率電壓值那說明這個(gè)旋轉(zhuǎn)方向是正向。第三部分 將它們綜合起來。融合加速度計(jì)和陀螺儀的數(shù)據(jù)。如果你在閱讀這篇文章你可能已經(jīng)有了或準(zhǔn)備購買一個(gè)IMU設(shè)備,或者你準(zhǔn)備用獨(dú)立的加速度計(jì)和陀螺儀搭建一個(gè)。注:具體的代碼實(shí)現(xiàn)和算法測試,請閱讀這篇文章:在使用整合了加速度計(jì)和陀螺儀的IMU設(shè)備時(shí),首先要做的就是統(tǒng)一它們的坐標(biāo)系。最簡單的辦法就是將加速度計(jì)作為參考坐標(biāo)系。大多數(shù)的加速度計(jì)技術(shù)說明書都會(huì)指出對應(yīng)于物理芯片

23、或設(shè)備的XZY軸方向。例如,下面就是Acc Gyro板的說明書中給出的XYZ軸方向:接下來的步驟是:- 確定陀螺儀的輸出對應(yīng)到上述討論的RateAxz,RateAyz值。- 根據(jù)陀螺儀和加速度計(jì)的位置決定是否要反轉(zhuǎn)輸出值不要設(shè)想陀螺儀陀的輸出有XY,它會(huì)適應(yīng)加速度計(jì)坐標(biāo)系里的任何軸,盡管這個(gè)輸出是IMU模塊的一部分。最好的辦法就是測試。接下來的示例用來確定哪個(gè)陀螺儀的輸出對應(yīng)RateAxz。- 首先將設(shè)備保持水平。加速度計(jì)的XY軸輸出會(huì)是零加速度電壓(Acc Gyro板的值是1.65V)- 接下來將設(shè)備繞Y軸旋轉(zhuǎn),換句話說就是將設(shè)備在XZ平面內(nèi)旋轉(zhuǎn),所以X、Z的加速度輸出值會(huì)變化而Y軸保持不變

24、。-當(dāng)以勻速旋轉(zhuǎn)設(shè)備的時(shí)候,注意陀螺儀的哪個(gè)通道輸出值變化了,其他輸出應(yīng)該保持不變。- 在陀螺儀繞Y軸旋轉(zhuǎn)(在XZ平面內(nèi)旋轉(zhuǎn))的時(shí)候輸出值變化的就是AdcGyroXZ,用于計(jì)算RateAxz-最后一步,確認(rèn)旋轉(zhuǎn)的方向是否和我們的模型對應(yīng),因?yàn)橥勇輧x和加速度的位置關(guān)系,有時(shí)候你可能要把RateAxz值反向-重復(fù)上面的測試,將設(shè)備繞Y軸旋轉(zhuǎn),這次查看加速度計(jì)的X軸輸出(也就是AdcRx)。如果AdcRx增大(從水平位置開始旋轉(zhuǎn)的第一個(gè)90),那AdcGyroXZ應(yīng)當(dāng)減小。這是因?yàn)槲覀冇^察的是重力矢量,當(dāng)設(shè)備朝一個(gè)方向旋轉(zhuǎn)時(shí)矢量會(huì)朝相反的方向旋轉(zhuǎn)(相對坐標(biāo)系運(yùn)動(dòng))。所以,如果你不想反轉(zhuǎn)RateAxz

25、,你可以在公式3中引入正負(fù)號(hào)來解決這個(gè)問題:RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 VzeroRate) / Sensitivity ,其中InvertAxz= 1 或-1同樣的方法可以用來測試RateAyz,將設(shè)備繞X軸旋轉(zhuǎn),你就能測出陀螺儀的哪個(gè)輸出對應(yīng)于RateAyz,以及它是否需要反轉(zhuǎn)。一旦你確定了InvertAyz,你就能可以用下面的公式來計(jì)算RateAyz:RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 VzeroRate) / Sensitivity如果對Acc Gyro板進(jìn)行這些

26、測試,你會(huì)得到下面的這些結(jié)果:- RateAxz的輸出管腳是GX4,InvertAxz = 1- RateAyz輸出管腳是GY4,InvertAyz = 1從現(xiàn)在開始我們認(rèn)為你已經(jīng)設(shè)置好了IMU模塊并能計(jì)算出正確的Axr,Ayr,Azr值(在第一部分加速度計(jì)中定義)以及RateAyz,RateAyz(在第二部分陀螺儀中)。下一步,我們分析這些值之間的關(guān)系并得到更準(zhǔn)確的設(shè)備和地平面之間的傾角。你可能會(huì)問自己一個(gè)問題,如果加速度計(jì)已經(jīng)告訴我們Axr,Ayr,Azr的傾角,為什么還要費(fèi)事去得到陀螺儀的數(shù)據(jù)?答案很簡單:加速度計(jì)的數(shù)據(jù)不是100%準(zhǔn)確的。有幾個(gè)原因,還記加速度計(jì)測量的是慣性力,這個(gè)力可

27、以由重力引起(理想情況只受重力影響),當(dāng)也可能由設(shè)備的加速度(運(yùn)動(dòng))引起。因此,就算加速度計(jì)處于一個(gè)相對比較平穩(wěn)的狀態(tài),它對一般的震動(dòng)和機(jī)械噪聲很敏感。這就是為什么大部分的IMU系統(tǒng)都需要陀螺儀來使加速度計(jì)的輸出更平滑。但是怎么辦到這點(diǎn)呢?陀螺儀不受噪聲影響嗎?陀螺儀也會(huì)有噪聲,但由于它檢測的是旋轉(zhuǎn),因此對線性機(jī)械運(yùn)動(dòng)沒那么敏感,不過陀螺儀有另外一種問題,比如漂移(當(dāng)選擇停止的時(shí)候電壓不會(huì)回到零速率電壓)。然而,通過計(jì)算加速度計(jì)和陀螺儀的平均值我們能得到一個(gè)相對更準(zhǔn)確的當(dāng)前設(shè)備的傾角值,這比單獨(dú)使用加速度計(jì)更好。接下來的步驟我會(huì)介紹一種算法,算法受卡爾曼濾波中的一些思想啟發(fā),但是它更簡單并且更

28、容易在嵌入式設(shè)備中實(shí)現(xiàn)。在此之前,讓我們先看看我們需要算法計(jì)算什么值。所要算的就是重力矢量R=Rx,Ry,Rz,它可由其他值推導(dǎo)出來,如Axr,Ayr,Azr或者cosX,cosY,cosZ,由這些值我們能得到設(shè)備相對地平面的傾角值,這些關(guān)系我們在第一部分已經(jīng)討論過。有人可能會(huì)說-根據(jù)第一部分的公式2我們不是已經(jīng)得到Rx,Ry,Rz的值了嗎?是的,但是記住,這些值只是由加速度計(jì)數(shù)據(jù)推導(dǎo)出來的,如果你直接將它們用于你的程序你會(huì)得到難以忍受的噪聲。為了避免進(jìn)一步的混亂,我們重新定義加速度計(jì)的測量值:Racc 是由加速度計(jì)測量到得慣性力矢量,它可分解為下面的分量(在XYZ軸上的投影):RxAcc =

29、 (AdcRx * Vref / 1023 VzeroG) / SensitivityRyAcc = (AdcRy * Vref / 1023 VzeroG) / SensitivityRzAcc = (AdcRz * Vref / 1023 VzeroG) / Sensitivity現(xiàn)在我們得到了一組只來自于加速度計(jì)ADC的值。我們把這組數(shù)據(jù)叫做“vector”,并使用下面的符號(hào):Racc = RxAcc,RyAcc,RzAcc因?yàn)檫@些Racc的分量可由加速度計(jì)數(shù)據(jù)得到,我們可以把它當(dāng)做算法的輸入。請注意Racc測量的是重力,如果你得到的矢量長度約等于1g那么你就是正確的:|Racc| =

30、SQRT(RxAcc2 +RyAcc2 + RzAcc2),但是請確定把矢量轉(zhuǎn)換成下面的矢量非常重要:Racc(normalized) = RxAcc/|Racc| , RyAcc/|Racc| , RzAcc/|Racc|.這可以確保標(biāo)準(zhǔn)化Racc始終是1。接來下我們引進(jìn)一個(gè)新的向量:Rest = RxEst,RyEst,RzEst這就是算法的輸出值,它經(jīng)過陀螺儀數(shù)據(jù)的修正和基于上一次估算的值。這是算法所做的事:-加速度計(jì)告訴我們:“你現(xiàn)在的位置是Racc”我們回答:“謝謝,但讓我確認(rèn)一下”-然后根據(jù)陀螺儀的數(shù)據(jù)和上一次的Rest值修正這個(gè)值并輸出新的估算值Rest。-我們認(rèn)為Rest是當(dāng)前

31、設(shè)備姿態(tài)的“最佳值”。讓我們看看它是怎么實(shí)現(xiàn)的。數(shù)列的開始,我們先認(rèn)為加速度值正確并賦值:Rest(0) = Racc(0)Rest和Racc是向量,所以上面的式子可以用3個(gè)簡單的式子代替,注意別重復(fù)了:RxEst(0)= RxAcc(0)RyEst(0)= RyAcc(0)RzEst(0)= RzAcc(0)接下來我們在每個(gè)等時(shí)間間隔T秒做一次測量,得到新的測量值,并定義為Racc(1),Racc(2),Racc(3)等等。同時(shí),在每個(gè)時(shí)間間隔我們也計(jì)算出新的估算值Rest(1),Rest(2),Rest(3),等等。假設(shè)我們在第n步。我們有兩列已知的值可以用:Rest(n-1) 前一個(gè)估算

32、值,Rest(0) = Racc(0)Racc(n) 當(dāng)前加速度計(jì)測量值在計(jì)算Rest(n)前,我們先引進(jìn)一個(gè)新的值,它可由陀螺儀和前一個(gè)估算值得到。叫做Rgyro,同樣它是個(gè)矢量并由3個(gè)分量組成:Rgyro = RxGyro,RyGyro,RzGyro我們分別計(jì)算這個(gè)矢量的分量,從RxGyro開始。首先觀察陀螺儀模型中下面的關(guān)系,根據(jù)由Rz和Rxz組成的直角三角形我們能推出:tan(Axz) = Rx/Rz = Axz = atan2(Rx,Rz)你可能從未用過atan2這個(gè)函數(shù),它和atan類似,但atan返回值范圍是(-PI/2,PI/2),atan2返回值范圍是(-PI,PI),并且他

33、有兩個(gè)參數(shù)。它能將Rx,Rz值轉(zhuǎn)換成360(-PI,PI)內(nèi)的角度。更多信息請閱讀atan2.所以,知道了RxEst(n-1)和RzEst(n-1)我們發(fā)現(xiàn):Axz(n-1) = atan2( RxEst(n-1) , RzEst(n-1) ).記住,陀螺儀測量的是Axz角度變化率,因此,我們可以按如下方法估算新的角度Axz(n):Axz(n) = Axz(n-1) + RateAxz(n) * T請記住,RateAxz可由陀螺儀ADC讀取得到。通過使用平均轉(zhuǎn)速可由得到一個(gè)更準(zhǔn)確的公式:RateAxzAvg =(RateAxz(N)+ RateAxz(N-1)/ 2Axz(n) = Axz(n

34、-1) + RateAxzAvg * T同理可得:Ayz(n) = Ayz(n-1) + RateAyz(n) * T好了,現(xiàn)在我們有了Axz(n),Ayz(n)?,F(xiàn)在我們?nèi)绾瓮茖?dǎo)出RxGyro/RyGyro?根據(jù)公式1我們可以把Rgyro長度寫成下式:| Rgyro | = SQRT(RxGyro 2 + RyGyro 2 + RzGyro 2)同時(shí),因?yàn)槲覀円呀?jīng)將Racc標(biāo)準(zhǔn)化,我們可以認(rèn)為它的長度是1并且旋轉(zhuǎn)后保持不變,所以寫成下面的方式相對比較安全:| Rgyro | = 1我們暫時(shí)采用更短的符號(hào)進(jìn)行下面的計(jì)算:x =RxGyro , y=RyGyro, z=RzGyro根據(jù)上面的關(guān)系

35、可得:x = x / 1 = x / SQRT(x2+y2+z2)分子分母同除以SQRT(X 2 + Z 2)x = ( x / SQRT(x2 + z2) ) / SQRT( (x2 + y2 + z2) / (x2 + z2) )注意x / SQRT(x2 + z2) = sin(Axz), 所以:x = sin(Axz) / SQRT (1 + y2 / (x2 + z2) )將SQRT內(nèi)部分式的分子分母同乘以z2x = sin(Axz) / SQRT (1 + y2 * z 2 / (z2 * (x2 + z2) )注意 z / SQRT(x2 + z2) = cos(Axz), y

36、/ z = tan(Ayz), 所以最后可得:x = sin(Axz) / SQRT (1 + cos(Axz)2 * tan(Ayz)2 )替換成原來的符號(hào)可得:RxGyro = sin(Axz(n) / SQRT (1 + cos(Axz(n)2 * tan(Ayz(n)2 )同理可得:RyGyro = sin(Ayz(n) / SQRT (1 + cos(Ayz(n)2 * tan(Axz(n)2 )提示:這個(gè)公式還可以更進(jìn)一步簡化。分式兩邊同除以sin(axz(你)可得:RxGyro = 1 / SQRT (1/ sin(Axz(n)2 + cos(Axz(n)2 / sin(Axz(

37、n)2 * tan(Ayz(n)2 )RxGyro = 1 / SQRT (1/ sin(Axz(n)2 + cot(Axz(n)2 * sin(Ayz(n)2 / cos(Ayz(n)2 )現(xiàn)在加減 cos(Axz(n)2/sin(Axz(n)2 = cot(Axz(n)2RxGyro = 1 / SQRT (1/ sin(Axz(n)2 - cos(Axz(n)2/sin(Axz(n)2 + cot(Axz(n)2 * sin(Ayz(n)2 / cos(Ayz(n)2 + cot(Axz(n)2 )綜合條件1、2和3、4可得:RxGyro = 1 / SQRT (1 + cot(Axz(n)2 * sec(Ayz(n)2 ), 其中 cot(x) = 1 / tan(x) , sec(x) = 1 / cos(x)這個(gè)公式只用了2個(gè)三角函數(shù)并且計(jì)算量更低

溫馨提示

  • 1. 本站所有資源如無特殊說明,都需要本地電腦安裝OFFICE2007和PDF閱讀器。圖紙軟件為CAD,CAXA,PROE,UG,SolidWorks等.壓縮文件請下載最新的WinRAR軟件解壓。
  • 2. 本站的文檔不包含任何第三方提供的附件圖紙等,如果需要附件,請聯(lián)系上傳者。文件的所有權(quán)益歸上傳用戶所有。
  • 3. 本站RAR壓縮包中若帶圖紙,網(wǎng)頁內(nèi)容里面會(huì)有圖紙預(yù)覽,若沒有圖紙預(yù)覽就沒有圖紙。
  • 4. 未經(jīng)權(quán)益所有人同意不得將文件中的內(nèi)容挪作商業(yè)或盈利用途。
  • 5. 人人文庫網(wǎng)僅提供信息存儲(chǔ)空間,僅對用戶上傳內(nèi)容的表現(xiàn)方式做保護(hù)處理,對用戶上傳分享的文檔內(nèi)容本身不做任何修改或編輯,并不能對任何下載內(nèi)容負(fù)責(zé)。
  • 6. 下載文件中如有侵權(quán)或不適當(dāng)內(nèi)容,請與我們聯(lián)系,我們立即糾正。
  • 7. 本站不保證下載資源的準(zhǔn)確性、安全性和完整性, 同時(shí)也不承擔(dān)用戶因使用這些下載資源對自己和他人造成任何形式的傷害或損失。

評(píng)論

0/150

提交評(píng)論