慣性傳感器單元 IMU

IMU 是 Inertial Measurement Unit 的縮寫, 直接翻譯過來就是慣性測量單元, 常見的有單獨的三軸加速度(Accelerometer)計 ADXL345, L3G4200D, L3GD20等, 單獨的三軸角速度計(又稱陀螺儀, Gyroscope) LIS3DH, L3GD20H, BMG160, 以及包含了加速度計和陀螺儀的六軸運動傳感器 MPU6050, MPU6500, MPU6881, BMI160等, 以及帶電子羅盤的九軸運動傳感器 MPU9250, MPU9255等.

在判斷物體在空間中的姿態以及運動軌跡時, 用得最多的是加速度和角速度傳感器. 加速度傳感器可以計算傾角, 陀螺儀可以計算角速度, 這兩種傳感器各自的特點為

• 陀螺儀: 動態特性好, 因為測量噪聲(誤差)的存在, 以及各向靈敏度的差異, 通過積分計算角度會累積誤差, 導致結果越來越不準
• 加速度計: 不會累積誤差, 所以準確度有保證, 但是動態響應差, 不適用于角度變化快速的場景.

如果設備運動速度較慢, 作用于系統的加速度力主要是重力, 可以使用加速度計來計算傾角, 利用重力矢量及其在加速度計軸上的投影(即對應讀數)來確定傾角. 由于重力是恒定加速度, 如果存在額外的恒定加速度也會影響計算結果. 額外的恒定加速度包括發動機持續加速以及設備自身勻速的旋轉等.

通過加速度計算傾角

靜止和慢速物體因為主要加速度來源為重力, 用加速度傳感器可以計算得到物體的傾角.

單軸數據計算傾角

假設X軸上測到的加速度值為 $a_x$ 定義傾角 $\alpha$ 為X軸與水平面(與重力矢量垂直的平面)的夾角, 計算為

$$\alpha =si{n}^{?1}(\frac{a_x}{g})$$

當 $a_x=0$ 時, 傾角為0, X軸處于水平位置, 當$a_x=g$ 時, 傾角90°, 處于垂直位置, 當 $a_x$ 的值很小時, 可以用近似公式 $sin(\alpha )\approx \alpha$, 于是

$$\alpha \approx k(\frac{a_x}{g})$$

比例系數k用于角度的線性近似計算

$a_x$ 的讀數匹配 $sin(\alpha )$ 曲線, 讀數值范圍為 $?1g1g$, 在讀數為0(水平位置)時靈敏度最高, 在讀數為 $+?1g$ 時靈敏度最低.

雙軸加速度數據計算傾角

單軸無法判斷方向, 因為在傾角為 $\alpha$ 和 $180°?\alpha$ 時讀數是一樣的. 如果增加一個與X軸垂直的軸, 假定為Z軸, 且XZ軸形成的平面垂直于水平面, 那么XZ軸在這個平面里形成的對水平面的傾角就可以判斷方向, 并且結果較為精確, 因為總會有一個軸處于靈敏度較高的區間.

當XZ軸形成的平面垂直于水平面時, X軸傾角可以用兩個軸的讀數進行計算.

$$\alpha =ta{n}^{?1}(\frac{a_x}{a_z})$$

$a_x$為0時, 傾角為0, X軸處于水平, 當$a_z$為0時要注意避免零除. 如果XZ軸平面不垂直于水平面, 這個結果會小于實際的傾角, 傾斜越厲害誤差越大.

三軸加速度數據計算傾角

假設傳感器Z軸垂直朝下, X軸朝正前方, 則X軸與水平面之間的夾角為俯仰角(pitch) $\alpha$, Y軸與水平面間的夾角為橫滾角(roll) $\beta$, 航向角yaw需要地磁傳感器, 無法通過加速度傳感器計算. Z軸垂直于X軸和Y軸, 和兩軸數值是相關的, 并沒有獨立性, 僅用于判斷設備上下的朝向. 令Z軸與水平面的夾角為 $\gamma$

重力加速度在XYZ三個軸上的投影即為三個軸傳感器的讀數, 可以將三軸傾角和重力加速度想像為一個斜立的長方體, 長方體的對角線為重力加速度, 對角線就是這個角對應的三條邊, 傾角的計算方法為

$$\alpha =si{n}^{?1}(\frac{a_x}{g})$$
$$\beta =si{n}^{?1}(\frac{a_y}{g})$$
$$\gamma =si{n}^{?1}(\frac{a_z}{g})$$

帶運動加速度的傾角計算

上面的計算方式適合相對靜止和慢速的場景, 當物體受作用于多個外力時, 作用于傳感器的綜合加速度為重力與各外力的疊加, 此時加速度的方向就不是重力的方向, 上面的計算方式就不適用了, 因為綜合加速度可能比 $g$ 更大或更小.

對于一個物體, 整體加速度等于三軸加速度的矢量和, 其大小 $G$ 可以通過三個向量的大小計算得到

$$G=\sqrt[2]{{a_x}^2+{a_y}^2+{a_z}^2}$$

由此可以得到運動狀態下傾角的計算

$$\alpha =si{n}^{?1}(\frac{a_x}{G})$$
$$\beta =si{n}^{?1}(\frac{a_y}{G})$$
$$\gamma =si{n}^{?1}(\frac{a_z}{G})$$

因為 $a_x$, $G$, $\sqrt[2]{{a_y}^2+{a_z}^2}$ 三個矢量形成直角三角形, 上面的式子可以也可以用 $ta{n}^{?1}$ 計算

$$\alpha =ta{n}^{?1}(\frac{a_x}{\sqrt[2]{{a_y}^2+{a_z}^2}})$$
$$\beta =ta{n}^{?1}(\frac{a_y}{\sqrt[2]{{a_x}^2+{a_z}^2}})$$
$$\gamma =ta{n}^{?1}(\frac{a_z}{\sqrt[2]{{a_x}^2+{a_y}^2}})$$

此時的傾角并非相對重力加速度的傾角, 而是相對物體整體加速度矢量的傾角, 例如物體向前(X軸方向)加速運動時, 整體加速度方向會向后傾斜, 當物體左轉時, 離心力會導致整體加速度方向向右傾斜. 計算此時的姿態傾角, 可以用于幫助物體在當前的運動狀態上保持平衡.

互補濾波

通過加速度傳感器(Accelerometer)可以使用反三角函數$si{n}^{?1}$和$ta{n}^{?1}$求靜止和慢速運動物體的傾角, 對于高速運動的物體, 需要結合陀螺儀的角速度讀數快速響應傾角變化. 對于這兩種傳感器讀數的結合, 通常采用互補濾波算法.

互補濾波就是在短時間內采用陀螺儀得到的角度做為最優值, 定時用加速度值來校正陀螺儀的得到的角度. 加速度計要濾掉高頻信號, 陀螺儀要濾掉低頻信號, 互補濾波器就是根據傳感器特性不同, 通過不同的濾波器, 相加得到整個頻帶的信號.

互補濾波的公式為

$${\alpha }_n=k?({\alpha }_{n?1}+{\delta }_{\alpha }?dt)+(1?k)?{\alpha }^{\prime }$$

其中

• ${\alpha }_n$ 互補計算得到的角度
• ${\alpha }_{n?1}$ 前一次計算得到的角度
• ${\delta }_{\alpha }$ 陀螺儀得到的角速度
• $dt$ 兩次計算的時間間隔
• ${\alpha }^{\prime }$ 通過加速度計得到的傾角
• $k$ 和 $1?k$ 為加權系數, 和為 1

加權系數的確定. 在 《The Balance Filter》 中提到關于加權系數的求解公式, 先設濾波器的加權系數為 $\alpha$, 時間常數為為 $\tau$, 運行周期為 $dt$, 那么公式為

$$\alpha =\frac{\tau }{\tau +dt}$$

運行周期 dt 根據運行周期確定, 如果互補濾波器方法的調用頻率為 200次每秒, 那么 $dt=\frac{1000ms}{200}=5ms$

時間常數 $\tau$ 的取值根據系統的實際需求調整,
不同的系統的 $\tau$ 值不一定相同. $\tau$取值越大則陀螺儀權重越大, $\tau$取值越小則加速度傳感器的權重越大. 通常互補濾波器對陀螺儀的權重會大些, 以降低加速度傳感器中噪聲的影響. 例如互補濾波器運行間隔為 10ms, 時間常數 $\tau =0.49$, 那么此時加權系數為：

$$\alpha =\frac{\tau }{\tau +dt}=\frac{0.49}{0.49+0.01}=0.98$$

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;
}

在實際應用中, 因為加速度計和角速度計讀取的數據存在很大的噪音, 直接使用會造成反饋的不穩定(抖動), 需要在計算前通過卡爾曼濾波器等進行平滑.

參考

• 傾角計算 https://www.analog.com/cn/resources/app-notes/an-1057.html