在判斷物體在空間中的姿態以及運動軌跡時, 用得最多的是加速度和角速度感測器. 加速度感測器可以計算傾角, 陀螺儀可以計算角速度, 本文介紹常用的傾角計算方法, 結合陀螺儀提高動態精度的互補濾波. ...
慣性感測器單元 IMU
IMU 是 Inertial Measurement Unit 的縮寫, 直接翻譯過來就是慣性測量單元, 常見的有單獨的三軸加速度(Accelerometer)計 ADXL345, L3G4200D, L3GD20等, 單獨的三軸角速度計(又稱陀螺儀, Gyroscope) LIS3DH, L3GD20H, BMG160, 以及包含了加速度計和陀螺儀的六軸運動感測器 MPU6050, MPU6500, MPU6881, BMI160等, 以及帶電子羅盤的九軸運動感測器 MPU9250, MPU9255等.
在判斷物體在空間中的姿態以及運動軌跡時, 用得最多的是加速度和角速度感測器. 加速度感測器可以計算傾角, 陀螺儀可以計算角速度, 這兩種感測器各自的特點為
- 陀螺儀: 動態特性好, 因為測量雜訊(誤差)的存在, 以及各向靈敏度的差異, 通過積分計算角度會累積誤差, 導致結果越來越不准
- 加速度計: 不會累積誤差, 所以準確度有保證, 但是動態響應差, 不適用於角度變化快速的場景.
如果設備運動速度較慢, 作用於系統的加速度力主要是重力, 可以使用加速度計來計算傾角, 利用重力矢量及其在加速度計軸上的投影(即對應讀數)來確定傾角. 由於重力是恆定加速度, 如果存在額外的恆定加速度也會影響計算結果. 額外的恆定加速度包括發動機持續加速以及設備自身勻速的旋轉等.
通過加速度計算傾角
靜止和慢速物體因為主要加速度來源為重力, 用加速度感測器可以計算得到物體的傾角.
單軸數據計算傾角
假設X軸上測到的加速度值為 \(a_x\) 定義傾角 \(α\) 為X軸與水平面(與重力矢量垂直的平面)的夾角, 計算為
\[α = sin^{-1}(\frac{a_x}{g}) \]當 \(a_x = 0\) 時, 傾角為0, X軸處於水平位置, 當\(a_x = g\) 時, 傾角90°, 處於垂直位置, 當 \(a_x\) 的值很小時, 可以用近似公式 \(sin(α) ≈ α\), 於是
\[α ≈ k(\frac{a_x}{g}) \]比例繫數k用於角度的線性近似計算
\(a_x\) 的讀數匹配 \(sin(α)\) 曲線, 讀數值範圍為 \(-1g ~ 1g\), 在讀數為0(水平位置)時靈敏度最高, 在讀數為 \(+-1g\) 時靈敏度最低.
雙軸加速度數據計算傾角
單軸無法判斷方向, 因為在傾角為 \(α\) 和 \(180° - α\) 時讀數是一樣的. 如果增加一個與X軸垂直的軸, 假定為Z軸, 且XZ軸形成的平面垂直於水平面, 那麼XZ軸在這個平面里形成的對水平面的傾角就可以判斷方向, 並且結果較為精確, 因為總會有一個軸處於靈敏度較高的區間.
當XZ軸形成的平面垂直於水平面時, X軸傾角可以用兩個軸的讀數進行計算.
\[α = tan^{-1}(\frac{a_x}{a_z}) \]\(a_x\)為0時, 傾角為0, X軸處於水平, 當\(a_z\)為0時要註意避免零除. 如果XZ軸平面不垂直於水平面, 這個結果會小於實際的傾角, 傾斜越厲害誤差越大.
三軸加速度數據計算傾角
假設感測器Z軸垂直朝下, X軸朝正前方, 則X軸與水平面之間的夾角為俯仰角(pitch) \(α\), Y軸與水平面間的夾角為橫滾角(roll) \(β\), 航向角yaw需要地磁感測器, 無法通過加速度感測器計算. Z軸垂直於X軸和Y軸, 和兩軸數值是相關的, 並沒有獨立性, 僅用於判斷設備上下的朝向. 令Z軸與水平面的夾角為 \(γ\)
重力加速度在XYZ三個軸上的投影即為三個軸感測器的讀數, 可以將三軸傾角和重力加速度想像為一個斜立的長方體, 長方體的對角線為重力加速度, 對角線就是這個角對應的三條邊, 傾角的計算方法為
\[α = sin^{-1}(\frac{a_x}{g}) \]\[β = sin^{-1}(\frac{a_y}{g}) \]\[γ = sin^{-1}(\frac{a_z}{g}) \]帶運動加速度的傾角計算
上面的計算方式適合相對靜止和慢速的場景, 當物體受作用於多個外力時, 作用於感測器的綜合加速度為重力與各外力的疊加, 此時加速度的方向就不是重力的方向, 上面的計算方式就不適用了, 因為綜合加速度可能比 \(g\) 更大或更小.
對於一個物體, 整體加速度等於三軸加速度的矢量和, 其大小 \(G\) 可以通過三個向量的大小計算得到
\[G = \sqrt[2]{{a_x}^2 + {a_y}^2 + {a_z}^2} \]由此可以得到運動狀態下傾角的計算
\[α = sin^{-1}(\frac{a_x}{G}) \]\[β = sin^{-1}(\frac{a_y}{G}) \]\[γ = sin^{-1}(\frac{a_z}{G}) \]因為 \(a_x\), \(G\), \(\sqrt[2]{{a_y}^2 + {a_z}^2}\) 三個矢量形成直角三角形, 上面的式子可以也可以用 \(tan^{-1}\) 計算
\[α = tan^{-1}(\frac{a_x}{\sqrt[2]{{a_y}^2 + {a_z}^2}}) \]\[β = tan^{-1}(\frac{a_y}{\sqrt[2]{{a_x}^2 + {a_z}^2}}) \]\[γ = tan^{-1}(\frac{a_z}{\sqrt[2]{{a_x}^2 + {a_y}^2}}) \]此時的傾角並非相對重力加速度的傾角, 而是相對物體整體加速度矢量的傾角, 例如物體向前(X軸方向)加速運動時, 整體加速度方向會向後傾斜, 當物體左轉時, 離心力會導致整體加速度方向向右傾斜. 計算此時的姿態傾角, 可以用於幫助物體在當前的運動狀態上保持平衡.
互補濾波
通過加速度感測器(Accelerometer)可以使用反三角函數\(sin^{-1}\)和\(tan^{-1}\)求靜止和慢速運動物體的傾角, 對於高速運動的物體, 需要結合陀螺儀的角速度讀數快速響應傾角變化. 對於這兩種感測器讀數的結合, 通常採用互補濾波演算法.
互補濾波就是在短時間內採用陀螺儀得到的角度做為最優值, 定時用加速度值來校正陀螺儀的得到的角度. 加速度計要濾掉高頻信號, 陀螺儀要濾掉低頻信號, 互補濾波器就是根據感測器特性不同, 通過不同的濾波器, 相加得到整個頻帶的信號.
互補濾波的公式為
\[α_n = k * (α_{n-1} + \delta_α ∗ dt) + (1 - k) ∗ α^{\prime} \]其中
- \(α_n\) 互補計算得到的角度
- \(α_{n-1}\) 前一次計算得到的角度
- \(\delta_α\) 陀螺儀得到的角速度
- \(dt\) 兩次計算的時間間隔
- \(α^{\prime}\) 通過加速度計得到的傾角
- \(k\) 和 \(1-k\) 為加權繫數, 和為 1
加權繫數的確定. 在 《The Balance Filter》 中提到關於加權繫數的求解公式, 先設濾波器的加權繫數為 \(α\), 時間常數為為 \(τ\), 運行周期為 \(dt\), 那麼公式為
\[α = \frac{τ}{τ+dt} \]運行周期 dt 根據運行周期確定, 如果互補濾波器方法的調用頻率為 200次每秒, 那麼 \(dt = \frac{1000ms}{200} = 5ms\)
時間常數 \(τ\) 的取值根據系統的實際需求調整,
不同的系統的 \(τ\) 值不一定相同. \(τ\)取值越大則陀螺儀權重越大, \(τ\)取值越小則加速度感測器的權重越大. 通常互補濾波器對陀螺儀的權重會大些, 以降低加速度感測器中雜訊的影響. 例如互補濾波器運行間隔為 10ms, 時間常數 \(τ =0.49\), 那麼此時加權繫數為：
C語言代碼
// a = tau / (tau + dt)
// acc = 加速度感測器數據
// gyro = 陀螺儀數據
// dt = 運行周期
float angle;
float a;
float ComplementaryFliter(float acc, float gyro, float dt)
{
a = 0.98;
angle = a * (angle + gyro * dt) + (1 - a) * (acc);
return angle;
}
在實際應用中, 因為加速度計和角速度計讀取的數據存在很大的噪音, 直接使用會造成反饋的不穩定(抖動), 需要在計算前通過卡爾曼濾波器等進行平滑.
