力位混合控制库使用指南
力位混合控制库使用指南
SDK Ver.V1.11.0
概述
力位混合控制（Hybrid Force-Position Control）是一种结合力控制和位置控制的阻抗控制方法，广泛应用于机器人关节控制、柔顺装配、人机交互等场景。本库实现了基于PD+前馈的阻抗控制算法，集成于HPM MCL电机控制库中。
控制原理
阻抗控制模型
力位混合控制的核心思想是让关节表现出期望的机械阻抗特性，即弹簧-阻尼系统特性。控制律如下：
\tau = \tau_{ff} + k_p (q_{des} - q) + k_d (\dot{q}_{des} - \dot{q})其中：
\tau为输出力矩 (N·m)\tau_{ff}为前馈力矩，用于重力补偿等 (N·m)k_p为位置刚度增益 (N·m/rad)k_d为阻尼增益 (N·m·s/rad)q_{des}为期望位置 (rad)q为实际位置 (rad)\dot{q}_{des}为期望速度 (rad/s)，位置保持时通常为0\dot{q}为实际速度 (rad/s)
物理意义
该控制律模拟了一个虚拟的弹簧-阻尼系统：
\tau = k_p \cdot e_p + k_d \cdot e_v- 位置刚度
k_p：决定关节对位置偏差的恢复力，类似弹簧刚度 - 阻尼系数
k_d：决定关节对速度的阻尼力，抑制振荡
临界阻尼条件
为获得无超调的快速响应，通常采用临界阻尼设计：
k_d = 2\sqrt{k_p \cdot J}其中 J 为关节等效转动惯量。对于未知惯量的系统，可简化为：
k_d \approx 2\sqrt{k_p}系统架构
力位混合控制作为位置/力外环，输出力矩指令给FOC电流内环：
力位混合控制 (20kHz) → 力矩指令τ → 力矩常数kt转换 → q轴电流iq → FOC电流环 → PWM力矩到电流的转换公式：
i_q = \frac{\tau}{k_t}其中 k_t 为电机转矩常数 (N·m/A)。
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;核心函数
| 函数 | 说明 |
|---|---|
mcl_hybrid_ctrl_init() |
初始化配置，所有参数置零 |
mcl_hybrid_ctrl_step() |
执行一次控制计算 |
mcl_hybrid_ctrl_set_kp() |
设置位置刚度 |
mcl_hybrid_ctrl_set_kd() |
设置阻尼系数 |
mcl_hybrid_ctrl_set_position() |
设置期望位置 |
mcl_hybrid_ctrl_set_velocity() |
设置期望速度 |
mcl_hybrid_ctrl_set_limits() |
设置力矩限幅 |
mcl_hybrid_ctrl_set_speed_filter() |
设置速度滤波参数 |
使用示例
参数初始化
以下为 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
位置刚度决定了关节对位置偏差产生的恢复力矩：
\tau_p = k_p \cdot (q_{des} - q)| kp范围 | 特性 | 应用场景 |
|---|---|---|
| 0.01-0.1 | 非常柔顺 | 空载直驱电机测试 |
| 0.1-1.0 | 柔顺 | 小型关节、人机交互 |
| 1.0-10 | 中等刚度 | 带减速器的关节 |
| 10-100 | 刚性 | 工业伺服定位 |
| 100-500 | 非常刚性 | 高精度定位 |
阻尼系数 kd
阻尼系数决定了关节对速度的阻尼力矩：
\tau_d = k_d \cdot (\dot{q}_{des} - \dot{q})推荐按临界阻尼设计：
k_d = 2\sqrt{k_p}| 现象 | 原因 | 调节方法 |
|---|---|---|
| 振荡/超调 | kd过小 | 增大kd |
| 响应迟缓 | kd过大 | 减小kd |
| 抖动 | 速度噪声 | 增加速度滤波 |
力矩限幅
保护电机和驱动器，防止过流：
\tau_{min} \leq \tau_{output} \leq \tau_{max}根据电机额定电流和转矩常数计算最大力矩：
\tau_{max} = k_t \cdot I_{max}速度滤波
速度信号通常噪声较大，建议使用低通滤波：
\dot{q}_{filtered}[n] = \alpha \cdot \dot{q}[n] + (1-\alpha) \cdot \dot{q}_{filtered}[n-1]\alpha越小，滤波越强（推荐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=100，电机端转矩常数 k_t=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);参数整定步骤
- 确定力矩限幅：根据电机额定电流和转矩常数计算
- 设置初始kp：从较小值开始（如1.0）
- 设置kd为临界阻尼：
k_d = 2\sqrt{k_p} - 测试响应：
- 振荡 → 增大kd
- 太慢 → 增大kp（同时调整kd）
- 抖动 → 减小速度滤波系数alpha
- 逐步增大kp：直到达到期望的刚度
注意事项
- 电机转矩常数：需要根据实际电机参数设置，可从电机规格书获取或通过标定测量
- 编码器精度：位置和速度反馈精度直接影响控制效果
- 安全限幅：务必设置合理的力矩限幅，防止失控