一天精通无人机第 41 讲 高级篇系列:磁罗盘与水平仪校准

file

**一、磁罗盘校准

磁罗盘又称磁力计,是一种可以测量当前磁场强度的传感器。磁罗盘同样可以测量x、y、z这3轴的磁场强度。但是磁罗盘测量的3轴磁场强度是地磁向量在3维空间中的分量。为了能够测量地磁的方向,通常将地磁向量分解为垂直和水平两个分量,而水平分量可以近似的表示地磁的方向。但是在地球上磁轴与地轴还存在一个磁偏角。磁偏角在不同的地理位置上也是不同的,在无人机的航向计算时可以通过GPS获取当前的经纬度,然后差表得到当前位置的磁偏角,对航向进行修正。

虽然地磁场在不同的地理位置上的强度和方向不同,但是在固定区域内地磁场的变化很小,基本上可以认为是一个固定的向量。

因此,在理想状态下,且不考虑其他磁场干扰的情况下,磁罗盘在水平方向上旋转360°之后x轴和y轴的读数为一个正圆形,如下图中第1个图所示:

file

但是,如果无人机机体上存在磁场对磁罗盘的读数产生干扰,则磁罗盘在水平方向上旋转360°之后读数所形成圆的圆心将偏离坐标原点的位置。此时圆心坐标x、y就是水平方向上x轴和y轴的零偏。如果测量到的结果并不是一个正圆,而是一个椭圆。那么除了圆心坐标作为零偏之外,其两个长短半径就表示x轴和y轴的标度因数。

需要注意的是,磁罗盘的校准只能处理无人机机体本身磁场对磁罗盘的影响,而不能处理无人机外部其他磁场对磁罗盘的影响。但是,我们认为无人机在高空飞行时,通常会远离地面,因此地面设备的磁场对无人机磁罗盘的影响可以忽略不计。

int mag_calibration_worker(void *data)
{
    mag_worker_data_t *worker_data = (mag_worker_data_t *) (data);
    //用于记录当前以及采集了多少组磁罗盘数据
    int counter_side = 0;
    while (counter_side < worker_data->points_perside) {
        px4_pollfd_struct_t fds;
        fds.fd = worker_data->sub_mag;
        fds.events = POLLIN;
        int poll_ret = px4_poll(&fds, 1, 1000);
        if (poll_ret > 0) {
            orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag, &mag);
            //磁罗盘x、y、z这3轴的数据准备进行校准用
            worker_data->x[counter_side] = mag.x;
            worker_data->y[counter_side] = mag.y;
            worker_data->z[counter_side] = mag.z;

            counter_side ++;
        }
    }

    float offset_x = 0.0f;
    float offset_y = 0.0f;
    float offset_z = 0.0f;
    float scale_x = 1.0f;
    float scale_y = 1.0f;
    float scale_z = 1.0f;
    //将磁罗盘的3轴数据拟合成一个椭球体,并计算出将3轴的零偏offset和标度因数
    ellipsoid_fit_least_squares(worker_data.x, worker_data.y, worker_data.z,
            &offset_x, &offset_y, &offset_z,
            &scale_x, &scale_y, &scale_z);

    return OK;
}

**二、水平仪校准

首先,我们需要注意的是水平仪本身并不是一个真实存在的传感器,而是一个用软件程序通过加速度计进行计算来进行当前角度误差计算的程序。此外,我们还需要注意,水平仪并不是用于测量无人机机身在水平方向上的倾斜角度的,而是用于计算飞控与无人机之间安装误差。如下图所示:

file

由于机械结构、机身外壳、螺丝或胶固定等其他因素可能会导致飞控的安装平面并没有与无人机机身保持平行,因此飞控中传感器所测量出的姿态就只是飞控的姿态,而不是无人机机身的姿态,因此有必要将此安装的误差角消除,于是飞控程序引入一个叫做“水平仪”的校准功能,但实际上这并不是一个真正的传感器,也不是用于测量飞机水平姿态的传感器,而是用于计算飞控与机身的误差。

水平仪校准的方法也很简单,在校准时需要将无人机机身置于水平桌面上,并且持续一段时间,此时校准程序会根据当前加速度计和陀螺仪的读数对飞控当前的姿态进行解算,得到当前飞控的俯仰角和滚转角。校准程序持续采集俯仰角和滚转角一段时间,最后求出它们的平均数,就得到了飞控与机身之间的安装误差。

int do_level_calibration()
{
    //采集10秒的数据进行校准
    const unsigned cal_time = 10;
    int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
    struct vehicle_attitude_s att;
    int counter = 0;
    float roll_mean = 0.0f;
    float pitch_mean = 0.0f;

    while (hrt_elapsed_time(&start) < cal_time * 1000000) {
        int pollret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
        if (pollret <= 0) {
            return -1; 
        }   
        orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
        //将姿态信息中的四元数q转为欧拉角
        matrix::Eulerf euler = matrix::Quatf(att.q);
        roll_mean += euler.phi();
        pitch_mean += euler.theta();

        counter++;
    }   
    //各自的平均值,也就是得到了安装误差
    roll_mean /= counter;
    pitch_mean /= counter;
    //滚转角和俯仰角由弧度制转为角度制
    roll_mean *= (float) M_RAD_TO_DEG;
    pitch_mean *= (float) M_RAD_TO_DEG;

    return OK;
}

下期预告:《卡尔曼滤波原理》

file