一、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 (速度阻尼系数 / 虚拟阻尼) 的效果如下二、代码逻辑2.1 总览2.2 AppMotionParam 结构体下面的结构描述了 MIT 中所需要的变量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() 中选择合适的启动模式程序通过函数指针方式调用所有模式的初始化、运行、启动、停止函数等均在这个表格内// 应用程序列表 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() 从中根据配置信息查找合适的函数进行调用// 运行应用程序 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) 换算为电机端的实际执行参数。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 模式不许用轨迹规划直接透传即可// 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)同时附加了一些工业级驱动器必备的平滑和补偿策略。// 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 计算函数/* 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 MitCtlInputtypedef 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 MitCtlOutputtypedef struct { /* 目标q轴电流 */ real32_T iq_tar_A; } MitCtlOutput;4.1.3 MotorCtlSmConfigtypedef 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