在正确读出陀螺仪,加速度计和磁力计原始数据的基础上,使用如下的代码可以实现姿态解算
如果使用的是mpu6050的话,将磁力计的传入参数置为0即可,在姿态解算函数内部会自动忽略,不会加入对磁力计的处理
首先定义一个结构体用于存储读取出的陀螺仪,加速度计和磁力计值:
typedef struct
{
float x;
float y;
float z;
}Axis3f
下面是姿态解算的核心代码:
#define DEG2RAD 0.017453293f /* 度转弧度 π/180 */
#define RAD2DEG 57.29578f /* 弧度转度 180/π */
//数值越大越相信加速度计
float A_Kp = 0.4f; /*比例增益*/
float A_Ki = 0.001f; /*积分增益*/
float A_exInt = 0.0f;
float A_eyInt = 0.0f;
float A_ezInt = 0.0f; /*积分误差累计*/
//数值越大越相信磁力计
float M_Kp = 1.0f; /*比例增益*/
float M_Ki = 0.001f; /*积分增益*/
float M_exInt = 0.0f;
float M_eyInt = 0.0f;
float M_ezInt = 0.0f; /*积分误差累计*/
static float q0 = 1.0f; /*四元数*/
static float q1 = 0.0f;
static float q2 = 0.0f;
static float q3 = 0.0f;
static float rMat[3][3];/*旋转矩阵*/
/*快速开平方求倒*/
float invSqrt(float x)
{
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
/*计算旋转矩阵*/
void imuComputeRotationMatrix(void)
{
float q1q1 = q1 * q1;
float q2q2 = q2 * q2;
float q3q3 = q3 * q3;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
float q0q3 = q0 * q3;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q2q3 = q2 * q3;
rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
rMat[0][1] = 2.0f * (q1q2 + -q0q3);
rMat[0][2] = 2.0f * (q1q3 - -q0q2);
rMat[1][0] = 2.0f * (q1q2 - -q0q3);
rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
rMat[1][2] = 2.0f * (q2q3 + -q0q1);
rMat[2][0] = 2.0f * (q1q3 + -q0q2);
rMat[2][1] = 2.0f * (q2q3 - -q0q1);
rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
}
//传入参数分别为加速度计,陀螺仪和磁力计值,Angle用于存放解算后的角度值,dt为调用此函数的时间间隔
void imuUpdate(Axis3f acc, Axis3f gyro, Axis3f mag, Axis3f *Angle , float dt) /*数据融合 互补滤波*/
{
float normalise;
float ex, ey, ez;
float halfT = 0.5f * dt;
float accBuf[3] = {0.f};
Axis3f tempacc = acc;
Axis3f tempmag = {0};
Axis3f tempmag_2 = {0};
gyro.x = gyro.x * DEG2RAD; /* 度转弧度 */
gyro.y = gyro.y * DEG2RAD;
gyro.z = gyro.z * DEG2RAD;
/* 加速度计输出有效时,利用加速度计补偿陀螺仪*/
if((acc.x != 0.0f) || (acc.y != 0.0f) || (acc.z != 0.0f))
{
/*单位化加速计测量值*/
normalise = invSqrt(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
acc.x *= normalise;
acc.y *= normalise;
acc.z *= normalise;
/*加速计读取的方向与重力加速计方向的差值,用向量叉乘计算*/
ex = (acc.y * rMat[2][2] - acc.z * rMat[2][1]);
ey = (acc.z * rMat[2][0] - acc.x * rMat[2][2]);
ez = (acc.x * rMat[2][1] - acc.y * rMat[2][0]);
/*误差累计,与积分常数相乘*/
A_exInt += A_Ki * ex * dt ;
A_eyInt += A_Ki * ey * dt ;
A_ezInt += A_Ki * ez * dt ;
/*用叉积误差来做PI修正陀螺零偏,即抵消陀螺读数中的偏移量*/
gyro.x += A_Kp * ex + A_exInt;
gyro.y += A_Kp * ey + A_eyInt;
gyro.z += A_Kp * ez + A_ezInt;
}
/* 磁力计输出有效时,利用磁力计补偿陀螺仪*/
if((mag.x != 0.0f) || (mag.y != 0.0f) || (mag.z != 0.0f))
{
/*单位化磁力计测量值*/
normalise = invSqrt(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
mag.x *= normalise;
mag.y *= normalise;
mag.z *= normalise;
/*磁力计从机体到地球*/
tempmag.x = rMat[0][0] * mag.x + rMat[0][1] * mag.y + rMat[0][2] * mag.z;
tempmag.y = rMat[1][0] * mag.x + rMat[1][1] * mag.y + rMat[1][2] * mag.z;
tempmag.z = rMat[2][0] * mag.x + rMat[2][1] * mag.y + rMat[2][2] * mag.z;
/*让导航坐标系中X轴指向正北方*/
tempmag.x = sqrt(tempmag.x * tempmag.x + tempmag.y * tempmag.y);
tempmag.y = 0;
/*磁力计从地球到机体*/
tempmag_2.x = rMat[0][0] * tempmag.x + rMat[1][0] * tempmag.y + rMat[2][0] * tempmag.z;
tempmag_2.y = rMat[0][1] * tempmag.x + rMat[1][1] * tempmag.y + rMat[2][1] * tempmag.z;
tempmag_2.z = rMat[0][2] * tempmag.x + rMat[1][2] * tempmag.y + rMat[2][2] * tempmag.z;
/*磁力计转换后的方向与磁力计方向的差值,用向量叉乘计算*/
ex = (mag.y * tempmag_2.z - mag.z * tempmag_2.y);
ey = (mag.z * tempmag_2.x - mag.x * tempmag_2.z);
ez = (mag.x * tempmag_2.y - mag.y * tempmag_2.x);
/*误差累计,与积分常数相乘*/
M_exInt += M_Ki * ex * dt ;
M_eyInt += M_Ki * ey * dt ;
M_ezInt += M_Ki * ez * dt ;
/*用叉积误差来做PI修正陀螺零偏,即抵消陀螺读数中的偏移量*/
gyro.x += M_Kp * ex + M_exInt;
gyro.y += M_Kp * ey + M_eyInt;
gyro.z += M_Kp * ez + M_ezInt;
}
/* 一阶近似算法,四元数运动学方程的离散化形式和积分 */
float q0Last = q0;
float q1Last = q1;
float q2Last = q2;
float q3Last = q3;
q0 += (-q1Last * gyro.x - q2Last * gyro.y - q3Last * gyro.z) * halfT;
q1 += ( q0Last * gyro.x + q2Last * gyro.z - q3Last * gyro.y) * halfT;
q2 += ( q0Last * gyro.y - q1Last * gyro.z + q3Last * gyro.x) * halfT;
q3 += ( q0Last * gyro.z + q1Last * gyro.y - q2Last * gyro.x) * halfT;
/*单位化四元数*/
normalise = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= normalise;
q1 *= normalise;
q2 *= normalise;
q3 *= normalise;
imuComputeRotationMatrix(); /*计算旋转矩阵*/
/*计算roll pitch yaw 欧拉角*/
Angle->x = -asinf(rMat[2][0]) * RAD2DEG;
Angle->y = atan2f(rMat[2][1], rMat[2][2]) * RAD2DEG;
Angle->z = atan2f(rMat[1][0], rMat[0][0]) * RAD2DEG;
}
其它资料:
MPU6050的姿态解算方法有多种,包括硬件方式的DMP解算,软件方式的欧拉角与旋转矩阵解算,软件方式的轴角法与四元数解算。本篇先介绍最易操作的DMP方式。
MPU6050基本功能
3轴陀螺仪
陀螺仪,测量的是绕xyz轴转动的角速度,对角速度积分可以得到角度。
3轴加速度计
加速度计,测量的是xyz方向受到的加速度。在静止时,测量到的是重力加速度,因此当物体倾斜时,根据重力的分力可以粗略的计算角度。在运动时,除了重力加速度,还叠加了由于运动产生的加速度。
DMP简介
DMP就是MPU6050内部的运动引擎,全称Digital Motion Processor,直接输出四元数,可以减轻外围微处理器的工作负担且避免了繁琐的滤波和数据融合。Motion Driver是Invensense针对其运动传感器的软件包,并非全部开源,核心的算法部分是针对ARM处理器和MSP430处理器编译成了静态链接库,适用于MPU6050、MPU6500、MPU9150、MPU9250等传感器。
四元数转欧拉角
四元数可以方便的表示3维空间的旋转,但其概念不太好理解,可以先类比复数,复数表示的其实是2维平面中的旋转。
四元数的基本表示形式为:q0+q1*i+q2*j+q3*k,即1个实部3个虚部,具体细节本篇先不做展开介绍。
四元数虽然方便表示旋转,但其形式不太直观,旋转转换成pitch、roll、yaw的表示形式,方便观察姿态。
转换公式为:
程序表示为:
pitch = asin(-2 * q1 * q3 + 2 * q0* q2)
roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)
yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3)