2015-11-16
加計和陀螺儀都能計算出姿態，但為何要對它們融合呢，是因為加速度計對振動之類的擾動很敏感，但長期資料計算出的姿態可信，而陀螺儀雖然對振動這些不敏感，但長期使用陀螺儀會出現漂移，因此我們要進行互補，短期相信陀螺，長期相信加計。不過，其實加計無法對航向角進行修正，修正航向角需要磁力計，也就是下次要總結的9軸資料融合。
在融合之前先要對感測器原始資料進行一些處理。理想情況下，加速度計水平放置時，xy軸應該是0輸出的，僅z軸輸出1個g，因此，我們需要對加速度計進行xy軸的零點校準（注意z軸可不能一起校準去了~）；同樣的，陀螺儀在水平靜止放置時各軸輸出應為0，因此需對陀螺儀進行三軸的校準。方法就是把機體標準水平靜止放置時採集它箇一兩百次資料求個平均作為校準值儲存起來嘍，然後工作狀態下各軸輸出的資料就是採集來的資料減去校準值嘍。僅此還不夠，陀螺儀不進行濾波還可以接受，但加速度計雜訊比較大，所以至少也得來個滑動視窗濾波，我用了20深度的滑動視窗，資料還是有很大波動，不過最後計算出的姿態角只有0.3度左右的抖動（我看大家一般都是建議8深度就夠了，所以單滑動視窗濾波效果是沒法做到更好了，可以考慮加個卡爾曼濾波？還沒研究，不過這樣處理器運算量就上去了）。（11.28補充：後面改對加計使用8深度滑動濾波，陀螺儀進行16階凱撒窗濾波後只有0.1不到的波動，不過凱撒濾波引數的整定還不會，用的別人整定的一組引數，近期繼續研究）。濾波效果如圖：
接下來上六軸資料融合**：（****網路，我在前人的基礎上再加些注釋，稍作改動）
#definekp 10.0f // 這裡的kpki是用於調整加速度計修正陀螺儀的速度#defineki 0.008f
#definehalft 0.001f // 取樣週期的一半，用於求解四元數微分方程時計算角增量
floatq0 = 1, q1 = 0, q2 = 0, q3 = 0; // 初始姿態四元數，由上篇博文提到的變換四元數公式得來
floatexint = 0, eyint = 0, ezint = 0; //當前加計測得的重力加速度在三軸上的分量
//與用當前姿態計算得來的重力在三軸上的分量的誤差的積分
voidimuupdate(float gx, float gy, float gz, float ax, float ay, float az)//g表陀螺儀，a表加計
float q0temp,q1temp,q2temp,q3temp;//四元數暫存變數，求解微分方程時要用
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)//加計處於自由落體狀態時不進行姿態解算，因為會產生分母無窮大的情況
return;
norm = sqrt(ax*ax + ay*ay + az*az);//單位化加速度計，
ax = ax /norm;// 這樣變更了量程也不需要修改kp引數，因為這裡歸一化了
ay = ay / norm;
az = az / norm;
//用當前姿態計算出重力在三個軸上的分量，
//參考座標n系轉化到載體座標b系的用四元數表示的方向余弦矩陣第三列即是（博文一中有提到）
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
//計算測得的重力與計算得重力間的誤差，向量外積可以表示這一誤差
//原因我理解是因為兩個向量是單位向量且sin0等於0
//不過要是夾角是180度呢~這個還沒理解
ex = (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 measurements
gx = gx + kp*ex + exint; //將誤差pi後補償到陀螺儀，即補償零點漂移
gy = gy + kp*ey + eyint;
gz = gz + kp*ez + ezint; //這裡的gz由於沒有觀測者進行矯正會產生漂移，表現出來的就是積分自增或自減
//下面進行姿態的更新，也就是四元數微分方程的求解
q0temp=q0;//暫存當前值用於計算
q1temp=q1;//網上傳的這份演算法大多沒有注意這個問題，在此更正
q2temp=q2;
q3temp=q3;
//採用一階畢卡解法，相關知識可參見《慣性器件與慣性導航系統》p212
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;
//單位化四元數在空間旋轉時不會拉伸，僅有旋轉角度，這類似線性代數裡的正交變換
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
//四元數到尤拉角的轉換，公式推導見博文一
//其中yaw航向角由於加速度計對其沒有修正作用，因此此處直接用陀螺儀積分代替
q_angle.z = gyro_i.z; // yaw
q_angle.y = asin(-2 * q1 * q3 + 2 * q0* q2)*57.3; // pitch
q_angle.x = atan2(2 * q2 * q3 + 2 * q0 * q1,-2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
姿態角解算（MPU6050 加速度計加陀螺儀）
本文持續更新 i2c通訊 ahrs是自動航向基準系統 automatic heading reference system 的簡稱。目前，使用四元數來進行ahrs姿態解算的演算法被廣泛採用於四軸飛行器上。imu部分 imu是慣性測量裝置 inertial measurement unit 的簡稱，通...
三軸加速度計 三軸陀螺儀 三軸磁力計
1.今天要做的事情 三軸加速度計 1.加速度計的原理 三軸加速度 輸出的速度是去掉重力後的整體加速度，因此，三軸加速度計可以測量物體的比力。即，去掉重力後的整體加速度，應用 測量角度 比力 通過彈簧壓縮長度來測量 在沒有外力的作用下，加速度能夠精準地測量俯仰角和滾轉角並且沒有累計誤差 mems 主要...
三軸陀螺儀與加速度計如何輔助 iPhone 定位的
本文 在所有之前解釋一下陀螺儀 根據wiki的定義 陀螺儀是用於測量角度或維持方向的裝置，基於角動量守恆原理。這句話的要點是測量角度或維持方向，這是 iphone 4 為何搭載此類裝置的原因。機械陀螺儀 例如下面這只 中間有一轉盤，用以偵測方向的改變。iphone 4 採用了微型的，電子化的振動陀螺...