四旋翼飞行器教学笔记2.2——姿态读取的计算

最新推荐文章于 2024-12-27 23:30:00 发布

小希0-0

最新推荐文章于 2024-12-27 23:30:00 发布

阅读量1.1k

点赞数 2

CC 4.0 BY-SA版权

分类专栏：四旋翼飞行器 c语言文章标签：四旋翼四元数四元数姿态估计

本文链接：https://blog.youkuaiyun.com/jerryandtony/article/details/77931033

c语言同时被 2 个专栏收录

13 篇文章

订阅专栏

四旋翼飞行器

4 篇文章

订阅专栏

本文介绍了一种从加速度计和陀螺仪数据中获取飞行器姿态的方法，通过计算四元数并进一步得到欧拉角来实现。文中提供了一个详细的C语言函数示例，展示了如何整合传感器数据并应用误差校正。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

在能够成功读取并且已经对读取到的数据进行误差处理后，就要对得到的数据进行处理，进行计算，小希是通过得到四元数，计算出欧拉角来进行读取飞行器姿态的。
具体的公式小希就不去搜了，po在这上面了，网上有很多，大家随便搜一下就知道了。
小希就直接po源码了。
通过三个加速度和三个角速度先计算出四元数，再得到欧拉角！
void IMU_Get_Attitude(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
float norm;
float vx, vy, vz;
float wx, wy, wz;
float ex, ey, ez;
float hx, hy, hz, bx, bz;
float q0_temp, q1_temp ,q2_temp, q3_temp;
float t11, t12, t13, t23, t33;

float q0q0 = q0*q0;
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;

//  ax *= ACC_G; //*ACC_G?
//  ay *= ACC_G;
//  az *= ACC_G;
gx *= GYRO_GR;
gy *= GYRO_GR;
gz *= GYRO_GR;

norm = invSqrt(ax*ax + ay*ay + az*az);
ax = ax*norm;
ay = ay*norm;
az = az*norm;

norm = invSqrt(mx*mx + my*my + mz*mz);          
mx = mx*norm;
my = my*norm;
mz = mz*norm;

hx = 2*mx*(0.5f - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5f - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5f - q1q1 - q2q2);         
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;

vx = 2*(q1q3 - q0q2);                                                           
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;

wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2); 

// error is sum of cross product between reference direction of fields and direction measured by sensors
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);

if((ex != 0.0f )&& (ey != 0.0f) && (ez != 0.0f))
{
    exInt = exInt + ex*kI_IMU*deltaT;                                                       
    eyInt = eyInt + ey*kI_IMU*deltaT;
    ezInt = ezInt + ez*kI_IMU*deltaT;

//  exInt = LIMIT(exInt, -IMU_INT_LIMIT, IMU_INT_LIMIT);
//  eyInt = LIMIT(eyInt, -IMU_INT_LIMIT, IMU_INT_LIMIT);
//  ezInt = LIMIT(ezInt, -IMU_INT_LIMIT, IMU_INT_LIMIT);

    // adjusted gyroscope measurements
    gx = gx + kP_IMU*ex + exInt;                                                        
    gy = gy + kP_IMU*ey + eyInt;
    gz = gz + kP_IMU*ez + ezInt;    // kP = 10?
}   

// integrate quaternion rate and normalise                  
q0_temp = q0 +(-q1*gx - q2*gy - q3*gz)*halfT;
q1_temp = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2_temp = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3_temp = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

// normalise quaternion
norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0_temp*norm;
q1 = q1_temp*norm;
q2 = q2_temp*norm;
q3 = q3_temp*norm;

t11 = q0*q0 + q1*q1 - q2*q2 - q3*q3;
t12 = 2*(q1*q2 + q0*q3);
t13 = 2*(q1*q3 - q0*q2);
t23 = 2*(q2*q3 + q0*q1);
t33 = q0*q0 + q3*q3 - q1*q1 - q2*q2;

Roll = fast_atan2(t23,t33)*57.3 - rol_offset;
Pitch = asin(t13)*57.3 - pit_offset;  
Yaw = -fast_atan2(t12, t11)*57.3 - yaw_offset; // yaw

}