YuanHub 源码分析【六】MIT 模式

目录

[一、MIT 模式简介](#一、MIT 模式简介)

[1.1 MIT 简介](#1.1 MIT 简介)

[1.2 弹簧与阻尼](#1.2 弹簧与阻尼)

[1.3 Kd 项的效果](#1.3 Kd 项的效果)

二、代码逻辑

[2.1 总览](#2.1 总览)

[2.2 AppMotionParam 结构体](#2.2 AppMotionParam 结构体)

[2.2 AppRun() 系列逻辑](#2.2 AppRun() 系列逻辑)

[2.2.1 AppRun()](#2.2.1 AppRun())

[2.2.2 MitModeRun()](#2.2.2 MitModeRun())

[2.2.3 MitTrajectoryPlanningHandle()](#2.2.3 MitTrajectoryPlanningHandle())

[2.3 ModeRun() 系列逻辑](#2.3 ModeRun() 系列逻辑)

[2.3.1 MitCtlStep()](#2.3.1 MitCtlStep())

[2.3.2 mit_ctl()](#2.3.2 mit_ctl())

四、仿真分析

[4.1 数据结构](#4.1 数据结构)

[4.1.1 MitCtlInput](#4.1.1 MitCtlInput)

[4.1.2 MitCtlOutput](#4.1.2 MitCtlOutput)

[4.1.3 MotorCtlSmConfig](#4.1.3 MotorCtlSmConfig)

[4.2 仿真分析](#4.2 仿真分析)

一、MIT 模式简介

1.1 MIT 简介

MIT 模式 (Mixed Integrated Torque，混合集成扭矩控制) 在现代四足机器人和柔性机械臂领域，已经成为了一个行业标准级的底层关节控制接口协议。它最早由 MIT Cheetah (麻省理工猎豹) 团队提出、应用并开源，因此在圈内得名。

其计算输出力矩方式如下：

= 位置比例系数 / 虚拟刚度 (决定虚拟弹簧有多硬)
= 目标位置
= 现在的位置
= 速度阻尼系数 / 虚拟阻尼 (决定减震器有多粘稠，吸收震荡)
= 目标速度
= 现在的速度
= 前馈力矩 (上层动力学模型提前算好的基础力矩，比如用来抵消腿部自身的重力或惯性力)

TIM 作为给机器人所使用的控制方式去掉了 Ki 积分项，相当于允许速度、位置和力矩产生当前值与目标值的误差，从而形成控制的刚性。

1.2 弹簧与阻尼

MIT 的理想控制是把电机仿真成一根泡在粘稠硅油里的弹簧。

Kp 变成了刚度 (模拟弹簧)：

它模拟的是一根虚拟弹簧。你可以把它想象成在机器狗的实际位置和目标位置之间，拉了一根弹簧。Kp 越大，弹簧越硬。位置误差就是弹簧被拉伸的长度，产生的力矩就是弹簧的拉力。

Kd 变成了阻尼 (模拟硅油)：

它模拟的是一个虚拟减震器 (就像泡在粘稠的油里)。它不看位置差，只看速度差。Kd 越大，对抗速度变化的能力越强，主要用来吸收弹簧产生的震荡能量，防止机器狗的腿像装了弹簧一样一直抖个不停。

其弹簧有经典的胡克定律：

F = 恢复力 (N)
k = 弹簧硬度系数越大弹簧越硬 (N/m)
x = 弹簧拉伸位移量 (m)

其中恢复力是 MIT 中 Kp 项的输出值；k 就是 Kp 常数；x 是目标位置和实际位置的差。

硅油有粘性阻尼定律：

Fd = 阻尼力 (N)
c = 阻尼系数代表粘稠度 (N·s/m)
v = 速度 (m/s)

其中阻尼力是 MIT 中 Kd 项的输出值；c 就是 Kd 常数；v 是目标速度和实际速度的差。

至此，我们得到了 TIM 真正想仿真的东西：

MITout = MIT 计算出的力矩
F = 恢复力 (N)
Fd = 阻尼力 (N)

1.3 Kd 项的效果

我们知道位置的微分就是速度，对速度做差变成了天然的微分项。

在 Kp 项 (位置比例系数 / 虚拟刚度) 不变的情况下，调整 Kd (速度阻尼系数 / 虚拟阻尼) 的效果如下：

在阻尼系数为 1 的时候，弹簧反复回弹：

而在阻尼系数为 6 的时候，回弹周期明显减少：

二、代码逻辑

2.1 总览

2.2 AppMotionParam 结构体

下面的结构描述了 MIT 中所需要的变量：

cpp 复制代码

typedef struct
{
    int64_t Target_position; //位置目标值
    float Profile_velocity; //轮廓速度
    float Profile_acceleration; //轮廓加速度
    float Profile_deceleration; //轮廓减速度
    float Quick_stop_deceleration; //快速停止减速度
    int32_t Motion_profile_type; //运动轮廓模式
    int64_t Home_offset; //回零偏移
    int8_t Homing_method; //回零方法
    float Target_velocity; //速度目标值
    float Target_torque; //力矩目标值
    float Torque_slope; //力矩上升斜率
    float Encoder_calibration_speed; //编码器校准速度
    float MIT_feedforward_torque; //MIT前馈力矩
    int64_t MIT_target_position; //MIT目标位置
    float MIT_max_current; //MIT最大输出电流
    float MIT_target_velocity; //MIT目标速度
    float MIT_kp; //位置刚度
    float MIT_kd; //速度阻尼系数
    uint8_t Interp_time_value; //插值时间基数
    int8_t Interp_time_index; //插值时间指数
    float Homing_speed_search_for_switch; //回零搜索开关速度
    float Homing_speed_search_for_zero; //回零搜索零点速度
    float Homing_acceleration; //回零加速度
}AppMotionParam;

2.2 AppRun() 系列逻辑

2.2.1 AppRun()

在 AppRun() 中选择合适的启动模式程序，通过函数指针方式调用，所有模式的初始化、运行、启动、停止函数等均在这个表格内：

cpp 复制代码

// 应用程序列表
static const AppTable app_table[APP_NUM] =
{
    {APP_PP_MODE, PpModeInit, PpModeStart, PpModeRun, PpModeStop, APP_TYPE_PDS},
    {APP_PV_MODE, PvModeInit, PvModeStart, PvModeRun, PvModeStop, APP_TYPE_PDS},
    {APP_PT_MODE, PtModeInit, PtModeStart, PtModeRun, PtModeStop, APP_TYPE_PDS},
    {APP_MIT_MODE, MitModeInit, MitModeStart, MitModeRun, MitModeStop, APP_TYPE_PDS},
    {APP_HM_MODE, HomingModeInit, HomingModeStart, HomingModeRun, HomingModeStop, APP_TYPE_PDS},
    {APP_CSP_MODE, CspModeInit, CspModeStart, CspModeRun, CspModeStop, APP_TYPE_PDS},
    {APP_CSV_MODE, CsvModeInit, CsvModeStart, CsvModeRun, CsvModeStop, APP_TYPE_PDS},
    {APP_CST_MODE, CstModeInit, CstModeStart, CstModeRun, CstModeStop, APP_TYPE_PDS},
    {APP_DIR_ID_MODE, IdDirModeInit, IdDirModeStart, IdDirModeRun, IdDirModeStop, APP_TYPE_PDS},
    {APP_ELEC_ANGLE_ID_MODE, IdElecAngleModeInit, IdElecAngleModeStart, IdElecAngleModeRun, IdElecAngleModeStop, APP_TYPE_PDS},
    {APP_ELEC_ID_MODE, IdElecModeInit, IdElecModeStart, IdElecModeRun, IdElecModeStop, APP_TYPE_PDS},
    {APP_MEC_ID_MODE, IdMecModeInit, IdMecModeStart, IdMecModeRun, IdMecModeStop, APP_TYPE_PDS},
    {APP_POLE_PAIRS_ID_MODE, IdPolePairsModeInit, IdPolePairsModeStart, IdPolePairsModeRun, IdPolePairsModeStop, APP_TYPE_PDS},
    {APP_TQ_FC_ID_MODE, IdTqFcModeInit, IdTqFcModeStart, IdTqFcModeRun, IdTqFcModeStop, APP_TYPE_PDS},
    {APP_NULL, NULL, NULL, NULL, NULL}, // 用于表示空闲状态
};

AppRun() 从中根据配置信息查找合适的函数进行调用：AppRun() 从中根据配置信息查找合适的函数进行调用：

cpp 复制代码

// 运行应用程序
void AppRun(uint8_t hw_ready_state)
{
    // 遍历应用程序列表
    for (int i = 0; i < APP_NUM; i++)
    {
        if (kAppInfo[i].enable)
        {

            // 启动应用程序
            if (kAppInfo[i].app_state == APP_STA_READY)
            {

                if (kAppInfo[i].app_ptr->start != NULL)
                {
                    kAppInfo[i].app_result = kAppInfo[i].app_ptr->start();
                    if (kAppInfo[i].app_result == APP_RET_RUNNING)
                    { // start处于等待状态
                        kAppInfo[i].app_state = APP_STA_READY;
                    }
                    else if (kAppInfo[i].app_result == APP_RET_SUCCESS)
                    { // start成功,进入run状态
                        kAppInfo[i].app_state = APP_STA_RUNNING;
                    }
                    else if (kAppInfo[i].app_result == APP_RET_FAIL)
                    { // start失败,进入错误状态
                        kAppInfo[i].app_state = APP_STA_ERROR;
                    }
                }
            }

            // 运行应用程序
            if (kAppInfo[i].app_state == APP_STA_RUNNING)
            {
                if (kAppInfo[i].app_ptr->run != NULL)
                {
                    // 回读PWM准备好状态，准备好则运行APP 否则等待处于 READY 状态
                    if (hw_ready_state)
                    {
                        kAppInfo[i].app_result = kAppInfo[i].app_ptr->run();
                        if (kAppInfo[i].app_result == APP_RET_RUNNING)
                        {
                            kAppInfo[i].app_state = APP_STA_RUNNING;
                        }
                        else if (kAppInfo[i].app_result == APP_RET_SUCCESS)
                        {
                            kAppInfo[i].app_state = APP_STA_SUCCESS;
                        }
                        else if (kAppInfo[i].app_result == APP_RET_FAIL)
                        {
                            kAppInfo[i].app_state = APP_STA_ERROR;
                        }
                    }
                    else
                    {
                        kAppInfo[i].app_state = APP_STA_READY;
                    }
                }
            }

            // 停止应用程序
            if ((kAppInfo[i].app_state == APP_STA_SUCCESS) || (kAppInfo[i].app_state == APP_STA_ERROR))
            {
                if (kAppInfo[i].app_ptr->stop != NULL)
                {
                    kAppInfo[i].app_ptr->stop();
                }
                kAppInfo[i].enable = false;           // 禁用应用程序
                kAppInfo[i].app_state = APP_STA_IDLE; // 设置应用程序状态为空闲
            }
        }
    }
}

2.2.2 MitModeRun()

MitModeRun() 是 MIT 控制模式的周期执行主函数。它根据上位机下发的控制指令、制动器状态和急停信号，决定电机当前应处于正常运行、暂停 (锁止) 还是紧急停车状态，并负责将负载端的控制参数 (位置、速度、扭矩、Kp、Kd) 换算为电机端的实际执行参数。

cpp 复制代码

AppResult MitModeRun()
{
    int64_t pos_tar_p_add = 0;

    kMitMode.now_Controlword = (APP_CONTROL_WORD)get_app_Controlword();

    if (kMitMode.now_Controlword == APP_CTRL_ENABLE)
    {
        if (!get_app_Emergency_brake_requested())
        {
            if (kMitMode.pre_Controlword == APP_CTRL_DISABLE)
            {
                // 重新使能后初始化规划器
                kMitMode.traj.iq_max_A = get_app_MIT_max_current();
                MitTrajectoryPlanningInit(&kMitMode.traj);
            }

            // 规划速度为0的情况：1. 暂停指令生效 2. 抱闸状态不为松闸
            if (get_app_Halt_running_cmd() == true || get_app_Brake_state() != BRAKE_STATE_RELEASED)
            {
                kMitMode.traj.pos_tar_p = get_app_Motor_position_actual_value();
                kMitMode.traj.speed_tar_p_s = 0;
            }
            else
            {
                kMitMode.pos_tar_p_add = (float)(get_app_MIT_target_position() - get_app_Position_actual_value()) *
                                         get_app_Reduction_ratio() * get_app_P_load_2_motor();
                kMitMode.traj.pos_tar_p = kMitMode.pos_tar_p_add + get_app_Motor_position_actual_value();

                kMitMode.traj.speed_tar_p_s = get_app_MIT_target_velocity() * get_app_Motor_rpm_2_pps() *
                                              get_app_Reduction_ratio();
            }

            kMitMode.traj.iq_max_A = get_app_MIT_max_current();
            kMitMode.traj.tq_set_NM = get_app_MIT_feedforward_torque() * get_app_Reduction_ratio_inv(); // 负载端转矩  转化 为电机端
            kMitMode.traj.kp_pos_NM_rad = get_app_MIT_kp() * get_app_Reduction_ratio_inv();             // 转化为电机端增益
            kMitMode.traj.kd_spd_NM_rad_s = get_app_MIT_kd() * get_app_Reduction_ratio_inv();           // 转化为电机端增益
        }
        else
        {
            set_app_Controlword(APP_CTRL_EMERGENCY_BRAKE); // 强制进入紧急停车（QuickStop）状态
            kMitMode.now_Controlword = APP_CTRL_EMERGENCY_BRAKE;
        }
    }

    // 紧急停车（QuickStop）处理
    if (kMitMode.now_Controlword == APP_CTRL_EMERGENCY_BRAKE)
    {
        kMitMode.traj.pos_tar_p = get_app_Motor_position_actual_value();
        kMitMode.traj.speed_tar_p_s = 0; // 如果处于急停状态，规划速度为0

        kMitMode.emergency_brake_mode = get_app_Quick_stop_option_code();
        // 在急停后失能电机模式下，检测到零速后电机失能
        if (kMitMode.emergency_brake_mode <= EMERGENCY_BRAKE_MODE_VOLTAGE_LIMIT)
        {
            kMitMode.check_status_val = (CheckStatusVal_t)app_get_check_status_val();
            if (kMitMode.emergency_brake_mode == EMERGENCY_BRAKE_MODE_DISABLED ||
                kMitMode.check_status_val.bits.velocity_zero == true)
            {
                set_app_Controlword(APP_CTRL_DISABLE);
                kMitMode.now_Controlword = APP_CTRL_DISABLE;
            }
        }
    }

    if (kMitMode.now_Controlword != APP_CTRL_DISABLE)
    {
        MitTrajectoryPlanningHandle(&kMitMode.traj);
    }

    kMitMode.pre_Controlword = kMitMode.now_Controlword;

    return APP_RET_RUNNING;
}

2.2.3 MitTrajectoryPlanningHandle()

MIT 模式不许用轨迹规划，直接透传即可：

cpp 复制代码

// MIT模式轨迹规划器处理函数
void MitTrajectoryPlanningHandle(MIT_TRAJECTORY_DATA *mit_traj_data)
{
    axis->mit_ctl_input.pos_tar_p = mit_traj_data->pos_tar_p;
    axis->mit_ctl_input.speed_tar_p_s = mit_traj_data->speed_tar_p_s;
    axis->mit_ctl_input.tq_set_NM = mit_traj_data->tq_set_NM;
    axis->mit_ctl_input.iq_max_A = mit_traj_data->iq_max_A;

    axis->mit_ctl_config.kp_pos_NM_rad = mit_traj_data->kp_pos_NM_rad;
    axis->mit_ctl_config.kd_spd_NM_rad_s = mit_traj_data->kd_spd_NM_rad_s;
}

2.3 ModeRun() 系列逻辑

2.3.1 MitCtlStep()

本函数负责在 MIT 控制模式下，获取实时的传感器反馈 (位置、速度)，将其喂给 MIT 控制算法 (PD 阻抗 + 前馈)，并将计算出的目标电流送入底层的电流环（FOC)，同时附加了一些工业级驱动器必备的平滑和补偿策略。

cpp 复制代码

// MIT模式
static void MitCtlStep(Axis *const axis, AxisDw *const axis_dw)
{
   // axis->mit_ctl_pd_input.tq_set_A  用户指令设定
   // axis->mit_ctl_pd_input.pos_tar_p  用户指令设定
   // axis->mit_ctl_pd_input.speed_tar_rad_s  用户指令设定

   axis->mit_ctl_input.pos_now_p = axis->motor_pos_sensor_output.enc_sum_p;   // 获取电机端当前绝对位置
   axis->mit_ctl_input.speed_now_rad_s = axis->speed_obs_pll_output.ev_rad_s; // 速度观测器输出

   mit_ctl(&axis->mit_ctl_input, &axis->mit_ctl_config,
           &axis->mit_ctl_output);

   axis->pos_speed_ctl_output.iq_tar_A = axis->mit_ctl_output.iq_tar_A; // 设置电流环目标电流

   // 转矩脉动与摩擦补偿
   TqFcComStep(axis, axis_dw);

   // 指令电流滤波
   CommandCurrentFilter(axis, axis_dw);
}

2.3.2 mit_ctl()

由 Simulink 模型生成的 MIT 计算函数：

cpp 复制代码

/* Output and update for referenced model: 'mit_ctl' */
void mit_ctl(const MitCtlInput *rtu_input, const MitCtlConfig *rtu_config,
             MitCtlOutput *rty_output)
{
    real32_T rtb_Divide;

    /* Product: '<S1>/Divide' incorporates:
     *  DataTypeConversion: '<S1>/Data Type Conversion'
     *  Gain: '<S3>/Gain1'
     *  Gain: '<S4>/Gain1'
     *  Product: '<S1>/Product'
     *  Product: '<S1>/Product1'
     *  Product: '<S3>/Product'
     *  Product: '<S4>/Product'
     *  Sum: '<S1>/Add'
     *  Sum: '<S1>/Add1'
     *  Sum: '<S1>/Add2'
     */

    ...
}

四、仿真分析

4.1 数据结构

4.1.1 MitCtlInput

cpp 复制代码

typedef struct
{
    /* 前馈力矩电流 */
    real32_T tq_set_NM;

    /* 目标位置 */
    int64_T pos_tar_p;

    /* 当前位置 */
    int64_T pos_now_p;

    /* q轴最大输出电流 */
    real32_T iq_max_A;

    /* 目标速度 */
    real32_T speed_tar_p_s;

    /* 当前速度 */
    real32_T speed_now_rad_s;
}

MitCtlInput;

4.1.2 MitCtlOutput

cpp 复制代码

typedef struct
{
    /* 目标q轴电流 */
    real32_T iq_tar_A;
}

MitCtlOutput;

4.1.3 MotorCtlSmConfig

cpp 复制代码

typedef struct
{
    /* 运行模式 */
    int8_T mode;

    /* 欠压保护阈值 */
    real32_T under_voltage_protection_V;

    /* 过压保护阈值 */
    real32_T over_voltage_protection_V;

    /* 过速保护阈值 */
    real32_T over_speed_protection_rad_s;

    /* 欠温保护阈值 */
    real32_T under_temperature_protection_d;

    /* 过温保护阈值 */
    real32_T over_temperature_protection_d;

    /* 位置跟随误差保护阈值 */
    int32_T position_following_error_protection;

    /* 保护生效 */
    uint32_T error_enable;

    /* 过流保护阈值 */
    real32_T over_current_protection_A;
}

4.2 仿真分析

Mit_ctl 模型如下：

MIT 模式的建模比较简单，如同公式一样，将 Kp、Kd和前馈扭矩相加。

最后求和 Add1 模块将三个来源 tq_set (前馈扭矩) + P 项 + D 项相加，得到总需求扭矩。扭矩转电流：Divide 模块将总扭矩除以电机扭矩常数 kt，计算出理想的 q 轴电流 Iq：