本博客与本人在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或零,如果偏差较大,校准后看好转了多少。