本文使用 Zhihu On VSCode 創作併發布
在Mahony姿態解算演算法筆記（一）中，我們介紹了融合加速度計與陀螺儀共六軸資料的姿態解算演算法，該篇將介紹融合加速度計、陀螺儀和磁力計共九軸資料的姿態解算演算法。
該演算法可到其網站下載原始碼
https：//
x-io。co。uk/open-source-
imu-and-ahrs-algorithms/
由於筆者水平有限，文中難免存在一些不足和錯誤之處，誠請各位批評指正。
1 引入磁力計資料的目的
在六軸資料融合姿態解算演算法中，我們透過理論和實際重力加速度向量來補償陀螺儀誤差。但重力加速度向量垂直於大地座標系的
平面，即平行於Yaw軸轉軸，故無法用於修正Yaw軸的角度資料。因此我們引入磁力計資料，由於地球磁場方向在中低緯度地區與地面大致平行，因此透過磁力計資料我們可以有效的修正Yaw軸的角度資料。
但在一部分情況中，複雜的磁場環境會導致九軸資料融合的結果並不能遠好於六軸資料，甚至劣於六軸資料融合的結果，比如RoboMaster機器人云臺在某些情況下不使用磁力計資料。原因是雲臺複雜多變的磁場環境會給磁力計引入很大的噪聲。綜上所述，具體選擇九軸還是六軸資料的融合還需要根據具體環境來決定。
2 橢球擬合
由於
三軸的單位存在差異，測量資料的向量頂點會落在橢球面上而非理想狀態下的球面上。另外，電路板產生的電磁場會使磁力計測量資料出現偏移，這則會導致橢球的中心不在原點。因此，在磁力計進行橢球擬合是很有必要的。具體橢球擬合算法可以參考
https：//
blog。csdn。net/shenshike
xmu/article/details/70143455
3 融合磁力計資料
九軸資料融合於六軸資料融合演算法的思路與框架是完全一致的，只需在六軸資料融合演算法的基礎上加入磁力計修正部分即可。與加速度計補償方法大致相同，唯一的區別在於標準重力加速度在地球表面任何地方始終固定不變，可直接從地理座標系變換到機體座標系。但地磁的大小與方向並非固定不變，**因此需要磁力計測量值本身的資料確定理論地磁向量，由於磁力計測得的磁場向量在機體座標系中，該向量還需透過
變換到地理座標系中，經過一定處理後，再透過
變換回機體座標系，從而得到理論地磁向量。**經過兩次四元數確定的座標轉換矩陣即可使理論地磁向量與實際地磁向量的偏差反應角速度誤差，就好比從白湯中取出肉片在紅湯中蘸一蘸便有了辣味。
3。1 將磁場資訊表示為向量
地球磁場可以分解為（投影到）水平和豎直兩個分量。其中豎直分量與水平分量的比值由磁場與大地的傾角決定，而這個傾角在不同緯度地區會有所不同，在赤道地區磁場方向與大地幾乎平行，而在磁極處則垂直於大地，因此在磁極處磁力計無法用於校正航向。我們將由測量和姿態矩陣轉換得到的
理論磁場向量
與僅透過測量得到的
實際磁場向量
表示為：
![\hat{\boldsymbol{m}}=\left[\begin{array}{lll}
b_{x}\\
0 \\
b_{z}
\end{array}\right] \\
\overline{\boldsymbol{m}}=\left[\begin{array}{lll}
m_{x} \\
m_{y} \\
m_{z}
\end{array}\right]\\](https://img.bearask.com/upload/website_attach/+lPuU5=b2tHDkMVG7=kLw4eNTEvm6FSiKJ5uTcsmjqwPV583ieS9-cZgtlMHtdlAIF_aUti0fKHCZhWtJbdsyzeAP_gGT.jpeg%3D%5Cleft%5B%5Cbegin%7Barray%7D%7Blll%7D%0Ab_%7Bx%7D%5C%5C%0A0+%5C%5C+%0Ab_%7Bz%7D%0A%5Cend%7Barray%7D%5Cright%5D+%5C%5C%0A%5Coverline%7B%5Cboldsymbol%7Bm%7D%7D%3D%5Cleft%5B%5Cbegin%7Barray%7D%7Blll%7D%0Am_%7Bx%7D+%5C%5C%0Am_%7By%7D+%5C%5C%0Am_%7Bz%7D%0A%5Cend%7Barray%7D%5Cright%5D%5C%5C)
3。2 如何得到兩個磁場向量
實際磁場向量
可透過三軸磁力計直接得到。但考慮到由測量得到的磁場向量還需要經過矩陣變換等操作才能最終得到機體座標系下的理論磁場向量
。因此我們需要引入一箇中間向量
：
![{\boldsymbol{h}}=\left[\begin{array}{l}
{h_x} \\
{h_y} \\
{h_z}
\end{array}\right]\\](https://img.bearask.com/upload/website_attach/sSrUqtsERUvVll6NDuI01_mFI-OWzw3el3bUo+uUnAadDPsgJtsyb9+U6-9H5xbPuQgZmZqc=ReVLiZALhxPK.jpeg%3D%5Cleft%5B%5Cbegin%7Barray%7D%7Bl%7D%0A%7Bh_x%7D+%5C%5C%0A%7Bh_y%7D+%5C%5C%0A%7Bh_z%7D+%0A%5Cend%7Barray%7D%5Cright%5D%5C%5C)
首先將測量得到的實際磁場向量
透過
轉換得到地理座標系中的磁場向量
：
![{\boldsymbol{h}}=C_{b}^{R} \cdot \overline{\boldsymbol{m}}=\left[\begin{array}{lll}
{1-2\left(q_{2}^{2}+q_{3}^{2}\right)} & {2\left(q_{1} q_{2}-q_{0} q_{3}\right)} & {2\left(q_{1} q_{3}+q_{0} q_{2}\right)} \\
{2\left(q_{1} q_{2}+q_{0} q_{3}\right)} & {1-2\left(q_{1}^{2}+q_{3}^{2}\right)} & {2\left(q_{2} q_{3}-q_{0} q_{1}\right)} \\
{2\left(q_{1} q_{3}-q_{0} q_{2}\right)} & {2\left(q_{2} q_{3}+q_{0} q_{1}\right)} & {1-2\left(q_{1}^{2}+q_{2}^{2}\right)}
\end{array}\right]\cdot
\left[\begin{array}{lll}
m_{x} \\
m_{y} \\
m_{z}
\end{array}\right] =\left[\begin{array}{l}
{h_x} \\
{h_y} \\
{h_z}
\end{array}\right]\\](https://img.bearask.com/upload/website_attach/sSrUqtsERUvVll6NDuI01_mFI-OWzw3el3bUo+uUnAadDPsgJtsyb9+U6-9H5xbPuQgZmZqc=ReVLiZALhxPK.jpeg%3DC_%7Bb%7D%5E%7BR%7D+%5Ccdot+%5Coverline%7B%5Cboldsymbol%7Bm%7D%7D%3D%5Cleft%5B%5Cbegin%7Barray%7D%7Blll%7D%0A%7B1-2%5Cleft%28q_%7B2%7D%5E%7B2%7D%2Bq_%7B3%7D%5E%7B2%7D%5Cright%29%7D+%26+%7B2%5Cleft%28q_%7B1%7D+q_%7B2%7D-q_%7B0%7D+q_%7B3%7D%5Cright%29%7D+%26+%7B2%5Cleft%28q_%7B1%7D+q_%7B3%7D%2Bq_%7B0%7D+q_%7B2%7D%5Cright%29%7D+%5C%5C%0A%7B2%5Cleft%28q_%7B1%7D+q_%7B2%7D%2Bq_%7B0%7D+q_%7B3%7D%5Cright%29%7D+%26+%7B1-2%5Cleft%28q_%7B1%7D%5E%7B2%7D%2Bq_%7B3%7D%5E%7B2%7D%5Cright%29%7D+%26+%7B2%5Cleft%28q_%7B2%7D+q_%7B3%7D-q_%7B0%7D+q_%7B1%7D%5Cright%29%7D+%5C%5C%0A%7B2%5Cleft%28q_%7B1%7D+q_%7B3%7D-q_%7B0%7D+q_%7B2%7D%5Cright%29%7D+%26+%7B2%5Cleft%28q_%7B2%7D+q_%7B3%7D%2Bq_%7B0%7D+q_%7B1%7D%5Cright%29%7D+%26+%7B1-2%5Cleft%28q_%7B1%7D%5E%7B2%7D%2Bq_%7B2%7D%5E%7B2%7D%5Cright%29%7D%0A%5Cend%7Barray%7D%5Cright%5D%5Ccdot%0A%5Cleft%5B%5Cbegin%7Barray%7D%7Blll%7D%0Am_%7Bx%7D+%5C%5C%0Am_%7By%7D+%5C%5C%0Am_%7Bz%7D%0A%5Cend%7Barray%7D%5Cright%5D+%3D%5Cleft%5B%5Cbegin%7Barray%7D%7Bl%7D%0A%7Bh_x%7D+%5C%5C%0A%7Bh_y%7D+%5C%5C%0A%7Bh_z%7D+%0A%5Cend%7Barray%7D%5Cright%5D%5C%5C)
為使航向角參考方向與地磁方向對準，我們需要將
中
軸的兩個分量合併為
，而豎直分量
保持不變，由此即可求出大地座標系下的理論地磁向量
，即：
![\boldsymbol{\hat{m’}}=\left[\begin{array}{lll}
b_{x}\\
0 \\
b_{z}
\end{array}\right]
=
\left[\begin{array}{lll}
\sqrt{h_{x}^{2}+h_{y}^{2}}\\
\qquad0\\
\qquad h_z
\end{array}\right]\\](https://img.bearask.com/upload/website_attach/UpPZYYpeUM6bVmA1zkE+wqt+kZIfK9jvkR5ZWEn0rcyKWuVu9uvCx9lDYa0fSoJeZzkreuTuFXuYaiBDO3_unCterpzbtzgZPshcip44F.jpeg%3D%5Cleft%5B%5Cbegin%7Barray%7D%7Blll%7D%0Ab_%7Bx%7D%5C%5C%0A0+%5C%5C+%0Ab_%7Bz%7D%0A%5Cend%7Barray%7D%5Cright%5D%0A+%3D+%0A+%5Cleft%5B%5Cbegin%7Barray%7D%7Blll%7D%0A+%5Csqrt%7Bh_%7Bx%7D%5E%7B2%7D%2Bh_%7By%7D%5E%7B2%7D%7D%5C%5C%0A+%5Cqquad0%5C%5C%0A+%5Cqquad+h_z%0A+%5Cend%7Barray%7D%5Cright%5D%5C%5C)
現在還不能透過向量外積計算理論向量與實際向量的誤差。因為實際磁場向量
在機體座標系中，而剛剛計算出的理論地磁向量
在地理座標系中，因此我們還需要將
透過矩陣
轉換到機體座標系：
![\boldsymbol{\hat{m}}=
C_{R}^{b}\cdot\boldsymbol{\hat{m’}}=
\left[\begin{array}{lll}
{1-2\left(q_{2}^{2}+q_{3}^{2}\right)} & {2\left(q_{1} q_{2}+q_{0} q_{3}\right)} & {2\left(q_{1} q_{3}-q_{0} q_{2}\right)} \\
{2\left(q_{1} q_{2}-q_{0} q_{3}\right)} & {1-2\left(q_{1}^{2}+q_{3}^{2}\right)} & {2\left(q_{2} q_{3}+q_{0} q_{1}\right)} \\
{2\left(q_{1} q_{3}+q_{0} q_{2}\right)} & {2\left(q_{2} q_{3}-q_{0} q_{1}\right)} & {1-2\left(q_{1}^{2}+q_{2}^{2}\right)}
\end{array}\right]
\cdot
\left[\begin{array}{lll}
b_{x}\\
0 \\
b_{z}
\end{array}\right]
=
\left[\begin{array}{lll}
b_{x}^{](https://img.bearask.com/upload/website_attach/sSrUqtsERUvVll6NDuI01_mFI-OWzw3el3bUo+uUnAadDPsgJabx2RHc7UkW1R_OBMsJmZIX==jwBX7RtXrEncmlWfrKpIHd9GfFmZqc1VeVLiZELXrE7hEOjS7YdHJWvRgUmZIXstvavv_CtftC2cmlVorSowpxl+MkZvDWmbMdKPggKYLybjrOjS7Yg-9zQcacTbSxtJnwBXdRBYMzHh1MK2RYd-Bx=OEq8YdSmReV95bPt_rEncmaV9r-o8dPQpMwZDfWt0ZdQx-f5SrE7nEOjS7YdHHzJTDnmZqYNbmK=d1gJts-trty8LMblmlQQpMkajdQMAmhv=-DBYsAtrty9KrJdqYzJTDnmZIXl=JTliZUKsXqtrti59r-pIZe56MkZVLW6DZdQcrf5SrE7ndOjS7y53izJTDomZqYNbJe=5O0ss7yyn0OjS7ylmAEC6MkZVLW6dTaveIDBYsF3GgOjSsGlmlhCwbFmZqcSUeVLitf5SrE7ndOjS7y53izJTDnmZqYNbJe=5O0ss7yyn0OjS7ylm3gBM0dmZQbNbrKQUZE9l9y0ArO5qIG07HF9racT=YODIudJsqUBY4gP0co8L4egIHFL+W26TdSMXtdJskgJabT1HtmnfrJeMHFL+VctV6ONbrKQWsUBYMz2i15nfrJeTerBMsJtYdSmRexLWZA9ksybjrO50tG07HF9ricT=YW6dedJsqUBY4gtr1P7jkC3T9zQrHcT=YONbMkCRZA9ksyb9+BJ20RlmAA=2zcT=L+NbrfClz8BY4jy+mlWfrJoPerBMsJtjdSmRz35RZA9kWybjrOjmbJ34l0l+Mn-YdSmRydJZ4gJayyyYbOjSsJlm3gtR0Srjd+7ljABX7RQXrEncXb+Kr-o8QzJTqcteMMvUeV9xggJt723GgOjSsJlmlhBM0dqvgA=lZdQxcgJt72trE=9Kr-o8AzJGfZn+7jNbMW=d1gJtsTtrty9UX=lmlQQ6MkajEMvUeV9xggJt723GgOjSsGlmlhBM0dqvgA=lZdQxcgJt72trE=9Kr-o8dPQpMwZDfWt0ZdQx-f5SrE7nbOjS7YdHHzJTDnmZqYNbmK=d1gJtsTtrty8LMblmlQQpMkajdQMAmhv=-DBYsAtrty8LZdlm3gv3w3mZqcsKmoufcgJt7yb9+RK24NuIHDL+M1ZYdQMFT4tlkgQzWyb9+BJ20Rlm3QBM0d7vIA=JCdJsrk=mtw5qmlWfr-oR1Ot+MkajdLMJmABX7RD_rEncmtVKrSo7Hd9McimZIXNbJeCRZe9ftqtrtioor-pIHd9LMwZDIUtReV95zU=uXAtrty8LMZ5RbHsSqcTbYWSi_dQskgQzWyb9+BJ20Rlm3QBM0d7vIA=JCdJsrk=mtw5qmlWfr-oR1Ot+MkajdLMJmABX7RD_rEncmtW9r-oqH-J6MkajdQMUex9RZe9YY2tr1P8LMZD7Hd93EFmZqcFVeVLiZELXrE76mRjKr-pIHd9LMwZDIUtReV95zU=uXAtrty8LMZ5RbHsSqcTbYW6dtdJZ4zK.jpeg)
3。3 誤差補償
經過上述步驟，我們已經得到了同為機體座標系的兩個磁場向量，接下來只需要透過求兩個向量外積得到誤差，並與由加速度計得到的誤差相加即可：
在外積運算前對上式中向量進行
單位化
處理，有：
考慮到實際情況中理論向量
和實際向量
偏差角不會超過45°，而當
在±45°內時，
與
的值非常接近，因此上式可進一步簡化為：
這樣一來，我們就得到了透過加速度計和磁力計顯化得到的角速度誤差，接下來就可以透過PI補償器來計算補償值。這裡與Mahony姿態解算演算法筆記（一）中完全一致，故不再贅述。
3。4 程式碼分析
// Definitions
#define sampleFreq 512。0f
// sample frequency in Hz
#define twoKpDef （2。0f * 0。5f）
// 2 * proportional gain
#define twoKiDef （2。0f * 0。0f）
// 2 * integral gain
// Variable definitions
volatile
float
twoKp
=
twoKpDef
；
// 2 * proportional gain （Kp）
volatile
float
twoKi
=
twoKiDef
；
// 2 * integral gain （Ki）
volatile
float
q0
=
1。0f
，
q1
=
0。0f
，
q2
=
0。0f
，
q3
=
0。0f
；
// quaternion of sensor frame relative to auxiliary frame
volatile
float
integralFBx
=
0。0f
，
integralFBy
=
0。0f
，
integralFBz
=
0。0f
；
// integral error terms scaled by Ki
void
MahonyAHRSupdate
（
float
gx
，
float
gy
，
float
gz
，
float
ax
，
float
ay
，
float
az
，
float
mx
，
float
my
，
float
mz
）
{
float
recipNorm
；
float
q0q0
，
q0q1
，
q0q2
，
q0q3
，
q1q1
，
q1q2
，
q1q3
，
q2q2
，
q2q3
，
q3q3
；
float
hx
，
hy
，
hz
，
bx
，
bz
；
float
halfvx
，
halfvy
，
halfvz
，
halfwx
，
halfwy
，
halfwz
；
float
halfex
，
halfey
，
halfez
；
float
qa
，
qb
，
qc
；
// Use IMU algorithm if magnetometer measurement invalid （avoids NaN in magnetometer normalisation）
// 在磁力計資料無效時使用六軸融合演算法
if
（（
mx
==
0。0f
）
&&
（
my
==
0。0f
）
&&
（
mz
==
0。0f
））
{
MahonyAHRSupdateIMU
（
gx
，
gy
，
gz
，
ax
，
ay
，
az
）；
return
；
}
// Compute feedback only if accelerometer measurement valid （avoids NaN in accelerometer normalisation）
// 只在加速度計資料有效時才進行運算
if
（
！
（（
ax
==
0。0f
）
&&
（
ay
==
0。0f
）
&&
（
az
==
0。0f
）））
{
// Normalise accelerometer measurement
// 將加速度計得到的實際重力加速度向量v單位化
recipNorm
=
invSqrt
（
ax
*
ax
+
ay
*
ay
+
az
*
az
）；
ax
*=
recipNorm
；
ay
*=
recipNorm
；
az
*=
recipNorm
；
// Normalise magnetometer measurement
// 將磁力計得到的實際磁場向量m單位化
recipNorm
=
invSqrt
（
mx
*
mx
+
my
*
my
+
mz
*
mz
）；
mx
*=
recipNorm
；
my
*=
recipNorm
；
mz
*=
recipNorm
；
// Auxiliary variables to avoid repeated arithmetic
// 輔助變數，以避免重複運算
q0q0
=
q0
*
q0
；
q0q1
=
q0
*
q1
；
q0q2
=
q0
*
q2
；
q0q3
=
q0
*
q3
；
q1q1
=
q1
*
q1
；
q1q2
=
q1
*
q2
；
q1q3
=
q1
*
q3
；
q2q2
=
q2
*
q2
；
q2q3
=
q2
*
q3
；
q3q3
=
q3
*
q3
；
// Reference direction of Earth‘s magnetic field
// 透過磁力計測量值與座標轉換矩陣得到大地座標系下的理論地磁向量
hx
=
2。0f
*
（
mx
*
（
0。5f
-
q2q2
-
q3q3
）
+
my
*
（
q1q2
-
q0q3
）
+
mz
*
（
q1q3
+
q0q2
））；
hy
=
2。0f
*
（
mx
*
（
q1q2
+
q0q3
）
+
my
*
（
0。5f
-
q1q1
-
q3q3
）
+
mz
*
（
q2q3
-
q0q1
））；
hz
=
2。0f
*
（
mx
*
（
q1q3
-
q0q2
）
+
my
*
（
q2q3
+
q0q1
）
+
mz
*
（
0。5f
-
q1q1
-
q2q2
））；
bx
=
sqrt
（
hx
*
hx
+
hy
*
hy
）；
bz
=
2。0f
*
（
mx
*
（
q1q3
-
q0q2
）
+
my
*
（
q2q3
+
q0q1
）
+
mz
*
（
0。5f
-
q1q1
-
q2q2
））；
// Estimated direction of gravity and magnetic field
// 將理論重力加速度向量與理論地磁向量變換至機體座標系
halfvx
=
q1q3
-
q0q2
；
halfvy
=
q0q1
+
q2q3
；
halfvz
=
q0q0
-
0。5f
+
q3q3
；
halfwx
=
bx
*
（
0。5f
-
q2q2
-
q3q3
）
+
bz
*
（
q1q3
-
q0q2
）；
halfwy
=
bx
*
（
q1q2
-
q0q3
）
+
bz
*
（
q0q1
+
q2q3
）；
halfwz
=
bx
*
（
q0q2
+
q1q3
）
+
bz
*
（
0。5f
-
q1q1
-
q2q2
）；
// Error is sum of cross product between estimated direction and measured direction of field vectors
// 透過向量外積得到重力加速度向量和地磁向量的實際值與測量值之間誤差
halfex
=
（
ay
*
halfvz
-
az
*
halfvy
）
+
（
my
*
halfwz
-
mz
*
halfwy
）；
halfey
=
（
az
*
halfvx
-
ax
*
halfvz
）
+
（
mz
*
halfwx
-
mx
*
halfwz
）；
halfez
=
（
ax
*
halfvy
-
ay
*
halfvx
）
+
（
mx
*
halfwy
-
my
*
halfwx
）；
// Compute and apply integral feedback if enabled
// 在PI補償器中積分項使能情況下計算並應用積分項
if
（
twoKi
>
0。0f
）
{
// integral error scaled by Ki
// 積分過程
integralFBx
+=
twoKi
*
halfex
*
（
1。0f
/
sampleFreq
）；
integralFBy
+=
twoKi
*
halfey
*
（
1。0f
/
sampleFreq
）；
integralFBz
+=
twoKi
*
halfez
*
（
1。0f
/
sampleFreq
）；
// apply integral feedback
// 應用誤差補償中的積分項
gx
+=
integralFBx
；
gy
+=
integralFBy
；
gz
+=
integralFBz
；
}
else
{
// prevent integral windup
// 避免為負值的Ki時積分異常飽和
integralFBx
=
0。0f
；
integralFBy
=
0。0f
；
integralFBz
=
0。0f
；
}
// Apply proportional feedback
// 應用誤差補償中的比例項
gx
+=
twoKp
*
halfex
；
gy
+=
twoKp
*
halfey
；
gz
+=
twoKp
*
halfez
；
}
// Integrate rate of change of quaternion
// 微分方程迭代求解
gx
*=
（
0。5f
*
（
1。0f
/
sampleFreq
））；
// pre-multiply common factors
gy
*=
（
0。5f
*
（
1。0f
/
sampleFreq
））；
gz
*=
（
0。5f
*
（
1。0f
/
sampleFreq
））；
qa
=
q0
；
qb
=
q1
；
qc
=
q2
；
q0
+=
（
-
qb
*
gx
-
qc
*
gy
-
q3
*
gz
）；
q1
+=
（
qa
*
gx
+
qc
*
gz
-
q3
*
gy
）；
q2
+=
（
qa
*
gy
-
qb
*
gz
+
q3
*
gx
）；
q3
+=
（
qa
*
gz
+
qb
*
gy
-
qc
*
gx
）；
// Normalise quaternion
// 單位化四元數 保證四元數在迭代過程中保持單位性質
recipNorm
=
invSqrt
（
q0
*
q0
+
q1
*
q1
+
q2
*
q2
+
q3
*
q3
）；
q0
*=
recipNorm
；
q1
*=
recipNorm
；
q2
*=
recipNorm
；
q3
*=
recipNorm
；
//Mahony官方程式到此結束，使用時只需在函式外進行四元數反解尤拉角即可完成全部姿態解算過程
}
