卡曼濾波器做姿態估測
用陀螺儀+加速規做姿態估測。左上是加速規輸出,左下是陀螺儀輸出。右下是遙控器輸入資料。資料傳輸是用900MHz無線傳輸。<--沒錯,是無線傳輸,因為要放在飛機上的。其實本身陀螺儀很穩定,姿態可以撐個30秒不飄掉,有點出乎我預料之外。中間有一段晃動,Y軸方向加速規受到約2G左右的力。不過姿態幾乎不受影響,反而是我手晃時造成姿態轉動影響比較大。如果放在桌面水平晃動,姿態變化大概在正負1、2度左右。裡面使用動態方差調整型卡曼濾波,效果還可以。整個姿態估測+控制在800us搞定,控制週期20Hz。
http://diydrones.ning.com/video/kalmanestimate-1 看了楼主这些年帖子,也在姿态测量方向孜孜不倦地专研了很久了呀,敬仰一下,呵呵。
是用四元数算法更新姿态的呢,还是用余弦矩阵的算法来估算姿态的呀? 兩個都有在用,這個版本是泛化版DCM做的。姿態估測+控制約需800us@14.74MIPS
目前工作是將所有的Algorithm都定點化,用DSP引擎跑。因為後期要跑真正的捷聯慣導,運算量不小。而且自己希望整個運算能壓制在5ms內,這樣才有其他空間寫別的功能。四元素還沒測,定點版的DCM速度增加60%,相當滿意。
另外還有個PIC32的版本運作中。換湯不換藥。
雖然慣導的書說用四元素法做精度會提高,不過實際跑的經驗是用DCM做運算的開銷比較小。<--如果有DSP引擎
不過最主要是ST的Gyro很穩定,積分後飄的很慢,即便Kalman一秒介入一次都可以。這讓我做外力分解時省不少力氣。
但是安裝在飛機上,與機體的避震措施要做好,不然用哪家的Sensor都會一樣效果很差。 哦,我想过一段时间试试看四元数Algorithm,据惯导书说四元数应该会使计算量大大降低呀,而且不用处理浮点数(float)运算。
DCM是什么的简称?Digital Compass Module? Direction Cosine Matrix 方向餘弦矩陣。如果是玩慣導,都直接寫DCM比較方便。除非有必要,我個人是不太用電羅盤,這種東西在複雜電磁環境下很不可靠。
四元素算法比DCM精度高的因素可以參見Strapdown Inertial Naviation Technology Ch11.2,不過那個是「數學上」的過程。
實際跑的狀況,DCM直接求解比較快,四元素到由拉角這段消耗的運算量不會比較小。另外兩者經過正交、歸一化校正後,定點運算DCM的穩定性比四元素穩定,這應該跟使用的uC定點精度有關,在PIC32實驗的結果,浮點版本則差異不大。
兩者都能用定點或浮點做,四元素開銷大不在於四元素更新部分,而是用四元素算尤拉角的部分。如果要考慮方向性和座標空間,其公式求解用掉的開銷會比單純用DCM求解多,在實務上如果要控制飛機的Roll Pitch Yaw,看DCM最下面一行就行了,不需要真的求由拉角。(我是有求出來) 用單晶片運算的敵人:浮點、三角函數、反矩陣 <---能查表就查表、能線性化就線性化、能用定點就用定點、能降階就降階。 回复【5楼】g921002
用單晶片運算的敵人:浮點、三角函數、反矩陣 <---能查表就查表、能線性化就線性化、能用定點就用定點、能降階就降階。
-----------------------------------------------------------------------
非常同意以上看法 :)我甚至考虑过用带cache的晶片的来做运算,比如SAMSUNG的S3C2440或S3C6410,不跑操作系统,纯粹只做定点运算。
《Strapdown Inertial Naviation Technology》这本书我也看过,只是浮光掠影看了看,有些东西还没理解透,等我做的时候碰到问题了再来向兄台请教。 mark g921002所言非虚,利用四元数只不过是一个中间过程,到最后还是要转换成坐标变换矩阵(也就是方向余弦矩阵)
另外,从坐标变换矩阵计算姿态角需要用到反正弦,反正切,这是运算量最大的
我做过一个小测试,使用C8051F330单片机(8位,运行于24.5MHZ,实际上达到了30MHZ左右),进行一次32位的浮点运算(float),一次乘法需要大概10us,一次正弦或者余弦运算需要50us,而一次反正弦需要200us
所以我觉得,无论是使用四元数还是坐标变换阵的时候,姿态更新算法,对于微分方程的求解,都应该避开三角函数,而是采用展开式
(以上纯属YY,如有不适,可适当喷我) mark mk mark 楼主视频中发现个问题 你的实验板的姿态与 地平仪姿态不相符啊 俯仰是正确的 滚转是反的啊 而且我在做实验的时候 也发现 有这方面的问题! 我现在解决的方法是 以姿态正确为准 不管传感器 标的是什么轴 愿闻高手详解 ! 滾轉是反的沒錯,那時傳感器與body frame正負號沒修正。後來都在做試飛,沒啥時間拍新的影片上去。
http://www.facebook.com/hochung.chang 後來的一些零星的紀錄 这个一定得MARK 回复【8楼】lijieamd
-----------------------------------------------------------------------
问题在于用四元数更新完,还是需要余弦函数和正切函数来求解。本质在于,我们必须适应,可以DSP,这个一定可以。 回复【15楼】asha
-----------------------------------------------------------------------
问题在于用四元数更新完,还是需要余弦函数和正切函数来求解。本质在于,我们必须适应,可以dsp,这个一定可以。
-----------------------------------------------------------------------
DSP??
如果先把DSP的課本翻開來,在對照選用的DSP的datasheet,你會發現很多東西不是針對這類運算優化的,即便到最後反推姿態,還是需要餘弦函數和正切函數來求解,這也不是DSP擅長的。
重點在於這些函數的Numerical approximation.在犧牲精度的條件下加速運算。
若是以矩陣運算來說,DSP定點帶來的效益不足以取代MCU(non-DSP)定點運算,這個實際做過的人應該會有體會。
如果要用到浮點DSP,你要不要先算算你打算賣多少錢? 回复【16楼】g921002
-----------------------------------------------------------------------
呵呵,我不做民用产品,成本不是所重点考虑的。因此一贯用浮点DSP。我上面也说了,必须用余弦和正切,就算DSP不擅长,可是我们还能选择什么呢?要不来个FPGA吧? mark mark 很深奥,有没有DSP的参考代码 沒有電羅盤嗎?
畫面上看不出對方向的刻度,不知道對方向是不是有飄移?? 回复【21楼】jade0606
沒有電羅盤嗎?
畫面上看不出對方向的刻度,不知道對方向是不是有飄移??
-----------------------------------------------------------------------
沒有,方向靠GPS校正。姿態經過估測後,方向漂移不嚴重。30秒飄5~8度。我的GPS是5Hz的,已經做150次方向修正。
回复【20楼】dongyedtx
很深奥,有没有dsp的参考代码
-----------------------------------------------------------------------
代碼參考UAVDevboard的。需要的直接參考http://diydrones.com/page/uav-devboard。 Kalman的代碼版上一堆自己蒐吧!!^_^
裡面的code用了dsPIC的庫跟built-in function,你看了也沒啥用。 mark mark mark mark 学习了。。。。 。。。。楼主能不能联系我。。。
ricky@u-comm.net 請問
1. 您的定點版的DCM與泛化版DCM指的是同一件事嗎?能不能告訴我們長什麼樣子?
2. Kalman一秒介入一次, 如果已經壓到5ms, 也就是說200次才做一次校正,方差/誤差累計200倍,沒有關係嗎?
3. 定點版的DCM與泛化版DCM有奇異點嗎?如果有請問要如何避開?加一個很小的值嗎?
4. 用GPS, 角度30秒飄5~8度,您的飛機如果飛的夠快,角度修正可以用GPS,但飛機起降時會使用您的系統嗎? 回复【29楼】jade0606
請問
1. 您的定點版的dcm與泛化版dcm指的是同一件事嗎?能不能告訴我們長什麼樣子?
2. kalman一秒介入一次, 如果已經壓到5ms, 也就是說200次才做一次校正,方差/誤差累計200倍,沒有關係嗎?
3. 定點版的dcm與泛化版dcm有奇異點嗎?如果有請問要如何避開?加一個很小的值嗎?
4. 用gps, 角度30秒飄5~8度,您的飛機如果飛的夠快,角度修正可以用gps,但飛機起降時會使用您的系統嗎?
-----------------------------------------------------------------------
1.一個是用Q2.14(因為角度表示就是+-pi),一個是用float...基本上運算過程是一樣的,只是code寫法有變。
2.方向的Kalman是200ms介入一次(GPS 5Hz),姿態更新+Kalman(gyro+accel)是20ms一次,每次運算消耗0.6ms,控制是20ms一次(配合RC Servo),ST的陀螺其實沒有飄的那樣厲害,另外陀螺的bias offset開機時會花上2秒校正。
3.有,大部分是出在atan計算,但atan我是用asin近似查表獲得,所以實際上跑不到奇異點,姿態在奇異點會跳到附近的值。
4. gps方位的信度來自於地速,位置信度來自於DOP值。靜止時的Heading幾乎就是gyro算出來的Heading。5~8度的漂移是沒有GPS的時候
飛機沒有超音波高度計,基本降落是人工用半自動模式下降。 回复【30楼】g921002
-----------------------------------------------------------------------
我發現我有問題問的不太對,感謝您耐心回答 楼主,我最近刚接触陀螺仪。我们设计一款软件需要陀螺仪的motion探测。目前我买了一块板子,上面传感器是MPU3050与LSM303.收到数据有陀螺仪,加速计,地磁。不过我对于算法如何将这些数据转变成我想要的陀螺仪的转动角度,请楼主指点一下。搜到了一点资料但是很粗略,不太明白。
我的QQ:2314343016
先谢谢了。 顶一个希望楼主早日联系! 这里真是高手多多啊 楼主理论很强大。不过还是建议加上电子罗盘,因为实际飞行时会出现飞机机头方向与飞行方向不一致的情况,这样gps修正便会出错,反映不了飞机的真实航向角 ratar 发表于 2012-3-31 05:15 static/image/common/back.gif
楼主理论很强大。不过还是建议加上电子罗盘,因为实际飞行时会出现飞机机头方向与飞行方向不一致的情况,这 ...
有全维加表的话,就没有这个问题。 ratar 发表于 2012-3-31 05:15 static/image/common/back.gif
楼主理论很强大。不过还是建议加上电子罗盘,因为实际飞行时会出现飞机机头方向与飞行方向不一致的情况,这 ...
有全维加表的话,就没有这个问题。 littleGnss 发表于 2012-3-31 05:58 static/image/common/back.gif
有全维加表的话,就没有这个问题。
全维加表能测方向吗? ratar 发表于 2012-4-1 10:35 static/image/common/back.gif
全维加表能测方向吗?
有陀螺啊。 你上电后的初始方向陀螺仪是怎么知道的?而且陀螺仪是会漂移的,用什么来修正它? 请问有人在用互补滤波做姿态估计吗?我最近在研究用互补滤波做姿态估计,看到这方面的论文也很多,但是感觉个人研究起来有点力不从心啊,希望能找到同样在研究这个方法的人异同讨论。 MARK...... cy19861230 发表于 2012-4-5 14:39 static/image/common/back.gif
请问有人在用互补滤波做姿态估计吗?我最近在研究用互补滤波做姿态估计,看到这方面的论文也很多,但是感觉 ...
你是想用傳統的方式還是有其他的ideal?
如果是傳統的話,直接讀這檔案
http://web.mit.edu/scolton/www/filter.pdf
mark{:smile:}{:smile:}{:smile:}{:smile:} mark~~~学习 g921002 发表于 2012-4-7 23:30
你是想用傳統的方式還是有其他的ideal?
如果是傳統的話,直接讀這檔案
http://web.mit.edu/scolton/www/f ...
这个文档被撤销了,可以再传下吗? SMC 发表于 2014-6-9 16:31
这个文档被撤销了,可以再传下吗?
http://ocw.mit.edu/courses/aeronautics-and-astronautics/16-333-aircraft-stability-and-control-fall-2004/lecture-notes/lecture_15.pdf g921002 发表于 2014-6-11 19:25
http://ocw.mit.edu/courses/aeronautics-and-astronautics/16-333-aircraft-stability-and-control-fall ...
谢了{:handshake:}
页:
[1]