ArduPilot开源代码之NavEKF2_core

lida2003

已于 2024-07-22 08:54:13 修改

阅读量966

点赞数 28

CC 4.0 BY-SA版权

分类专栏： ArduPilot 文章标签： ardupilot 开源 c++

于 2024-07-21 19:43:30 首次发布

本文链接：https://blog.youkuaiyun.com/lida2003/article/details/140592645

ArduPilot开源代码之NavEKF2_core

1. 源由
2. 框架设计
3. 重要例程
4. 总结
5. 参考资料

1. 源由

NavEKF2_core类EKF2的第二层抽象，对于无人机等系统中的导航任务至关重要，提供实时状态估计和传感器融合功能。

2. 框架设计

2.1 构造函数

NavEKF2_core: 使用前端引用初始化核心EKF。

2.1.1 NavEKF2_core

没有具体的初始化流程，主要是成员变量赋值。

frontend = _frontend
dal = AP::dal()

NavEKF2_core::NavEKF2_core(NavEKF2 *_frontend) :
    frontend(_frontend),
    dal(AP::dal())
{
   
   
}

初始化流程

NavEKF2::InitialiseFilter
 └──> core = (NavEKF2_core*)AP::dal().malloc_type(sizeof(NavEKF2_core)*num_cores, AP_DAL::MEM_FAST);

2.2 设置和初始化

setup_core: 使用特定索引配置核心后端。
InitialiseFilterBootstrap: 在静止状态下使用加速度计和磁力计数据初始化状态。

2.2.1 setup_core

核心后端设置缓冲(ring buffer)大小。

bool NavEKF2_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
{
   
   
    imu_index = _imu_index;
    gyro_index_active = _imu_index;
    accel_index_active = _imu_index;
    core_index = _core_index;

    /*
      imu_buffer_length 需要处理在 100Hz 最大融合速率下的 260ms 延迟。
      非 IMU 数据以超过 100Hz 的速度输入时会被下采样。
      对于 50Hz 的主循环速率，我们需要较短的缓冲区。
     */
    if (dal.ins().get_loop_rate_hz() < 100) {
   
   
        imu_buffer_length = 13;
    } else {
   
   
        // 在 100 Hz 融合速率下最多 260 毫秒延迟
        imu_buffer_length = 26;
    }
    if(!storedGPS.init(OBS_BUFFER_LENGTH)) {
   
   
        return false;
    }
    if(!storedMag.init(OBS_BUFFER_LENGTH)) {
   
   
        return false;
    }
    if(!storedBaro.init(OBS_BUFFER_LENGTH)) {
   
   
        return false;
    } 
    if(!storedTAS.init(OBS_BUFFER_LENGTH)) {
   
   
        return false;
    }
    if(!storedOF.init(FLOW_BUFFER_LENGTH)) {
   
   
        return false;
    }
    // 注意：使用双测距仪可能会加倍存储量
    if(!storedRange.init(2*OBS_BUFFER_LENGTH)) {
   
   
        return false;
    }
    // 注意：测距信标数据一次读取一个，并且可能以高速度到达
    if(!storedRangeBeacon.init(imu_buffer_length)) {
   
   
        return false;
    }
    if(!storedExtNav.init(EXTNAV_BUFFER_LENGTH)) {
   
   
        return false;
    }
    if(!storedIMU.init(imu_buffer_length)) {
   
   
        return false;
    }
    if(!storedOutput.init(imu_buffer_length)) {
   
   
        return false;
    }
    if(!storedExtNavVel.init(EXTNAV_BUFFER_LENGTH)) {
   
   
       return false;
    }

    if ((yawEstimator == nullptr) && (frontend->_gsfRunMask & (1U<<core_index))) {
   
   
        // 检查是否有足够的内存来创建 EKF-GSF 对象
        if (dal.available_memory() < sizeof(EKFGSF_yaw) + 1024) {
   
   
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF2 IMU%u GSF: 内存不足", (unsigned)imu_index);
            return false;
        }

        // 尝试实例化
        yawEstimator = NEW_NOTHROW EKFGSF_yaw();
        if (yawEstimator == nullptr) {
   
   
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF2 IMU%u GSF: 分配失败", (unsigned)imu_index);
            return false;
        }
    }
    
    return true;
}

2.2.2 InitialiseFilterBootstrap

滤波器初始化

// 从加速度计和磁力计数据初始化状态（如果存在）
// 该方法只能在车辆静止时使用
bool NavEKF2_core::InitialiseFilterBootstrap(void)
{
   
   
    // 如果是飞机且没有GPS锁定，则不初始化
    if (assume_zero_sideslip() && dal.gps().status(dal.gps().primary_sensor()) < AP_DAL_GPS::GPS_OK_FIX_3D) {
   
   
        dal.snprintf(prearm_fail_string,
                           sizeof(prearm_fail_string),
                           "EKF2 init failure: No GPS lock");
        statesInitialised = false;
        return false;
    }

    if (statesInitialised) {
   
   
        // 我们已经初始化，但在IMU缓冲区填满之前不返回true。这防止在滤波器启动期间IMU数据暂停的时间漏洞
        readIMUData();
        readMagData();
        readGpsData();
        readBaroData();
        return storedIMU.is_filled();
    }

    // 将重复使用的变量置零
    InitialiseVariables();

    const auto &ins = dal.ins();

    // 初始化IMU数据
    dtIMUavg = ins.get_loop_delta_t();
    readIMUData();
    storedIMU.reset_history(imuDataNew);
    imuDataDelayed = imuDataNew;

    // 加速度计测得的XYZ机体轴上的加速度向量（m/s^2）
    Vector3F initAccVec;

    // TODO 我们应该在多个周期内平均加速度读数
    initAccVec = ins.get_accel(accel_index_active).toftype();

    // 读取磁力计数据
    readMagData();

    // 规范化加速度向量
    ftype pitch=0, roll=0;
    if (initAccVec.length() > 0.001f) {
   
   
        initAccVec.normalize();

        // 计算初始俯仰角
        pitch = asinF(initAccVec.x);

        // 计算初始滚转角
        roll = atan2F(-initAccVec.y , -initAccVec.z);
    }

    // 计算初始滚转和俯仰方向
    stateStruct.quat.from_euler(roll, pitch, 0.0f);

    // 初始化动态状态
    stateStruct.velocity.zero();
    stateStruct.position.zero();
    stateStruct.angErr.zero();

    // 初始化静态过程模型状态
    stateStruct.gyro_bias.zero();
    stateStruct.gyro_scale.x = 1.0f;
    stateStruct.gyro_scale.y = 1.0f;
    stateStruct.gyro_scale.z = 1.0f;
    stateStruct.accel_zbias = 0.0f;
    stateStruct.wind_vel.zero();
    stateStruct.earth_magfield.zero();
    stateStruct.body_magfield.zero();

    // 读取GPS并设置位置和速度状态
    readGpsData();
    ResetVelocity();
    ResetPosition();

    // 读取气压计并设置高度状态
    readBaroData();
    ResetHeight();

    // 在NED导航框架中定义地球旋转向量
    calcEarthRateNED(earthRateNED, dal.get_home().lat);

    // 初始化协方差矩阵
    CovarianceInit();

    // 重置输出状态
    StoreOutputReset();

    // 现在状态已初始化，设置为true
    statesInitialised = true;

    // 重置不活动的偏差
    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
   
   
        inactiveBias[i].gyro_bias.zero();
        inactiveBias[i].accel_zbias = 0;
        inactiveBias[i].gyro_scale.x = 1;
        inactiveBias[i].gyro_scale.y = 1;
        inactiveBias[i].gyro_scale.z = 1;
    }

    // 我们最初返回false以等待IMU缓冲区填满
    return false;
}

2.3 更新和健康检查

UpdateFilter: 在新IMU数据可用时更新滤波器状态。
healthy: 检查滤波器的健康状况。
errorScore: 提供滤波器健康状况的错误评分。

2.3.1 UpdateFilter

更新滤波器状态 - 每当有新的IMU数据时都应该调用。

void NavEKF2_core::UpdateFilter(bool predict)
{
   
   
    // 设置标志，指示滤波器前端已经允许开始新的状态预测周期
    startPredictEnabled = predict;

    // 如果状态尚未初始化，则不运行滤波器更新
    if (!statesInitialised) {
   
   
        return;
    }

    // 启动用于负载测量的定时器
#if ENABLE_EKF_TIMING
    void *istate = hal.scheduler->disable_interrupts_save();
    static uint32_t timing_start_us;
    timing_start_us = dal.micros();
#endif

    fill_scratch_variables();

    // TODO - 在飞行中重启的方法

    // 获取更新步骤的起始时间
    imuSampleTime_ms = frontend->imuSampleTime_us / 1000;

    // 检查臂状态并执行所需的检查和模式更改
    controlFilterModes();

    // 读取IMU数据作为增量角度和速度
    readIMUData();

    // 如果缓冲区中有新的IMU数据，运行EKF方程以估计融合时间地平线
    if (runUpdates) {
   
   
        // 使用来自延迟时间地平线的IMU数据预测状态
        UpdateStrapdownEquationsNED();

        // 预测协方差增长
        CovariancePrediction();

        // 运行IMU预测步骤以获得GSF偏航估计器算法
        // 使用IMU和可选的真空气速数据。
        // 必须在SelectMagFusion()之前运行，以提供最新的偏航估计
        runYawEstimatorPrediction();

        // 使用磁力计数据更新状态
        SelectMagFusion();

        // 使用GPS和高度计数据更新状态
        SelectVelPosFusion();

        // 运行GPS速度修正步骤以获得GSF偏航估计器算法
        // 并使用偏航估计值重置主EKF偏航（如果请求）
        // 必须在SelectVelPosFusion()之后运行，以确保有最新的GPS数据
        runYawEstimatorCorrection();

#if AP_BEACON_ENABLED
        // 使用范围信标数据更新状态
        SelectRngBcnFusion();
#endif

        // 使用光流数据更新状态
        SelectFlowFusion();

        // 使用空气速度数据更新状态
        SelectTasFusion();

        // 使用侧滑约束假设更新状态（适用于前进飞行的车辆）
        SelectBetaFusion();

        // 更新滤波器状态
        updateFilterStatus();
    }

    // 从融合中将风输出转发到输出时间地平线
    calcOutputStates();

    // 停止用于负载测量的定时器
#if ENABLE_EKF_TIMING
    static uint32_t total_us;
    static uint32_t timing_counter;
    total_us += dal.micros() - timing_start_us;
    if (timing_counter++ == 4000) {
   
   
        DEV_PRINTF("ekf2 avg %.2f us\n", total_us / float(timing_counter));
        total_us = 0;
        timing_counter = 0;
    }
    hal.scheduler->restore_interrupts(istate);
#endif

    /*
      这是一个检查，用来应对车辆静止在地面时过于自信的状态。
      这种情况下，用户尝试解锁时会出现“陀螺仪仍在稳定”现象。
      在这种状态下，EKF无法恢复，所以我们进行硬重置，让它再试一次。
     */
    if (filterStatus.value != 0) {
   
   
        last_filter_ok_ms = dal.millis();
    }
    if (filterStatus.value == 0 &&
        last_filter_ok_ms != 0 &&
        dal.millis() - last_filter_ok_ms > 5000 &&
        !dal.get_armed()) {
   
   
        // 在健康状态之后不健康5秒，重置滤波器
        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 IMU%u 强制重置", (unsigned)imu_index);
        last_filter_ok_ms = 0;
        statesInitialised = false;
        InitialiseFilterBootstrap();
    }
}

2.3.2 healthy

检查基本的滤波器健康指标，并返回合并的健康状态

bool NavEKF2_core::healthy(void) const
{
   
   
    uint16_t faultInt;
    getFilterFaults(faultInt);
    if (faultInt > 0) {
   
   
        return false;
    }
    if (velTestRatio > 1 && posTestRatio > 1 && hgtTestRatio > 1) {
   
   
        // 如果三个指标都大于1，则说明滤波器的状态
        // 非常不健康。
        return false;
    }
    // 在使用前给滤波器一个时间来稳定
    if ((imuSampleTime_ms - ekfStartTime_ms) < 1000 ) {
   
   
        return false;
    }
    // 当处于地面且在静态模式下时，位置和高度的创新必须在限制范围内
    ftype horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
    if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsF(hgtInnovFiltState) > 1.0f))) {
   
   
        return false;
    }

    // 一切正常
    return true;
}

2.3.3 errorScore

返回一个合并的错误分数，数值越高表示错误越大；旨在供前端使用，以确定哪个是主要的EKF

ftype NavEKF2_core::errorScore() const
{
   
   
    ftype score = 0.0f;
    if (tiltAlignComplete && yawAlignComplete) {
   
   
        // 检查GPS融合性能
        score = MAX(score, 0.5f * (velTestRatio + posTestRatio));
        // 检查高度计融合性能
        score = MAX(score, hgtTestRatio);
        // 检查姿态修正
        const ftype tiltErrThreshold = 0.05f;
        score = MAX(score, tiltErrFilt / tiltErrThreshold);
    }
    return score;
}

2.4 位置和速度

getPosNE, getPosD: 获取东北和下方向位置。
getVelNED: 提供NED速度。

2.4.1 getPosNE

获取NE坐标系下机体相对于起始点的二维矢量。

bool NavEKF2_core::getPosNE(Vector2f &posNE) const
{
   
   
    // 有三种操作模式：绝对位置（GPS 融合），相对位置（光学流融合）和恒定位置（没有位置估计可用）
    if (PV_AidingMode != AID_NONE) {
   
   
        // 这是正常的操作模式，我们可以使用 EKF 位置状态
        // 校正 IMU 偏移（EKF 计算在 IMU 处）
        posNE.x = outputDataNew.position.x + posOffsetNED.x;
        posNE.y = outputDataNew.position.y + posOffsetNED.y;
        return true;

    } else {
   
   
        // 在恒定位置模式下，EKF 位置状态在原点，因此我们不能将它们用作位置估计
        if(validOrigin) {
   
   
            if ((dal.gps().status(dal.gps()