所以在軟件解算中，我們要首先把加速度計采集到的值(三維向量)轉化為單位向量，即向量除以模，傳入參數是陀螺儀x、y、z值和加速度計x、y、z值：
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{ float norm;
float vx, vy, vz; float ex, ey, ez;
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
下面把四元數換算成方向余弦中的第三行的三個元素。剛好vx、vy、vz 。其實就是上一次的歐拉角（四元數）的機體坐標參考系換算出來的重力的單位向量。
estimated direction of gravity vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
axyz是機體坐標參照系上，加速度計測出來的重力向量，也就是實際測出來的重力向量。
axyz是測量得到的重力向量，vxyz是陀螺積分后的姿態來推算出的重力向量，它們都是機體坐標參照系上的重力向量。
那它們之間的誤差向量，就是陀螺積分后的姿態和加計測出來的姿態之間的誤差。
向量間的誤差，可以用向量叉積（也叫向量外積、叉乘）來表示，exyz就是兩個重力向量的叉積。
這個叉積向量仍舊是位于機體坐標系上的，而陀螺積分誤差也是在機體坐標系，而且叉積的大小與陀螺積分誤差成正比，正好拿來糾正陀螺。（你可以自己拿東西想象一下）由于陀螺是對機體直接積分，所以對陀螺的糾正量會直接體現在對機體坐標系的糾正。
integral error scaled integral gain exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
用叉積誤差來做PI修正陀螺零偏
integral error scaled integral gain exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki; // adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
四元數微分方程，其中T為測量周期，為陀螺儀角速度，以下都是已知量，這里使用了一階龍哥庫塔求解四元數微分方程：
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;
最后根據四元數方向余弦陣和歐拉角的轉換關系，把四元數轉換成歐拉角，所以有：
ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
ANGLE.Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
ANGLE.Roll= atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll |
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
