ODrive0.5.1程序分析#4 闭环控制程序（run_closed_loop_control_loop）

趁起东风

已于 2022-10-23 15:32:52 修改

阅读量2.8k

点赞数 10

文章标签：算法 python 嵌入式硬件

于 2022-08-31 11:24:27 首次发布

本文链接：https://blog.csdn.net/ledros/article/details/126605316

版权

闭环控制算法程序分析

固件版本fw-0.5.1（0积分下载）

代码分析

1. 状态输入

当输入状态为AXIS_STATE_CLOSED_LOOP_CONTROL时，检查电机校准和编码器校准是否已完成（参考文章#1与#2），然后运行run_closed_loop_control_loop程序

        bool status;
        switch (current_state_) {
            case AXIS_STATE_CLOSED_LOOP_CONTROL: {
                if (!motor_.is_calibrated_ || motor_.config_.direction==0)
                    goto invalid_state_label;
                if (!encoder_.is_ready_)
                    goto invalid_state_label;
                watchdog_feed();
                status = run_closed_loop_control_loop();
            } break;
         }

2. 加载编码器

进入run_closed_loop_control_loop，load_encoder_axis为设定的编码器数量，其作为输入进入select_encoder函数，AXIS_COUNT值为2，检测编码器数量不超过两个时，对一系列位置速度数据进行赋值（具体含义在下面讨论）。

bool Axis::run_closed_loop_control_loop() {
    if (!controller_.select_encoder(controller_.config_.load_encoder_axis)) {
        return error_ |= ERROR_CONTROLLER_FAILED, false;
    }

bool Controller::select_encoder(size_t encoder_num) {
    if (encoder_num < AXIS_COUNT) {
        Axis* ax = axes[encoder_num];
        pos_estimate_circular_src_ = &ax->encoder_.pos_circular_;
        pos_wrap_src_ = &config_.circular_setpoint_range;
        pos_estimate_linear_src_ = &ax->encoder_.pos_estimate_;
        pos_estimate_valid_src_ = &ax->encoder_.pos_estimate_valid_;
        vel_estimate_src_ = &ax->encoder_.vel_estimate_;
        vel_estimate_valid_src_ = &ax->encoder_.vel_estimate_valid_;
        return true;
    } else {
        return set_error(Controller::ERROR_INVALID_LOAD_ENCODER), false;
    }
}

3.初始位置设定

为了避免进入闭环产生瞬态运动，将当前位置设定为期望位置
第一个if判断的circular_setpoints在该版本值始终为false（当值为真时，进入循环位置模式，适用于无限转动的机械，取消位置设置上限，同时因为浮点舍入，而损失了位置精度）
因此执行else里的判断pos_estimate_linear_src_在上步select_encoder函数中取址了pos_estimate_
在函数encoder_.updata中利用采样时间current_meas_period * 估计速度vel_estimate_counts_（初值为0）更新估计位置pos_estimate_counts_（初值为0），然后计算与实际位置值shadow_count_的差值delta_pos_counts（单位count）,差值乘以锁相环kp（单位count/s/count）再乘以采样时间current_meas_period(单位s)对位置估计值进行补偿得到新的估计位置pos_estimate_counts_，然后除以设置的cpr值得到已圈数turn为单位的位置值pos_estimate_。接上面分析pos_estimate_ 传到 select_encoder() 函数中，pos_estimate_linear_src_ 取址pos_estimate_，然后返回run_closed_loop_control_loop() 函数而pos_setpoint_ 和input_pos_ 取址 pos_estimate_linear_src_ 得到设定位置。
然后进入 input_pos_updated函数，内容只是对input_pos_updated bool型变量赋值为ture,为之后控制做准备。

    //axis.cpp-接run_closed_loop_control_loop()函数
    // To avoid any transient on startup, we intialize the setpoint to be the current position
    if (controller_.config_.circular_setpoints) {
        if (!controller_.pos_estimate_circular_src_) {
            return error_ |= ERROR_CONTROLLER_FAILED, false;
        }
        else {
            controller_.pos_setpoint_ = *controller_.pos_estimate_circular_src_;
            controller_.input_pos_ = *controller_.pos_estimate_circular_src_;
        }
    }
    else {
        if (!controller_.pos_estimate_linear_src_) {
            return error_ |= ERROR_CONTROLLER_FAILED, false;
        }
        else {
            controller_.pos_setpoint_ = *controller_.pos_estimate_linear_src_;
            controller_.input_pos_ = *controller_.pos_estimate_linear_src_;
        }
    }
    controller_.input_pos_updated();

    //encoder.cpp-updata()函数
     run pll (for now pll is in units of encoder counts)
    // Predict current pos
    pos_estimate_counts_ += current_meas_period * vel_estimate_counts_;
    pos_cpr_counts_      += current_meas_period * vel_estimate_counts_;
    // discrete phase detector
    float delta_pos_counts = (float)(shadow_count_ - (int32_t)std::floor(pos_estimate_counts_));
    float delta_pos_cpr_counts = (float)(count_in_cpr_ - (int32_t)std::floor(pos_cpr_counts_));
    delta_pos_cpr_counts = wrap_pm(delta_pos_cpr_counts, 0.5f * (float)(config_.cpr));
    // pll feedback
    pos_estimate_counts_ += current_meas_period * pll_kp_ * delta_pos_counts;
    pos_cpr_counts_ += current_meas_period * pll_kp_ * delta_pos_cpr_counts;
    pos_cpr_counts_ = fmodf_pos(pos_cpr_counts_, (float)(config_.cpr));
    vel_estimate_counts_ += current_meas_period * pll_ki_ * delta_pos_cpr_counts;
    bool snap_to_zero_vel = false;
    if (std::abs(vel_estimate_counts_) < 0.5f * current_meas_period * pll_ki_) {
        vel_estimate_counts_ = 0.0f;  //align delta-sigma on zero to prevent jitter
        snap_to_zero_vel = true;
    }

    // Outputs from Encoder for Controller
    float pos_cpr_last = pos_cpr_;
    pos_estimate_ = pos_estimate_counts_ / (float)config_.cpr;
    vel_estimate_ = vel_estimate_counts_ / (float)config_.cpr;

4.主循环框架

回到run_closed_loop_control_loop() 函数，为避免力矩积分项饱和，对其vel_integrator_torque_ 进行初始化，该参数在下方 controller_.update() 函数中进行更新；
set_step_dir_active() 函数的输入为enable_step_dir默认设置为false，该函数为步进电机配置GPIO口为接收脉冲step及方向dir的功能。对控制伺服电机该参数配置为false即可；
run_control_loop(） 函数为循环函数，当下面主循环内容返回false时循环停止，同时包含对于编码器等数据的更新，详细分析看#2文章；主循环内容包括控制模式更新controller_.update()以及电机运动参数更新motor_.update。

    //axis.cpp-接run_closed_loop_control_loop()函数
    // Avoid integrator windup issues
    controller_.vel_integrator_torque_ = 0.0f;

    set_step_dir_active(config_.enable_step_dir);
    run_control_loop([this](){
        // Note that all estimators are updated in the loop prefix in run_control_loop
        float torque_setpoint;
        if (!controller_.update(&torque_setpoint))
            return error_ |= ERROR_CONTROLLER_FAILED, false;

        float phase_vel = (2*M_PI) * encoder_.vel_estimate_ * motor_.config_.pole_pairs;
        if (!motor_.update(torque_setpoint, encoder_.phase_, phase_vel))
            return false; // set_error should update axis.error_

        return true;
    });
    set_step_dir_active(config_.enable_step_dir && config_.step_dir_always_on);
    return check_for_errors();

5.控制模式更新

控制模式更新函数controller_.update()，首先是三目运算符判断pos_estimate_valid_src_ 不为0时，对pos_estimate_linear进行赋值，另两个参数同理。
anticogging_pos为补偿齿槽效应位置，下面判断参数calib_anticogging决定是否运行抗齿槽效应校准函数anticogging_calibration(),默认为false不进行抗齿槽效应校准。
circular_setpoints循环位置模式，上文已解释，为false，不执行if函数内容。

bool Controller::update(float* torque_setpoint_output) {
    float* pos_estimate_linear = (pos_estimate_valid_src_ && *pos_estimate_valid_src_)
            ? pos_estimate_linear_src_ : nullptr;
    float* pos_estimate_circular = (pos_estimate_valid_src_ && *pos_estimate_valid_src_)
            ? pos_estimate_circular_src_ : nullptr;
    float* vel_estimate_src = (vel_estimate_valid_src_ && *vel_estimate_valid_src_)
            ? vel_estimate_src_ : nullptr;

    // Calib_anticogging is only true when calibration is occurring, so we can't block anticogging_pos
    float anticogging_pos = axis_->encoder_.pos_estimate_ / axis_->encoder_.getCoggingRatio();
    if (config_.anticogging.calib_anticogging) {
        if (!axis_->encoder_.pos_estimate_valid_ || !axis_->encoder_.vel_estimate_valid_) {
            set_error(ERROR_INVALID_ESTIMATE);
            return false;
        }
        // non-blocking
        anticogging_calibration(axis_->encoder_.pos_estimate_, axis_->encoder_.vel_estimate_);
    }

    // TODO also enable circular deltas for 2nd order filter, etc.
    if (config_.circular_setpoints) {
        // Keep pos setpoint from drifting
        input_pos_ = fmodf_pos(input_pos_, config_.circular_setpoint_range);
    }

接控制模式更新函数controller_.update()，判断控制信号的输入模式，分别有闲置模式INPUT_MODE_INACTIVE,直通模式INPUT_MODE_PASSTHROUGH，速度斜波模式INPUT_MODE_VEL_RAMP，力矩斜波模式INPUT_MODE_TORQUE_RAMP，位置滤波模式INPUT_MODE_POS_FILTER，梯形轨迹控制模式INPUT_MODE_TRAP_TRAJ。本文分析闭环程序流程，以直通模式为例，其余模式分析在后面文章进行，在直通模式中，对位置速度力矩设定值pos/vel/torque_setpoint_ 进行赋值，而位置/速度/力矩的输入值 input_pos_/vel_/torque_ 通过CAN信号在函数set_input_pos_callback()中进行输入，CAN设定为8个字节即64位，前32位为input_pos_，32-48位为input_vel_,48-64为input_torque_。从而得到了设定值。

    switch (config_.input_mode) {
        case INPUT_MODE_INACTIVE: {
            // do nothing
        } break;
        case INPUT_MODE_PASSTHROUGH: {
            pos_setpoint_ = input_pos_;
            vel_setpoint_ = input_vel_;
            torque_setpoint_ = input_torque_; 
        } break;
       case INPUT_MODE_VEL_RAMP: {...
       } break;
       case INPUT_MODE_TORQUE_RAMP:{... 
       } break;
       case INPUT_MODE_POS_FILTER: {...
       } break;
       case INPUT_MODE_TRAP_TRAJ: {...
       } break;
       default: {
            set_error(ERROR_INVALID_INPUT_MODE);
            return false;
        }
        
    }

void CANSimple::set_input_pos_callback(Axis* axis, can_Message_t& msg) {
    axis->controller_.input_pos_ = can_getSignal<float>(msg, 0, 32, true);
    axis->controller_.input_vel_ = can_getSignal<int16_t>(msg, 32, 16, true, 0.001f, 0);
    axis->controller_.input_torque_ = can_getSignal<int16_t>(msg, 48, 16, true, 0.001f, 0);
    axis->controller_.input_pos_updated();
}

6.输出力矩计算

定义好位置速度力矩的设定值，继续往下执行程序controller_update()
首先令速度期望值等于速度设定值，然后判断控制模式（control_mode），电压模式为0，力矩模式为1，速度模式为2，位置模式为3，if语句中判断条件为是否为位置控制模式（POSITION_CONTROL），下面的if判断是否为循环位置（circular_setpoints），上文已说明不使用循环位置，此处为否，故循环位置的if中函数省略，else中判断是否已获取编码器位置，位置控制模式下，利用CAN传入的位置设定值pos_setpoint_ 减去编码器位置获取值 pos_estimate_linear，计算得到位置偏差pos_err。速度设定值vel_des等于CAN输入的设定值加上位置增益pos_gain单位（turn/s/turn）乘以位置偏差pos_err单位（turn）。
然后利用clmp函数保证速度设定值vel_des在设定的最大最小值之间。
之后用if函数判断编码器速度获取值vel_estimate_src是否大于1.2（默认系数）速度限制值vel_lim，是的话报错过速ERROR_OVERSPEED返回false。

    float vel_des = vel_setpoint_;
    if (config_.control_mode >= CONTROL_MODE_POSITION_CONTROL) {
        float pos_err;

        if (config_.circular_setpoints) {...
        } else {
            if(!pos_estimate_linear) {
                set_error(ERROR_INVALID_ESTIMATE);
                return false;
            }
            pos_err = pos_setpoint_ - *pos_estimate_linear;
        }

        vel_des += config_.pos_gain * pos_err;
    }
    float vel_lim = config_.vel_limit;
    if (config_.enable_vel_limit) {
        vel_des = std::clamp(vel_des, -vel_lim, vel_lim);
    }
    if (config_.enable_overspeed_error) {  // 0.0f to disable
        if (!vel_estimate_src) {
            set_error(ERROR_INVALID_ESTIMATE);
            return false;
        }
        if (std::abs(*vel_estimate_src) > config_.vel_limit_tolerance * vel_lim) {
            set_error(ERROR_OVERSPEED);
            return false;
        }
    }

对位置速度设定好之后，进入该controller_updata（） 函数的输出—力矩的设定
首先对速度增益以及速度积分项增益进行赋值，该值在config中也即在上位机能够进行配置，if函数判断是否为感应电机，感应电机相关内容忽略，之后令变量torque等于力矩设定值torque_setpoint_，再判断齿隙补偿是否使能，否忽略函数内容。
创建变量速度偏差v_err
当控制模式大于等于速度控制模式时，即在位置控制和速度控制模式下，利用速度设定值vel_des减去编码器速度获取值vel_estimate_src得出值，计算力矩torque加等于速度增益vel_gain乘以速度偏差v_err再加上速度积分增益得出的力矩vel_integrator_torque_，速度积分增益的力矩初值为0，数值在下方程序计算。
当控制模式小于速度控制模式，即在电压控制和扭矩控制模式下，先if函数判断编码器是否能正常获取数据，否的话报错ERROR_INVALID_ESTIMATE返回false。之后计算力矩torque等于limitVel（） 函数内计算内容，为力矩最值TMAX/TMIN等于正/负速度增益vel_gain乘以（速度限制值vel_limit减去编码器速度获取值vel_estimate_src），然后利用clamp函数保证此时的力矩值torque也即力矩输入值torque_setpoint_ 在最值范围内。
在上面两种控制模式情况下都能算出力矩torque的值，再算出力矩的限制值Tlim,Tlim的值是根据力矩常数torque_constant乘以电流限制值current_lim得出的，力矩常数和电流限制值都可通过上位机配置。再判断力矩torque是否超过限制值，超过时取torque=±Tlim，并将布尔型变量limited置true，对于位置和速度控制模式下速度积分增益计算力矩，如果limited为false时，速度积分增益力矩vel_integrator_torque加等于速度积分增益单位[Nm/(turn/s * s)] 乘以电流采样周期 current_meas_period单位（s）再乘以 速度偏差v_err单位（turn/s）得出力矩值；当limited置true时，说明积分项饱和，不再继续增加值，而是原值乘以0.99，对积分增益力矩进行衰减。
最后将torque赋值torque_setpoint_output，也即函数的输出，进行对外力矩值的发送。

    float vel_gain = config_.vel_gain;
    float vel_integrator_gain = config_.vel_integrator_gain;
    if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_ACIM) {...}//感应电机内容忽略

    float torque = torque_setpoint_;
    if (anticogging_valid_ && config_.anticogging.anticogging_enabled) {...}//齿隙补偿内容忽略
    float v_err = 0.0f;
    if (config_.control_mode >= CONTROL_MODE_VELOCITY_CONTROL) {
        if (!vel_estimate_src) {
            set_error(ERROR_INVALID_ESTIMATE);
            return false;
        }

        v_err = vel_des - *vel_estimate_src;
        torque += (vel_gain * gain_scheduling_multiplier) * v_err;

        // Velocity integral action before limiting
        torque += vel_integrator_torque_;
    }

    // Velocity limiting in current mode
    if (config_.control_mode < CONTROL_MODE_VELOCITY_CONTROL && config_.enable_current_mode_vel_limit) {
        if (!vel_estimate_src) {
            set_error(ERROR_INVALID_ESTIMATE);
            return false;
        }
        torque = limitVel(config_.vel_limit, *vel_estimate_src, vel_gain, torque);
    }

    // Torque limiting
    bool limited = false;
    float Tlim = axis_->motor_.max_available_torque();
    if (torque > Tlim) {
        limited = true;
        torque = Tlim;
    }
    if (torque < -Tlim) {
        limited = true;
        torque = -Tlim;
    }

    // Velocity integrator (behaviour dependent on limiting)
    if (config_.control_mode < CONTROL_MODE_VELOCITY_CONTROL) {
        // reset integral if not in use
        vel_integrator_torque_ = 0.0f;
    } else {
        if (limited) {
            // TODO make decayfactor configurable
            vel_integrator_torque_ *= 0.99f;
        } else {
            vel_integrator_torque_ += ((vel_integrator_gain * gain_scheduling_multiplier) * current_meas_period) * v_err;
        }
    }

    if (torque_setpoint_output) *torque_setpoint_output = torque;
    return true;

7.电机控制参数更新

回到run_closed_loop_control_loop() 函数，controller_update 函数传出力矩值，之后进入对力矩的处理转化为对电机的控制参数，也即 motor_.update（） 函数内容。
首先计算相位电角速度phase_vel，其中vel_estimate为编码器的角速度获取值也即机械角速度，单位turn/s乘以2pi,变为以弧度表示的角速度，单位rad/s，再乘以极对数pole_pairs，将机械角速度变为电角速度。
然后将上一把传出的力矩设定值torque_setpoint、编码器检测位置phase_以及相位电角速度phase_vel作为输出参数进入函数motor_.update(）

        float phase_vel = (2*M_PI) * encoder_.vel_estimate_ * motor_.config_.pole_pairs;**相位电角速度phase_vel**
        if (!motor_.update(torque_setpoint, encoder_.phase_, phase_vel))
            return false;

进入电机控制更新程序motor_.update(），首先定义电流设定值current_setpoint，然后将相位和相速度乘以方向direction（1或-1，在编码器校准时得出此值，见#2文章），之后判断电机类型是否为感应电机，执行否的else内程序，电流设定值current_setpoint等于传入的力矩设定值torque_setpoint除以扭矩常数，电流设定值再乘以方向，根据电流限制函数得出电流限制值ilim，id等于上位机设置的id设定值再用clamp函数保证范围，iq等于由扭矩算出的电流设定值current_setpoint再用clamp函数保证输出范围，之后计算下一时刻相位pwm_phase，利用传入相位phase加上相位电角速度phase_vel乘以1.5倍的采样周期current_meas_period，得出相位差的下一时刻计算相电角度。
之后将iq,id,phase,pwm_phase参数传入对应电机类型的FOC计算函数，详情可见**#3 FOC算法程序**。

bool Motor::update(float torque_setpoint, float phase, float phase_vel) {
    float current_setpoint = 0.0f;
    phase *= config_.direction;
    phase_vel *= config_.direction;

    if (config_.motor_type == MOTOR_TYPE_ACIM) {...}
    else {
        current_setpoint = torque_setpoint / config_.torque_constant;
    }
    current_setpoint *= config_.direction;

    // TODO: 2-norm vs independent clamping (current could be sqrt(2) bigger)
    float ilim = effective_current_lim();
    float id = std::clamp(current_control_.Id_setpoint, -ilim, ilim);
    float iq = std::clamp(current_setpoint, -ilim, ilim);

    if (config_.motor_type == MOTOR_TYPE_ACIM) {...}

    float pwm_phase = phase + 1.5f * current_meas_period * phase_vel;

    // Execute current command
    switch(config_.motor_type){
        case MOTOR_TYPE_HIGH_CURRENT: return FOC_current(id, iq, phase, pwm_phase); break;
        case MOTOR_TYPE_ACIM: return FOC_current(id, iq, phase, pwm_phase); break;
        case MOTOR_TYPE_GIMBAL: return FOC_voltage(id, iq, pwm_phase); break;
        default: set_error(ERROR_NOT_IMPLEMENTED_MOTOR_TYPE); return false; break;
    }
    return true;
}