四轴飞控：Mahony姿态解算代码及思路_half the sample period

---

前言

Mahony算法常用于无人机的姿态解算，通过将加速度计和陀螺仪的数据进行融合计算四元数，再将融合后的四元数用于反解欧拉角，得到精确的机体姿态。本文以常见的IMU传感器MPU6050/MPU9250为例，介绍四轴飞控的姿态解算原理，即根据加速度和角速度计算无人机姿态。

---

一、为什么要融合传感器数据？

陀螺仪可以测量出机体的角速度，通过对角速度积分就可以直接得到机体的姿态角。同理，加速度计可以测量机体的加速度，通过计算重力加速度在坐标系各轴的分量也可以计算机体的姿态角，那么为什么还要同时用两个传感器计算姿态角呢？

对于陀螺仪而言，陀螺仪的动态性能更好，利用角速度计算姿态角的灵敏度更高，但由于姿态角是通过对角速度积分得到的，积分存在的误差累计问题使其很难独立应用于现实任务。假设陀螺仪计算的角速度存在-0.1°/s的误差，对角速度进行积分10s后会产生-1°误差，积分1分钟后会出现-6°误差。这样的角度偏差，对于无人机任务而言会导致机体产生严重倾斜，是完全无法接受的。
对于加速度计而言，加速度计在测量物体姿态时，主要依赖于重力加速度的分量，静态性能较好，不存在积分累计误差。但是，利用加速度计计算姿态时默认将机体视为稳态，只受重力影响，这里会存在一定问题。当机体处于运动状态时（如前进、侧翻等），加速度计无法区分重力加速度和其他外部加速度，测量的加速度数据会掺杂运动产生的加速度，引入误差。

如果融合陀螺仪和加速度计的数据，就可以利用加速度计修正陀螺仪的积分累计误差，并结合陀螺仪提供高频姿态变化信息，获得更准确和稳定的姿态信息。

二、姿态解算代码

推荐一个视频：视频解析
姿态解算主要分为以下五步：
（1）利用陀螺仪数据计算一个重力向量
（2）利用加速度计测量一个重力向量
（3）对两个重力向量进行叉积运算，得到误差值
（4）将误差值用于补偿角速度
（5）利用补偿后的角速度计算四轴姿态

代码：

//kp=ki=0 就是完全相信陀螺仪
#define Kp 1.50f                         // proportional gain governs rate of convergence to accelerometer/magnetometer
                                         //比例增益控制加速度计，磁力计的收敛速率
#define Ki 0.005f                        // integral gain governs rate of convergence of gyroscope biases  
                                         //积分增益控制陀螺偏差的收敛速度
#define halfT 0.005f                     // half the sample period 采样周期的一半（该部分对应四元数微分方程的1/2*δt）

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;     // quaternion elements representing the estimated orientation（四元数初值）
float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error（用于暂存加速度计补偿陀螺仪的PID积分值）

void IMUupdate(FLOAT_XYZ *Gyr_rad,FLOAT_XYZ *Acc_filt,FLOAT_ANGLE *Att_Angle)	//传入陀螺仪数据Gyr_rad、加速度数据Acc_filt，返回姿态角Att_Angle
{
	uint8_t i;
	float matrix[9] = {1.f,  0.0f,  0.0f, 0.0f,  1.f,  0.0f, 0.0f,  0.0f,  1.f };//初始化矩阵
	float ax = Acc_filt->X,ay = Acc_filt->Y,az = Acc_filt->Z;	//获取加速度计数据
	float gx = Gyr_rad->X,gy = Gyr_rad->Y,gz = Gyr_rad->Z;		//获取陀螺仪数据
	float vx, vy, vz;	//四元数推导出的理论重力加速度向量
	float ex, ey, ez;	//理论和实际重力加速度向量的误差
	float norm;	//求模
	
	float q0q0 = q0*q0;	//由于四元数矩阵存在大量重复的q?*q?计算，且各个q求解较麻烦，需结合加速度求解
	float q0q1 = q0*q1;	//故提前暂存四元数乘积，降低解算计算量
	float q0q2 = q0*q2;
	float q0q3 = q0*q3;
	float q1q1 = q1*q1;
	float q1q2 = q1*q2;
	float q1q3 = q1*q3;
	float q2q2 = q2*q2;
	float q2q3 = q2*q3;
	float q3q3 = q3*q3;
	
	if(ax*ay*az==0)
		return;
	
	//加速度计测量的重力向量(机体坐标系) 
	norm = invSqrt(ax*ax + ay*ay + az*az); 	//norm = 1/根号(x²+y²+z²)
	ax = ax * norm;	//单位化后，向量为单位向量，只需考虑方向。单位化后，向量长度=根号(ax²+ay²+az²)=1
	ay = ay * norm;
	az = az * norm;
	
	//陀螺仪积分估计重力向量(机体坐标系) 
	vx = 2*(q1q3 - q0q2);	//四元数矩阵的第三列为理论的重力加速度向量
	vy = 2*(q0q1 + q2q3);	//该向量也是单位向量
	vz = q0q0 - q1q1 - q2q2 + q3q3 ;
	
	//测量的重力向量与估算的重力向量差积求出向量间的误差 
	ex = (ay*vz - az*vy); //+ (my*wz - mz*wy);   //叉积可用于衡量两个向量之间的差异，叉积后的向量垂直于这两个向量。   
	ey = (az*vx - ax*vz); //+ (mz*wx - mx*wz);
	ez = (ax*vy - ay*vx); //+ (mx*wy - my*wx);	//叉积后的向量长度为|axb|=|a||b|sinθ。叉积后的向量值也可通过代码中的方法表示
	
	//用上面求出误差进行积分
	exInt = exInt + ex * Ki;								 
	eyInt = eyInt + ey * Ki;
	ezInt = ezInt + ez * Ki;
	
	//将误差PI后补偿到陀螺仪
	gx = gx + Kp*ex + exInt;					   		  	
	gy = gy + Kp*ey + eyInt;
	gz = gz + Kp*ez + ezInt;//这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减
	
	//四元素的微分方程
	q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;		//求解四元数
	q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
	q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
	q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
	
	//单位化四元数 
	norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
	q0 = q0 * norm;
	q1 = q1 * norm;
	q2 = q2 * norm;  
	q3 = q3 * norm;
	//方向余弦矩阵，可求可不求
	//矩阵R 将惯性坐标系(n)转换到机体坐标系(b) 
	matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11(前列后行)
	matrix[1] = 2.f * (q1q2 + q0q3);	    // 12
	matrix[2] = 2.f * (q1q3 - q0q2);	    // 13
	matrix[3] = 2.f * (q1q2 - q0q3);	    // 21
	matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22
	matrix[5] = 2.f * (q2q3 + q0q1);	    // 23
	matrix[6] = 2.f * (q1q3 + q0q2);	    // 31
	matrix[7] = 2.f * (q2q3 - q0q1);	    // 32
	matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33
	 
	//四元数转换成欧拉角(Z->Y->X) 
	Att_Angle->yaw += Gyr_rad->Z *RadtoDeg*0.01f;     //yaw轴不用加速度计校准，直接对角速度积分也可，因为加速度计无法矫正yaw。看哪个更准用哪个
	//乘0.01f是因为姿态解算任务的频率为100Hz，累加角速度时应以0.01s为单位
	
	//	Att_Angle->yaw = atan2(2.f * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3)* 57.3f; // yaw
	Att_Angle->pit = -asin(2.f * (q1q3 - q0q2))* 57.3f;                                 // pitch(负号要注意) 
	Att_Angle->rol = atan2(2.f * q2q3 + 2.f * q0q1, q0q0 - q1q1 - q2q2 + q3q3)* 57.3f ; // roll
	for(i=0;i<9;i++)
	{
	  *(&(DCMgb[0][0])+i) = matrix[i];
	}
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97

三、代码思路

（1）利用陀螺仪数据计算一个重力向量

首先介绍一些基础概念：
假设初始状态下，大地坐标系和机体坐标系完全重合。任取飞机上一点A，该点在大地系的坐标为A0=（x0,y0,z0），（显然此时该点在机体系的坐标是一模一样的），那么当飞机绕自己的X轴旋转θ弧度后，问该点在大地系的新坐标A1？

答：物体绕自己的各个轴旋转可以用旋转矩阵R来表示。假设Rx为绕x轴旋转的旋转矩阵，则新坐标A1=Rx * A0。如果飞机先绕自己的X轴转（横滚Roll），再绕自己的Y轴转（俯仰Pitch），最后绕自己的Z轴转（航向Yaw），想要求解A在大地系的新坐标，只需把坐标A0依次左乘3个旋转矩阵即可，即A1 = Rz * Ry * Rx * A0。

有了上述基础知识，接下来介绍如何计算Rz 、 Ry 、Rx：
以绕z轴旋转为例，计算Rz：
假设图中的向量1绕z轴旋转为向量2：

那么向量1与x轴的夹角为α，向量2与向量1的夹角为ψ，向量1长度为r，向量1记为(x,y,z)，向量2记为(x’,y’,z’)，则有：

至此，z轴旋转矩阵Rz求出了。我们将向量1左乘Rz即可得到绕z轴旋转后的向量2。Rx和Ry的计算方法同理，以下给出结果：

因此，假设一个向量e绕x、y、z轴旋转后得到向量b，则计算过程可表示为：

Rz×Ry×Rx我们定义为方向余弦矩阵R，b=R×e。以下为Rz×Ry×Rx结果，即R的元素值：

我们假设rij为R的第i行第j列元素（如r32代表R的第3行第2列元素sinφcosθ），则有：

经过上述一系列证明，我们得知一个向量绕x,y,z轴旋转可以被视为左乘一个方向余弦矩阵R。除了用旋转角度表示方向余弦矩阵之外，还可以用四元数表示方向余弦矩阵，相关课程：

假设重力加速度向量(0,0,1)经过机体旋转后产生了变化，我们通过左乘方向余弦矩阵R即可得到变化后的重力加速度向量（Vx,Vy,Vz）：

这个向量的参数q0~q3可以由陀螺仪测得的角速度求出，计算过程如下：

将求出的q0~q3带入（Vx,Vy,Vz）即可得到理论重力加速度向量。至此，我们利用陀螺仪计算出了一个理论的重力加速度向量。
对应的源码：

vx = 2*(q1q3 - q0q2);	//四元数矩阵的第三列为理论的重力加速度向量
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
1
2
3

//四元素的微分方程
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;		//求解四元数
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

//单位化四元数 
norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 * norm;
q1 = q1 * norm;
q2 = q2 * norm;  
q3 = q3 * norm;
1
2
3
4
5
6
7
8
9
10
11
12

（2）利用加速度计测量一个重力向量

为了利用加速度计补偿陀螺仪，还需要利用加速度计测量一个重力向量，对应的源码：

norm = invSqrt(ax*ax + ay*ay + az*az); 	//norm = 1/根号(x²+y²+z²)
ax = ax * norm;	//单位化后，向量为单位向量，只需考虑方向。单位化后，向量长度=根号(ax²+ay²+az²)=1
ay = ay * norm;
az = az * norm;
1
2
3
4

（3）对两个重力向量进行叉积运算，得到误差值

对理论重力向量和实际重力向量进行叉积运行可以得出两个向量的误差：

ex = (ay*vz - az*vy); //+ (my*wz - mz*wy);   //叉积可用于衡量两个向量之间的差异，叉积后的向量垂直于这两个向量。   
ey = (az*vx - ax*vz); //+ (mz*wx - mx*wz);
ez = (ax*vy - ay*vx); //+ (mx*wy - my*wx);	//叉积后的向量长度为|axb|=|a||b|sinθ。叉积后的向量值也可通过代码中的方法表示
1
2
3

（4）将误差值用于补偿角速度

通过对误差进行PI计算得到补偿值，将补偿值直接加到陀螺仪测量出来的角速度即可得到补偿后的角速度。由于角速度经过PI补偿，故利用这个新的角速度推算出的角度值会更精确。

//用上面求出误差进行积分
exInt = exInt + ex * Ki;								 
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;

//将误差PI后补偿到陀螺仪
gx = gx + Kp*ex + exInt;					   		  	
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;//这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减
1
2
3
4
5
6
7
8
9

（5）利用补偿后的角速度计算四轴姿态

由于旋转角表达的方向余弦矩阵和四元数表达的方向余弦矩阵相等，因此直接对四元数表达的方向余弦矩阵相应的元素做反三角运算即可得到姿态角：


对应的源码为：

Att_Angle->pit = -asin(2.f * (q1q3 - q0q2))* 57.3f;                                 // pitch(负号要注意) 
Att_Angle->rol = atan2(2.f * q2q3 + 2.f * q0q1, q0q0 - q1q1 - q2q2 + q3q3)* 57.3f ; // roll
1
2

---

总结

本文详细介绍了飞控姿态解算中的难点，包括姿态结算为什么要使用传感器数据融合，如何进行数据融合，最后详细解析了Mahony姿态解算的代码。