解析odrive抗齿槽算法讲解

解析odrive抗齿槽算

电机齿槽扭矩效应定义、产生原因等这里不再赘述，我前面编码器线性校准的文章也提到过，之前一直在用MITCheetah的方法，因为他的方法操作比较简洁，但是具体建表和查表的原理有点不好理解，odrive的原理非常的简单，并且效果不错所以这里讲一讲。

odirve的原理其实就是将电机一圈分为N个点，这个点数根据实际情况来定，也可以直接使用odrive的3600个点，点数并不重要。然后使用位置控制的方法来遍历这些点，将固定在这个位置的扭矩进行建表，然后在后面根据编码器的位置反馈来查表将对应的扭矩补偿上去即可，代码贴出来并加上注释。

/*
 * This anti-cogging implementation iterates through each encoder position,
 * waits for zero velocity & position error,
 * then samples the current required to maintain that position.
 * 
 * This holding current is added as a feedforward term in the control loop.
 */
bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) {
    float pos_err = input_pos_ - pos_estimate;
    // 这个if语句就是判断当前位置和期望位置以及速度和0小于一个阀值，来确定是否到了采样位置，如果到了就记录该点的扭矩vel_integrator_torque_
    if (std::abs(pos_err) <= config_.anticogging.calib_pos_threshold / (float)axis_->encoder_.config_.cpr &&
        std::abs(vel_estimate) < config_.anticogging.calib_vel_threshold / (float)axis_->encoder_.config_.cpr) {
        config_.anticogging.cogging_map[std::clamp<uint32_t>(config_.anticogging.index++, 0, 3600)] = vel_integrator_torque_;// 这里的config_.anticogging.index++要注意，后面期望位置是通过 input_pos_ = config_.anticogging.index * axis_->encoder_.getCoggingRatio()计算的，位置的递增在这里产生。
    }
    if (config_.anticogging.index < 3600) {
        config_.control_mode = CONTROL_MODE_POSITION_CONTROL;
        input_pos_ = config_.anticogging.index * axis_->encoder_.getCoggingRatio();// 采样完一个点index++后乘以encoder_.getCoggingRatio()，该函数返回1/3600，这里就是就是每个点单位input_pos_ + 1/3600（圈）个角度到下一个采样点。
        input_vel_ = 0.0f;
        input_torque_ = 0.0f;
        input_pos_updated();
        return false;
    } else {
        config_.anticogging.index = 0;
        config_.control_mode = CONTROL_MODE_POSITION_CONTROL;
        input_pos_ = 0.0f;  // Send the motor home
        input_vel_ = 0.0f;
        input_torque_ = 0.0f;
        input_pos_updated();
        anticogging_valid_ = true;
        config_.anticogging.calib_anticogging = false;
        return true;
    }
}

这个就是主要的函数，其实还是蛮短的，这里对函数进行解析一下。要搞懂这个函数首先要知道几个变量的意义，if语句中的config_.anticogging.calib_pos_threshold和onfig_.anticogging.calib_vel_threshold，分别是位置阀值和速度阀值，小于这两个值就认为是到达了采样点。

	// Anti-cogging is enabled after calibration
    // We get the current position and apply a current feed-forward
    // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1)
    if (anticogging_valid_ && config_.anticogging.anticogging_enabled) {
        if (!anticogging_pos_estimate.has_value()) {
            set_error(ERROR_INVALID_ESTIMATE);
            return false;
        }
        float anticogging_pos = *anticogging_pos_estimate / axis_->encoder_.getCoggingRatio();
        torque += config_.anticogging.cogging_map[std::clamp(mod((int)anticogging_pos, 3600), 0, 3600)];
    }

校准完后，直接通过编码器反馈的位置找到表中对应的力矩补偿上去，其中查表方法要搞明白，建表前其实有个先到绝对位置的0的操作，所以建表是从index=0开始的，对应就是绝对角度的0/3600，每次补偿通过编码器反馈的绝对角度！绝对角度！绝对角度！说三遍是一圈的绝对角度，然后通过这个绝对角度找到离这个绝对角度最近的采样位置对应的力矩进行补偿。