HPM知识库
先楫半导体官方公众号全新上线「HPM 知识库」专栏。我们将在这里不定期更新技术文档、开发指南与实战教程，打造先楫 MCU 开发的 “一站式技术参考指南” ! 了解更多，欢迎访问 https://kb.hpmicro.com/
概述
力位混合控制（Hybrid Force-Position Control）是一种结合力控制和位置控制的阻抗控制方法，广泛应用于机器人关节控制、柔顺装配、人机交互等场景。本库实现了基于PD+前馈的阻抗控制算法，集成于HPM MCL电机控制库中。
控制原理
力位混合控制的核心思想是让关节表现出期望的机械阻抗特性，即弹簧-阻尼系统特性。控制律如下：
其中：
该控制律模拟了一个虚拟的弹簧-阻尼系统：
为获得无超调的快速响应，通常采用临界阻尼设计：
其中 J 为关节等效转动惯量。对于未知惯量的系统，可简化为：
系统架构
力位混合控制作为位置/力外环，输出力矩指令给FOC电流内环：
力矩到电流的转换公式：
API说明
配置结构体
typedef struct {float kp; /* 位置刚度 (N·m/rad) */float kd; /* 阻尼系数 (N·m·s/rad) */float tau_ff; /* 前馈力矩 (N·m) */float q_des; /* 期望位置 (rad) */float dq_des; /* 期望速度 (rad/s) */float tau_max; /* 最大输出力矩 (N·m) */float tau_min; /* 最小输出力矩 (N·m) */float speed_lpf_alpha; /* 速度低通滤波系数 (0-1) */float speed_deadzone; /* 速度死区 (rad/s) */} mcl_hybrid_ctrl_cfg_t;
状态结构体
typedef struct {float q_actual; /* 实际位置 (rad) - 输入 */float dq_actual; /* 实际速度 (rad/s) - 输入 */float tau_output; /* 输出力矩 (N·m) - 输出 */float pos_error; /* 位置误差 (rad) - 输出 */float vel_error; /* 速度误差 (rad/s) - 输出 */float speed_lpf; /* 滤波后速度 (rad/s) - 内部 */} mcl_hybrid_ctrl_state_t;
使用示例
以下为 bldc_foc 示例中的初始化代码，适用于空载直驱电机：
void motor0_hybrid_ctrl_init(void){/* 初始化配置结构体 */mcl_hybrid_ctrl_init(&motor0.hybrid_ctrl_cfg);/* 清零状态结构体 */memset(&motor0.hybrid_ctrl_state, 0, sizeof(motor0.hybrid_ctrl_state));/* 设置PD参数 - 空载直驱电机参数较小 */mcl_hybrid_ctrl_set_kp(&motor0.hybrid_ctrl_cfg, 0.06f);mcl_hybrid_ctrl_set_kd(&motor0.hybrid_ctrl_cfg, 0.001429f);/* 设置初始期望位置和速度 */mcl_hybrid_ctrl_set_position(&motor0.hybrid_ctrl_cfg, 0.0f);mcl_hybrid_ctrl_set_velocity(&motor0.hybrid_ctrl_cfg, 0.0f);/* 设置力矩限幅，防止过流 */mcl_hybrid_ctrl_set_limits(&motor0.hybrid_ctrl_cfg, -0.5f, 0.5f);/* 设置速度滤波：alpha=0.003（强滤波），死区=0.1 rad/s */mcl_hybrid_ctrl_set_speed_filter(&motor0.hybrid_ctrl_cfg, 0.003f, 0.1f);}
在ADC中断（PWM周期触发，20kHz）中执行控制算法：
void isr_adc(void){uint32_t status;mcl_user_value_t user_current;status = hpm_adc_get_status_flags(&hpm_adc_u);if ((status & BOARD_BLDC_ADC_TRIG_FLAG) != 0) {hpm_adc_clear_status_flags(&hpm_adc_u, BOARD_BLDC_ADC_TRIG_FLAG);/* 编码器数据处理 */hpm_mcl_encoder_process(&motor0.encoder,motor0.cfg.mcl.physical.time.mcu_clock_tick / PWM_FREQUENCY);if (hybrid_ctrl_mode) {/* 步骤1：获取编码器反馈 */hpm_mcl_encoder_get_absolute_theta(&motor0.encoder,&motor0.hybrid_ctrl_state.q_actual);motor0.hybrid_ctrl_state.dq_actual =hpm_mcl_encoder_get_speed(&motor0.encoder);/* 步骤2：执行力位混合控制算法 */mcl_hybrid_ctrl_step(&motor0.hybrid_ctrl_cfg,&motor0.hybrid_ctrl_state);/* 步骤3：力矩转电流，发送给FOC电流环 */user_current.enable = true;/* kt = 0.053 N·m/A（电机转矩常数） */user_current.value = motor0.hybrid_ctrl_state.tau_output / 0.053f;hpm_mcl_loop_set_current_q(&motor0.loop, user_current);}/* 执行FOC电流环 */hpm_mcl_loop(&motor0.loop);}}
主循环中的模式选择和位置输入：
if (user_mode == 2) {/* 初始化力位混合控制 */motor0_hybrid_ctrl_init();hybrid_ctrl_mode = true;/* 禁用速度环，由力位混合控制接管 */user_speed.enable = false;hpm_mcl_loop_set_speed(&motor0.loop, user_speed);printf("\r\nHybrid control mode\r\n");printf("kp=%.3f, kd=%.3f, tau_limit=%.3f\r\n",(double)motor0.hybrid_ctrl_cfg.kp,(double)motor0.hybrid_ctrl_cfg.kd,(double)motor0.hybrid_ctrl_cfg.tau_max);while (1) {/* 读取用户输入的目标位置（度） */position = atoi(input_data);/* 角度转弧度：deg * (π/180) ≈ deg * 0.01745 */mcl_hybrid_ctrl_set_position(&motor0.hybrid_ctrl_cfg,(float)position * 0.00157079632f); /* 实际为 deg * π/180 / 10 */printf("Pos: %d deg, Tau: %.4f Nm, Pos_err: %.4f rad\r\n",position,(double)motor0.hybrid_ctrl_state.tau_output,(double)motor0.hybrid_ctrl_state.pos_error);}}
参数调节指南
位置刚度 kp
位置刚度决定了关节对位置偏差产生的恢复力矩：
阻尼系数 kd
阻尼系数决定了关节对速度的阻尼力矩：
推荐按临界阻尼设计：
力矩限幅
保护电机和驱动器，防止过流：
根据电机额定电流和转矩常数计算最大力矩：
速度滤波
速度信号通常噪声较大，建议使用低通滤波：
α 越小，滤波越强（推荐0.003-0.1）
死区用于消除静止时的小幅抖动
空载直驱电机（测试用）
mcl_hybrid_ctrl_set_kp(&cfg, 0.06f);mcl_hybrid_ctrl_set_kd(&cfg, 0.001429f);mcl_hybrid_ctrl_set_limits(&cfg, -0.5f, 0.5f);mcl_hybrid_ctrl_set_speed_filter(&cfg, 0.003f, 0.1f);
特点：惯量小，无减速器，参数需要较小以避免过激响应。
带减速器的关节电机
假设减速比 N=100N=100，电机端转矩常数 kt=0.1kt=0.1 N·m/A，最大电流3A：
/* 输出端等效刚度 = 电机端刚度 × 减速比² */mcl_hybrid_ctrl_set_kp(&cfg, 50.0f);mcl_hybrid_ctrl_set_kd(&cfg, 14.0f); /* ≈ 2*sqrt(50) *//* 输出端最大力矩 = 电机端力矩 × 减速比 */mcl_hybrid_ctrl_set_limits(&cfg, -30.0f, 30.0f); /* 0.1 × 3 × 100 */mcl_hybrid_ctrl_set_speed_filter(&cfg, 0.05f, 0.01f);
柔顺人机交互
需要较低刚度，允许人手推动：
mcl_hybrid_ctrl_set_kp(&cfg, 5.0f);mcl_hybrid_ctrl_set_kd(&cfg, 4.5f); /* ≈ 2*sqrt(5) */mcl_hybrid_ctrl_set_limits(&cfg, -5.0f, 5.0f);
高精度定位
需要较高刚度和强阻尼：
mcl_hybrid_ctrl_set_kp(&cfg, 200.0f);mcl_hybrid_ctrl_set_kd(&cfg, 28.0f); /* ≈ 2*sqrt(200) */mcl_hybrid_ctrl_set_limits(&cfg, -50.0f, 50.0f);
注意事项
电机转矩常数：需要根据实际电机参数设置，可从电机规格书获取或通过标定测量
编码器精度：位置和速度反馈精度直接影响控制效果
安全限幅：务必设置合理的力矩限幅，防止失控
相关阅读
“先楫半导体”（HPMicro）是一家致力于高性能嵌入式解决方案的半导体公司，产品覆盖微控制器及其配套的开发工具和生态系统。公司总部坐落于上海市浦东软件园区，并在天津、苏州和深圳均设立分公司，入选2025年福布斯亚洲Top100最具潜力的企业榜单。先楫半导体以产品质量为本，所有产品均通过严格的可靠性测试。目前已经量产八大系列高性能通用MCU产品，产品性能及通用性领先国际同类产品并通过AEC-Q100认证。公司已完成ISO9001质量管理和ISO 26262/IEC61508功能安全管理体系双认证，全力服务中国乃至全球的工业自动化、机器人、能源和汽车市场。更多信息，请访问 https://hpmicro.com/
