解讀pixhawk加速度計校準代碼

本博客與本人在csdn上的博客同步:https://blog.csdn.net/qiuzhizhecsd/article/details/75093967 轉載請註明出處

1.傳感器校準是什麼東東？

      理想狀況下，傳感器的讀數與實際物理量相同，但因爲一些隨機因素的影響，傳感器測量到的數據與實際數據存在誤差，且誤差不停變化。誤差大致分爲兩種，零位誤差和標度誤差。

      對於一個傳感器來講，實際物理量=K*傳感器度數+B。

      理想狀態下，K=1,B=0,可是實際中K與B的值會不停的隨機變化，K的改變引發的誤差叫標度誤差，B的改變引發的誤差叫零位誤差。

      傳感器校準，就是經過對已知物理量的測量，求出K與B的值，並將這兩個值傳入傳感器驅動，使得傳感器驅動能夠解算出準確的物理量。，

2.速度計校準幹嗎要轉6個面

       這個過程的學術名詞叫「六面較準法」。Pixhawk的加速度計是3軸的，也就是在x,y,z三個方向上測得三個加速度，因此存在三個y=kx+b的公式，根據兩點肯定一條直線的原則，每一個軸都要測量到兩個不一樣的已知值，因此將每一個軸垂直向下採集一個負G數據，垂直向上採集一個正G數據，從而算出每一個軸的k和b值。

3.啥都不說了講代碼

      入口函數do_accel_calibration

3.1 去除以前校準對傳感器度數的影響

      設置驅動中的校準參數爲默認值，」px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);「的含義是將校準參數的默認值寫入加速度計驅動，默認三個scale是1，三個offset是0.

3.2 機頭朝向肯定

      Calibration_routines.cpp文件中的749行：do_accel_calibration_measurements -> calibrate_from_orientation -> detect_orientation函數的返回值就是機頭朝向，六個朝向都會進行一次校準數據採集。

3.3 校準數據採集

      774行calibration_worker函數指針指向accel_calibration_worker函數accel_calibration_worker -> read_accelerometer_avg函數負責採集某個機頭朝向的加速度計值，三個軸各取3000個數據平均，而後將平均數從傳感器座標系轉換到機體座標系，將轉換後的結果存放到worker_data -> accel_ref[3][6][3]（三個羅盤六個方向三個軸）

3.4 校準結果計算

        零偏計算

         六個方向的數據採集完畢後，由calculate_calibration_values函數完成校準結果的計算，offset的計算就是取同一個軸上的兩個數據的平均值。

         比例計算

         將機體系x軸朝上，y軸朝上，z軸朝上的數據減去各個軸的offset後存放到3*3矩陣mat_A的三個行中，mat_A是機體座標系的值，mat_A的三行表明三個不一樣的朝向，mat_A的三列表明在某個朝向減去offset後的X、Y、Z值。

      有未知3*3矩陣Accel_T:

      mat_A * Accel_T  = [g      0      0]  //X軸朝上

                                      |0     g       0 | //y軸朝上

                                      [0      0      g]  //z軸朝上

      mat_invert3函數用伴隨矩陣法求出mat_A的逆陣，等式左右兩邊同時左乘mat_A^-1解出Accel_T。若是飛機與加速度計的座標系重合，則Accel_T中的主對角線的三個元素就是加速度計三個軸的scale信息。 

3.5 飛機與加速度計之間的夾角處理

      若是飛機與加速度計的座標系重合，則矩陣Accel_T的主對角線上的三個值就是加速度計三個軸的比例（scale）

      若是夾角不爲0，根據pxhawk本身導出的公式計算傳感器座標系下的校準參數。轉換公式以下：

      accel_T= rot^-1 * Accel_T_r *rot

      Accel_offs= rot^-1 * Accel_offs_r

      1) rot的含義

      rot是傳感器與機體系之間的旋轉矩陣，傳感器座標系值*rot = 機體座標系值

      2) Accel_offs_r與Accel_offs的含義

      Accel_offs_r是機體座標系下三個軸的offset組成的三維向量，也就是accel_offs[3][3]。Accel_offs是傳感器座標系下的offset三維向量，Accel_offs是須要求得的三個軸offset校準參數的來源。

      3) Accel_T_r與accel_T的含義

      Accel_T_r是時機體座標系下的比例(scale)3*3矩陣，也就是咱們在以前比例計算中得出的矩陣Accel_T。accel_T是傳感器座標系下的比例(scale)3*3矩陣，accel_T是咱們須要求得的三個軸scale校準參數的來源。

 

      正交矩陣的轉置（transpose）等於正交矩陣的逆陣，因此用board_rotation_t就是board_rotation的逆陣。

      最後經過accel_scale結構體將六個校準參數存入參數列表並經過「px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale)」將校準結果寫入驅動。

 

3.6 公式的推導過程：（極可能對,但僅供參考）

         Pixhawk註釋的公式推到過程連括號都不匹配，因此我試着本身推導

     其中mat_A_r是機體座標系下的三個方向三個軸組成的3*3矩陣。mat_A是傳感器座標系下三個方向三個軸組成的3*3矩陣。Accel_T_r機體座標系下的主對角線scale參數3*3矩陣。accel_T是傳感器座標系的scale參數3*3矩陣。

      證實:accel_T = rot^-1* Accel_T_r *rot

      mat_A_r* Accel_T_r =    [g  0  0]

                                             |0  g  0|

                                             [0  0  g]

         展開mat_A_r和右側矩陣得出

      mat_A* rot*Accel_T_r = xyz分別向上組成的傳感器座標系3*3矩陣* rot

         左右同時右乘rot^-1,得出

      mat_A*rot*Accel_T_r*rot^-1 = xyz分別向上組成的傳感器座標系3*3矩陣

         因此

      mat_A* （rot*Accel_T_r*rot^-1）= xyz分別向上組成的傳感器座標系3*3矩陣

         由於mat_A*accel_T = xyz分別向上組成的傳感器座標系3*3矩陣

         因此括號中的東西（rot*Accel_T_r*rot^-1）是傳感器座標系下的accel_T

 

      證實:Accel_offs= rot^-1 * Accel_offs_r

      機體座標系offset三維向量 = （正G *ROT + 負G*ROT）/2 = （(正G + 負G)/2）*ROT = 傳感器offset三維向量*ROT

      其中正G是三個軸朝上造成的三維向量，負G是三個軸朝下造成的三維向量，ROT是座標系轉換旋轉矩陣。

 

4.我怎麼能感覺到校準後的好處

      1）加速度計主管飛機姿態，加速度計校準後，在不考慮水平校準的狀況下。飛機儀表盤上的滾轉俯仰角更加準確。

      2）能夠用qGroundControl的分析工具實時查看加速度計的原始數據曲線，注意這裏的數據是機體座標系的，因此不用考慮旋轉問題。將pixhawk的某個軸向上或水平，看讀數是否是一個G或零，若是誤差較大，校準後看好轉了多少。