理想狀況下,傳感器的讀數與實際物理量相同,但因爲一些隨機因素的影響,傳感器測量到的數據與實際數據存在誤差,且誤差不停變化。誤差大致分爲兩種,零位誤差和標度誤差。markdown
對於一個傳感器來講,實際物理量=K*傳感器度數+B。函數
理想狀態下,K=1,B=0,可是實際中K與B的值會不停的隨機變化,K的改變引發的誤差叫標度誤差,B的改變引發的誤差叫零位誤差。工具
傳感器校準,就是經過對已知物理量的測量,求出K與B的值,並將這兩個值傳入傳感器驅動,使得傳感器驅動能夠解算出準確的物理量。,spa
這個過程的學術名詞叫「六面較準法」。Pixhawk的加速度計是3軸的,也就是在x,y,z三個方向上測得三個加速度,因此存在三個y=kx+b的公式,根據兩點肯定一條直線的原則,每一個軸都要測量到兩個不一樣的已知值,因此將每一個軸垂直向下採集一個負G數據,垂直向上採集一個正G數據,從而算出每一個軸的k和b值。.net
入口函數do_accel_calibration指針
設置驅動中的校準參數爲默認值,」px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);「的含義是將校準參數的默認值寫入加速度計驅動,默認三個scale是1,三個offset是0.blog
Calibration_routines.cpp文件中的749行:do_accel_calibration_measurements -> calibrate_from_orientation -> detect_orientation函數的返回值就是機頭朝向,六個朝向都會進行一次校準數據採集。rem
774行calibration_worker函數指針指向accel_calibration_worker函數accel_calibration_worker -> read_accelerometer_avg函數負責採集某個機頭朝向的加速度計值,三個軸各取3000個數據平均,而後將平均數從傳感器座標系轉換到機體座標系,將轉換後的結果存放到worker_data -> accel_ref[3][6][3](三個羅盤六個方向三個軸)同步
六個方向的數據採集完畢後,由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信息。
若是飛機與加速度計的座標系重合,則矩陣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)」將校準結果寫入驅動。
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是座標系轉換旋轉矩陣。
1)加速度計主管飛機姿態,加速度計校準後,在不考慮水平校準的狀況下。飛機儀表盤上的滾轉俯仰角更加準確。
2)能夠用qGroundControl的分析工具實時查看加速度計的原始數據曲線,注意這裏的數據是機體座標系的,因此不用考慮旋轉問題。將pixhawk的某個軸向上或水平,看讀數是否是一個G或零,若是誤差較大,校準後看好轉了多少。