從零開始做四軸飛行器 (六) - 感測器校正

從零開始做四軸系列離上一篇大概有四個月了，本來是打算在第六部分寫飛行器模型的，不過只敘述模型不加控制器感覺沒甚麼內容，也就是普通的旋轉加直線運動方程式而已，但加控制器的方法又有多到數不清，而且要透過 MATLAB 來驗證又要花上不少時間寫程式，幾乎難產，所以就暫時先插入已經完成的感測器校正部分，之後看飛控板和遙控器更新改板的進度再來調整飛行器模型篇的位子吧。

這裡的感測器校正不探討溫度對於感測器在誤差方面的影響，因為沒有特別用溫度來校正過 IMU 相關感測器，不確定在溫度產生的誤差方面對於校正是否有顯著或是影響大的結果，之後如果有空來測量不同的工作時間長度下，芯片工作溫度的變化後，或許可以評估一下是否有這些必要，但目前就先假設感測器的溫度固定，不會因為外在或內部的溫度而影響噪音的大小。

陀螺儀（Gyroscope or Gyro）校正

對陀螺儀校正的部分主要校正偏差（Bias）的誤差，不對安裝誤差做校正，因為對於安裝物差的校正需要一些＂絕對＂的測量數據，這些數據通常都需要外部的儀器來輔助校正與測量，舉一個簡單的例子，今天我想要找出下列方程式中的 S_g 矩陣，找出這一個矩陣的一個方法就是在不同的角速度下測量，外部的裝置可以產生一個固定的角速度給予感測器，或是透過另一個可靠的感測器來做提供實際的結果（可能有些人會想為什麼不直接用感測器的輸出？因為感測器的輸出是包含誤差的，所以才需要被校正，如果感測器沒有誤差當然沒有問題，可以使用，不過就不需要做校正了...），而感測器將測量到的結果與實際的結果作比較，就可以得出 S_g 矩陣，那不用外部的儀器可做嗎？可以，如果有人可以產生一個固定或是準確的角速度，就可以透過這個角速度來校正陀螺儀，不過個人是沒辦法實現這高難度的動作，只能先假設 S_g 矩陣是一個單位矩陣或是一個對角矩陣（Diagonal Matrix）。

$\large \begin{align*} &y_{g}=S_{g}\,y_{g,c}+b_{g}+n_g,\\ & where\; y_{g} = \begin{bmatrix} \omega _{1} \\ \omega _{2} \\ \omega _{3} \end {bmatrix}, S_{g}= \begin{bmatrix} S_{g11} & S_{g12} & S_{g13}\\ S_{g21} & S_{g22} & S_{g23}\\ S_{g31} & S_{g32} & S_{g33} \end{bmatrix} b_{g}= \begin{bmatrix} b_{g1}\\ b_{g2} \\ b_{g3} \end{bmatrix} \end{align*}$

在假設 S_g 是一個對角矩陣後，問題就簡化了許多，在 n_b 為高斯噪音（Gaussian Noise）下，平均值為 0，靜止下對陀螺儀取樣 y_g = 0，所以這時測量到的就是 b_g，只要在每次測量的結果減掉 b_g，並乘上一個放大縮小的變數，就可以得出理想的陀螺儀資料了，說的簡單一點就是在靜止下取平均值就可以得出 b_g。

$\large \begin{align*} &y_{g,c} = S_{g}^{-1}\,(y_{g}-b_{g}-n_{b}),\\ &where\; S_{g}^{-1}= \begin{bmatrix} S_{g11}^{-1} & 0 & 0\\ 0 & S_{g22}^{-1} & 0\\ 0 & 0 & S_{g33}^{-1} \end{bmatrix} \end{align*}$
　

加速度計（Accelerometer or G-Sensor）校正

首先先放上感測器的模型

$\large \begin{align*} &y_{a}=S_{a}\,y_{a,c}+b_{a}+n_a,\\ & where\; y_{a} = \begin{bmatrix} a_{1} \\ a_{2} \\ a_{3} \end {bmatrix}, S_{a}= \begin{bmatrix} S_{a11} & S_{a12} & S_{a13}\\ S_{a21} & S_{a22} & S_{a23}\\ S_{a31} & S_{a32} & S_{a33} \end{bmatrix} b_{a}= \begin{bmatrix} b_{a1}\\ b_{a2} \\ b_{a3} \end{bmatrix} \end{align*}$

對加速度計校正的部分除了 b_a 矩陣的偏差外，還會找出 S_a 矩陣來校正比例誤差與焊接、安裝產生的未對準誤差，這邊提供兩種校正的方法，第一種是基於最小平方法的校正方法，需要放置在特定角度下做校正，另一種則是高斯牛頓法，透過重力加速度固定的特性校準，不需放置在特定角度下來校準。

2017/07/16 補上最小平方法校正

．加速度計的最小平方法校正

先整理成以下形式

$\large \begin{align*} &y_{a,c} = S_{a}^{-1}(y_{a} - b_{a}) = S_{a}^{-1}\begin{bmatrix} I & -b_{a} \end{bmatrix}\begin{bmatrix} y_{a}^{T} & 1 \end{bmatrix}^{T} = KY, \\ &where\; K = S_{a}^{-1}\begin{bmatrix} I & -b_{a} \end{bmatrix} = \begin{bmatrix} S_{a}^{-1} & -S_{a}^{-1}b_{a} \end{bmatrix}\in \mathbb{R}^{3\times 4}, Y = \begin{bmatrix} y_{a}^{T} & 1 \end{bmatrix}^{T}\in \mathbb{R}^{4\times 1} \end{align*}$

擴展延伸至 N 種不同姿態，則 y_a,c 與測量矩陣 Y 則可重新寫成

$\large \begin{align*} &y_{a,c} \in \mathbb{R}^{3\times N}, Y = \begin{bmatrix} y_{a}^{(1)} & y_{a}^{(2)} & ... & y_{a}^{(N)} \\ 1 & 1 & ... & 1 \end{bmatrix} \in \mathbb{R}^{4\times N} \end{align*}$

若已知不同姿態下的，則可以透過最小二乘法求解矩陣 K

$\large \begin{align*} K = (y_{a,c}Y^{T})(YY^{T})^{-1} \end{align*}$

經由上式關係，可以反推回 S_a 與 b_a 矩陣

$\large \begin{align*} S_{a} = K(1:3, 1:3)^{-1}, b_{a} = - S_{a} K(1:3, 4) \end{align*}$

．加速度計的高斯牛頓法校正

為了減少運算，三軸間的正交誤差不特別做校正，因為使用的感測器是三軸集成於一顆芯片的，所以直接假設三軸互相正交，則 S 的反矩陣（K 矩陣）會是一個對證矩陣，並且表示成無偏差的形式，為的是減少展開的複雜度，所以問題可以化簡成下面式子

$\large \begin{align*} &y_{a,c} = S_{a}^{-1}(y_{a} - b_{a}) = K(y_{a} - b_{a}) = K\bar{y}_{a}, \\ &where\; \bar{y}_{a} = \begin{bmatrix} \bar{a}_{1} \\ \bar{a}_{2} \\ \bar{a}_{3} \end{bmatrix} = \begin{bmatrix} a_{1} - b_{a1} \\ a_{2} - b_{a2} \\ a_{3} - b_{a3} \end{bmatrix}, K = \begin{bmatrix} k_{11} & k_{12} & k_{13}\\ k_{12} & k_{22} & k_{23}\\ k_{13} & k_{23} & k_{33} \end{bmatrix} \end{align*}$

把右式展開則可以表示如下，其中 k 表示第 k 次的加速度計資料

$\large \begin{align*} (&y_{a,c})_{k} = {\begin{bmatrix} a_{c1} \\ a_{c2} \\ a_{c3} \end{bmatrix}}_{k} = \begin{bmatrix} k_{11}\bar{a}_{1} + k_{12}\bar{a}_{2} + k_{13}\bar{a}_{3} \\ k_{12}\bar{a}_{1} + k_{22}\bar{a}_{2} + k_{23}\bar{a}_{3} \\ k_{13}\bar{a}_{1} + k_{23}\bar{a}_{2} + k_{33}\bar{a}_{3} \end{bmatrix}_{k} \end{align*}$

再來就是透過在沒有運動加速度的條件下，加速度計三軸輸出平方和會等於重力加速度的關係來做校正，此種方法不需要依預先設定好的角度與位置來校正，校正的自由度較大

$\large \begin{align*} g^{2} = a_{c1}^{2} + a_{c2}^{2} + a_{c3}^{2} \end{align*}$

將上式表示成誤差函數，並展開與整理，結果如下

$\large \begin{align*} e & \equiv a_{c1}^{2} + a_{c2}^{2} + a_{c3}^{2} - g^{2}\\ & = (k_{11}^{2} + k_{12}^{2} + k_{13}^{2})(a_{1} - b_{1})^{2}\\ & + (k_{12}^{2} + k_{22}^{2} + k_{23}^{2})(a_{2} - b_{2})^{2}\\ & + (k_{13}^{2} + k_{23}^{2} + k_{33}^{2})(a_{3} - b_{3})^{2}\\ & + 2(k_{11}k_{12} + k_{12}k_{22} + k_{13}k_{23})(a_{1} - b_{1})(a_{2} - b_{2})\\ & + 2(k_{12}k_{13} + k_{22}k_{23} + k_{23}k_{33})(a_{2} - b_{2})(a_{3} - b_{3})\\ & + 2(k_{11}k_{13} + k_{12}k_{23} + k_{13}k_{33})(a_{3} - b_{3})(a_{1} - b_{1}) - g^{2}\\ & = A\bar{a}_{1}^{2} + B\bar{a}_{2}^{2} + C\bar{a}_{3}^{2} + 2D\bar{a}_{1}\bar{a}_{2} + 2E\bar{a}_{2}\bar{a}_{3} + 2F\bar{a}_{3}\bar{a}_{1} - g^{2} \end{align*}$

定義 E 函數為誤差的平方和，若可以找出 E 函數的極值（最小值）時，就可以得出 K 矩陣與 B 矩陣了

$\large \begin{align*} &E(X) = \sum_{k=1}^{N}e_{k}^{2}(X) = e^{T}e,\\ &where \,\, e = \begin{bmatrix} e_{1} & e_{2} & ... & e_{N} \end{bmatrix}^{T} \end{align*}$

因為 E 函數可以表示成 e'*e 的形式，所以這裡解極值的方法我們採用高斯牛頓法（Gauss-Newton Method），詳細的演算法說明請自行參考 wikipedia: Gauss-Newton Method ，迭代過程如下式，對誤差的 Jocabian 與 Hessian 反矩陣相乘，其中 Hessian 矩陣中省略掉了二次微分項，自己實際用 MATLAB 模擬過，有無省略結果其實差不多，有時好、有時差，但在運算量上差別還頗大的，所以這裡把二次微分向給忽略掉。

$\large \begin{align*} X_{k+1} & = X_{k} - \alpha \begin{pmatrix}\nabla^{2} E(X))^{-1}(\nabla E(X)\end{pmatrix}\\ & = X_{k} - \alpha \begin{pmatrix} \sum_{k=1}^{N}(\nabla e_{k}^{T}\nabla e_{k} + e_{k} \nabla^{2} e_{k}) \end{pmatrix}^{-1} \begin{pmatrix} \sum_{k=1}^{N}e_{k} \nabla e_{k}^{T} \end{pmatrix}\\ &\approx X_{k} - \alpha \begin{pmatrix} \sum_{k=1}^{N}\nabla e_{k}^{T}\nabla e_{k} \end{pmatrix}^{-1} \begin{pmatrix} \sum_{k=1}^{N}e_{k} \nabla e_{k}^{T} \end{pmatrix}\\ \end{align*}$

而 X 就是我們想要求得的 K 矩陣與 B 矩陣，透過不斷的迭代而得出來，如果要校正正交誤差就在加入 3 項 k 矩陣沒有對稱的變數即可，但實際效果如何我倒是沒有試過，有興趣可以自己試看看

$\large \begin{align*} X&= \begin{bmatrix} x_{1} & x_{2} & x_{3} & x_{4} & x_{5} & x_{6} & x_{7} & x_{8} & x_{9} \end{bmatrix}^{T}\\&= \begin{bmatrix} k_{11} & k_{12} & k_{13} & k_{22} & k_{23} & k_{33} & b_{a1} & b_{a2} & b_{a3} \end{bmatrix}^{T} \end{align*}$

2016/02/10 更正

之後想了一下，此方法主要以重力來校正，若裝置與重力有固定夾角的話，無法校正未對準或安裝的誤差

因為之前使用 Least Squares 在不同角度的重力值來做校正，所以可以校正安裝誤差，但缺點是需要外部的儀器精度才容易比較高，不過現在換成三軸輸出平方和為重力平方的條件後，因為沒有參考的角度，所以就無法校正與重力有夾角的裝置，但相對的，不需要外部儀器可以有一定的校正精度了。

MATLAB 的模擬結果

詳細的微分結果與程式都放在 Github 上

C 語言驗證版本 https://github.com/Hom-Wang/C-Language/tree/master/newtonMethod
MATLAB 驗證版本 https://github.com/Hom-Wang/MATLAB/tree/master/gauss-Newton_Methods

↑ 修正量，也就是 Jocabian 與 Hessian 反矩陣的相乘，最後卻趨近 0，表示收斂。
　
↑ 校正參數，在迭代的過程中不斷的更新，最後卻趨於穩定的一個值。
　
↑ 誤差平方和。
　
↑ 最後整裡的一張實際數值與迭代結果得比較表格，誤差大概都在 0.01% 以下。
　
　

磁力計（Magnetometer or Compass）校正

磁力計的校正之前是使用橢圓擬合，詳細可以參考Github: 推導_橢圓擬合，不過橢圓擬合僅適用於平面的校正，離開該平面，像是傾斜等情況，容易產生誤差，但理論上電子羅盤也可以用上述的高斯牛頓法來求得校正參數，把 g 改成 1 即可，實現歸一化的作用，因為最後在求航向角時，僅需要知道比例關係，不需要知道實際的磁場強度。

2016/06/29 補充

今天經過實際測試後，發現橢圓擬合方無法有效校正電子羅盤的資料，主要是因為實際應用是在三維空間中，橢圓擬合校準僅會校準二維平面上的偏移，之後再補上其他的電子羅盤校正方法

2016/07/30 新增磁力計校正方法 - 四參數校正模型

Freescale 的磁力計校準方法 (有另外的七參數和十參數校正)
https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs

假設從磁力計中讀出，未經校正軟磁干擾 S 與硬磁干擾 H 的磁場為 h_m，已校正過的磁場為 h_c，地球磁場為 h_r，兩磁場間的轉換關係為 R，則其關係可表示如下：

$\begin{align*} &h_{c} = R h_{r},\ h_{m} = S h_{c} + H\\ &\Rightarrow h_{m} = S R h_{r} + H\\ &\Rightarrow h_{c} = S h_{r} = S^{-1} (h_{m} - H)\\ \end{align*}$

將以校正過的三軸磁力計任意旋轉而產生出來軌跡，會在的半徑為的球面上，其數值表示為地球磁場強度，關係如下：

$\begin{align*} \left \| h_{c} \right \|^{2} = h_{c}^{T} h_{c} = (R h_{r})^{T}R h_{r} = h_{r}^{T} R^{T} R h_{r} = B^{2} \end{align*}$

帶入可得到

$\begin{align*} &\left \| h_{c} \right \|^{2} = (S^{-1}(h_{m} - H))^{T} (S^{-1}(h_{m} - H)) = B^{2}\\ &\Rightarrow (h_{m} - H)^{T} (S^{-1})^{T} (S^{-1}) (h_{m} - H) = B^{2}\\ &\Rightarrow (h_{m} - H)^{T} A (h_{m} - H) = B^{2}\\ \\ & where A = (S^{-1})^{T} (S^{-1}) = A^{T} \end{align*}$

定義誤差函數

$\begin{align*} E = \left \| h_{c} \right \|^{2}-B^{2} \end{align*}$

．四參數校正模型 Four Parameter Magnetic Calibration Model

假設在無軟磁干擾，僅有硬磁干擾的情況下，原式可改寫如下

$\begin{align*} & h_{m} = h_{c} + H \\ & \Rightarrow h_{c} = h_{m} - H \\ & \Rightarrow \left \| h_{c} \right \|^{2} = h_{c}^{T} h_{c} = (h_{m} - H)^{T} (h_{m} - H) = h_{m}^{T} h_{m} -2 H^{T} h_{m} + H^{T} H \end{align*}$

並定義誤差函數

$\begin{align*} E = \left \| h_{c} \right \|^{2} - B^{2} = h_{m}^{T} h_{m} - 2 H^{T} h_{m} + H^{T} H - B^{2} \end{align*}$

將其展開與整理

$\begin{align*} E & = h_{mx}^{2} + h_{my}^{2} + h_{mz}^{2} - 2 H_{x} h_{mx} - 2 H_{y} h_{my} - 2 H_{z} h_{mz} + H_{x}^{2} + H_{y}^{2} + H_{z}^{2} - B^{2}\\ & = (h_{mx}^{2} + h_{my}^{2} + h_{mz}^{2}) - \begin{bmatrix} h_{mx}\\ h_{my}\\ h_{mz}\\ 1 \end{bmatrix}^{T} \begin{bmatrix} 2H_{x}\\ 2H_{y}\\ 2H_{z}\\ B^{2} - H_{x}^{2} - H_{y}^{2} - H_{z}^{2} \end{bmatrix} \\ & = (h_{mx}^{2} + h_{my}^{2} + h_{mz}^{2}) - \begin{bmatrix} h_{mx}\\ h_{my}\\ h_{mz}\\ 1 \end{bmatrix}^{T} \begin{bmatrix} \beta_{1}\\ \beta_{2}\\ \beta_{3}\\ \beta_{4} \end{bmatrix}\\ & = Y - X \beta \end{align*}$

當殘差為零時

$\begin{align*} & E = 0 \\ & \Rightarrow Y-X \beta = 0 \\ & \Rightarrow X \beta = Y \\ & \Rightarrow X^{T}X \beta = X^{T}Y \\ & \Rightarrow \beta = (X^{T}X )^{-1} X^{T}Y \\ \end{align*}$

擴展至 N 筆資料

$\begin{align*} Y-X \beta = \begin{bmatrix} h_{mx1}^{2} + h_{my1}^{2} + h_{my1}^{2}\\ h_{mx2}^{2} + h_{my2}^{2} + h_{my2}^{2}\\ \vdots \\ h_{mxN}^{2} + h_{myN}^{2} + h_{myN}^{2} \end{bmatrix} - \begin{bmatrix} h_{mx1} & h_{my1} & h_{mz1} & 1\\ h_{mx2} & h_{my2} & h_{mz2} & 1\\ \vdots & \vdots & \vdots & \vdots\\ h_{mxN} & h_{myN} & h_{mzN} & 1\\ \end{bmatrix} \begin{bmatrix} \beta_{1}\\ \beta_{2}\\ \beta_{3}\\ \beta_{4} \end{bmatrix} \end{align*}$

整理後

$\begin{align*} & \beta = (X^{T} X)^{-1} X^{T} Y \\ & \Rightarrow \begin{bmatrix} \beta_{1}\\ \beta_{2}\\ \beta_{3}\\ \beta_{4} \end{bmatrix} = \begin{pmatrix} \sum_{i=1}^{N} \begin{bmatrix} h_{mxi}^{2} & h_{mxi}h_{myi} & h_{mxi}h_{mzi} & h_{mxi}\\ h_{mxi}h_{myi} & h_{myi}^{2} & h_{myi}h_{mzi} & h_{myi}\\ h_{mxi}h_{mzi} & h_{myi}h_{mzi} & h_{mzi}^{2} & h_{mzi}\\ h_{mxi} & h_{myi} & h_{mzi} & 1 \end{bmatrix} \end{pmatrix}^{-1} \begin{bmatrix} h_{mx1} & h_{mx2} & \cdots & h_{mxN}\\ h_{my1} & h_{my2} & \cdots & h_{myN}\\ h_{mz1} & h_{mz2} & \cdots & h_{mzN}\\ 1 & 1 & \cdots & 1\\ \end{bmatrix} \begin{bmatrix} h_{mx1}^{2} + h_{my1}^{2} + h_{mz1}^{2}\\ h_{mx2}^{2} + h_{my2}^{2} + h_{mz2}^{2}\\ \vdots \\ h_{mxN}^{2} + h_{myN}^{2} + h_{mzN}^{2}\\ \end{bmatrix} \end{align*}$

$\begin{align*} &H = \begin{bmatrix} H_{x}\\ H_{y}\\ H_{z} \end{bmatrix} =\frac{1}{2} \begin{bmatrix} \beta_{1}\\ \beta_{2}\\ \beta_{3} \end{bmatrix}\\ & B=(\beta_{4} + H_{x}^{2} + H_{y}^{2} + H_{z}^{2})^{1/2} \end{align*}$

上一篇 → 從零開始做四軸 (五) - 感測器原理
下一篇 → 從零開始做四軸 (七) - 姿態計算

KitSprout