无人机底层开发-MPU6050+磁力计的四元数解算姿态

最新推荐文章于 2025-03-31 04:13:42 发布

飘飘仙儿__

最新推荐文章于 2025-03-31 04:13:42 发布

阅读量1.2w

点赞数 28

分类专栏：无人机开发应用

本文链接：https://blog.csdn.net/qq_41422043/article/details/83955983

版权

无人机开发应用专栏收录该内容

5 篇文章

订阅专栏

MPU6050主要包含陀螺仪和加速度计。陀螺仪主要测量角速度，即可以测出某一时间段物体转过的角度。加速度计测量的是物体的加速度，重力加速度即物体受重力作用的情况下具有的加速度，物体静止时，加速度计测出来的加速度等于重力加速度g，约等于9.8米每平方秒，重力加速度总是竖直向下，通过计算重力加速度在X,Y的分量，则可以计算出物体相对于水平面的倾斜角度。

我们先来看看如何用欧拉角描述旋转(坐标变换)：

绕Z轴旋转即滚转角，绕Y轴旋转即偏航角，绕X旋转即俯仰角。

这里需要注意绕X,Y,Z轴方向旋转的先后顺序不一样，得出余弦矩阵的顺序（也就是公式）也不一样。并且在一组旋转里X,Y,Z不可交换(Euler旋转)，所以有先后顺序之分（程序中）。旋转变换有12种表示方式，分别为：XYZ、XZY、XYX、XZX、YXZ、YZX、YXY、YZY、ZXY、ZYX、ZXZ、ZYZ。这里还有一点需要注意，那就是"Gimbal Lock"。图中（ZYX，以下均为此顺序）的绕Z旋转会导致YX轴变化，但是Y影响X不会影响Z，X不会影响ZY。因此，如果旋转Y±90°，那么Z轴方向不变，但是会改变X轴方向，导致YX轴同向。此时保持Y值不变，那么改变Z或者X的效果相同。

Gimbal Lock总结就是，其源自Euler旋转原理，此原理旋转变量不可交换，所以有先后之分，所以可以改变后两个轴而第一轴方向不变，所以产生轴共线，即Gimbal Lock。

上图中得出旋转后的欧拉角公式，但是无人机的姿态结算中不能用欧拉角公式计算，一方面是因为欧拉角微分方程中包含了大量的三角运算，这给实时结算带来了一定的困难。而且当俯仰角为90度时方程式会出现神奇的“Gimbal Lock”。所以欧拉角方法只适合水平姿态变化不大的情况，而不适用于飞行器的姿态确定。

下面对四元数姿态进行结算：

重力加速度归一化。即将加速度计的三维向量（ax,ay,az）转换为单位向量，因为是单位矢量到参考性的投影，所以要把加速度计数据单位化。归一化只是改变这三个向量的长度，方向不改变，也就是只改变了相同的倍数，只是为了与单位四元数对应。ax,ay,az是机体坐标参照系上，加速度测出来的重力向量。
四元数换成方向余弦中的第三行的三个元素。将当前姿态重力在三个轴的分量分离出来，把四元数换算成方向余弦中的第三行的三个元素(vx,vy,vz)。惯性测量器件测量的都是关于b系的值。vx,vy,vz是陀螺仪的值积分后的姿态来推算出的重力向量。
向量叉积得出姿态误差。将ax,ay,az和vx,vy,vz对应的进行向量叉积（向量外积、叉乘）。分别得出ex,ey,ez。这个叉积向量仍然是位于机体坐标系，并和积分误差成正比，正好矫正陀螺。
对误差进行积分。将ax,ay,az和vx,vy,vz的误差进行积分消除误差。
进行滤波。陀螺仪测的值不断的进行更新，相应的积分误差也不断的修正，最后将积分误差反馈给陀螺仪，修正陀螺仪的值。
四元数微分方程。通过一阶龙格库塔法更新四元数。
四元数归一化。对四元数的单位化，单位化的四元数可以表示一个完整的旋转，只有单位四元数才可以表示旋转，这就是四元数表示旋转的约束条件。
四元数转欧拉角。

程序如下：

float Kp = 0.4f; // 比例增益
float Ki = 0.001f; // 积分增益
float exInt = 0.f; // 重力在X轴上的分量
float eyInt = 0.0f;
float	ezInt = 0.0f;

//四元数
static float q0 = 1.0f;
static float q1 = 0.0f;
static float q2 = 0.0f;
static float q3 = 0.0f;

float Rot_matrix[3][3];
Vector3f angle1;
	
void	IMU_Updata(Vector3f acc,Vector3f gyro,Attitude *state,float dt)
{
	
    float norm;
    float ex,ey,ez;
    float halfT	= dt/2.0f;
    //重力加速度在X,Y,Z方向的分量
    static float verxZ,veryZ,verzZ;
    angle1.x = 0.0f;
    angle1.y = 0.0f;
    float q0q0,q0q1,q0q2,q0q3,q1q1,q1q2,q1q3,q2q2,q2q3,q3q3;
	
    //1、获取原始值
    //角速度转弧速度
    gyro.x = gyro.x	* DEG2RAD;
    gyro.y = gyro.y	* DEG2RAD;
    gyro.z = gyro.z	* DEG2RAD;
	
    //2、加速度计量化
    if((acc.x!=0.0f)||(acc.y!=0.0f)||(acc.z!=0.0f))
    {
	norm =	sqrtf3(acc.x , acc.y , acc.z);
	acc.x = acc.x /norm ;
	acc.y = acc.y /norm ;
	acc.z = acc.z /norm ;
		
		
	//3、叉乘
	ex = (acc.y * verzZ) - (veryZ * acc.z);
	ey = (acc.x * verzZ) - (verxZ * acc.z);
	ez = (acc.x * veryZ) - (verxZ * acc.y);
		
	//4、积分误差滤波
	gyro.x += ex	* Kp + exInt;
	gyro.y += ey	* Kp + eyInt;
	gyro.z += ez	* Kp + ezInt;	
		
}
	
    float q0_last =q0;
    float q1_last =q1;
    float q2_last =q2;
    float q3_last =q3;
	
    //解四元微分方程
    q0 += (-q1_last*gyro.x - q2_last * gyro.y - q3_last *gyro.z)*halfT;
    q1 += (	q0_last*gyro.x + q3_last * gyro.y - q2_last *gyro.z)*halfT;
    q2 += (-q3_last*gyro.x + q0_last * gyro.y + q1_last *gyro.z)*halfT;
    q3 += (	q2_last*gyro.x - q1_last * gyro.y + q0_last *gyro.z)*halfT;

    norm = sqrtf4(q0,q1,q2,q3);
    q0 = q0/norm;
    q1 = q1/norm;
    q2 = q2/norm;
    q3 = q3/norm;
	
	
    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;
	
    verxZ = 2.0f * (q1q3 - q0q2); 
    veryZ = 2.0f * (q2q3 + q0q1);
    verzZ = q0q0 - q1q1 - q2q2 + q3q3;
	
    Rot_matrix[0][0] = q0q0 + q1q1 - q2q2 - q3q3;
    Rot_matrix[0][1] = 2.0f * (q1q2 - q0q3);
    Rot_matrix[0][2] = 2.0f * (q1q3 + q0q2);
    Rot_matrix[1][0] = 2.0f * (q1q2 + q0q3);
    Rot_matrix[1][1] = q0q0 - q1q1 + q2q2 - q3q3;
    Rot_matrix[1][2] = 2.0f * (q2q3 - q0q1);
    Rot_matrix[2][0] = 2.0f * (q1q3 - q0q2);
    Rot_matrix[2][1] = 2.0f * (q2q3 + q0q1);
    Rot_matrix[2][2] = q0q0 - q1q1 - q2q2 + q3q3;
	
    //四元数转换为欧拉角输出
    state->pitch = asin(Rot_matrix[1][2]);
    state->roll	 = atan2(-Rot_matrix[0][2],Rot_matrix[2][2]);
    state->yaw   = atan2(-Rot_matrix[1][0], Rot_matrix[1][1]);
	
}

运算宏定义如下：

#define MM2M 			0.001f
#define DEG2RAD 	0.017453292519943295769236907684886f	//角度转弧度
#define RAD2DEG 	57.295779513082320876798154814105f	//弧度转角度
	
#define sq(a)		((a) * (a))	//平方、
#define	sqrtf4(a,b,c,d)	__sqrtf(sq(a) + sq(b) + sq(c) + sq(d))
#define sqrtf3(a,b,c)	__sqrtf(sq(a) + sq(b) + sq(c))
#define	sqrtf2(a,b)	__sqrtf(sq(a) + sq(b))

在加速度计矫正的基础上+磁力计：
在三维空间中，根据重力加速度，加速度为我们提供一个水平位置的参考，但是无法获得方向的参考，这时就需要磁力计，它能给人们提供一个正北方向的绝对参考。

在上述姿态结算的基础上进行磁力计矫正：

把磁力计的数据进行归一化处理（进行量化）。
根据当前四元数姿态值估算各重力分量vx,vy,vz，再根据vx,vy,vz预估磁场的方向wx,wy,wz。
再根据wx,wy,wz对磁力计测出的值进行误差矫正。
把加速度计和磁力计修正后的陀螺仪数据整合到四元数中。
最后进行角度运算。

float KpDef = 0.5f;
float KiDef	=	0.025f;

float q0 = 1.0f,q1 = 0.0f,q2 = 0.0f,q3 = 0.0f;

float integralFBx = 0.0f,integralFBy = 0.0f,integralFBz = 0.0f;

static float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;

static float Rot_matrix[3][3];	//方向余弦矩阵

//使用加速度传感器测量重力向量，使用磁力计测量方向
void AHRS_Uptate(Vector3f acc,Vector3f gyro,Vector3i mag,Attitude *state,float dt)
{

    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;
//大地坐标系(e)  --> 机体坐标系(b)
    Rot_matrix[0][0] = q0q0 + q1q1 - q2q2 - q3q3;
    Rot_matrix[0][1] = 2.f * (q1q2 + q0q3);
    Rot_matrix[0][2] = 2.f * (q1q3 - q0q2);
    Rot_matrix[1][0] = 2.f * (q1q2 - q0q3);
    Rot_matrix[1][1] = q0q0 - q1q1 + q2q2 - q3q3;
    Rot_matrix[1][2] = 2.f * (q2q3 + q0q1);
    Rot_matrix[2][0] = 2.f * (q1q3 + q0q2);
    Rot_matrix[2][1] = 2.f * (q2q3 - q0q1);
    Rot_matrix[2][2] = q0q0 - q1q1 - q2q2 + q3q3;
	
    float ex = 0.0f, ey = 0.0f, ez = 0.0f;
    float norm;
	
    if((mag.x != 0) || (mag.y != 0) || (mag.z !=0))
    {
	//磁力计量化
	norm = sqrtf3(mag.x,mag.y,mag.z);
	mag.x /= norm;
	mag.y /= norm;
	mag.z /= norm;
		
	//地球磁场的参考方向
	float hx = Rot_matrix[0][0] * mag.x + Rot_matrix[1][0] * mag.y + Rot_matrix[2][0] * mag.z ;//计算出x轴的方向
	float hy = Rot_matrix[0][1] * mag.x + Rot_matrix[1][1] * mag.y + Rot_matrix[2][1]	* mag.z	;//计算出Y轴的方向
	float by = sqrtf2(hx,hy);	//预估磁场的方向
	float bz = Rot_matrix[0][2] * mag.x + Rot_matrix[1][2] * mag.y + Rot_matrix[2][2] * mag.z	;//计算出Z轴方向
		
	//估计磁场的方向
	float wx = Rot_matrix[0][1] *	by + Rot_matrix[0][2] * bz;
	float wy = Rot_matrix[1][1]	* by + Rot_matrix[1][2] * bz;
	float wz = Rot_matrix[2][1]	* by + Rot_matrix[2][2]	*	bz;
		
	//误差是估计方向和场矢量测量方向的乘积
	ex = mag.y * wz - mag.z * wy;
	ey = mag.z * wx - mag.x * wz;
	ez = mag.x * wz - mag.z * wx;
		
	if((acc.x !=0)||(acc.y !=0)||(acc.z !=0))
	{
	 //加速度值量化
	norm = sqrtf3(acc.x,acc.y,acc.z);
	acc.x /= norm;
	acc.y	/= norm;
	acc.z	/= norm;
			
	//根据当前四元数的姿态值估算出各重力分量Vx,Vy,Vz
	float vx = Rot_matrix[0][2];
	float vy = Rot_matrix[1][2];
	float vz = Rot_matrix[2][2];
			
	//使用叉积来计算重力误差
	ex += acc.y * vz - acc.z * vy;
	ey += acc.z * vx - acc.x * vz;
	ez += acc.x * vy - acc.y * vx;
	}
		
        //只有当从加速度计或磁力计收集有效数据时才应用反馈
	if(ex != 0.0f && ey != 0.0f && ez !=0.0f)
	{
	//把上述计算得到的重力和磁力差进行积分运算
	if(KiDef > 0.0f)
	{
	    integralFBx += KiDef * ex * dt;
	    integralFBy += KiDef * ey *	dt;
	    integralFBz += KiDef * ez *	dt;
				
	    gyro.x += integralFBx;
	    gyro.y += integralFBy;
	    gyro.z += integralFBz;
	}
	else
	{
	    integralFBx = 0.0f;
	    integralFBy = 0.0f;
	    integralFBz = 0.0f;
	}
	//把上述计算得到的重力差和磁力差进行比例运算
			
	    gyro.x += KpDef * ex;
	    gyro.y += KpDef * ey;
	    gyro.z += KpDef *	ez;
	}
	float dq0 = 0.5f*(-q1 * gyro.x - q2 * gyro.y - q3 * gyro.z);
	float dq1 = 0.5f*( q0 * gyro.x + q2 * gyro.z - q3 * gyro.y);
	float dq2 = 0.5f*( q0 * gyro.y - q1 * gyro.z + q3 * gyro.x);
	float dq3 = 0.5f*( q0 * gyro.z + q1 * gyro.y - q2 * gyro.x); 
	
	q0 += dt * dq0;
	q1 += dt * dq1;
	q2 += dt * dq2;
	q3 += dt * dq3;
		
	norm = sqrtf4(q0, q1, q2, q3);
	q0 /= norm;
	q1 /= norm;
	q2 /= norm;
	q3 /= norm;		
		
	Rot_matrix[0][0] = q0q0 + q1q1 - q2q2 - q3q3;
	Rot_matrix[0][1] = 2.f * (q1q2 + q0q3);
	Rot_matrix[0][2] = 2.f * (q1q3 - q0q2);
	Rot_matrix[1][0] = 2.f * (q1q2 - q0q3);
	Rot_matrix[1][1] = q0q0 - q1q1 + q2q2 - q3q3;
	Rot_matrix[1][2] = 2.f * (q2q3 + q0q1);
	Rot_matrix[2][0] = 2.f * (q1q3 + q0q2);
	Rot_matrix[2][1] = 2.f * (q2q3 - q0q1);
	Rot_matrix[2][2] = q0q0 - q1q1 - q2q2 + q3q3;
		
	//四元数转换为欧拉角输出
	state->pitch = asin(Rot_matrix[1][2]);
	state->roll	 = atan2(-Rot_matrix[0][2],Rot_matrix[2][2]);
	state->yaw   = atan2(-Rot_matrix[1][0], Rot_matrix[1][1]);	
    }
}

运算宏定义如下：

#define MM2M 		0.001f
#define DEG2RAD 	0.017453292519943295769236907684886f	// 角度转弧度
#define RAD2DEG 	57.295779513082320876798154814105f	// 弧度转角度 
	
#define sq(a)		((a) * (a))	//平方、
#define	sqrtf4(a,b,c,d)	__sqrtf(sq(a) + sq(b) + sq(c) + sq(d))
#define sqrtf3(a,b,c)	__sqrtf(sq(a) + sq(b) + sq(c))
#define	sqrtf2(a,b)	__sqrtf(sq(a) + sq(b))

以上仅为本人的理解，仅供参考，如有细节上的错误希望大神指点指点！