IMU的內參標定2

作者：由林明發表于文化時間：2021-03-08

2。4 不用轉檯的標定

2。4。1 利用建立的IMU模型和測量的值構建殘差模型

轉檯的作用就是讓

$a_{x}, a_{y}, a_{z}$

變成已知的。這樣才可以透過最小二乘法得到其餘的標定引數。

那麼不利用轉檯的情況下，我們是不知道IMU的理想輸出值

$a_{x}, a_{y}, a_{z}$

的。IMU上電後，我們測量到的值只能是

$A_{x}, A_{y},A_{z}$

的。

加速度誤差模型【深藍學院】

不過就算我們不知道理想狀態的

$a_{x}, a_{y}, a_{z}$

的值，我們還是知道靜止狀態下重力是［0，0， g］是已知的。那麼我們只要將靜止狀態下的IMU的資料存好，然後利用加速度輸入輸出模型，利用儲存的資料倒推

$a_{x}, a_{y}, a_{z}$

的數值之後，跟重力做差得到殘差函數了。得到殘差函式，那麼就好辦了，就可以透過對內參求偏導後，得到對應的雅克比。有了殘差函式和雅克比，就可以利用迭代的方式找出合適的內參（標定引數）。

整體步驟就是參考下面的論文進行的。

加速度計代表定的引數為

$\theta^{acc} = [S_{ayx}, S_{azx}, S_{azy}, K_{ax}, K_{ay}, K_{az}, b_{ax}, b_{ay}, b_{az}]$

按照內參定義，加速度的輸入與輸出的模型如下

$A=K_{a}(I+S_{a})a+b_{a}$

那麼為了透過測量值得到的真實值，變換成下面的形態

$a=(I+S_{a})^{-1}K^{-1}_{a}(A-b_{a})$

為了方便求解，將定義一個

$K^{$

$K^{-1}_{a}=diag(K^{-1}_{ax},K^{-1}_{ay},K^{-1}_{az})$

最終將非常小的項省略之後最終的a等於

$a\approx (I-S_{a})K^{$

加速度向量定義為

$g=[0, 0, g_{0}]^{T}$

，

$g_{0}$

為當地重力加速度大小。

當內參完全正確的時候，有

$||g||^{2}=||a||^{2}$

。但是這是不可能的。肯定因為內參的原因會產生誤差。所以這個誤差就可以當做殘差函式

$f(\theta^{acc})=||g||^{2} - ||a||^{2}$

來使用。

那麼為了利用高斯牛頓進行迭代最佳化，需要計算

$f(\theta^{acc})$

對

$\theta^{acc}$

的偏導數。

推導

先計算

。

$a\approx\begin{bmatrix} 1, 0, 0 \\ S_{ayx},1,0\\ S_{azx},S_{azy},1 \end{bmatrix} \cdot \begin{bmatrix} K^{$

$a\approx \begin{bmatrix} K^{$

$||a|| = |K^{$

是一個標量，

是一個3x1的向量。

利用殘差函式求對內參的偏導數。

$f(\theta^{acc})=||g||^{2} - ||a||^{2}$

$\frac{\partial||g||^{2}-||a||^{2}}{\partial\theta_{acc}} =\frac{\partial||g||^{2}}{\partial\theta_{acc}}-\frac{\partial||a||^{2}}{\partial\theta_{acc}}$

因為g其實是一個常量，所以其實

$\frac{\partial||g||^{2}}{\partial\theta_{acc}}$

等於0。

那麼其實要計算的是

$-\frac{\partial||a||^{2}}{\partial\theta_{acc}}$

。由鏈式法則

$\frac{\partial||a||^{2}}{\partial\theta_{acc}} = \frac{\partial||a||^{2}}{\partial||a||}\cdot \frac{\partial||a||}{\partial a}\cdot\frac{\partial a}{\partial \theta_{acc}}$

其中

$\frac{\partial||a||^{2}}{\partial||a||}=2||a||$

$\frac{\partial||a||}{\partial a}=\frac{a}{||a||}$

（因為公式

$\frac{\partial||X||}{\partial X} = \frac{X}{||X||}$

）

$\frac{\partial||a||^{2}}{\partial||a||}\cdot \frac{\partial||a||}{\partial a}=2a$

那麼對於最後

$\frac{\partial a}{\partial \theta_{acc}}$

計算偏導

$\frac{\partial a}{\partial \theta_{acc}}= \begin{bmatrix} \frac{\partial a_{11}}{\partial \theta_{1}}&\frac{\partial a_{11}}{\partial \theta_{2}}&\frac{\partial a_{11}}{\partial \theta_{3}}&\frac{\partial a_{11}}{\partial \theta_{4}}&\frac{\partial a_{11}}{\partial \theta_{5}}&\frac{\partial a_{11}}{\partial \theta_{6}}&\frac{\partial a_{11}}{\partial \theta_{7}}&\frac{\partial a_{11}}{\partial \theta_{8}}&\frac{\partial a_{11}}{\partial \theta_{9}}\\ \frac{\partial a_{21}}{\partial \theta_{1}}&\frac{\partial a_{21}}{\partial \theta_{2}}&\frac{\partial a_{21}}{\partial \theta_{3}}&\frac{\partial a_{21}}{\partial \theta_{4}}&\frac{\partial a_{21}}{\partial \theta_{5}}&\frac{\partial a_{21}}{\partial \theta_{6}}&\frac{\partial a_{21}}{\partial \theta_{7}}&\frac{\partial a_{21}}{\partial \theta_{8}}&\frac{\partial a_{21}}{\partial \theta_{9}}\\ \frac{\partial a_{31}}{\partial \theta_{1}}&\frac{\partial a_{31}}{\partial \theta_{2}}&\frac{\partial a_{31}}{\partial \theta_{3}}&\frac{\partial a_{31}}{\partial \theta_{4}}&\frac{\partial a_{31}}{\partial \theta_{5}}&\frac{\partial a_{31}}{\partial \theta_{6}}&\frac{\partial a_{31}}{\partial \theta_{7}}&\frac{\partial a_{31}}{\partial \theta_{8}}&\frac{\partial a_{31}}{\partial \theta_{9}}\\ \end{bmatrix}$