姿態估計(1)——互補濾波(Complementary filter )

注:本文轉載自 博主:Gen_Ye 博客:《Pixhawk-姿態解算-互補濾波    

    

    想要了解用於姿態解算的擴展卡爾曼濾波的可以看這裏姿態估計(2)—— 擴展卡爾曼濾波(Extended Kalman Filter—EKF)

 

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

 

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

 

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

 

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

 

          

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

 

   下面先說點基礎內容,之後再貼源碼:

   下面介紹三部分內容:

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

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

 

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

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

 

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

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

 

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

 

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

 

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

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

 

 

 

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

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

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

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

 

3、四元數姿態解算的步驟:

 

 

 

 

好了到這裏姿態解算的四路已經比較清晰了,最後給大家一個“殺手鐗”:

 

   看到這些之後再結合源碼,想必思路會清晰很多!

發佈了21 篇原創文章 · 獲贊 52 · 訪問量 10萬+
發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章