解读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或零，如果偏差较大，校准后看好转了多少。