加速度計與陀螺儀融合問題

[zxc2694]

你好，我在QCopterFC_ahrs.c看到

define Kp 15.0f

define Ki 0.020f

請問一下 如果要改pid參數 是在這裡改??

[shengwen-tw]

你所謂的PID是哪組控制器的參數呢?

[hom-wang]

目前飛控有兩個 PID，
１．QCopterFC_ahrs.c 裡面的 PID 是用來調整加速度計與陀螺儀融合的，也就是你所說的。
２．調整四軸 Pitch, Roll, Yaw 平衡的 PID 在 algorithm_pid.c 裡面的 PID_Init 函式可以做初始化，或是向下面一樣，直接修改 PID 參數。

https://github.com/QCopter/QCopterFlightControl/blob/6b3d1668f50d4b16774aef3b2704a975c109c289/Software/QCopterFC_FlightControl/Program_System/QCopterFC.c

[james54068]

針對QCopterFC_ahrs.c 裡面的 PID 我有個疑問：
就如同你所說的是用來調整加速度計與陀螺儀融合，請問一下這個作法有甚麼參考資料嗎？
還是是自己調整試出來的呢？
我仔細看了一下仔細看了一下errorr你在數學上的解釋是用外積來看，所以我猜想你的你的error是飛行器測得的運動時的g方向和靜止時的的g方向夾角，但是從GyrX = GyrX + Kp * ErrX + exInt;
程式碼來看，GyrX單位是角速度，ErrX單位是角度，單位好像不太對，所以想確認一下關於這種融和方法的概念，是不是我哪裡誤會了？QQ

[hom-wang]

該算法是參考對岸 amobbs 捷联惯导算法心得，看程式理解的，另外也有參考慣性技術 鄧正隆著一書。

關於你另一問題：

有兩單位向量 A, B，兩向量夾角為 theta，
以上存在 A Cross B = |A|_|B|_Sin(theta) = Sin(theta) 的關係，
由上式可以看出來，A, B 兩個向量"""重合"""的程度決定了 A Cross B 的數值，
( 當完全重合 theta = 0 deg，A Cross B = 0，若 theta = 90 deg，A Cross B = 1 )
所以這裡透過上述概念應用至誤差 Error 上，

A：飛行器上的加速度計測得的加速度。
B：重力加速度透過四元數旋轉至飛行器上。
以上倆者皆需要經過歸一化( normalize )，也就是只參考他們的方向。

透過 Cross Product 來得知兩向量重合的程度，也就是說，若加速度計沒有誤差，且重力加速度也沒有問題，那誤差 Error 就是直接反應出四元素的誤差，同時也因為算法中四元數是以角速度所更新的，所以就間接的反映出陀螺儀所產生的誤差。

所以總結以上，誤差 Error 反映的是重合狀況，僅與方向有關，與單位無關。

可以參考之前寫的 推導_四元數.pdf 的最後面 "使用四元數融合加速度計" 部分

[fboris]

我發現程式碼有部份非常類似於Magwich所寫的Mahony filter
code: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
然後我又在這個有人整理IMU data fusion的網站找到這一段：

Mahony’s papers:
[RM05] Complementary filter design on the special orthogonal group SO(3) (Dec. ’05) [.pdf] – by Robert Mahony, Tarek Hamel, Jean-Michel Pflimlin
[RM07] Complementary filter design on the Special Euclidean group SO(3) (’07) [.pdf] – by Grant Baldwin, Robert Mahony, Jochen Trumpf, Tarek Hamel, Thibault Cheviron
[RM08] Nonlinear Complementary Filters on the Special Orthogonal Group (Jun. ’08) [link] – by Robert Mahony, Tarek Hamel, Jean-Michel Pflimlin (it is also hosted on gentlenav)

簡單來說，濾波器也是一種控制器，所以在Maphony的paper中他把他設計的complementary filter(互補式濾波器)當作一個傳統的回授控制系統。在[RM08]的APPENDIX A有稍微提到所謂的互補式濾波實際上在控制系統怎麼表示。

很多人的誤解就是控制系統是控制系統，濾波器就是濾波器要用數位訊號處理的技巧處理。但是實際上是要看你怎麼分析這個系統。以互補式濾波器為例，他可以等效於特殊的IIR filter，也可以當成傳統的回授控制系統。

[hom-wang]

搜尋了一下，使用的算法應該是 Magwich 所設計的沒錯，
另外 @fboris 提供的 IMU data fusion 網站蠻不錯的，有時間會詳細去看。