Mahony姿態解算算法筆記（二）

在Mahony姿態解算算法筆記（一）中，我們介紹了融合加速度計與陀螺儀共六軸數據的姿態解算算法，該篇將介紹融合加速度計、陀螺儀和磁力計共九軸數據的姿態解算算法。

該算法可到其網站下載源碼https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/

由於筆者水平有限，文中難免存在一些不足和錯誤之處，誠請各位批評指正。

1 引入磁力計數據的目的

在六軸數據融合姿態解算算法中，我們通過理論和實際重力加速度向量來補償陀螺儀誤差。但重力加速度向量垂直於大地坐標系的\(xoy\)平面，即平行於Yaw軸轉軸，故無法用於修正Yaw軸的角度數據。因此我們引入磁力計數據，由於地球磁場方向在中低緯度地區與地面大致平行，因此通過磁力計數據我們可以有效的修正Yaw軸的角度數據。

但在一部分情況中，復雜的磁場環境會導致九軸數據融合的結果並不能遠好於六軸數據，甚至劣於六軸數據融合的結果，比如RoboMaster機器人雲台在某些情況下不使用磁力計數據。原因是雲台復雜多變的磁場環境會給磁力計引入很大的噪聲。綜上所述，具體選擇九軸還是六軸數據的融合還需要根據具體環境來決定。

2 橢球擬合

由於 \(x,y,z\) 三軸的單位存在差異，測量數據的向量頂點會落在橢球面上而非理想狀態下的球面上。另外，電路板產生的電磁場會使磁力計測量數據出現偏移，這則會導致橢球的中心不在原點。因此，在磁力計進行橢球擬合是很有必要的。具體橢球擬合算法可以參考https://blog.csdn.net/shenshikexmu/article/details/70143455

3 融合磁力計數據

九軸數據融合於六軸數據融合算法的思路與框架是完全一致的，只需在六軸數據融合算法的基礎上加入磁力計修正部分即可。與加速度計補償方法大致相同，唯一的區別在於標准重力加速度在地球表面任何地方始終固定不變，可直接從地理坐標系變換到機體坐標系。但地磁的大小與方向並非固定不變，因此需要磁力計測量值本身的數據確定理論地磁向量，由於磁力計測得的磁場向量在機體坐標系中，該向量還需通過 \(C_{b}^{R}\) 變換到地理坐標系中，經過一定處理后，再通過 \(C_{R}^{b}\) 變換回機體坐標系，從而得到理論地磁向量。經過兩次四元數確定的坐標轉換矩陣即可使理論地磁向量與實際地磁向量的偏差反應角速度誤差，就好比從白湯中取出肉片在紅湯中蘸一蘸便有了辣味。

3.1 將磁場信息表示為向量

地球磁場可以分解為（投影到）水平和豎直兩個分量。其中豎直分量與水平分量的比值由磁場與大地的傾角決定，而這個傾角在不同緯度地區會有所不同，在赤道地區磁場方向與大地幾乎平行，而在磁極處則垂直於大地，因此在磁極處磁力計無法用於校正航向。我們將由測量和姿態矩陣轉換得到的理論磁場向量 \(\hat{\boldsymbol{m}}\) 與僅通過測量得到的實際磁場向量 \(\overline{\boldsymbol{m}}\) 表示為：

\[\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]\\ \]

3.2 如何得到兩個磁場向量

實際磁場向量 \(\overline{\boldsymbol{m}}\) 可通過三軸磁力計直接得到。但考慮到由測量得到的磁場向量還需要經過矩陣變換等操作才能最終得到機體坐標系下的理論磁場向量 \(\hat{\boldsymbol{m}}\) 。因此我們需要引入一個中間向量 \({\boldsymbol{h}}\) ：

\[{\boldsymbol{h}}=\left[\begin{array}{l} {h_x} \\ {h_y} \\ {h_z} \end{array}\right]\\ \]

首先將測量得到的實際磁場向量 \(\overline{\boldsymbol{m}}\) 通過 \(C_{b}^{R}\) 轉換得到地理坐標系中的磁場向量\({\boldsymbol{h}}\)：

\[{\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]\\ \]

為使航向角參考方向與地磁方向對准，我們需要將 \(\hat{\boldsymbol{h}}\) 中\(x,y\)軸的兩個分量合並為\(b_x\)，而豎直分量 \(h_z\) 保持不變，由此即可求出大地坐標系下的理論地磁向量 \(\boldsymbol{\hat{m’}}\) ，即：

\[\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]\\ \]

現在還不能通過向量外積計算理論向量與實際向量的誤差。因為實際磁場向量 \(\overline{\boldsymbol{m}}\) 在機體坐標系中，而剛剛計算出的理論地磁向量 \(\boldsymbol{\hat{m’}}\) 在地理坐標系中，因此我們還需要將 \(\boldsymbol{\hat{m’}}\) 通過矩陣 \(C_{R}^{b}\) 轉換到機體坐標系：

\[\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}^{'}\\ 0 \\ b_{z}^{'} \end{array}\right]\\ \]

3.3 誤差補償

經過上述步驟，我們已經得到了同為機體坐標系的兩個磁場向量，接下來只需要通過求兩個向量外積得到誤差，並與由加速度計得到的誤差相加即可：

\[|\boldsymbol{\rho}|=|\overline{\boldsymbol{v}}| \cdot|\hat{\boldsymbol{v}}| \cdot \sin \theta_1 +|\overline{\boldsymbol{m}}| \cdot|\hat{\boldsymbol{m}}| \cdot \sin \theta_2\\ \]

在外積運算前對上式中向量進行單位化處理，有：

\[|\boldsymbol{\rho}|=\sin \theta_1 +\sin \theta_2\\ \]

考慮到實際情況中理論向量 \(\hat{\boldsymbol{v}}\) 和實際向量 \(\overline{\boldsymbol{v}}\) 偏差角不會超過45°，而當\(θ\)在±45°內時，\(sinθ\) 與\(θ\)的值非常接近，因此上式可進一步簡化為：

\[|\boldsymbol{\rho}|=\theta_1 +\theta_2\\ \]

這樣一來，我們就得到了通過加速度計和磁力計顯化得到的角速度誤差，接下來就可以通過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官方程序到此結束，使用時只需在函數外進行四元數反解歐拉角即可完成全部姿態解算過程
}