ArduCopter —— ArduPilot —— 气压计地效补偿处理

版权声明：本文为博主原创博文，未经允许不得转载，若要转载，请说明出处并给出博文链接 

       以前总是听到地效（地面效应）这个词语，对飞机的影响，尤其是对大飞机的影响还是很明显的，最近看到了相关的代码，特此来学习一下并且写下这个博客。

相关的视频可以进入该网址观看：http://ardupilot.org/copter/docs/ground-effect-compensation.html?highlight=ground%20effect

ArduCopter3.4（及更高版本）才包含地面效应补偿的，当飞机可能起飞或降落时，该地面效应补偿减少了气压计的权重（这有利于加速度计）。与降低螺旋桨长度的飞机相比，这会降低有时看到的弹跳。

 

如何启用或关闭地效补偿？

地面站（Misson Planner）连接上飞控后，修改全参数 GND_EFFECT_COMP 为 1，则开启地效补偿；设置为0，则关闭地效补偿。（如果您的飞机没有出现着陆时的反弹，最好将此功能禁用，因为它会略微增加高振动水平导致高度估计的风险。）

 

下面介绍下代码中的  地效补偿  是如何实现的：

首先是这里，代码进程调用throttle_loop函数，每50hz运行一次，预估75us运行完。

SCHED_TASK(throttle_loop,         50,     75),

进入到throttle_loop函数，可以看到如果允许，进入update_ground_effect_detector()函数。

// throttle_loop - should be run at 50 hz
// ---------------------------
void Copter::throttle_loop()
{
    // update throttle_low_comp value (controls priority of throttle vs attitude control)
    update_throttle_thr_mix();

    // check auto_armed status
    update_auto_armed();

#if FRAME_CONFIG == HELI_FRAME
    // update rotor speed
    heli_update_rotor_speed_targets();

    // update trad heli swash plate movement
    heli_update_landing_swash();
#endif

    // compensate for ground effect (if enabled)
    update_ground_effect_detector();
}

继续进入该函数，可以跳进baro_ground_effect.cpp文件里，看到这个函数主要是提供一些起飞和降落的逻辑，当然是启用地效之后的逻辑判断，判断起飞和降落阶段是否需要EKF来做地效补偿（gndeffect_state.takeoff_expected = 真，起飞阶段要补偿，gndeffect_state.touchdown_expected = 真，降落阶段要补偿）。所以核心的地效补偿是在EKF里面做的，接着我们进入到EKF里面查看到底如何做的补偿。

void Copter::update_ground_effect_detector(void)
{
    if(!g2.gndeffect_comp_enabled || !motors->armed()) {
        // 上锁就关闭地效
        gndeffect_state.takeoff_expected = false;
        gndeffect_state.touchdown_expected = false;
        ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected);
        ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected);
        return;
    }

    // variable initialization
    uint32_t tnow_ms = millis();
    float xy_des_speed_cms = 0.0f;
    float xy_speed_cms = 0.0f;
    float des_climb_rate_cms = pos_control->get_desired_velocity().z;

    if (pos_control->is_active_xy()) {
        Vector3f vel_target = pos_control->get_vel_target();
        vel_target.z = 0.0f;
        xy_des_speed_cms = vel_target.length();
    }

    if (position_ok() || optflow_position_ok()) {
        Vector3f vel = inertial_nav.get_velocity();
        vel.z = 0.0f;
        xy_speed_cms = vel.length();
    }

    // 起飞的逻辑设定

    // if we are armed and haven't yet taken off
    if (motors->armed() && ap.land_complete && !gndeffect_state.takeoff_expected) {
        gndeffect_state.takeoff_expected = true;
    }

    // if we aren't taking off yet, reset the takeoff timer, altitude and complete flag
    const bool throttle_up = flightmode->has_manual_throttle() && channel_throttle->get_control_in() > 0;
    if (!throttle_up && ap.land_complete) {
        gndeffect_state.takeoff_time_ms = tnow_ms;
        gndeffect_state.takeoff_alt_cm = inertial_nav.get_altitude();
    }

    // if we are in takeoff_expected and we meet the conditions for having taken off
    // end the takeoff_expected state
    if (gndeffect_state.takeoff_expected && (tnow_ms-gndeffect_state.takeoff_time_ms > 5000 || inertial_nav.get_altitude()-gndeffect_state.takeoff_alt_cm > 50.0f)) {
        gndeffect_state.takeoff_expected = false;
    }

    // 降落的逻辑设定
    Vector3f angle_target_rad = attitude_control->get_att_target_euler_cd() * radians(0.01f);
    bool small_angle_request = cosf(angle_target_rad.x)*cosf(angle_target_rad.y) > cosf(radians(7.5f));
    bool xy_speed_low = (position_ok() || optflow_position_ok()) && xy_speed_cms <= 125.0f;
    bool xy_speed_demand_low = pos_control->is_active_xy() && xy_des_speed_cms <= 125.0f;
    bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control->is_active_xy()) || (control_mode == ALT_HOLD && small_angle_request);

    bool descent_demanded = pos_control->is_active_z() && des_climb_rate_cms < 0.0f;
    bool slow_descent_demanded = descent_demanded && des_climb_rate_cms >= -100.0f;
    bool z_speed_low = fabsf(inertial_nav.get_velocity_z()) <= 60.0f;
    bool slow_descent = (slow_descent_demanded || (z_speed_low && descent_demanded));

    gndeffect_state.touchdown_expected = slow_horizontal && slow_descent;

    // 无论起飞还是降落被需要，EKF都要做好准备来补偿地效
    ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected);
    ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected);
}

看到了set...函数，代码里面一般会有一个get...函数与之对应。果不其然：

// determine if a takeoff is expected so that we can compensate for expected barometer errors due to ground effect
bool NavEKF2_core::getTakeoffExpected()
{
    if (expectGndEffectTakeoff && imuSampleTime_ms - takeoffExpectedSet_ms > frontend->gndEffectTimeout_ms) {
        expectGndEffectTakeoff = false;
    }

    return expectGndEffectTakeoff;
}

// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect
void NavEKF2_core::setTakeoffExpected(bool val)
{
    takeoffExpectedSet_ms = imuSampleTime_ms;
    expectGndEffectTakeoff = val;
}


// determine if a touchdown is expected so that we can compensate for expected barometer errors due to ground effect
bool NavEKF2_core::getTouchdownExpected()
{
    if (expectGndEffectTouchdown && imuSampleTime_ms - touchdownExpectedSet_ms > frontend->gndEffectTimeout_ms) {
        expectGndEffectTouchdown = false;
    }

    return expectGndEffectTouchdown;
}

// called by vehicle code to specify that a touchdown is expected to happen
// causes the EKF to compensate for expected barometer errors due to ground effect
void NavEKF2_core::setTouchdownExpected(bool val)
{
    touchdownExpectedSet_ms = imuSampleTime_ms;
    expectGndEffectTouchdown = val;
}

那么具体EKF库里是如何做的地效补偿，我们就要追踪代码里面什么时候调用了get...函数，如下几个地方：

                                             

起飞的get函数共四个地方被调用，另外两个文件分别是函数的定义与声明。

                                     

同理，降落的get函数也只有两个地方被调用，其中一个还和起飞的get函数在一起被调用，另外两个文件为函数的定义与声明。

首先EKF需要先得到气压计的数据，所以有readBaroData()这个函数来读取数据，在这个函数中就有

// 如果我们处于起飞模式，则气压计数据不得低于起飞开始时的测量值
// 这可以防止由于飞机下洗气流造成的负面气压干扰，在最初上升时破坏EKF高度。
        if (getTakeoffExpected()) {
            baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff);
        }

如果用气压计数据来融合计算高度的话，便有下面

// filtered baro data used to provide a reference for takeoff
        // it is is reset to last height measurement on disarming in performArmingChecks()
        if (!getTakeoffExpected()) {
            const float gndHgtFiltTC = 0.5f;
            const float dtBaro = frontend->hgtAvg_ms*1.0e-3f;
            float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
            meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha;
        }

// reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
        if (getTakeoffExpected() || getTouchdownExpected()) {
            posDownObsNoise *= frontend->gndEffectBaroScaler;
        }
        // If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
        // This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
        if (motorsArmed && getTakeoffExpected()) {
            hgtMea = MAX(hgtMea, meaHgtAtTakeOff);
        }

这里我们可以看到，如果启用了地效，且地效补偿被期望使用的时候，会将高度位置的观测噪声乘以一个地效气压尺度因子（代码中的该因子默认数值是4.0），也就是增加了原有的高度方向上的观测噪声，目的在于减弱气压计的权重，增加对加速度计的信任。

else if (obsIndex == 5) {
  innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex];
  const float gndMaxBaroErr = 4.0f;
  const float gndBaroInnovFloor = -0.5f;

  if(getTouchdownExpected()) {
   // when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor
   // constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr
   // this function looks like this:
   //         |/
   //---------|---------
   //    ____/|
   //   /     |
   //  /      |
     innovVelPos[5] += constrain_float(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr);
    }
}

这里是对高度方向的EKF公式中的革新（或  新息）进行限幅操作，以防发散。

总而言之，地面效应的补偿终究是针对气压计这个传感器在近地面阶段的气流影响。

这里涉及到EKF的5个核心公式，有兴趣的可以继续了解下EKF，可以参考看姿态估计（2）—— 扩展卡尔曼滤波（Extended Kalman Filter—EKF）。