体感9D姿态角计算

来源：互联网发布：英语口语交流软件app 编辑：程序博客网时间：2024/06/05 17:56

最近有做一款体感的可穿戴设备，所以呢，有涉及到一些9D姿态角度计算，当然此时就涉及到一些算法，之前没有接触过四轴什么的，只是知道江湖传闻：“很难”，但是实际从应用角度---还好，现在就说说关于姿态角度思路：

主要就三个部分：采集原始数据，欧拉角计算，磁力计补偿。。。

一、采集数据

用的MPU9250这款传感器，采集的原始数据包括：加速度、角速度、磁力计数据。这个可以用I2C和SPI两种方式通讯采集，具体的配置与采集详见我的博文：http://blog.csdn.net/black_yu/article/details/51815049

二、欧拉角计算

很多大侠老是卡在这里，其实我觉得没啥必要去研究四元数算法有多么精妙，直接用老外madgwick的函数就行了：

unsigned char BS004_IMU_Update(float ax,float ay,float az,float gx,float gy,float gz)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
  //
//四元数乘法运算
float q0q0 = q0 * q0;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
float q1q1 = q1 * q1;
float q1q3 = q1 * q3;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q3q3 = q3 * q3;
//
//归一化处理
norm = sqrt(ax*ax + ay*ay + az*az);
if(norm==0) return 0;
ax = ax / norm;
ay = ay / norm;
az = az / norm;
  //
//建立坐标系
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
//
//坐标系和重力叉积运算
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);
//
//比例运算
exInt = exInt + ex*bs004_quad_Ki;
eyInt = eyInt + ey*bs004_quad_Ki;
ezInt = ezInt + ez*bs004_quad_Ki;
//
//陀螺仪融合
gx = gx + bs004_quad_Kp*ex + exInt;
gy = gy + bs004_quad_Kp*ey + eyInt;
gz = gz + bs004_quad_Kp*ez + ezInt;
//
//整合四元数率
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*bs004_quad_halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*bs004_quad_halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*bs004_quad_halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*bs004_quad_halfT;
//
//归一化处理
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
if(norm==0) return 0;
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
//
//欧拉角转换
bs004_imu_roll=asin(-2*q1q3 + 2*q0q2)*57.30f;
  bs004_imu_pitch=atan2(2*q2q3 + 2*q0q1, -2*q1q1-2*q2q2 + 1)*57.30f;
  bs004_imu_yaw=bs004_imu_yaw-gz*bs004_mpu6050_gyro_scale;
//
return 1;
}

当然还是要调试的，用串口调试会比较方便，然后查看最终数据：bs004_imu_roll,bs004_imu_pitch, bs004_imu_yaw是否正常，如果计算有问题，基本上是采集的数据有问题，请移步到第一步采集上瞧瞧。。。

三、磁力计补偿

如果以上的BS004_IMU_Update 函数没有添加以下代码，也能计算出欧拉角，但是大侠们调试的时候就会发现，偏航角会漂移。。。然后就会想到我们的磁力计补偿了，其实上面的代码已经补偿过，只要保证原始磁力计数据是对的就行啦

//比例运算
exInt = exInt + ex*bs004_quad_Ki;
eyInt = eyInt + ey*bs004_quad_Ki;
ezInt = ezInt + ez*bs004_quad_Ki;
//
//陀螺仪融合
gx = gx + bs004_quad_Kp*ex + exInt;
gy = gy + bs004_quad_Kp*ey + eyInt;
gz = gz + bs004_quad_Kp*ez + ezInt;

啊啊啊。。。。。。。。可以啦，基本就已经得出空间姿态啊。。。。。。啊啊啊！！！

0 0