MEMS慣性傳感器初始姿態角的確定

9軸MEMS慣性傳感器初始姿態角的計算

隨着微機電系統( Micro Electromechanical System，MEMS) 技術的不斷髮展，基於MEMS的慣性傳感器得到了更加廣泛的應用，比如機器人跟蹤導航，行人定位。慣性導航系統利用初始的位置、速度和姿態信息，通過陀螺儀和加速度測量的角速率和比力進行二次積分推算得到目標的位置、速度和姿態信息。因此，初始對準的好壞將確定慣性導航系統的精度和穩定性。
MEMS慣性傳感器中加速度計的精度通常優於1mg，而陀螺儀無法敏感到地球自轉，因此可以利用加速度計確定慣性導航系統的橫滾角和俯仰角，但卻無法利用陀螺儀確定航向角信息。利用磁力計能確定慣性導航系統的初始航向角

導航座標系和載體座標系之間的關係

這裏的導航座標系採用北向-東向-地向（NED）座標系，記爲n系；載體座標系採用前向-右向-向下座標系，記爲b系。繞着載體的$\ x$軸旋轉爲橫滾角，繞着載體的$\ y$軸旋轉爲俯仰角，繞着載體的$\ z$軸旋轉爲航向角。導航座標系依次沿着$\ x$軸、$\ y$軸和$\ z$軸旋轉能變換到載體座標系中，那麼載體座標系和導航座標系的姿態旋轉矩陣DCM可表示爲：
$\ C{^n_b} = \begin{bmatrix} {cos\theta cos \psi} & {-cos\phi sin \psi +sin\phi sin\theta cos \psi} & {sin\phi sin \psi +cos\phi sin\theta cos \psi} \\ {cos\theta sin\psi} & {cos\phi cos\psi +sin\phi sin\theta sin\psi} & {-sin\phi cos\psi +cos\phi sin\theta sin\psi} \\ {-sin\theta} & {sin\phi cos\theta} &{cos\phi cos\theta} \end{bmatrix}$其中，$\theta$爲俯仰角，$\phi$爲橫滾角，$\psi$爲航向角。
注：選擇的導航座標系和載體座標系和這裏的不一致，以及旋轉順序和這裏的不一致，所產生的旋轉矩陣均不相同。
注：座標系統和姿態旋轉矩陣是慣性導航系統最基礎最核心的部分。

初始橫滾角和俯仰角的計算

當傳感器靜止時，加速度計測量的是重力在載體座標系下的投影。由於是MEMS傳感器器件誤差較大，可以不考慮重力在北方向和東方向的投影，即不考慮重力的垂線偏差，那麼重力和加速度計的比力（加速度）之間的關係爲：
$f^b_{ib} = C{^b_n}g^n$其中，$f^b_{ib}=\begin{bmatrix} f^b_{ib,x} &f^b_{ib,y}&f^b_{ib,z} \end{bmatrix}'$爲加速度計輸出的x，y和z軸的比力(或加速度)信息，$g^n=\begin{bmatrix} 0 &0&g^n_D \end{bmatrix}'$爲導航座標系下重力在北(North)、東(East)和地(Down)方向三個的投影。通過上面DCM矩陣可以展開爲：
$\begin{bmatrix} f^b_{ib,x} \\f^b_{ib,y}\\f^b_{ib,z} \end{bmatrix}=\begin{bmatrix} -sin\theta \\-cos\theta sin\phi \\-cos\theta cos\phi \end{bmatrix}g^n_D \space ......(1)$通過公式（1）可以計算靜止期間慣性傳感器的初始橫滾角和俯仰角，計算公式爲：
$\theta=atan(\cfrac{f^b_{ib,x}}{\sqrt{\smash[b]{{f^b_{ib,y}*{f^b_{ib,y}+{f^b_{ib,z}*{f^b_{ib,z} }}}}}}}) \space, \phi=atan2(-f^b_{ib,y},-f^b_{ib,z})$

初始磁航角的計算

由於陀螺儀無法敏感到地球自轉，因此可通過磁力計確定初始的航向角。當傳感器靜止時，測力計測量的是地磁場在載體座標系下的投影。磁力計量測的磁場和地磁場之間的關係可表示爲：
$m^n=C{^n_{b,pr}}m^b$其中,$m^n=\begin{bmatrix} m^n_x &m^n_y&m^n_z \end{bmatrix}'$爲地磁場，$m^b=\begin{bmatrix} m^b_x &m^b_y&m^b_z \end{bmatrix}'$爲磁力計測量的磁場。在通過加速度計確定橫滾角和俯仰角之後，姿態轉換矩陣DCM可以表示爲：
$C{^n_{b,pr}} =\begin{bmatrix} cos\theta & sin\phi sin\theta &cos\phi sin\theta \\ 0 & cos\phi& -sin\phi \\ {-sin\theta} & {sin\phi cos\theta} &{cos\phi cos\theta} \end{bmatrix}$磁航角的計算公式爲：$cos {\psi_m} = -atan2(m^n_y,m^n_x)$那麼磁航角計算公式可進一步表示爲：
$cos {\psi_m} = -atan2(m^b_ycos\phi-m^b_zsin\phi,m^b_xcos\theta+m^b_ysin\phi sin\theta+m^b_zcos\phi sin\theta)$

初始航向角的計算

由於地理北極和磁力北極之間存在一個磁偏角，慣性導航中初始的航向角是以地理北極（真北）進行計算的，因此計算初始航向角的時候需要減去磁偏角。磁偏角可以通過查表或者公式計算出來（一個城市內磁偏角相同）。
磁北和真北（地理北極）之間的關係可以通過下圖表示

磁偏角可以通過該網址查詢[武漢的磁偏角爲-4°26′]：magnetic-declination: http://www.magnetic-declination.com/

注：上述中加速度計和磁力計在靜止時間段內的輸出值進行平均，然後計算初始姿態角，能降低期間中隨機噪聲對結果的影響。

作爲學習中的記錄，僅供參考~~