卡曼濾波器做姿態估測

g921002 · 发表于 2010-6-25 20:24:14

用陀螺儀+加速規做姿態估測。左上是加速規輸出，左下是陀螺儀輸出。右下是遙控器輸入資料。資料傳輸是用900MHz無線傳輸。<--沒錯，是無線傳輸，因為要放在飛機上的。

其實本身陀螺儀很穩定，姿態可以撐個30秒不飄掉，有點出乎我預料之外。中間有一段晃動，Y軸方向加速規受到約2G左右的力。不過姿態幾乎不受影響，反而是我手晃時造成姿態轉動影響比較大。如果放在桌面水平晃動，姿態變化大概在正負1、2度左右。裡面使用動態方差調整型卡曼濾波，效果還可以。整個姿態估測+控制在800us搞定，控制週期20Hz。
http://diydrones.ning.com/video/kalmanestimate-1

timegate · 发表于 2010-6-25 21:38:24

看了楼主这些年帖子，也在姿态测量方向孜孜不倦地专研了很久了呀，敬仰一下，呵呵。

是用四元数算法更新姿态的呢，还是用余弦矩阵的算法来估算姿态的呀？

g921002 · 发表于 2010-6-26 00:11:44

兩個都有在用，這個版本是泛化版DCM做的。姿態估測+控制約需800us@14.74MIPS
目前工作是將所有的Algorithm都定點化，用DSP引擎跑。因為後期要跑真正的捷聯慣導，運算量不小。而且自己希望整個運算能壓制在5ms內，這樣才有其他空間寫別的功能。四元素還沒測，定點版的DCM速度增加60%，相當滿意。

另外還有個PIC32的版本運作中。換湯不換藥。

雖然慣導的書說用四元素法做精度會提高，不過實際跑的經驗是用DCM做運算的開銷比較小。<--如果有DSP引擎

不過最主要是ST的Gyro很穩定，積分後飄的很慢，即便Kalman一秒介入一次都可以。這讓我做外力分解時省不少力氣。
但是安裝在飛機上，與機體的避震措施要做好，不然用哪家的Sensor都會一樣效果很差。

timegate · 发表于 2010-6-26 09:29:14

哦，我想过一段时间试试看四元数Algorithm，据惯导书说四元数应该会使计算量大大降低呀，而且不用处理浮点数(float)运算。

DCM是什么的简称？Digital Compass Module?

g921002 · 发表于 2010-6-26 11:10:47

Direction Cosine Matrix 方向餘弦矩陣。如果是玩慣導，都直接寫DCM比較方便。除非有必要，我個人是不太用電羅盤，這種東西在複雜電磁環境下很不可靠。

四元素算法比DCM精度高的因素可以參見Strapdown Inertial Naviation Technology Ch11.2，不過那個是「數學上」的過程。
實際跑的狀況，DCM直接求解比較快，四元素到由拉角這段消耗的運算量不會比較小。另外兩者經過正交、歸一化校正後，定點運算DCM的穩定性比四元素穩定，這應該跟使用的uC定點精度有關，在PIC32實驗的結果，浮點版本則差異不大。

兩者都能用定點或浮點做，四元素開銷大不在於四元素更新部分，而是用四元素算尤拉角的部分。如果要考慮方向性和座標空間，其公式求解用掉的開銷會比單純用DCM求解多，在實務上如果要控制飛機的Roll Pitch Yaw，看DCM最下面一行就行了，不需要真的求由拉角。(我是有求出來)

g921002 · 发表于 2010-6-26 11:16:15

用單晶片運算的敵人：浮點、三角函數、反矩陣 <---能查表就查表、能線性化就線性化、能用定點就用定點、能降階就降階。

timegate · 发表于 2010-6-26 21:05:53

回复【5楼】g921002
用單晶片運算的敵人：浮點、三角函數、反矩陣 <---能查表就查表、能線性化就線性化、能用定點就用定點、能降階就降階。

-----------------------------------------------------------------------
非常同意以上看法：）我甚至考虑过用带cache的晶片的来做运算，比如SAMSUNG的S3C2440或S3C6410，不跑操作系统，纯粹只做定点运算。

《Strapdown Inertial Naviation Technology》这本书我也看过，只是浮光掠影看了看，有些东西还没理解透，等我做的时候碰到问题了再来向兄台请教。

aliveghost · 发表于 2010-6-26 22:57:06

mark

lijieamd · 发表于 2010-6-26 23:11:05

g921002所言非虚,利用四元数只不过是一个中间过程,到最后还是要转换成坐标变换矩阵(也就是方向余弦矩阵)
另外,从坐标变换矩阵计算姿态角需要用到反正弦,反正切,这是运算量最大的
我做过一个小测试,使用C8051F330单片机(8位,运行于24.5MHZ,实际上达到了30MHZ左右),进行一次32位的浮点运算(float),一次乘法需要大概10us,一次正弦或者余弦运算需要50us,而一次反正弦需要200us
所以我觉得,无论是使用四元数还是坐标变换阵的时候,姿态更新算法,对于微分方程的求解,都应该避开三角函数,而是采用展开式
(以上纯属YY,如有不适,可适当喷我)

denglu · 发表于 2010-7-11 15:17:12

mark

zht9961020 · 发表于 2011-5-16 12:01:23

lxw2087290 · 发表于 2011-5-17 21:51:26

mark

liguole · 发表于 2011-5-18 09:40:59

楼主视频中发现个问题你的实验板的姿态与地平仪姿态不相符啊俯仰是正确的滚转是反的啊而且我在做实验的时候也发现有这方面的问题！我现在解决的方法是以姿态正确为准不管传感器标的是什么轴愿闻高手详解！

g921002 · 发表于 2011-5-18 18:45:11

滾轉是反的沒錯，那時傳感器與body frame正負號沒修正。後來都在做試飛，沒啥時間拍新的影片上去。
http://www.facebook.com/hochung.chang 後來的一些零星的紀錄

mosidao · 发表于 2011-5-18 19:23:56

这个一定得MARK

asha · 发表于 2011-5-20 22:13:48

回复【8楼】lijieamd
-----------------------------------------------------------------------

问题在于用四元数更新完，还是需要余弦函数和正切函数来求解。本质在于，我们必须适应，可以DSP，这个一定可以。

g921002 · 发表于 2011-5-21 01:48:14

回复【15楼】asha
-----------------------------------------------------------------------
问题在于用四元数更新完，还是需要余弦函数和正切函数来求解。本质在于，我们必须适应，可以dsp，这个一定可以。
-----------------------------------------------------------------------
DSP??
如果先把DSP的課本翻開來，在對照選用的DSP的datasheet，你會發現很多東西不是針對這類運算優化的，即便到最後反推姿態，還是需要餘弦函數和正切函數來求解，這也不是DSP擅長的。
重點在於這些函數的Numerical approximation.在犧牲精度的條件下加速運算。
若是以矩陣運算來說，DSP定點帶來的效益不足以取代MCU(non-DSP)定點運算，這個實際做過的人應該會有體會。

如果要用到浮點DSP，你要不要先算算你打算賣多少錢？

asha · 发表于 2011-5-22 10:36:40

回复【16楼】g921002
-----------------------------------------------------------------------

呵呵，我不做民用产品，成本不是所重点考虑的。因此一贯用浮点DSP。我上面也说了，必须用余弦和正切，就算DSP不擅长，可是我们还能选择什么呢？要不来个FPGA吧？

fengleihu · 发表于 2011-6-16 00:49:57

mark

dujun168 · 发表于 2011-7-22 18:17:51

mark

dongyedtx · 发表于 2011-9-3 22:51:51

很深奥，有没有DSP的参考代码

jade0606 · 发表于 2011-12-9 15:10:20

沒有電羅盤嗎?
畫面上看不出對方向的刻度,不知道對方向是不是有飄移??

g921002 · 发表于 2011-12-9 17:11:40

回复【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，你看了也沒啥用。

whyjld · 发表于 2011-12-9 17:29:17

mark

xld007 · 发表于 2011-12-9 19:56:26

mark

dujun168 · 发表于 2011-12-9 21:28:40

mark

clqfly · 发表于 2011-12-10 14:58:47

mark

xibujingji · 发表于 2011-12-12 14:28:31

学习了。。。。

xibujingji · 发表于 2011-12-12 14:30:45

。。。。楼主能不能联系我。。。
ricky@u-comm.net

jade0606 · 发表于 2011-12-13 09:22:10

請問
1. 您的定點版的DCM與泛化版DCM指的是同一件事嗎?能不能告訴我們長什麼樣子?
2. Kalman一秒介入一次, 如果已經壓到5ms, 也就是說200次才做一次校正,方差/誤差累計200倍,沒有關係嗎?
3. 定點版的DCM與泛化版DCM有奇異點嗎?如果有請問要如何避開?加一個很小的值嗎?
4. 用GPS, 角度30秒飄5~8度,您的飛機如果飛的夠快,角度修正可以用GPS,但飛機起降時會使用您的系統嗎?

g921002 · 发表于 2011-12-13 18:21:35

回复【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的時候

飛機沒有超音波高度計，基本降落是人工用半自動模式下降。

jade0606 · 发表于 2011-12-14 17:55:29

回复【30楼】g921002
-----------------------------------------------------------------------

我發現我有問題問的不太對,感謝您耐心回答

wwjdwy · 发表于 2012-2-10 09:54:22

楼主，我最近刚接触陀螺仪。我们设计一款软件需要陀螺仪的motion探测。目前我买了一块板子，上面传感器是MPU3050与LSM303.收到数据有陀螺仪，加速计，地磁。不过我对于算法如何将这些数据转变成我想要的陀螺仪的转动角度，请楼主指点一下。搜到了一点资料但是很粗略，不太明白。
我的QQ：2314343016
先谢谢了。

wwjdwy · 发表于 2012-2-10 10:01:06

顶一个希望楼主早日联系！

thxl_kis · 发表于 2012-3-30 23:49:22

这里真是高手多多啊

ratar · 发表于 2012-3-31 05:15:44

楼主理论很强大。不过还是建议加上电子罗盘，因为实际飞行时会出现飞机机头方向与飞行方向不一致的情况，这样gps修正便会出错，反映不了飞机的真实航向角

littleGnss · 发表于 2012-3-31 05:57:42

ratar 发表于 2012-3-31 05:15
楼主理论很强大。不过还是建议加上电子罗盘，因为实际飞行时会出现飞机机头方向与飞行方向不一致的情况，这 ...

有全维加表的话，就没有这个问题。

littleGnss · 发表于 2012-3-31 05:58:08

ratar 发表于 2012-3-31 05:15
楼主理论很强大。不过还是建议加上电子罗盘，因为实际飞行时会出现飞机机头方向与飞行方向不一致的情况，这 ...

有全维加表的话，就没有这个问题。

ratar · 发表于 2012-4-1 10:35:45

littleGnss 发表于 2012-3-31 05:58
有全维加表的话，就没有这个问题。

全维加表能测方向吗？

littleGnss · 发表于 2012-4-1 11:20:46

ratar 发表于 2012-4-1 10:35
全维加表能测方向吗？

有陀螺啊。

ratar · 发表于 2012-4-1 12:39:06

你上电后的初始方向陀螺仪是怎么知道的？而且陀螺仪是会漂移的，用什么来修正它？

cy19861230 · 发表于 2012-4-5 14:39:16

请问有人在用互补滤波做姿态估计吗？我最近在研究用互补滤波做姿态估计，看到这方面的论文也很多，但是感觉个人研究起来有点力不从心啊，希望能找到同样在研究这个方法的人异同讨论。

liningsky · 发表于 2012-4-6 14:15:56

MARK......

g921002 · 发表于 2012-4-7 23:30:30

cy19861230 发表于 2012-4-5 14:39
请问有人在用互补滤波做姿态估计吗？我最近在研究用互补滤波做姿态估计，看到这方面的论文也很多，但是感觉 ...

你是想用傳統的方式還是有其他的ideal?
如果是傳統的話，直接讀這檔案
http://web.mit.edu/scolton/www/filter.pdf

wj0755 · 发表于 2012-9-27 17:33:44

mark

lph1987 · 发表于 2012-10-15 15:47:22

mark~~~学习

SMC · 发表于 2014-6-9 16:31:25

g921002 发表于 2012-4-7 23:30
你是想用傳統的方式還是有其他的ideal?
如果是傳統的話，直接讀這檔案
http://web.mit.edu/scolton/www/f ...

这个文档被撤销了，可以再传下吗？

g921002 · 发表于 2014-6-11 19:25:18

SMC 发表于 2014-6-9 16:31
这个文档被撤销了，可以再传下吗？

http://ocw.mit.edu/courses/aeron ... otes/lecture_15.pdf

SMC · 发表于 2014-6-11 20:54:22

g921002 发表于 2014-6-11 19:25
http://ocw.mit.edu/courses/aeronautics-and-astronautics/16-333-aircraft-stability-and-control-fall ...

谢了

卡曼濾波器做姿態估測

阿莫论坛20周年了！感谢大家的支持与爱护！！

浏览过的版块