機器人：（5）樹莓派自駕儀的開發備忘錄——互補濾波實現姿態解算
姿態解算是自駕儀最基本的內容之一。實現它的演算法非常多，這裡先使用最簡單的互補濾波。
航係為東-北-天，直接從word上截圖下來第一步：我們約定基本的坐標系，旋轉順序X-Y-Z，機頭為Y，右側為X，符合右手坐標系，導航係為東北天。
第二步：對感測器數據進行基本的粗校準（粗校準可以滿足基本的使用要求）
比較精確的測量模型如下：
K表示縮放比例矩陣，b表示偏置誤差，n表示高斯測量雜訊。
在這裡，只實現加速度計和陀螺儀零偏誤差的消除，磁強計粗校準主要消除偏心誤差和橢圓誤差。
具體操作簡單，對加速度計和陀螺儀水平靜置，在一段時間內取平均值，該值就是要消除的偏置誤差（加速度計Z軸要先減去g再做平均）
磁強計校準要將磁強計分別繞X、Y、Z軸進行360°旋轉，取每個軸讀數的最大值和最小值，二者相加除以2，就是零偏誤差，然後選定一個軸為基本軸，將橢球校正為球：
假設已經找到三個軸的最大最小值—— ，
,
,
,
和
則有x軸的零偏誤差: ，x軸的比例：
則有y軸的零偏誤差: ，y軸的比例：
則有z軸的零偏誤差: ，z軸的比例：
校正後，磁強計三個軸輸出為
三個感測器校正完成後，對9個軸的實際輸出數據進行適當的低通濾波，出去雜訊，即可進入下一步.（過分的低通會造成數據的延時）
第三步：互補濾波姿態解算
互補濾波，主要實現以陀螺儀為基本感測器敏感載體三軸運動，輔以加速度計和磁強計來修正漂移誤差，以達到獲得穩定姿態信息的目的。
思路及其簡單，直觀如下圖所示
實際代碼如下：
函數的輸入包括兩次函數執行的間隔dT,經過校正的三軸陀螺儀數據，三軸加速度計數據和三軸磁強計數據。
關於如何得到較為精確的dT，可以參考我的另一個小帖子
基於STM32CUBEMX的程序時間計時器（簡單易用）（STM32L496） - STM32 - 意法半導體STM32/STM8技術社區互補原理參見實際代碼注釋，相信已經非常簡單易懂了
#define DEG_TO_RAD 0.01745329251994329576f
#define RAD_TO_DEG 57.29577951308232087679f
void ins_update(float dT, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
float Kp_acc=15.0f;
float Ki_acc=0.008f;
float Kp_mag=5.0f;
float Ki_mag=0.003f;
static float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float q0_old,q1_old,q2_old,q3_old;
float quaternion_norm;
/**************************重力校正姿態*******************************/
static float acc_exInt = 0, acc_eyInt = 0, acc_ezInt = 0;
float acc_norm;
float acc_vx, acc_vy, acc_vz;
float acc_ex, acc_ey, acc_ez;
acc_norm = sqrt(ax*ax + ay*ay + az*az);
if(acc_norm==0) return; //感測器未讀取數據時，acc_norm為0,此時會出現NAN錯誤
ax = ax / acc_norm;
ay = ay / acc_norm;
az = az / acc_norm;
//重力向量提取
acc_vx = 2*(q1*q3 - q0*q2);
acc_vy = 2*(q0*q1 + q2*q3);
acc_vz = q0*q0 - q1*q1 - q2*q2 + q3*q3 ;
//重量向量與實測重力值叉乘得到誤差
acc_ex = (ay*acc_vz - az*acc_vy) ;
acc_ey = (az*acc_vx - ax*acc_vz) ;
acc_ez = (ax*acc_vy - ay*acc_vx) ;
//誤差積分+比例，校正x和y角速度值
acc_exInt = acc_exInt + acc_ex * Ki_acc;
acc_eyInt = acc_eyInt + acc_ey * Ki_acc;
acc_ezInt = acc_ezInt + acc_ez * Ki_acc;
gx = gx + Kp_acc*acc_ex + acc_exInt;
gy = gy + Kp_acc*acc_ey + acc_eyInt;
// gz = gz + Kp_acc*acc_ez + acc_ezInt; //重力向量只能校正x，y軸的角速度值
/**************************地磁場校正姿態*****************************/
static float mag_exInt = 0, mag_eyInt = 0, mag_ezInt = 0;
float mag_norm;
float mhx,mhy,mhz;
float mag_vx, mag_vy, mag_vz;
float mag_ex, mag_ey, mag_ez;
//將三軸磁強計測量值投影至水平面
mhx = mx*cos(t_roll.euler_rad) + my*sin(t_roll.euler_rad)*sin(t_pitch.euler_rad) + mz*sin(t_roll.euler_rad)*cos(t_pitch.euler_rad);
mhy = my*cos(t_pitch.euler_rad) - mz*sin(t_pitch.euler_rad);
// mhz = -mx*sin(t_roll.euler_rad) + my*cos(t_roll.euler_rad)*sin(t_pitch.euler_rad) + mz*cos(t_roll.euler_rad)*cos(t_pitch.euler_rad);
mag_norm = sqrt(mhx*mhx + mhy*mhy);
mhx = mhx / mag_norm;
mhy = mhy / mag_norm;
mhz = 0;
//磁場向量提取
mag_vx = 2*(q1*q2 + q0*q3);
mag_vy = 1-2*(q1*q1+q3*q3);
mag_vz = 2*(q2*q3 - q0*q1);
//磁場向量與實測磁場叉乘得到誤差
mag_ex = (mhy*mag_vz - mhz*mag_vy) ;
mag_ey = (mhz*mag_vx - mhx*mag_vz) ;
mag_ez = (mhx*mag_vy - mhy*mag_vx) ;
//誤差積分+比例，校正z軸角速度值
mag_exInt = mag_exInt + mag_ex * Ki_mag;
mag_eyInt = mag_eyInt + mag_ey * Ki_mag;
mag_ezInt = mag_ezInt + mag_ez * Ki_mag;
// gx = gx + Kp_mag*mag_ex + mag_exInt;
// gy = gy + Kp_mag*mag_ey + mag_eyInt;
gz = gz + Kp_mag*mag_ez + mag_ezInt;
//四元數迭代
q0_old=q0;
q1_old=q1;
q2_old=q2;
q3_old=q3;
//四元數更新
q0 = q0_old + (-q1_old*gx - q2_old*gy - q3_old*gz) * 0.5f * dT;
q1 = q1_old + (q0_old*gx + q2_old*gz -q3_old*gy) * 0.5f * dT;
q2 = q2_old + (q0_old*gy - q1_old*gz +q3_old*gx) * 0.5f * dT;
q3 = q3_old + (q0_old*gz + q1_old*gy -q2_old*gx) * 0.5f * dT;
//四元數歸一化
quaternion_norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / quaternion_norm;
q1 = q1 / quaternion_norm;
q2 = q2 / quaternion_norm;
q3 = q3 / quaternion_norm;
//輸出結果
t_pitch.euler_rad = atan2(2*(q2*q3-q0*q1),1-2*(q1*q1+q2*q2));
t_roll.euler_rad = -asin(2*(q1*q3+q0*q2));
t_yaw.euler_rad = atan2(2*(q1*q2-q0*q3),1-2*(q2*q2+q3*q3));
//角度單位弧度轉度
t_pitch.euler_deg = t_pitch.euler_rad * RAD_TO_DEG;
t_roll.euler_deg = t_roll.euler_rad * RAD_TO_DEG;
t_yaw.euler_deg = t_yaw.euler_rad * RAD_TO_DEG;
}
推薦閱讀：
