Pixhawk-姿態解算-互補濾波

阿新 • • 發佈：2019-01-11

根深方能葉茂在等待的日子裡，刻苦讀書，謙卑做人，養得深根，日後才能枝葉茂盛。 --Better(根爺)

終於說到了正題，姿態解算這一部分很重要，主要的基礎就是慣性導航和多感測器資料融合，很多公司都在招這方面的人才，如百度的無人駕駛在招感測器資料融合，網易的人機互動工程師也在找這方面的人，因為它是資訊流的源泉，準確的姿態資訊需要靠他們解算出來才能進行後續的步驟。

鑑於加速度計低頻特性比較好，因為加速度的角度可以直接算出來，沒有累積誤差，所以長時間後也比較準。而陀螺儀長時間後由於積分誤差的累加，會造成輸出誤差比較大，甚至無法使用。所以用互補濾波法根據他們的特性取長補短進行姿態解算，每過一段時間就讓加速度計去校準一下陀螺儀。互補濾波就是在短時間內採用陀螺儀得到的角度做為最優值，定時對加速度取樣來的加速度值進行取平均值來校正陀螺儀的得到的角度。短時間內用陀螺儀比較準確，以它為主；長時間用加速度計比較準確，這時候加大它的比重，這就是互補了，不過加速度計要濾掉高頻訊號，陀螺儀要濾掉低頻訊號，互補濾波器就是根據感測器特性不同，通過不同的濾波器（高通或低通，互補的），然後再相加得到整個頻帶的訊號。

互補是給他們不同的權重加權求和。

當然這裡面還有一些問題：如加速度無法區分慣性加速度和運動加速度，在固定翼上這個問題更為顯著，再者磁力計準確的偏角怎麼得到？

下面融合的一些框架圖，先建立一個整體的概念：

下面這幅圖才是準確的闡述了互補濾波的過程。正常情況下用陀螺儀的資料就可以進行姿態的更新，但是由於陀螺儀的積分誤差，這裡用acc和mag去校正，求出他們的誤差用PI去彌補。注意看看pid的公式和作用，pid是作用於誤差（實際個期望之間的差值），最終反覆調節，讓實際值=期望值。

下面先說點基礎內容，之後再貼原始碼：

下面介紹三部分內容：

1、姿態的表示方法，在原始碼之中姿態的表示方法有DCM、四元數，尤拉角。尤拉角法在求解姿態時存在奇點（萬向節死鎖），不能用於全姿態的解算；方向餘弦可用於全姿態的解算但計算量大，不能滿足實時性要求。四元數法，其計算量小，無奇點且可以滿足飛行器運動過程中姿態的實時解算。

2、闡述一下姿態解算的原理。

姿態就是指飛行器的俯仰/橫滾/航向情況。在咱們地球上，就是指飛行器在地球座標系中的俯仰/橫滾/航向情況。飛行器需要實時知道當前自己的姿態，才能夠根據需要操控其接下來的動作，例如保持平穩，例如實現翻滾。
姿態是用來描述一個剛體的固連座標系和參考座標系之間的角位置關係，有一些數學表示方法。很常見的就是尤拉角，四元數，矩陣，軸角。

地球座標系又叫做地理座標系，是固定不變的。正北，正東，正向上構成了這個座標系的X，Y，Z軸，我們用座標系R表示。四軸飛行器上固定著一個座標系，我們一般稱之為機體座標系，用座標系r表示。那麼我們就可以用尤拉角，四元數等來描述r和R的角位置關係。這就是四軸飛行器姿態解算的數學模型和基礎。

尤拉角的姿態表示方法最為直觀，可以看做飛機繞固定軸的三次旋轉達到現在的姿態。

分解之後就是每次的旋轉：

方向餘弦矩陣是一個3*3階的矩陣，矩陣的列表示載體座標系中的單位向量在參考座標系中的投影。

這是一個總的旋轉的表達，分解為三次旋轉，可以理解為R=R3*R2*R1。

四元數姿態表示式是一個四引數的表示式。它的基本思路是：一個座標系到另一個座標系的變換可以通過繞一個定義在參考系中的向量的單次轉動來實現。四元數用符號q表示，它是一個具有4個元素的向量，這些元素是該向量方向和轉動大小的函式。定義的大小和方向是使參考系繞轉動一個角度，就能與載體座標系重合。

他們三者都可以表示姿態，求出一個就相當於知道其他的了。具體用什麼形式表達，可以按照你的要求自己去換算，他們的之間的關係如下：

2、下面對姿態解算的原理進行闡述。

姿態解算常用的演算法有尤拉角法、方向餘弦法和四元數法。尤拉角法在求解姿態時存在奇點（萬向節死鎖），不能用於全姿態的解算；方向餘弦可用於全姿態的解算但計算量大，不能滿足實時性要求。四元數法，其計算量小，無奇點且可以滿足飛行器運動過程中姿態的實時解算。

姿態解算的原理：對於一個確定的向量，用不同的座標系表示時，他們所表示的大小和方向一定是相同的。但是由於這兩個座標系的旋轉矩陣存在誤差，那麼當一個向量經過這麼一個有誤差存在的旋轉矩陣後，在另一個座標系中肯定和理論值是有偏差的，我們通過這個偏差來修正這個旋轉矩陣。這個旋轉矩陣的元素是四元數，我們修正的就是四元數，這樣姿態就被修正了。

陀螺儀動態響應特性良好，但計算姿態時會產生累積誤差。磁力計和加速度計測量姿態沒有累積誤差，但動態響應較差。因此他們在頻域上特性互補，所以採用互補濾波器融合這三種感測器的資料，提高測量精度和系統的動態效能。