加速計陀螺儀6軸資料融合演算法解析
阿新 • • 發佈：2019-02-04
四旋翼和雲臺都離不開加速計和陀螺儀。加速計提供實時的重力加速度方向，進而可以計算出機體姿態角度。陀螺儀提供實時的旋轉角度，通過積分也能換算成機體姿態角度。加速計對機體振動非常敏感，稍有擾動，加速度方向就會偏移（外力與重力的合力），但是長期執行時，經過平滑濾波演算法之後的加速計資料就比較可信。相比而言，陀螺儀則對外部擾動不敏感，反而是長時間積分後會出現角度漂移。因此實際應用中，都是將二者採集的資料進行融合，相互取長補短。
下面這段程式碼是網上比較常見的6軸融合演算法，原作者只添加了幾句簡單的英文註釋，幾處關鍵段落閱讀起來非常費解。而網上的大多數引用頁面對這段程式碼也都沒有做進一步的分析，我懷疑大家也都是一知半解，拿來就用，用得順手便沒有再去深究。這個週末我仔細研究了一下，對各關鍵公式進行了理論推導，供大家參考。三個關鍵段落的推導過程詳見後文Note1，Note2，Note3。
*Note1void IMUupdate() { float norm; float vx, vy, vz; float ex, ey, ez; float ax,ay,az,gx,gy,gz; float q0,q1,q2,q3; float q0temp,q1temp,q2temp,q3temp; ax=Data.pSensor->accel[0]; ay=Data.pSensor->accel[1]; az=Data.pSensor->accel[2]; gx=Data.pSensor->gyro[0]; gy=Data.pSensor->gyro[1]; gz=Data.pSensor->gyro[2]; q0=Data.q[0]; q1=Data.q[1]; q2=Data.q[2]; q3=Data.q[3]; // normalise the measurements norm = sqrt(ax*ax + ay*ay + az*az); ax = ax / norm; ay = ay / norm; az = az / norm; //*Note1 // 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; //*Note2 // error is sum of cross product between reference direction of field and direction measured by sensor ex = (ay*vz - az*vy); ey = (az*vx - ax*vz); ez = (ax*vy - ay*vx); // integral error scaled integral gain exInt = exInt + ex*Ki; eyInt = eyInt + ey*Ki; ezInt = ezInt + ez*Ki; // adjusted gyroscope measurements， PID gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt; q0temp=q0; q1temp=q1; q2temp=q2; q3temp=q3; //*Note3 // integrate quaternion rate and normalise q0 = q0temp + (-q1temp *gx - q2temp *gy - q3temp *gz)*halfT; q1 = q1temp + (q0temp *gx + q2temp *gz - q3temp *gy)*halfT; q2 = q2temp + (q0temp *gy - q1temp *gz + q3temp *gx)*halfT; q3 = q3temp + (q0temp *gz + q1temp *gy - q2temp *gx)*halfT; // normalise quaternion norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 = q0 * norm; q1 = q1 * norm; q2 = q2 * norm; q3 = q3 * norm; Data.q[0]=q0; Data.q[1]=q1; Data.q[2]=q2; Data.q[3]=q3; }
*Note2
*Note3
