1、無人機-姿態(tài)解析算法詳解電 子 技 術(shù)02姿態(tài)解析算法詳解姿態(tài)解算中的主角姿態(tài)解算中的傳感器的特性：加速度：使用三角公式得到絕對角度。 “動態(tài)不穩(wěn)定，靜態(tài)穩(wěn)定”角速度：通過角速度積分累加得到角度。 “動態(tài)穩(wěn)定，長時間不穩(wěn)定”磁力計：用于計算方位角即航向。 “易受磁干擾，需要傾角補償” 在現(xiàn)有較成熟的姿態(tài)解算算法中，主角還是角速度和加速度，磁力計只是起矯正糾正作用。姿態(tài)融合 在大部分飛控傳感器中通常使用的是成本低廉mems傳感器，這種傳感器的陀螺儀和加速度計的噪聲相對來說很大，以常見的陀螺儀為例進行積分1分鐘會漂移2度左右。在這種前提下，如果沒有磁場和重力來修正三軸陀螺儀的話，那么基本上3分鐘以

2、后物體的實際姿態(tài)和測量輸出姿態(tài)就完全變樣了。所以我們要了解常用的一些數(shù)據(jù)融合方法，來獲得無人機的姿態(tài)。慣性傳感器姿態(tài)融合1. 互補濾波 最簡單濾波 2. Mahony濾波 有反饋的互補濾波 3. 擴展卡爾曼濾波 復(fù)雜濾波、計算量龐大無人機中的坐標(biāo)系“地理”坐標(biāo)系地球上的“東北天（ENU）”坐標(biāo)系“載體”坐標(biāo)系四旋翼自身的坐標(biāo)系“場地”坐標(biāo)系 飛行場地的坐標(biāo)系一階互補濾波 我們已經(jīng)知道，MPU6500可以獲得加速度和角速度數(shù)據(jù)，但是由于傳感器的噪聲信號比較大，無法直接通過兩個數(shù)據(jù)來獲得該軸上的角度。因此，需要進行互補濾波來近似得到一個比較準(zhǔn)確的角度。 互補，即高頻信號與低頻信號都與我們將求得的量

3、相關(guān)，我們將其進行濾波運算，給兩部分一個權(quán)重得到一個相對準(zhǔn)確的值，或者得到一個很好的近似的過程。公式： Angle = a*(Angle + Gyro*dt) + (1-a)*AccAngle 簡單互補濾波要求 : 平放開始計算（盡量與地理坐標(biāo)系重合）。優(yōu)點： 算法簡單，適用于要求實時精度較低的場合，可使用二階互補濾波進行優(yōu)化。缺點： 并沒有把傳感器的噪聲和系統(tǒng)的噪聲考慮在內(nèi)。且互補濾波系數(shù)不能隨著實際的觀測數(shù)據(jù)做調(diào)整（沒有反饋和修正）。例程見：Complementary filtering_AHRS無人機姿態(tài)的表示方法 在姿態(tài)解算過程中，到底用什么表示無人機的姿態(tài)呢？姿態(tài)表示的方法有很多種，

4、比如歐拉角、四元數(shù)、DCM，各有的各的優(yōu)勢。歐拉角和四元數(shù) 歐拉角是用來唯一地確定定點轉(zhuǎn)動明體位置的三個一組獨立角參量，由章動角、進動角和自轉(zhuǎn)角組成。在航姿中我們把它們稱作俯仰角、翻滾角和偏航角。機體坐標(biāo)系與地理坐標(biāo)系之間的夾角就是飛機的姿態(tài)角，又稱歐拉角。 四元數(shù)在航姿中的意義，一個向量（x,y,z）繞角度旋轉(zhuǎn)。 特點：模等于1DCM方向余弦矩陣DCM方向余弦矩陣的表示方式方向余弦矩陣的作用Mahony互補濾波核心思想： 對于一個確定的向量，用不同的坐標(biāo)系表示時，它們所表示的大小和方向一定是相同的。但是由于這兩個坐標(biāo)系的旋轉(zhuǎn)矩陣存在誤差，那么當(dāng)一個向量經(jīng)過這么一個有誤差的旋轉(zhuǎn)矩陣后，在另一個

5、坐標(biāo)系中肯定和理論值是有偏差的，為我們通過這個偏差來修正這個旋轉(zhuǎn)矩陣，矩陣中的元素就可以表示姿態(tài)，姿態(tài)也就被修改了。核心步驟：加速度數(shù)據(jù)歸一化。將加速度向量(0,0,1)從地理坐標(biāo)系轉(zhuǎn)換到載體坐標(biāo)系得到預(yù)測向量。將預(yù)測加速度向量與實際加速度向量做叉積得到誤差（偏差角度的余弦）。將誤差做PI調(diào)節(jié)，補償疊加到原始角速度數(shù)據(jù)向量上。通過四元數(shù)微分方程，將補償過的角速度向量轉(zhuǎn)換為四元數(shù)向量。將四元數(shù)轉(zhuǎn)換為歐拉角。新的四元數(shù)重新代入方向余弦矩陣，生產(chǎn)新的方向余弦矩陣，用于數(shù)據(jù)從地理坐標(biāo)系到載體坐標(biāo)系的轉(zhuǎn)換。直到誤差為0，方向余弦矩陣正確，姿態(tài)數(shù)據(jù)正確。DCM方向余弦矩陣表示方法選擇 旋轉(zhuǎn)矩陣即方向余弦矩

6、陣的表示方式有3種，但要用于姿態(tài)解算，我們需要對比這個方法的優(yōu)缺點。 歐拉角法存在奇點（萬向節(jié)死鎖），不能全姿態(tài)解算；方向余弦可用于全姿態(tài)解算但是計算量大；四元數(shù)法，計算量較小，無奇點可滿足無人機運動過程中的姿態(tài)實時解算。Mahony濾波詳解Mahony濾波詳解計算估算的載體加速度向量v 與實際的加速度向量a的叉積（向量積）。 向量積的定義： a b = |a|b|*sin 在這里表示兩向量之間的夾角(共起點的前提下)（0 180） 在坐標(biāo)中可表示為：ab=(aybz -azby)i+(azbx -axbz)j+(axby - aybx)k 所以將a向量與b向量做叉積的公式如下，得到e向量（誤

7、差向量） ex = (ay * vz - az * vy); ey = (az * vx - ax * vz); ez = (ax * vy - ay * vx);解析： 當(dāng)方向余弦矩陣表示正確姿態(tài)時，向量v應(yīng)該與向量a重合，即e向量（誤差）的模為0，但是一開始方向余弦矩陣作為初始矩陣需要不停被修正，這里向量積的意義就是通過計算誤差來修正矩陣。 Mahony濾波詳解對誤差向量e做PI運算，將輸出累加到載體坐標(biāo)系下的角速度數(shù)據(jù)上，有反饋的對角速度數(shù)據(jù)進行實時的糾正補償。 integralFBx += Ki * ex * (1.0f / twosampleFreq);integralFBy +=

8、Ki * ey * (1.0f / twosampleFreq);integralFBz += Ki * ez * (1.0f / twosampleFreq);gx += integralFBx;gy += integralFBy;gz += integralFBz;gx += Kp * ex;gy += Kp * ey;gz += Kp * ez;注： twosampleFreq為采樣率，積分項需要乘以相應(yīng)的積分時間。Kp參數(shù)和Ki參數(shù)需要整定，Kp過大數(shù)據(jù)會有震蕩，Ki過大也會有震蕩，過小導(dǎo)致消除靜態(tài)誤差的時間很長。Mahony濾波詳解通過四元數(shù)微分方程（角速度與四元數(shù)的關(guān)系），將補償修

9、正過的角速度數(shù)據(jù)（g向量）轉(zhuǎn)換成四元數(shù)，并將四元數(shù)歸一化。 tempq0 = q0 + (-q1*gx - q2*gy - q3*gz)*(1.0f / twosampleFreq); tempq1 = q1 + (q0*gx + q2*gz - q3*gy)*(1.0f / twosampleFreq); tempq2 = q2 + (q0*gy - q1*gz + q3*gx)*(1.0f / twosampleFreq); tempq3 = q3 + (q0*gz + q1*gy - q2*gx)*(1.0f / twosampleFreq); recipNorm = invSqrt(t

10、empq0*tempq0 + tempq1*tempq1 + tempq2*tempq2 + tempq3*tempq3);q0 = tempq0 * recipNorm;q1 = tempq1 * recipNorm;q2 = tempq2 * recipNorm;q3 = tempq3 * recipNorm;Mahony濾波詳解通過修正補償過后的角速度求得的新四元數(shù)，將帶入方向余弦矩陣，成為一個修正過的新矩陣，即表示無人機的新姿態(tài)。再次得到預(yù)測的加速度向量v與原始加速度向量a做叉積，不停的循環(huán)糾正方向余弦矩陣。將四元數(shù)轉(zhuǎn)換為直觀的歐拉角表示無人機的航姿，公式如下：磁力計修正方向余弦矩陣左

11、側(cè)的圖為加速度修正姿態(tài)，將平面1與平面2重合即修正完畢。而同一平面上，無法用加速度計對航向進行修正，靜止?fàn)顟B(tài)下不同航向的3軸數(shù)據(jù)相同。而右側(cè)的圖則通過正北來修正同一平面上的航向漂移，以此來獲得準(zhǔn)確的航向 磁力計修正方向余弦矩陣1. 將磁力計的數(shù)據(jù)歸一化，變成單位向量mrecipNorm = invSqrt(mx * mx + my * my + mz * mz);mx *= recipNorm;my *= recipNorm;mz *= recipNorm;思考： 加速度計在靜止時，測量的是重力加速度（0,0,1），有大小和方向。同理，地磁計同樣測量的是地球磁場的大小和方向，只不過不再是豎直向

12、下的了。我們把磁場數(shù)據(jù)記做 (bx,by,bz)，將x軸對準(zhǔn)正北，by=0，即 (bx,0,bz)但實際上，當(dāng)?shù)卮艌鱿鄬|北天的夾角是無法測量的，那我們應(yīng)該怎么去做磁力計的補償呢？磁力計修正方向余弦矩陣磁力計修正方向余弦矩陣磁力計修正方向余弦矩陣6. 對誤差向量e做PI運算，將輸出累加到載體坐標(biāo)系下的角速度數(shù)據(jù)上，有反饋的對角速度數(shù)據(jù)進行實時的糾正補償。 integralFBx += Ki * ex * (1.0f / twosampleFreq);integralFBy += Ki * ey * (1.0f / twosampleFreq);integralFBz += Ki * ez *

13、(1.0f / twosampleFreq);gx += integralFBx;gy += integralFBy;gz += integralFBz;gx += Kp * ex;gy += Kp * ey;gz += Kp * ez;注： twosampleFreq為采樣率，積分項需要乘以相應(yīng)的積分時間。Kp參數(shù)和Ki參數(shù)需要整定，Kp過大數(shù)據(jù)會有震蕩，Ki過大也會有震蕩，過小導(dǎo)致消除靜態(tài)誤差的時間很長。磁力計修正方向余弦矩陣7. 通過四元數(shù)微分方程（角速度與四元數(shù)的關(guān)系），將補償修正過的角速度數(shù)據(jù)（g向量）轉(zhuǎn)換成四元數(shù)，并將四元數(shù)歸一化。 tempq0 = q0 + (-q1*gx -

14、q2*gy - q3*gz)*(1.0f / twosampleFreq); tempq1 = q1 + (q0*gx + q2*gz - q3*gy)*(1.0f / twosampleFreq); tempq2 = q2 + (q0*gy - q1*gz + q3*gx)*(1.0f / twosampleFreq); tempq3 = q3 + (q0*gz + q1*gy - q2*gx)*(1.0f / twosampleFreq); recipNorm = invSqrt(tempq0*tempq0 + tempq1*tempq1 + tempq2*tempq2 + tempq3*tempq3);q0 = tempq0 * recipNorm;q1 = tempq1 * recipNorm;q2 = tempq2 * recipNorm;q3 =