解讀pixhawk加速度計校準代碼

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

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

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

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

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

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

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

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

3.啥都不說了講代碼

      入口函數do_accel_calibration指針

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

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

3.2 機頭朝向肯定

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

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或零,若是誤差較大,校準後看好轉了多少。

相關文章
相關標籤/搜索