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官方程序到此结束，使用时只需在函数外进行四元数反解欧拉角即可完成全部姿态解算过程
}