距離我發電調那一貼,已經經過零零散散的一年時間,初步完成了姿態估計部分,因本人基礎學科較差,大部分時間都是在反覆閱讀理解各種資料,呵呵.俗話說的好,知識在於積累,四軸在於堅持.與大家共勉.
一個穩定的系統,要有三個部分,(1)聽話的執行模塊(2)精確的測量模塊(3)牛X的控制模塊
對於四旋翼來說,電調以及電機是執行模塊,姿態估計,高度估計,位置估計等等這些都是測量模塊,控制就不說了
這裏僅僅概述一下關於姿態估計部分的一些東西
【1】數學平臺
這裏所說的姿態,是用來描述一個剛體的固連座標系和參考座標系之間的角位置關係,有一些數學表示方法,很常見的就是歐拉角,也就是pitch,yaw,roll三個轉角,轉來轉去很容易把人轉糊塗,所以我們還是看一下另外一種
想象一下有兩根筷子,它們的一端相連,並且兩根筷子可以繞這一點旋轉,呈一定角度,比如30度,那麼現要讓他們重合,很簡單,把他們轉到一起就行了,我相信沒有人會先pitch XX度,然後yaw XX度 然後roll XX度,大家都會直接
把它們轉到一起,一次就可以搞定的事情,幹嘛分三步?
然後,更加理想化,固定其中一隻筷子,旋轉另外一隻,那麼很明顯我們需要旋轉30度,而旋轉軸呢? 兩根筷子確定一個平面A,沿它們的焦點做平面A的垂線,這就是旋轉軸. 於是這就是一次旋轉的姿態描述方式,愛爾蘭數學家哈密
頓發明了一種叫做[四元數]的東西可以用來描述這種一次旋轉,關於[四元數]的相關知識,可以看這個
四元數ourdev_643759VIYSS5.pdf(文件大小:94K) (原文件名:Shoemake-quattut.pdf)
我們要解決四旋翼和地球的相對姿態問題,地球太大了,換一個小一點的概念,將四軸放在水平地面上,四軸上固定着一個座標系r,地面上固定着一個座標系R,用四元數Q來描述r和R的角位置關係.於是現在有了基本的數學平臺。
【2】傳感器
再來說說測量的問題
1.陀螺儀,可以測量角速度,具有高動態特性,但是它是一個間接測量器件,它測量的是角度的導數,角速度,顯然我們要將角速度對時間積分才能得到角度
如果這個世界是理想的,美好的,那我們的問題到此就解決了,不過很遺憾,現實是殘酷的,誤差的引入,使得積分出現了問題
假設陀螺儀固定不動,理想角速度值是0dps(degree per second)，但是有一個偏置0.1dps加在上面,於是測量出來是0.1dps,積分一秒之後,得到的角度是0.1度,1分鐘之後是6度,還能忍受,一小時之後是360度,轉了一圈,也就是
說,陀螺儀在短時間內有很大的參考價值。
2.加速度計,可以測量加速度,包括重力加速度,於是在靜止或勻速運動的時候,加速度計僅僅測量的是重力加速度,而重力加速度與剛纔所說的R座標系是固連的,通過這種關係,可以得到加速度計所在平面 與 地面 的角度關係.
但是加速度計若是繞着重力加速度的軸轉動,則測量值不會改變,也就是說無法感知這種水平旋轉。
3.磁傳感器,可以測量磁場,在沒有其他磁場的情況下,僅僅測量的是地球的磁場,而地磁也是和R座標系固連的,通過這種關係,可以得到平面A和地平面的關係
(平面A：和磁場方向垂直的平面)，同樣的，若是沿着磁場方向的軸旋轉，測量值不會改變，無法感知這種旋轉。
綜合考慮，加速度計和磁傳感器都是極易受外部干擾的傳感器，都只能得到2維的角度關係，但是測量值隨時間的變化相對較小，結合加速度計和磁傳感器可以得到3維的角度關係。陀螺儀可以積分得到三維的角度關係，動態
性能好，受外部干擾小，但測量值隨時間變化比較大。可以看出，它們優缺點互補，結合起來纔能有好的效果。
【3】數據融合
現在有了三個傳感器，都能在一定程度上測量角度關係，但是究竟相信誰？
根據剛纔的分析，應該是在短時間內更加相信陀螺儀，隔三差五的問問加速度計和磁傳感器，我角度飄了多少了？
有一點必須非常明確，陀螺儀纔是主角，加速度計和磁傳感器僅僅是跑龍套的
關於數據融合，也有好多方法，這裏採用一種最爲形象，貼切的，簡單易懂的，[互補濾波]，可以先通過這個簡單的單軸融合的例子瞭解，來自CROSSBOW
CROSSBOWourdev_643801SBWOGM.pdf(文件大小:112K) (原文件名:Combine Angular Rate Sensors with
Accelerometers.pdf)
![]()
(原文件名:融合流程(CROSSBOW).JPG)
通過上圖可以看出,陀螺儀在融合後的角度基礎上積分,然後與加速度計測量的角度融合,然後又提供給下一次積分,看起來更像一個反饋系統.
【3】全姿態融合
上面的例子僅僅是單軸的融合，理解了單軸就可以結合四元數，得到3維的姿態融合方法了
![]()
(原文件名:姿態融合.JPG)
圖中的符號解釋：Bg是加速度計測得的值,擴充爲四元數,Bg=[0,Bgx,Bgy,Bgz],上標B代表是在體座標系(Body Frame)下的測量值
Bh是磁傳感器計測得的值,擴充爲四元數,Bh=[0,Bhx,Bhy,Bhz]
Eh是參考座標系下的固有磁場,地磁場,上標E代表是在參考座標系(earth Frame)下的值
Eg是參考座標系下的固有加速度,重力加速度
p,q,r是陀螺儀測得的角速度
[a,b,c,d]是姿態四元數Q
(1)關於用加速度計和磁傳感器求姿態
假設現在已經得到正確的姿態四元數Q,那麼可以利用四元數旋轉將參考座標系和體座標系下的向量互相轉換,將Eh和Eg轉換到體座標系下(BEh=Q×Eh×Q* , BEg=Q×Eg×Q*,BEh,BEg是參考座標系的Eh,Eg由Q旋轉到體座標系下)
如果這個世界是理想的,美好的,那麼Bg=BEg,Bh=BEh,也就是 { Q×Eh×Q* - Bh = 0} , { Q×Eg×Q* - Bg = 0}這兩個方程成立,聯立這兩個方程就可以解得姿態四元數Q,不過很遺憾,現實是殘酷的,由於各種誤差的存在,這個
方程組無解,因此我們只能找到最優解,找最優解的最小方差問題有許多方法可以採用,如[梯度下降][高斯牛頓],圖中是用的高斯牛頓法,由於高斯牛頓法含有矩陣求逆的計算,計算量超大,處理器不是DSP或FPGA的話就算了,關於
[梯度下降]和[高斯牛頓],可以在google,wiki百科搜索.
(2)陀螺儀積分求姿態
這個在網上搜索,陀螺儀,四元數微分方程 等等就可以找到了,秦永元[慣性導航]書上也有詳細的
下圖是一個很老的全姿態互補濾波的流程,不過很巧妙,PDF點擊此處下載 ourdev_643809A8FXXO.pdf(文件大小:339K) (
原文件名:Orientation Tracking for Humans and Robots.pdf)
加速度計和磁傳感器經過高斯牛頓迭代得到姿態誤差速率,陀螺儀直接通過四元數微分方程得到姿態四元數速率,兩個加起來積分得到姿態四元數
![]()
(原文件名:全部流程.JPG)
最後,附上我的測試視頻,手機拍攝,有點模糊
上位機:MFC+OpenGL
傳感器:LSM303,L3G4200D
MCU:TMS320F28335
點擊此處下載 ourdev_643811SFPPGR.rar(文件大小:530K) (原文件名:姿態演示.rar)
一個穩定的系統,要有三個部分,(1)聽話的執行模塊(2)精確的測量模塊(3)牛X的控制模塊
對於四旋翼來說,電調以及電機是執行模塊,姿態估計,高度估計,位置估計等等這些都是測量模塊,控制就不說了
這裏僅僅概述一下關於姿態估計部分的一些東西
【1】數學平臺
這裏所說的姿態,是用來描述一個剛體的固連座標系和參考座標系之間的角位置關係,有一些數學表示方法,很常見的就是歐拉角,也就是pitch,yaw,roll三個轉角,轉來轉去很容易把人轉糊塗,所以我們還是看一下另外一種
想象一下有兩根筷子,它們的一端相連,並且兩根筷子可以繞這一點旋轉,呈一定角度,比如30度,那麼現要讓他們重合,很簡單,把他們轉到一起就行了,我相信沒有人會先pitch XX度,然後yaw XX度 然後roll XX度,大家都會直接
把它們轉到一起,一次就可以搞定的事情,幹嘛分三步?
然後,更加理想化,固定其中一隻筷子,旋轉另外一隻,那麼很明顯我們需要旋轉30度,而旋轉軸呢? 兩根筷子確定一個平面A,沿它們的焦點做平面A的垂線,這就是旋轉軸. 於是這就是一次旋轉的姿態描述方式,愛爾蘭數學家哈密
頓發明了一種叫做[四元數]的東西可以用來描述這種一次旋轉,關於[四元數]的相關知識,可以看這個
四元數ourdev_643759VIYSS5.pdf(文件大小:94K) (原文件名:Shoemake-quattut.pdf)
我們要解決四旋翼和地球的相對姿態問題,地球太大了,換一個小一點的概念,將四軸放在水平地面上,四軸上固定着一個座標系r,地面上固定着一個座標系R,用四元數Q來描述r和R的角位置關係.於是現在有了基本的數學平臺。
【2】傳感器
再來說說測量的問題
1.陀螺儀,可以測量角速度,具有高動態特性,但是它是一個間接測量器件,它測量的是角度的導數,角速度,顯然我們要將角速度對時間積分才能得到角度
如果這個世界是理想的,美好的,那我們的問題到此就解決了,不過很遺憾,現實是殘酷的,誤差的引入,使得積分出現了問題
假設陀螺儀固定不動,理想角速度值是0dps(degree per second)，但是有一個偏置0.1dps加在上面,於是測量出來是0.1dps,積分一秒之後,得到的角度是0.1度,1分鐘之後是6度,還能忍受,一小時之後是360度,轉了一圈,也就是
說,陀螺儀在短時間內有很大的參考價值。
2.加速度計,可以測量加速度,包括重力加速度,於是在靜止或勻速運動的時候,加速度計僅僅測量的是重力加速度,而重力加速度與剛纔所說的R座標系是固連的,通過這種關係,可以得到加速度計所在平面 與 地面 的角度關係.
但是加速度計若是繞着重力加速度的軸轉動,則測量值不會改變,也就是說無法感知這種水平旋轉。
3.磁傳感器,可以測量磁場,在沒有其他磁場的情況下,僅僅測量的是地球的磁場,而地磁也是和R座標系固連的,通過這種關係,可以得到平面A和地平面的關係
(平面A：和磁場方向垂直的平面)，同樣的，若是沿着磁場方向的軸旋轉，測量值不會改變，無法感知這種旋轉。
綜合考慮，加速度計和磁傳感器都是極易受外部干擾的傳感器，都只能得到2維的角度關係，但是測量值隨時間的變化相對較小，結合加速度計和磁傳感器可以得到3維的角度關係。陀螺儀可以積分得到三維的角度關係，動態
性能好，受外部干擾小，但測量值隨時間變化比較大。可以看出，它們優缺點互補，結合起來纔能有好的效果。
【3】數據融合
現在有了三個傳感器，都能在一定程度上測量角度關係，但是究竟相信誰？
根據剛纔的分析，應該是在短時間內更加相信陀螺儀，隔三差五的問問加速度計和磁傳感器，我角度飄了多少了？
有一點必須非常明確，陀螺儀纔是主角，加速度計和磁傳感器僅僅是跑龍套的
關於數據融合，也有好多方法，這裏採用一種最爲形象，貼切的，簡單易懂的，[互補濾波]，可以先通過這個簡單的單軸融合的例子瞭解，來自CROSSBOW
CROSSBOWourdev_643801SBWOGM.pdf(文件大小:112K) (原文件名:Combine Angular Rate Sensors with
Accelerometers.pdf)
(原文件名:融合流程(CROSSBOW).JPG)
通過上圖可以看出,陀螺儀在融合後的角度基礎上積分,然後與加速度計測量的角度融合,然後又提供給下一次積分,看起來更像一個反饋系統.
【3】全姿態融合
上面的例子僅僅是單軸的融合，理解了單軸就可以結合四元數，得到3維的姿態融合方法了
(原文件名:姿態融合.JPG)
圖中的符號解釋：Bg是加速度計測得的值,擴充爲四元數,Bg=[0,Bgx,Bgy,Bgz],上標B代表是在體座標系(Body Frame)下的測量值
Bh是磁傳感器計測得的值,擴充爲四元數,Bh=[0,Bhx,Bhy,Bhz]
Eh是參考座標系下的固有磁場,地磁場,上標E代表是在參考座標系(earth Frame)下的值
Eg是參考座標系下的固有加速度,重力加速度
p,q,r是陀螺儀測得的角速度
[a,b,c,d]是姿態四元數Q
(1)關於用加速度計和磁傳感器求姿態
假設現在已經得到正確的姿態四元數Q,那麼可以利用四元數旋轉將參考座標系和體座標系下的向量互相轉換,將Eh和Eg轉換到體座標系下(BEh=Q×Eh×Q* , BEg=Q×Eg×Q*,BEh,BEg是參考座標系的Eh,Eg由Q旋轉到體座標系下)
如果這個世界是理想的,美好的,那麼Bg=BEg,Bh=BEh,也就是 { Q×Eh×Q* - Bh = 0} , { Q×Eg×Q* - Bg = 0}這兩個方程成立,聯立這兩個方程就可以解得姿態四元數Q,不過很遺憾,現實是殘酷的,由於各種誤差的存在,這個
方程組無解,因此我們只能找到最優解,找最優解的最小方差問題有許多方法可以採用,如[梯度下降][高斯牛頓],圖中是用的高斯牛頓法,由於高斯牛頓法含有矩陣求逆的計算,計算量超大,處理器不是DSP或FPGA的話就算了,關於
[梯度下降]和[高斯牛頓],可以在google,wiki百科搜索.
(2)陀螺儀積分求姿態
這個在網上搜索,陀螺儀,四元數微分方程 等等就可以找到了,秦永元[慣性導航]書上也有詳細的
下圖是一個很老的全姿態互補濾波的流程,不過很巧妙,PDF點擊此處下載 ourdev_643809A8FXXO.pdf(文件大小:339K) (
原文件名:Orientation Tracking for Humans and Robots.pdf)
加速度計和磁傳感器經過高斯牛頓迭代得到姿態誤差速率,陀螺儀直接通過四元數微分方程得到姿態四元數速率,兩個加起來積分得到姿態四元數
(原文件名:全部流程.JPG)
最後,附上我的測試視頻,手機拍攝,有點模糊
上位機:MFC+OpenGL
傳感器:LSM303,L3G4200D
MCU:TMS320F28335
點擊此處下載 ourdev_643811SFPPGR.rar(文件大小:530K) (原文件名:姿態演示.rar)
