Pixhawk學習5 1 感測器標定之加速度計

加速度計作為飛控系統的核心感測器，在pixhawk中主要充當姿態解算、加速度敏感等任務。由於pixhawk中採用的為mems加速度計，其使用過程中存在著零偏、刻度係數、安裝偏角等誤差，如果直接使用，即加速度計的輸出並非飛機本體的加速度，因此需要提前標定這些誤差，在使用時提前補償掉。

pixhawk中給出了加速度計的標定模型，在這裡稍作總結並提出自己的一些想法。

加速度計標定的程式主要涉及到以下檔案：

src/modules/commander/accelermeter_calibration.cpp

加速度計的標定模型如下：

accel_corr = accel_t * (accel_raw - accel_offs)

其中：accel_corr[3]——為飛機機體系下的加速度值；

accel_t[3][3]——為飛機機體系與飛控座標系（這裡假設加速度計與飛控機殼固連且加計的敏感軸座標系與飛控機殼座標系重合）；

accel_raw[3]——為加速度計敏感輸出的加速度；

accel_offs[3]——為加速度計敏感軸下的零偏。

該模型的含義為，加速度計敏感到的資訊，減去自身零偏之後，再乘以飛控與機體之間的旋轉矩陣，可以得到機體系下的加速度。那麼標定方法為採用機體系下的加速度值和感測器敏感到的加速度，反算 accel_t[3][3]和 accel_offs[3]。

標定方法採用六位置法，此時機體系下的加速度值為：

* accel_corr_ref[6]
[3]=
[  g  00]
// 機頭朝上  x *|
-g  00|
// 機頭朝下*|
0  g  0
|// 機身左傾  *|
0-g  0
|// 機身右傾  y*|
00  g |
// 機背朝上*[
00-g ]
// 機背朝下  z

那麼針對上述每乙個位置，都有公式：

accel_corr_ref[i] = accel_t*(accel_raw_ref[i] - accel_offs); i=0~5,對應每個位置。（公式1）

根據上述方法，總共可產生3*6=18個式子，而需要確定 9(accel_t) + 3(accel_offs) = 12個變數。

1、零偏accel_offs標定

零偏accel_offs可根據每個方向的正反兩次放置來得到。以i=0和i=1為例（機頭向上和向下），兩式相加==> 0=accel_t*(accel_raw_ref[0] + accel_raw_ref[1] - 2accel_offs)

由於i=0和i=1兩種情況時g主要在x軸上，y和z基本為零。因此：

accel_offs(x) = 1/2(accel_raw_ref[0] + accel_raw_ref[1])。accel_offs(y) 和 accel_offs(z) 一次類推。

2、零偏accel_offs標定

標定好accel_offs之後，選取公式1中i=0，i=2，i=4項，則有以下公式：

通過簡單矩陣運算就可以得到accel_t。