VSLAM：：[手寫VIO_課堂筆記]第二講_IMU測量模型+誤差模型+運動模型&&歐拉角微分推導

1. 第二講_IMU相關內容

1.1. 旋轉運動學

1.1.1. 運動半徑$r$對$\theta$的求導,再寫成矩陣形式

$\color{red}{下圖中:半徑a和高度h固定}$

從結果可以看到,矩陣形式的導數表示可以寫成矩陣形式$\omega \times r$,左側爲反對稱矩陣w

1.1.2. 考慮更復雜的情況,假設旋轉座標系下的情況

1.1.2.1. 旋轉座標系下的運動學(暫時不考慮平移)

考慮兩個座標系,一個靜止的座標系,慣性座標系i系,另外一個是旋轉的座標系即載體座標系b系.

其中,關於旋轉矩陣$R_{IB}$的導數$\dot{R_{IB}}$可看第一講的內容.

結果:一個有趣的現象是,物體在慣性系下的速度$\dot{r}$並不是直接等於物體在b系下的速度乘以旋轉矩陣$R_{IB}*v_{B}$,後面還多了一項$-\omega \times r_I$,這就是哥氏加速度帶來的影響.

• 補充:

• $\dot{R}_{IB}r_B=\omega \times r_I$的詳細證明
  (1)用到了性質$R[a]_\times=[Ra]_\times R$
  (2)用到了性質$\dot{R}=R\omega$
  $\begin{aligned} \dot{R}_{IB}r_B &= \lim_{\Delta t \to 0} \frac{R_{IB} \exp([\omega_{BB'} \Delta t]\hat{~})r_B-R_{IB}r_B}{\Delta t} \\ &= \lim_{\Delta t \to 0} \frac{R_{IB} (I+[\omega_{BB'}\Delta t]\hat{~}) r_B-R_{IB}r_B}{\Delta t} \\ &= \lim_{\Delta t \to 0} \frac{R_{IB} [\omega_{BB'}\Delta t]\hat{~} r_B}{\Delta t} \\ &= \lim_{\Delta t \to 0} \frac{-R_{IB} r_B\hat{~} (\omega_{BB'}\Delta t)}{\Delta t} \\ &= {-R_{IB} r_B\hat{~} (\omega_{BB'})} \\ &= R_{IB}(\omega_{BB'})\hat{~}r_B \\ &=[R_{IB}\omega_{BB'}]_\times R_{IB}r_B \\ &=\omega \times r_I \end{aligned}$

• 關於速度的求導部分的補充

• 因爲有$\dot{R_{IB}}=R_{IB}[\omega_b]_\times =[R_{IB}\omega_b]_\times R_{IB}$(根據性質-第一講)

• 所以$\dot{R_{IB}}v_b=R_{IB}[\omega_b]_\times v_b=[R_{IB}\omega_b]_\times R_{IB} v_{b}=\omega \times v$

• $\dot{R_{IB}}\omega_b=R_{IB}[\omega_b]_\times \omega_b=R_{IB} \omega_b \hat{~}\omega_b=0$

結果:結果表明,物體在慣性系的加速度a_i並不直接等於b系下的加速度乘以對應的旋轉矩陣,而是要加上後面幾項,其中,哥氏力是成對出現的,另外:由於w_{ie}爲常量,所以$\omega$求導後的$\dot{\omega}$爲0,所以一般歐拉力爲0?.

---

1.2. IMU測量模型和運動模型

1.2.1. 加速度測量

1.2.2. 角速度測量

主要利用科氏力來計算角速度

---

1.3. IMU誤差模型

1.3.1. 確定性誤差

• run-to-run : 每次上電的時候的bias疊加
• run-in-run : 每次上電時候的bias不一樣
• 溫度

1.3.2. 標定方法

1.3.2.1. 加速度計標定

1.3.2.2. 陀螺儀標定

1.3.2.3. 溫度相關係數

1.3.2.4. 不確定性誤差(隨機誤差)

1.3.2.5. Bias誤差模型

1.3.2.6. 艾倫方差標定(常用)

• 使用標定工具
  
  兩種標定工具
• 美國:calibration alan
• 港科大:imu_utils

1.3.2.7. 加速度計數學模型

1.3.2.8. 陀螺儀數學模型

---

1.4. 運動模型離散時間處理

1.4.1. 歐拉法

關於姿態q的離散積分:
$\begin{aligned} \because \dot{q}_{wb_k}=q_{wb_k} \otimes [0,\frac{1}{2}\omega]^T \end{aligned}$

$\begin{aligned} \therefore q_{wb_{k+1}}&=q_{wb_k}+\dot{q}_{wb_k} \delta t \\ &= q_{wb_k}+q_{wb_k} \otimes [0,\frac{1}{2}\omega]^T \\ &=q_{wb_k} \otimes [1,\frac{1}{2}\omega]^T \end{aligned}$

1.4.2. 中值法

比歐拉法稍微精確一點

---

1.5. 仿真

1.5.1. 知識補充

$\color{red}{注意歐拉角(3維向量)形式的旋轉積分:需要先使用旋轉矩陣將b系下的角速度轉換爲世界座標系的歐拉角速度}$

1.5.2. 個人理解

上面的關於歐拉角旋轉的描述是基於ZYX321順序的,其對應的座標系是前右下,對應的導航座標系是北東地,常用於無人機.下面用另外一種歐拉角順序推導一遍,該順序更常用於地面無人車.

1.5.2.1. 方向餘弦矩陣的基本形式

一個向量的方向（姿態）我們可以用他在參考座標系（地理座標系）各個軸向的夾角的餘弦來表示（及在各個軸的投影）。
類似的 一個座標系 可以看成是3個向量組成，所以三個向量分別在座標軸上的投影可以用來表示一個座標系與參考座標系的關係。這總共9個方向餘弦組成了一個三階矩陣，其對應方式如下圖。
$\quad C_b^n= \begin{bmatrix} c_{11} & c_{12} &c_{13} \\ c_{21} & c_{22} &c_{23} \\ c_{11} & c_{12} &c_{13} \end{bmatrix} \quad$
其中，第 i 行、 j 列的元素表示參考座標系 i 軸和姿態座標系 j 軸夾角的餘弦。事實上方向餘弦和歐拉角沒有本質區別，因爲方向餘弦實際上就是用歐拉角表示的。

1.5.2.2. 方向餘弦矩陣的舉例推導

一個二維的座標變換如下：

點F爲固定點，在座標系1下的表示爲$F(r_{x1},r_{y1})$，在座標系2下爲$F_{2}(r_{x2},r_{y2})$
由圖可知,注意觀察使用圖中兩個紅色的三角形
$\begin{aligned} r_{x2}&=r_{x1}*\cos \alpha+ r_{y1}*\sin \alpha \\ r_{y2}&=r_{x1}*(-\sin \alpha)+r_{y1}*\cos \alpha \end{aligned}$
推廣到三維的情況下，可看作是繞Z軸逆時針旋轉，並寫成矩陣形式：
$\quad \begin{bmatrix} r_{x2} \\ r_{y2} \\ r_{z2} \end{bmatrix}= \begin{bmatrix} \cos \alpha & \sin \alpha &0 \\ -\sin \alpha & \cos \alpha &0 \\ 0 & 0 &1 \end{bmatrix} \begin{bmatrix} r_{x1} \\ r_{y1} \\ r_{z1} \end{bmatrix} \quad$
由上面的例子，可推理餘弦矩陣各個元素的意義

• 第1行表示旋轉之後的X2軸在原座標系(X1,Y1,Z1)軸下的投影
• 第2行表示旋轉之後的Y2軸在原座標系(X1,Y1,Z1)軸下的投影
• 第3行表示旋轉之後的Z2軸在原座標系(X1,Y1,Z1)軸下的投影

1.5.2.3. 東北天ENU---->右前上的餘弦矩陣$C_{n}^{b}$推導

假設我們現在有一個東北天座標系和一個載體座標系，現需要將東北天座標系經過3次旋轉，使得最終得到的座標系與載體座標系重合。

在此之前，需要做出一些規定

• 旋轉的正方向爲：從旋轉軸看的逆時針方向
• 旋轉的順序爲：Z-X-Y
• 對應的歐拉角：Yaw-Pitch-Roll

1.5.2.3.1. 繞Z軸逆時針旋轉$\psi$——Yaw

得到旋轉矩陣$C_{n}^{1}$
$\quad C_{n}^{1}= \begin{bmatrix} \cos \psi & \sin \psi &0 \\ -\sin \psi & \cos \psi &0 \\ 0 & 0 &1 \end{bmatrix} \quad$

1.5.2.3.2. 繞X’軸逆時針旋轉$\theta$——Pitch

得到旋轉矩陣$C_{1}^{2}$
$\quad C_{1}^{2}= \begin{bmatrix} 1 & 0 &0 \\ 0 &\cos \theta & \sin \theta \\ 0 &-\sin \theta & \cos \theta \end{bmatrix} \quad$

1.5.2.3.3. 繞Y’'軸逆時針旋轉$\phi$——Roll

得到旋轉矩陣$C_{2}^{3}$
$\quad C_{2}^{3}= \begin{bmatrix} \cos \phi & 0 &-\sin \phi \\ 0 & 1& 0 \\ \sin \phi & 0& \cos \phi \end{bmatrix} \quad$

1.5.2.3.4. 得到ENU導航座標系到右前上載體座標系的方向餘弦矩陣$C_{n}^{b}$

$\begin{aligned} C_{n}^{b}=&C_{2}^{3}C_{1}^{2}C_{n}^{1} \\ =& \begin{bmatrix} \cos \phi & 0 &-\sin \phi \\ 0 & 1& 0 \\ \sin \phi & 0& \cos \phi \end{bmatrix} \begin{bmatrix} 1 & 0 &0 \\ 0 &\cos \theta & \sin \theta \\ 0 &-\sin \theta & \cos \theta \end{bmatrix} \begin{bmatrix} \cos \psi & \sin \psi &0 \\ -\sin \psi & \cos \psi &0 \\ 0 & 0 &1 \end{bmatrix} \\ =& \begin{bmatrix} \cos \psi \cos \phi -\sin \psi \sin \theta \sin \phi & \sin \psi \cos \phi+\cos \psi \sin \theta \sin \phi & -\cos \theta \sin \phi \\ -\sin \psi \cos \theta & \cos \psi \cos \theta & \sin \theta \\ \cos \psi \sin \phi+ \sin \psi \sin \theta \cos \phi & \sin \psi \sin \phi- \cos \psi \sin \theta \cos \phi & \cos \theta \cos \phi \end{bmatrix} \\ = & \begin{bmatrix} c_{11} & c_{12} &c_{13} \\ c_{21} & c_{22} &c_{23} \\ c_{11} & c_{12} &c_{13} \end{bmatrix} \end{aligned}$

1.5.2.4. $\bold{\color{red}{(ZXY-312順序)歐拉角微分}}$

上面的方向餘弦矩陣$C_n^b$描述了將一個點\向量從一個座標系轉換到另一個座標系的變換.

---

導航座標系下歐拉角離散積分形式推導

$\because$歐拉角形式積分: $\vartheta_{wb'}=\vartheta_{wb'}+ \frac{d \vartheta}{dt} \Delta t$.用到了歐拉角的微分形式
$\therefore$需要求$\frac{d \vartheta}{dt}$
其中:
(1)$\vartheta=[\theta_{pitch},\Phi_{roll},\psi_{yaw}]^T$表示在導航座標系下的歐拉角速度
(2)$\omega$表示在載體座標系b系下的三個角速度
求:利用$\omega$表示出導航座標系下的歐拉角微分$\frac{d \vartheta}{dt}$

求解:
以下部分參考來自:<歐拉角微分方程推導>

1. 假設已經完成繞三個軸的旋轉，則最後繞Y軸的角速度在載體座標系和導航座標系都是一樣的，所以有如下公式：
   $\begin{aligned} \omega_b(roll)=\begin{bmatrix} 0\\ \frac{d roll}{dt} \\ 0 \end{bmatrix} \\ \end{aligned}$
2. 假設已經完成了前兩組旋轉，最後一組旋轉沒有完成，如果接着完成最後一步旋轉，則和第一步一樣，繞X軸的角速度在兩個座標系下是一樣的，則有如下公式：
   $\begin{aligned} \omega_b(pitch)=C_2^{3}\begin{bmatrix} \frac{d pitch}{dt}\\ 0 \\ 0 \end{bmatrix} \end{aligned}$
3. 分析和第二步類似。假設已經完成了第一組旋轉，最後兩組旋轉沒有完成，如果接着完成最後兩組旋轉，則繞ZZZ軸的角速度在兩個座標系下是一樣的，則有如下公式：
   $\begin{aligned} \omega_b(yaw)=C_2^{3}C_1^{2}\begin{bmatrix} 0\\ 0 \\ \frac{d yaw}{dt} \end{bmatrix} \end{aligned}$

4. 通過對三次旋轉的理解,可以得出,載體座標系的三軸角速度$\omega$可以如下表示:
   $\begin{aligned} \omega_b&=\omega_b(yaw)+\omega_b(pitch)+\omega_b(roll) \\ &=C_2^{3}C_1^{2}\begin{bmatrix} 0\\ 0 \\ \frac{d yaw}{dt} \end{bmatrix}+C_2^{3}\begin{bmatrix} \frac{d pitch}{dt}\\ 0 \\ 0 \end{bmatrix}+\begin{bmatrix} 0\\ \frac{d roll}{dt} \\ 0 \end{bmatrix} \\ &=\begin{bmatrix} \cos \phi & 0 & -\sin \phi \cos \theta \\ 0 & 1 & 0 \\ \sin \phi & 0 & \cos \theta \cos \phi \end{bmatrix} \cdot \begin{bmatrix} \frac{d pitch}{dt} \\ \frac{d roll}{dt} \\ \frac{d yaw}{dt} \end{bmatrix} \\ &=\begin{bmatrix} \cos \phi & 0 & -\sin \phi \cos \theta \\ 0 & 1 & 0 \\ \sin \phi & 0 & \cos \theta \cos \phi \end{bmatrix} \cdot \frac{d \vartheta}{dt} \end{aligned}$

5. 於是,通過對矩陣求逆,可求出$\frac{d \vartheta}{dt}$
   $\begin{aligned} \frac{d \vartheta}{dt}= \begin{bmatrix} \frac{d pitch}{dt} \\ \frac{d roll}{dt} \\ \frac{d yaw}{dt} \end{bmatrix} = \begin{bmatrix} \cos \phi & 0 & \sin \phi \\ \sin \theta \sin \phi / \cos \theta & 1 & -\sin \theta \cos \phi / \cos \theta \\ -\sin \phi/ \cos \theta & 0 & \cos \phi /\cos \theta \end{bmatrix} \cdot \omega_b \end{aligned}$

6. 另外的,還可以參照第一講的旋轉矩陣R的導數$\dot{R}=Rw_\times$,具體參照下面資料<方法二>

這部分參考資料:
(1)<方法一:分步計算偏導:歐拉角微分方程推導>
(2)<方法二:直接對旋轉矩陣求偏導:歐拉角微分方程的推導>

---

1.6. 作業:IMU仿真