獲取陀螺儀和加速度計(jì)原始數(shù)據(jù)

short gryox,gyroy,gyroz; short accx,accy,accz; void IMU_GetData(void) {MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);MPU_Get_Accelerometer(&accx,&accy,&accz);}

定義變量

#define RtA 57.295779f //弧度->角度 #define AtR 0.0174533f //角度->弧度 #define Acc_G 0.0011963f #define Gyro_G 0.0610351f #define Gyro_Gr 0.0010653f #define Kp 18.0f #define Ki 0.008f #define halfT 0.008f float q0 = 1, q1 = 0, q2 = 0, q3 = 0; float exInt = 0, eyInt = 0, ezInt = 0; float Angle[3]={0};//最終角度

函數(shù)聲明

/************************* *函數(shù)名：IMU_Update *輸入：陀螺儀，加速度計(jì)原始數(shù)據(jù)（short類型) *************************/ void IMU_Update(short gyrox,short gyroy,short gyroz,short accx,short accy,short accz);

函數(shù)實(shí)現(xiàn)

void IMU_Update(short gyrox,short gyroy,short gyroz,short accx,short accy,short accz) {float ax=accx,ay=accy,az=accz;float gx=gyrox,gy=gyroy,gz=gyroz;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;if(ax*ay*az==0)//此時(shí)任意方向角加速度為0return; gx *= Gyro_Gr;gy *= Gyro_Gr;gz *= Gyro_Gr;norm = sqrt(ax*ax + ay*ay + az*az); ax = ax /norm;ay = ay / norm;az = az / norm;// estimated direction of gravity and flux (v and w) vx = 2*(q1q3 - q0q2); vy = 2*(q0q1 + q2q3);vz = q0q0 - q1q1 - q2q2 + q3q3 ;// error is sum of cross product between reference direction of fields and direction measured by sensorsex = (ay*vz - az*vy) ; ey = (az*vx - ax*vz) ;ez = (ax*vy - ay*vx) ;exInt = exInt + ex * Ki; eyInt = eyInt + ey * Ki;ezInt = ezInt + ez * Ki;// adjusted gyroscope measurementsgx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt;gz = gz + Kp*ez + ezInt; // integrate quaternion rate and normalise 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;// normalise quaternionnorm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);q0 = q0 / norm;q1 = q1 / norm;q2 = q2 / norm;q3 = q3 / norm;// angle->yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* RtA; // yawANgle[0] = asin(-2 * q1 * q3 + 2 * q0* q2)* RtA ; //pitchANgle[1] = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* RtA; //roll}

IMU.h

//IMU.h #ifndef _IMU_H #define _IMU_H /************************* *函數(shù)名：IMU_Update *輸入：陀螺儀，加速度計(jì)原始數(shù)據(jù)（short類型) *************************/ void IMU_Update(short gyrox,short gyroy,short gyroz,short accx,short accy,short accz);#endif

IMU.c

//IMU.c #include "IMU.h" short gryox,gyroy,gyroz; short accx,accy,accz; void IMU_GetData(void) {MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);MPU_Get_Accelerometer(&accx,&accy,&accz);}#define RtA 57.295779f //弧度->角度 #define AtR 0.0174533f //角度->弧度 #define Acc_G 0.0011963f #define Gyro_G 0.0610351f #define Gyro_Gr 0.0010653f #define Kp 18.0f #define Ki 0.008f #define halfT 0.008f float q0 = 1, q1 = 0, q2 = 0, q3 = 0; float exInt = 0, eyInt = 0, ezInt = 0; float Angle[3]={0};//最終角度void IMU_Update(short gyrox,short gyroy,short gyroz,short accx,short accy,short accz) {float ax=accx,ay=accy,az=accz;float gx=gyrox,gy=gyroy,gz=gyroz;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;if(ax*ay*az==0)//此時(shí)任意方向角加速度為0return; gx *= Gyro_Gr;gy *= Gyro_Gr;gz *= Gyro_Gr;norm = sqrt(ax*ax + ay*ay + az*az); ax = ax /norm;ay = ay / norm;az = az / norm;// estimated direction of gravity and flux (v and w) vx = 2*(q1q3 - q0q2); vy = 2*(q0q1 + q2q3);vz = q0q0 - q1q1 - q2q2 + q3q3 ;// error is sum of cross product between reference direction of fields and direction measured by sensorsex = (ay*vz - az*vy) ; ey = (az*vx - ax*vz) ;ez = (ax*vy - ay*vx) ;exInt = exInt + ex * Ki; eyInt = eyInt + ey * Ki;ezInt = ezInt + ez * Ki;// adjusted gyroscope measurementsgx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt;gz = gz + Kp*ez + ezInt; // integrate quaternion rate and normalise 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;// normalise quaternionnorm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);q0 = q0 / norm;q1 = q1 / norm;q2 = q2 / norm;q3 = q3 / norm;// angle->yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* RtA; // yawANgle[0] = asin(-2 * q1 * q3 + 2 * q0* q2)* RtA ; //pitchANgle[1] = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* RtA; //roll}

總結(jié)

以上是生活随笔為你收集整理的四元数姿态解算详细步骤的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。