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

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

導航坐標系和載體坐標系之間的關系

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

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

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

初始磁航角的計算

由于陀螺儀無法敏感到地球自轉，因此可通過磁力計確定初始的航向角。當傳感器靜止時，測力計測量的是地磁場在載體坐標系下的投影。磁力計量測的磁場和地磁場之間的關系可表示為：
$$m^n=C{}_{b,pr}^n{m}^b$$其中,$m^n={\left[\begin{array}{ccc}{m}_x^n& m_y^n& m_z^n\end{array}\right]}^{\prime }$為地磁場，$m^b={\left[\begin{array}{ccc}{m}_x^b& m_y^b& m_z^b\end{array}\right]}^{\prime }$為磁力計測量的磁場。在通過加速度計確定橫滾角和俯仰角之后，姿態轉換矩陣DCM可以表示為：
$$C{}_{b,pr}^n=?\begin{array}{ccc}cos\theta & sin?sin\theta & cos?sin\theta \\ 0& cos?& ?sin?\\ ?sin\theta & sin?cos\theta & cos?cos\theta \end{array}?$$磁航角的計算公式為：$${\psi }_m=?atan2(m_y^n,m_x^n)$$那么磁航角計算公式可進一步表示為：
$${\psi }_m=?atan2(m_y^{b}cos??m_z^{b}sin?,m_x^{b}cos\theta +m_y^{b}sin?sin\theta +m_z^{b}cos?sin\theta )$$

初始航向角的計算

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

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

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

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

總結

以上是生活随笔為你收集整理的MEMS惯性传感器初始姿态角的确定的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。