加速度傳感器算法：三步教你結(jié)合加速度傳感器和陀螺儀算法設(shè)計(jì)
這篇文章主要介紹加速度計(jì)和陀螺儀的數(shù)學(xué)模型和基本算法，以及如何融合這兩者，側(cè)重算法、思想的討論.
介紹
本指南旨在向興趣者介紹慣性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)算降低到最少。如果你知道什么是正弦、余弦、正切函數(shù)，那無論你的項(xiàng)目使用哪種平臺你應(yīng)該都會(huì)明白和運(yùn)用這篇文章中的思想，這些平臺如Arduino、Propeller、Basic Stamp、Ateml芯片、PIC芯片等等?？傆行┤苏J(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 Accelerometer + 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ì)介紹之。
第一部分 加速度計(jì)
要了解這個(gè)模塊我們先從加速度計(jì)開始。當(dāng)我們在想象一個(gè)加速度計(jì)的時(shí)候我們可以把它想作一個(gè)圓球在一個(gè)方盒子中。你可能會(huì)把它想作一個(gè)餅干或者甜圈，但我就把它當(dāng)做一個(gè)球好了：
請注意加速度計(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è)例子只是為了說明加速度計(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度?，F(xiàn)在球會(huì)與兩個(gè)面接觸：Z-和X-，見下圖：
0.71g這個(gè)值是不是任意的，它們實(shí)際上是1/2的平方根的近似值。我們介紹加速度計(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 ^ 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，SPI或USART方式獲取信息，而模擬加速度計(jì)的輸出是一個(gè)在預(yù)定范圍內(nèi)的電壓值，你需要用ADC（模擬量轉(zhuǎn)數(shù)字量）模塊將其轉(zhuǎn)換為數(shù)字值。我將不會(huì)詳細(xì)介紹ADC是怎么工作的，部分原因是這是個(gè)很廣的話題，另一個(gè)原因是不同平臺的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位ADC模塊得到了以下的三個(gè)軸的數(shù)據(jù)：
AdcRx=586AdcRy=630
AdcRz=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.03V
VoltsRz=561 * 3.3 / 1023=～1.81V
每個(gè)加速度計(jì)都有一個(gè)零加速度的電壓值，你可以在它的說明書中找到，這個(gè)電壓值對應(yīng)于加速度為0g。通過計(jì)算相對0g電壓的偏移量我們可以得到一個(gè)有符號的電壓值。比方說，0g電壓值 VzeroG=1.65V，通過下面的方式可以得到相對0g電壓的偏移量:
DeltaVoltsRx=1.89V - 1.65V=0.24VDeltaVoltsRy=2.03V - 1.65V=0.38V
DeltaVoltsRz=1.81V - 1.65V=0.16V
現(xiàn)在我們得到了加速度計(jì)的電壓值，但它的單位還不是g（9.8m/s^2），最后的轉(zhuǎn)換，我們還需要引入加速度計(jì)的靈敏度（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.79g
RZ=0.16V / 0.4785V / G=～0.33g
當(dāng)然，我們可以把所有的步驟全部放在一個(gè)式子里，但我想通過介紹每一個(gè)步驟以便讓你了解怎么讀取一個(gè)ADC值并將其轉(zhuǎn)換為單位為g的矢量力的分量。
Rx=(AdcRx * Vref / 1023 – VzeroG) / Sensitivity(公式2)Ry=(AdcRy * Vref / 1023 – VzeroG) / Sensitivity
Rz=(AdcRz * Vref / 1023 – VzeroG) / Sensitivity
現(xiàn)在我們得到了慣性力矢量的三個(gè)分量，如果設(shè)備除了重力外不受任何外力影響，那我們就可以認(rèn)為這個(gè)方向就是重力矢量的方向。如果你想計(jì)算設(shè)備相對于地面的傾角，可以計(jì)算這個(gè)矢量和Z軸之間的夾角。如果你對每個(gè)軸的傾角都感興趣，你可以把這個(gè)結(jié)果分為兩個(gè)分量：X軸、Y軸傾角，這可以通過計(jì)算重力矢量和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 / R
cos(Azr)=Rz / R
從公式1我們可以推導(dǎo)出 R=SQRT( Rx^2 + Ry^2 + Rz^2)
通過arccos()函數(shù)（cos()的反函數(shù)）我們可以計(jì)算出所需的角度：
Axr=arccos(Rx/R)
Ayr=arccos(Ry/R)
Azr=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 ^ 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)一些符號。首先我們定義：
Rxz – 慣性力矢量R在XZ平面上的投影Ryz – 慣性力矢量R在YZ平面的上投影在由Rxz和Rz組成的直角三角形中，運(yùn)用勾股定理可得：Rxz^2=Rx^2 + Rz^2 ，同樣：Ryz^2=Ry^2 + Rz^2同時(shí)注意：R^2=Rxz^2 + Ry^2 ，這個(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)的值。為了解釋這一點(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代替255，如果是12為ADC用4095代替1023）。
RateAxz=(AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity公式3RateAyz=(AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity
AdcGyroXZ，AdcGyroYZ - 這兩個(gè)值由ADC讀取，它們分別代表矢量R的投影在XZ和YZ平面內(nèi)里的轉(zhuǎn)角，也可等價(jià)的說，旋轉(zhuǎn)可分解為單獨(dú)繞Y和X軸的運(yùn)動(dòng)。
Vref – ADC的參考電壓，上例中我們使用3.3V
VzeroRate – 是零變化率電壓，換句話說它是陀螺儀不受任何轉(zhuǎn)動(dòng)影響時(shí)的輸出值，對Acc 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=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ù)號表示該設(shè)備朝著反方向旋轉(zhuǎn)。按照慣例，一個(gè)方向的旋轉(zhuǎn)是正值。一份好的陀螺儀說明書會(huì)告訴你哪個(gè)方向是正的，否則你就要自己測試出哪個(gè)旋轉(zhuǎn)方向會(huì)使得輸出腳電壓增加。最好使用示波器進(jìn)行測試，因?yàn)橐坏┠阃Ｖ沽诵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è)。
在使用整合了加速度計(jì)和陀螺儀的IMU設(shè)備時(shí)，首先要做的就是統(tǒng)一它們的坐標(biāo)系。最簡單的辦法就是將加速度計(jì)作為參考坐標(biāo)系。大多數(shù)的加速度計(jì)技術(shù)說明書都會(huì)指出對應(yīng)于物理芯片或設(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軸保持不變。
-當(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，你可以在公式3中引入正負(fù)號來解決這個(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)行這些測試，你會(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è)力可以由重力引起（理想情況只受重力影響），當(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ā)，但是它更簡單并且更容易在嵌入式設(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=(AdcRx * Vref / 1023 – VzeroG) / SensitivityRyAcc=(AdcRy * Vref / 1023 – VzeroG) / Sensitivity
RzAcc=(AdcRz * Vref / 1023 – VzeroG) / Sensitivity
現(xiàn)在我們得到了一組只來自于加速度計(jì)ADC的值。我們把這組數(shù)據(jù)叫做“vector”，并使用下面的符號：
Racc=[RxAcc,RyAcc,RzAcc]
因?yàn)檫@些Racc的分量可由加速度計(jì)數(shù)據(jù)得到，我們可以把它當(dāng)做算法的輸入。
請注意Racc測量的是重力，如果你得到的矢量長度約等于1g那么你就是正確的：
|Racc|=SQRT(RxAcc^2 +RyAcc^2 + RzAcc^2),
但是請確定把矢量轉(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)前設(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è)估算值，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），并且他有兩個(gè)參數(shù)。它能將Rx，Rz值轉(zhuǎn)換成360°（-PI，PI）內(nèi)的角度。
所以，知道了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））/ 2
Axz(n)=Axz(n-1) + RateAxzAvg * T
同理可得：
Ayz(n)=Ayz(n-1) + RateAyz(n) * T
好了，現(xiàn)在我們有了Axz（n），Ayz（n）。現(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í)采用更短的符號進(jìn)行下面的計(jì)算：
x=RxGyro , y=RyGyro, z=RzGyro
根據(jù)上面的關(guān)系可得：
x=x / 1=x / SQRT(x^2+y^2+z^2)分子分母同除以SQRT（X ^ 2 + Z ^ 2）x=( x / SQRT(x^2 + z^2) ) / SQRT( (x^2 + y^2 + z^2) / (x^2 + z^2) )注意x / SQRT(x^2 + z^2)=sin(Axz), 所以:x=sin(Axz) / SQRT (1 + y^2 / (x^2 + z^2) )將SQRT內(nèi)部分式的分子分母同乘以z^2x=sin(Axz) / SQRT (1 + y^2* z ^2 / (z^2 * (x^2 + z^2)) )注意 z / SQRT(x^2 + z^2)=cos(Axz)， y / z=tan(Ayz), 所以最后可得:
x=sin(Axz) / SQRT (1 + cos(Axz)^2 * tan(Ayz)^2 )
替換成原來的符號可得：
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(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ì)算量更低。如果你有Mathematica程序，通過使用 FullSimplify [Sin[A]^2/ ( 1 + Cos[A]^2 * Tan[B]^2)]你可以驗(yàn)證這個(gè)公式。
現(xiàn)在我們發(fā)現(xiàn)：
RzGyro=Sign(RzGyro)*SQRT(1 – RxGyro^2 – RyGyro^2).
其中，當(dāng) RzGyro>=0時(shí)，Sign(RzGyro)=1 , 當(dāng) RzGyro<0時(shí)，Sign(RzGyro)=-1 。
一個(gè)簡單的估算方法：Sign(RzGyro)=Sign(RzEst(n-1))在實(shí)際應(yīng)用中，當(dāng)心RzEst（n-1）趨近于0。這時(shí)候你可以跳過整個(gè)陀螺儀階段并賦值：Rgyro=Rest（n-1）。Rz可以用作計(jì)算Axz和Ayz傾角的參考，當(dāng)它趨近于0時(shí)，它可能會(huì)溢出并引發(fā)不好的后果。這時(shí)你會(huì)得到很大的浮點(diǎn)數(shù)據(jù)，并且tan()/atan()函數(shù)得到的結(jié)果會(huì)缺乏精度。
現(xiàn)在我們回顧一下已經(jīng)得到的結(jié)果，我們在算法中的第n步，并計(jì)算出了下面的值：
Racc – 加速度計(jì)讀取的當(dāng)前值Rgyro –根據(jù)Rest(-1)和當(dāng)前陀螺儀讀取值所得我們根據(jù)哪個(gè)值來更新Rest（n）呢？你可能已經(jīng)猜到，兩者都采用。我們會(huì)用一個(gè)加權(quán)平均值，得：Rest(n)=(Racc * w1 + Rgyro * w2 ) / (w1 + w2)分子分母同除以w1，公式可簡化成：Rest(n)=(Racc * w1/w1 + Rgyro * w2/w1 ) / (w1/w1 + w2/w1)令w2=w1=wGyro，可得：Rest(n)=(Racc + Rgyro * wGyro ) / (1 + wGyro)在上面的公式中，wGyro表示我們對加速度計(jì)和陀螺儀的相信程度。這個(gè)值可以通過測試確定，根據(jù)經(jīng)驗(yàn)值5-20之間會(huì)得到一個(gè)很好的結(jié)果。此算法和卡爾曼濾波最主要的差別是它的權(quán)重是相對固定的，而卡爾曼濾波中的權(quán)重會(huì)隨著加速度計(jì)讀取的噪聲而改變。卡爾曼濾波注重給你一個(gè)“最好”的理論結(jié)果，而此算法給你的是實(shí)際項(xiàng)目中“夠用”的結(jié)果。你可以實(shí)現(xiàn)一個(gè)算法，它能根據(jù)測量的噪聲而改變wGyro值，但對大部分應(yīng)用來說固定的權(quán)重也能工作的很好。現(xiàn)在得到最新的估算值還差一步：RxEst(n)=(RxAcc + RxGyro * wGyro ) / (1 + wGyro)RyEst(n)=(RyAcc + RyGyro * wGyro ) / (1 + wGyro)RzEst(n)=(RzAcc + RzGyro * wGyro ) / (1 + wGyro)現(xiàn)在，再次標(biāo)準(zhǔn)化矢量：R=SQRT(RxEst(n) ^2 + RyEst(n)^2 +RzEst(n)^2 )RxEst(n)=RxEst(n)/RRyEst(n)=RyEst(n)/RRzEst(n)=RzEst(n)/R現(xiàn)在，可以再次進(jìn)行下一輪循環(huán)了。
加速度傳感器算法：利用三軸加速度傳感器的計(jì)步測算法
現(xiàn)如今，很多現(xiàn)代人都非常注重自己的日常鍛煉，計(jì)步作為一種有效記錄監(jiān)控鍛煉的監(jiān)控手段，被廣泛應(yīng)用在移動(dòng)終端的應(yīng)用中。
目前，大部分的計(jì)步都是通過GPS信號來測算運(yùn)動(dòng)距離，再反推行走步數(shù)實(shí)現(xiàn)的。這種方法很是有效，但在室內(nèi)或沒有GPS信號的設(shè)備上無法工作。同時(shí)，GPS精度對結(jié)果的干擾也比較大。
為避免上述問題的出現(xiàn)，我們可以考慮一種新的測步方法，即：通過設(shè)備上的加速度傳感器來計(jì)算步數(shù)，在不支持GPS的設(shè)備上也可正常工作。還可以與GPS互相配合測步，這樣可令使用場景變得多樣。
1.先要摸清模型的特征
目前，大部分設(shè)備都提供了可以檢測各個(gè)方向的加速度傳感器。以iOS設(shè)備為例，我們利用了其三軸加速度傳感器（x，y，z軸代表方向如圖）的特性來分析。分別用以檢測人步行中三個(gè)方向的加速度變化。
iOS設(shè)備的三軸加速度傳感器示意圖
用戶在水平步行運(yùn)動(dòng)中，垂直和前進(jìn)兩個(gè)加速度會(huì)呈現(xiàn)周期性變化，如圖所示。在步行收腳的動(dòng)作中，由于重心向上單只腳觸地，垂直方向加速度是呈正向增加的趨勢，之后繼續(xù)向前，重心下移兩腳觸底，加速度相反。水平加速度在收腳時(shí)減小，在邁步時(shí)增加。
反映到圖表中，可以看到，在步行運(yùn)動(dòng)中，垂直和前進(jìn)產(chǎn)生的加速度與時(shí)間大致為一個(gè)正弦曲線，而且在某點(diǎn)有一個(gè)峰值。其中，垂直方向的加速度變化最大，通過對軌跡的峰值進(jìn)行監(jiān)測計(jì)算和加速度閥值決策，即可實(shí)時(shí)計(jì)算用戶運(yùn)動(dòng)的步數(shù)，還可依此進(jìn)一步估算用戶步行距離。
2.計(jì)步的合理算法
因?yàn)橛脩粼谶\(yùn)動(dòng)中可能用手平持設(shè)備，或者將設(shè)備置于口袋中。所以，設(shè)備的放置方向不定。為此，通過計(jì)算三個(gè)加速度的矢量長度，我們可以獲得一條步行運(yùn)動(dòng)的正弦曲線軌跡。
第二步是峰值檢測，我們記錄了上次矢量長度和運(yùn)動(dòng)方向，通過矢量長度的變化，可以判斷目前加速度的方向，并和上一次保存的加速度方向進(jìn)行比較。如果是相反的，即是剛過峰值狀態(tài)，則進(jìn)入計(jì)步邏輯進(jìn)行計(jì)步，否則舍棄。通過對峰值的次數(shù)累加，可得到用戶步行的步伐。
最后，就是去干擾。手持設(shè)備會(huì)有一些低幅度和快速的抽動(dòng)狀態(tài)，或是我們俗稱的手抖，或者某個(gè)惡作劇用戶想通過短時(shí)快速反復(fù)搖動(dòng)設(shè)備來模擬人走路，這些干擾數(shù)據(jù)如果不剔除，會(huì)影響記步的準(zhǔn)確值，對于這種干擾，我們可以通過給檢測加上閥值和步頻判斷來過濾。
人體最快的跑步頻率為5HZ，也就是說相鄰兩步的時(shí)間間隔的至少大于0.2秒，如圖所示，我們設(shè)置了timespan在記步過程中我們過濾了高頻噪聲，即步頻過快的情況。同時(shí)我們通過和上次加速度大小進(jìn)行比較，設(shè)置設(shè)立一定的閥值Threshold來判斷運(yùn)動(dòng)是否屬于有效，有效運(yùn)動(dòng)才可進(jìn)行記步。
3.關(guān)于計(jì)步器的擴(kuò)展
以上是一個(gè)依靠加速度測算的計(jì)步器實(shí)現(xiàn)原理，已知步行和跑步的步伐經(jīng)驗(yàn)值，那么稍微改進(jìn)下即可變成一個(gè)測距測速計(jì)。
通過三軸加速度傳感器，我們可以知道用戶的運(yùn)動(dòng)狀態(tài)。除了計(jì)步，還可以通過加速器的變化曲線判斷用戶摔倒?fàn)顟B(tài)，做成一個(gè)老人和兒童摔倒檢測自動(dòng)報(bào)警器。
加速度傳感器算法：基于三軸加速度傳感器計(jì)步算法
本文介紹可穿戴設(shè)備加速度傳感器－Lis3dh的特性原理和應(yīng)用場景。意法半導(dǎo)體研發(fā)的Lis3dh廣泛應(yīng)用在智能手環(huán)、智能計(jì)步鞋等智能穿戴產(chǎn)品中。
Lis3dh有兩種工作方式，一種是其內(nèi)置了多種算法來處理常見的應(yīng)用場景（如靜止檢測、運(yùn)動(dòng)檢測、屏幕翻轉(zhuǎn)、失重、位置識別、單擊和雙擊等等），用戶只需簡單配置算法對應(yīng)的寄存器即可開始檢測，一旦檢測到目標(biāo)事件，Lis3dh的外圍引腳INT1會(huì)產(chǎn)生中斷。另一種是支持用戶通過SPI/I2C來讀取底層加速度數(shù)據(jù)，并自行通過軟件算法來做進(jìn)一步復(fù)雜的處理，如計(jì)步等等。
本文以Lis3dh為講解案例，但工作原理和應(yīng)用場景對其他加速度傳感器同樣適用。更多嵌入式和物聯(lián)網(wǎng)原創(chuàng)技術(shù)分享敬請關(guān)注微信公眾號：嵌入式企鵝圈。
一、加速度傳感器工作原理
加速度傳感器自然是對自身器件的加速度進(jìn)行檢測。其自身的物理實(shí)現(xiàn)方式咱們就不去展開了，可以想象芯片內(nèi)部有一個(gè)真空區(qū)域，感應(yīng)器件即處于該區(qū)域，其通過慣性力作用引起電壓變化，并通過內(nèi)部的ADC給出量化數(shù)值。
Lis3dh是三軸加速度傳感器，因此其能檢測X、Y、Z的加速度數(shù)據(jù)，如下圖：
在靜止的狀態(tài)下，傳感器一定會(huì)在一個(gè)方向重力的作用，因此有一個(gè)軸的數(shù)據(jù)是1g（即9.8米/秒的二次）。在實(shí)際的應(yīng)用中，我們并不使用跟9.8相關(guān)的計(jì)算方法，而是以1g作為標(biāo)準(zhǔn)加速度單位，或者使用1/1000g，即mg。既然是ADC轉(zhuǎn)換，那么肯定會(huì)有量程和精度的概念。在量程方面，Lis3dh支持（+-）2g/4g/8g/16g四種。一般作為計(jì)步應(yīng)用來說，2g是足夠的，除去重力加速度1g，還能檢測出1g的加速度。至于精度，那就跟其使用的寄存器位數(shù)有關(guān)了。Lis3dh使用高低兩個(gè)8位（共16位）寄存器來存取一個(gè)軸的當(dāng)前讀數(shù)。由于有正反兩個(gè)方向的加速度，所以16位數(shù)是有符號整型，實(shí)際數(shù)值是15位。以（+-）2g量程來算，精度為2g/2^15= 2000mg/ =0.061mg。
當(dāng)以上圖所示的靜止?fàn)顟B(tài)，z軸正方向會(huì)檢測出1g，X、Y軸為0.如果調(diào)轉(zhuǎn)位置（如手機(jī)屏幕翻轉(zhuǎn)），那總會(huì)有一個(gè)軸會(huì)檢測出1g，其他軸為0，在實(shí)際的測值中，可能并不是0，而是有細(xì)微數(shù)值。
在運(yùn)動(dòng)過程中，x，y，z軸都會(huì)發(fā)生變化。計(jì)步運(yùn)動(dòng)也有其固有的數(shù)值規(guī)律，因?yàn)檫~步過程也有抬腳和放腳的規(guī)律過程，如下圖?！澳_蹬離地是一步的開始，此時(shí)由于地面的反作用力，垂直方向加速度開始增大，當(dāng)腳達(dá)到最高位置時(shí)，垂直方向加速度達(dá)到最大；然后腳向下運(yùn)動(dòng)，垂直加速度開始減小，直到腳著地，垂直加速度減到最小值。接著下一步邁步。前向加速度由腳與地面的摩擦力產(chǎn)生，雙腳觸地時(shí)增大，一腳離地時(shí)減小。”[此處引用韓文正等人發(fā)表的《基于加速度傳感器LIS3DH的計(jì)步器設(shè)計(jì)》]。
二、理解加速度傳感器的一個(gè)坐標(biāo)系誤區(qū)
意法半導(dǎo)體針對LIS3DH發(fā)布兩個(gè)文檔，官方規(guī)格書和應(yīng)用設(shè)計(jì)指導(dǎo)。單獨(dú)提出這點(diǎn)是為因?yàn)楸救酥霸谑褂肔IS3DH時(shí)可能是太久沒有運(yùn)用過立體幾何思維，導(dǎo)致在X,Y,Z坐標(biāo)系上混淆概念，對位置識別遲遲沒能理解，現(xiàn)在指出這個(gè)誤區(qū)。
下圖的X,Y,Z除了代表我們所認(rèn)識的三維坐標(biāo)系外，還有一個(gè)重要的認(rèn)知，那就是X,Y,Z軸對應(yīng)的寄存器分別按照芯片圖示（以芯片的圓點(diǎn)來確定）的方向來測加速度值，而不管芯片的位置如何，即X,Y,Z軸對應(yīng)的三個(gè)寄存器總是以這樣工作的：Z軸寄存器測芯片垂直方向的數(shù)據(jù)、Y軸測芯片左右方的數(shù)據(jù)、X軸測芯片前后的數(shù)據(jù)（前后左右的定義可能不夠形象，大家能理解就好）。例如，圖示靜止?fàn)顟B(tài)下，X軸寄存器測芯片前后方向的加速度；如果芯片如右邊圖示靜止時(shí)，X軸寄存器測的是坐標(biāo)系的Z軸方向加速度。
三、LIS3DH內(nèi)置硬件算法工作原理
由于計(jì)步等場景是需要先讀取底層X,Y,Z軸數(shù)據(jù)再進(jìn)行處理的，所以我們這里不去探討這個(gè)算法。這里主要闡述如何利用LIS3DH內(nèi)置的硬件算法來檢測常用的場景。
LIS3DH的內(nèi)置硬件算法主要由2個(gè)參數(shù)和1個(gè)模式選擇來確定。2個(gè)參數(shù)分別是閾值和持續(xù)時(shí)間。例如，在靜止的時(shí)候我們要去檢測芯片的運(yùn)動(dòng)（wakeup）時(shí)，我們可以設(shè)定一個(gè)運(yùn)動(dòng)對應(yīng)的閾值，并且要求芯片檢測數(shù)據(jù)在超過這個(gè)閾值時(shí)要持續(xù)一定的時(shí)間才可以認(rèn)為芯片是運(yùn)動(dòng)的。內(nèi)置算法基本都是基于閾值和持續(xù)時(shí)間來進(jìn)行檢測的。
LIS3DH一共有兩套能夠同時(shí)工作的硬件算法電路，一種是專門針對單擊、雙擊這種場景，如鼠標(biāo)應(yīng)用，另一種是針對其他所有場景的，如靜止運(yùn)動(dòng)檢測、運(yùn)動(dòng)方向識別、位置識別等等。這里我們主要講述后者，其有四種工作模式：
第一種：OR或電路，即X,Y,Z任一軸數(shù)據(jù)超過閾值即可完成檢測。
第二種：AND與電路，即X,Y,Z所有軸的數(shù)據(jù)均超過閾值才能完成檢測。當(dāng)然，其也允許只檢測任意兩個(gè)軸或者一個(gè)軸，不檢測的軸的閾值檢測可以認(rèn)為是永遠(yuǎn)為真。
以上兩種電路的閾值比較圖示如下，閾值比較是絕對值比較，沒有方向之分。不管在正方向還是負(fù)方向，只要絕對值超過閾值，那么XH(YH、ZH)為1，此時(shí)相應(yīng)的XL(YL、ZL)為0；否則XL(YL、ZL)為1，相應(yīng)的XH(YH、ZH)為0。XH(YH、ZH)、XL(YL、ZL)可以認(rèn)為是檢測條件是否滿足的pending指示位。
第三種和第四種是一個(gè)物體六個(gè)方向的檢測，movement檢測芯片的運(yùn)動(dòng)方向變化，即從一種方向變化到另一種方向；而position檢測芯片穩(wěn)定為一種確定的方向（如穩(wěn)定為平放朝上、平放朝下、豎立前后左右）等等。
其閾值比較電路如下，該閾值比較使用正負(fù)數(shù)真實(shí)數(shù)據(jù)比較。正方向超過閾值，則XH(YH、ZH)為1，否則為0；負(fù)方向超過閾值，XL(YL、ZL)為1，否則為0。XH(YH、ZH)、XL(YL、ZL)代表了六個(gè)方向。由于靜止穩(wěn)定狀態(tài)時(shí)，只有一個(gè)方向有重力加速度，因此可以據(jù)此知道當(dāng)時(shí)芯片的位置姿勢。
四、加速度傳感器應(yīng)用
如果能夠理解第三部分的工作原理，那么也能夠很好理解以下的應(yīng)用。
1.?靜止時(shí)進(jìn)行運(yùn)動(dòng)檢測
使用OR電路工作方式，設(shè)置一個(gè)較小的運(yùn)動(dòng)閾值，只檢測X,Y軸數(shù)據(jù)是否超過該閾值（Z軸這時(shí)有1g，咱不管這個(gè)軸了）即可。只要X,Y任一軸數(shù)據(jù)超過閾值一定時(shí)間即認(rèn)為設(shè)備處于wakeup狀態(tài)了。
2.?失重檢測
失重時(shí)Z軸的加速度和重力加速度抵消，在短時(shí)間內(nèi)會(huì)為0，而且X,Y軸沒有變化，因此在短時(shí)間內(nèi)三者都為0。這里使用AND電路工作方式，設(shè)置一個(gè)較小的運(yùn)動(dòng)閾值，當(dāng)三個(gè)方向的數(shù)據(jù)都小于閾值一定時(shí)間時(shí)即認(rèn)為是失重。
3.?位置姿勢識別
例如手機(jī)翻轉(zhuǎn)等應(yīng)用場景就是利用這個(gè)特性。這里在第三部分講解工作原理時(shí)已經(jīng)講得很清楚了。?
有了以上理解，以后在使用LIS3DH時(shí)直接找寄存器填數(shù)值就可以完成功能啦。
如轉(zhuǎn)載請務(wù)必全文轉(zhuǎn)載，保留嵌入式企鵝圈的微信公眾號，否則即視為侵權(quán)。
? ? 群猴報(bào)喜，祝愿大家在猴年事事如意！嵌入式企鵝圈堅(jiān)持百分百地原創(chuàng)高質(zhì)量技術(shù)研發(fā)文章。更多原創(chuàng)技術(shù)分享敬請關(guān)注微信公眾號：嵌入式企鵝圈
加速度傳感器算法：求助如何對加速度傳感器采集的信號進(jìn)行濾波？
小白，現(xiàn)在有一套加速度傳感器設(shè)備用于測量機(jī)械加工振動(dòng)，采集卡保存了加工過程中的加速度(g)、速度(mm/s)和位移（mm），供應(yīng)商那邊說這套設(shè)備沒有濾波功能，讓我自己濾波。搜論文、相關(guān)書籍看了好幾天，濾波方法很多，但是沒看懂要處理加速度還是速度，還是位移？也不知道我如何處理這些數(shù)據(jù)，請大佬指點(diǎn)。
就我現(xiàn)有的這些數(shù)據(jù)，我應(yīng)該如何做才能讓這些數(shù)據(jù)可以使用？
先不管你的物理量是什么，這里只是對你的數(shù)據(jù)進(jìn)行濾波，Python里面有很多濾波方法，下面舉幾個(gè)例子。你先導(dǎo)出自己的數(shù)據(jù)，可參考如下方式進(jìn)行濾波操作。
import numpy as np
import matplotlib.pyplot as plt
from obspy import read
from scipy.signal import savgol_filter as sgolay
from scipy import signal
tr=read()[0]
# 原始信號 s1, 及對應(yīng)的時(shí)間t
s1=tr.data; t=np.arange(len(s1)) * tr.stats.delta
# 相同權(quán)重卷積平均
s2=np.convolve(s1, np.ones(50)/50, mode='same')
# Savitsky-Golay 平滑濾波
s3=sgolay(s1, 91, 3)
# 低通濾波
fs=1 / (t[1]-t[0]) # 采樣率 (赫茲)
fc=2 # 截止頻率 （赫茲）
b, a=signal.butter(4, 2.0*fc/fs, 'lowpass')
s4=signal.filtfilt(b, a, s1)
# 高通濾波
fs=1 / (t[1]-t[0]) # 采樣率 (赫茲)
fc=1 # 截止頻率 （赫茲）
b, a=signal.butter(4, 2.0*fc/fs, 'highpass')
s5=signal.filtfilt(b, a, s1)
# 帶通濾波
fs=1 / (t[1]-t[0]) # 采樣率 (赫茲)
f1=.1; f2=2 # 通帶的兩個(gè)截止頻率 （赫茲）
b, a=signal.butter(4,[2.0*f1/fs, 2.0*f2/fs], 'bandpass')
s6=signal.filtfilt(b, a, s1)
# # 帶阻濾波
fs=1 / (t[1]-t[0]) # 采樣率 (赫茲)
f1=.5; f2=5 # 阻帶的兩個(gè)截止頻率
b, a=signal.butter(4,[2.0*f1/fs, 2.0*f2/fs], 'bandstop')
s7=signal.filtfilt(b, a, s1)
# 畫圖對比
l=6
plt.figure(figsize=(15, 20))
plt.subplot(l, 1, 1)
plt.plot(t, s1, label='Original signal')
plt.plot(t, s2, label='Convolved&filtered')
plt.legend(fontsize=15)
plt.subplot(l, 1, 2)
plt.plot(t, s1, label='Original signal')
plt.plot(t, s3, label='Savitzky-Golay&filtered')
plt.legend(fontsize=15)
plt.subplot(l, 1, 3)
plt.plot(t, s1, label='Original signal')
plt.plot(t, s4, label='Lowpass filtered')
plt.legend(fontsize=15)
plt.subplot(l, 1, 4)
plt.plot(t, s1, label='Original signal')
plt.plot(t, s5, label='Highpass filtered')
plt.legend(fontsize=15)
plt.subplot(l, 1, 5)
plt.plot(t, s1, label='Original signal')
plt.plot(t, s6, label='Bandpass filtered')
plt.legend(fontsize=15)
plt.subplot(l, 1, 6)
plt.plot(t, s1, label='Original signal')
plt.plot(t, s7, label='Bandstop filtered')
plt.legend(fontsize=15)
plt.show()
下面展示這些濾波方法的結(jié)果：
