目录
2.2.3 MitTrajectoryPlanningHandle()
一、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 中所需要的变量：
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 MitCtlInput
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
typedef struct
{
/* 目标q轴电流 */
real32_T iq_tar_A;
}
MitCtlOutput;
4.1.3 MotorCtlSmConfig
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;
}
MotorCtlSmConfig;4.2 仿真分析
4.2.1 仿真概览
Mit_ctl 模型如下：
MIT 模式的建模比较简单，如同公式一样，将 Kp、Kd 和前馈扭矩相加。
最后求和 Add1 模块将三个来源 tq_set (前馈扭矩) + P 项 + D 项相加，得到总需求扭矩。扭矩转电流：Divide 模块将总扭矩除以电机扭矩常数 kt，计算出理想的 q 轴电流 Iq：
4.2.2 Kp 环
将编码器误差相减，除总一圈的编码器的值，再乘 2π，得到 rad 值，最后乘上速度阻尼系数：
- pos_tar - pos_now = 位置误差
- enc_line = 编码器每转总脉冲数
- Kp = 位置刚度 (Nm/rad)
至此，我们得到 Kp 输出的力矩值。
4.2.3 Kd 环
- speed_tar = 目标速度 (pulse/s)
- enc_line = 编码器每转总脉冲数
- ω_now = 当前速度 (rad/s)
- Kd = 速度阻尼 (Nm·s/rad)
4.2.4 乘 kt
最后把这三项乘以 Kt 转矩常数 (Nm/A) 得到我们的输出 iq 电流值
736
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
