HPM知识库 | 力位混合控制库使用指南

概述

力位混合控制（Hybrid Force-Position Control）是一种结合力控制和位置控制的阻抗控制方法，广泛应用于机器人关节控制、柔顺装配、人机交互等场景。本库实现了基于PD+前馈的阻抗控制算法，集成于HPM MCL电机控制库中。

控制原理

1. 阻抗控制模型

力位混合控制的核心思想是让关节表现出期望的机械阻抗特性，即弹簧-阻尼系统特性。控制律如下：

其中：

2. 物理意义

该控制律模拟了一个虚拟的弹簧-阻尼系统：

3. 临界阻尼条件

为获得无超调的快速响应，通常采用临界阻尼设计：

其中 J 为关节等效转动惯量。对于未知惯量的系统，可简化为：

系统架构

力位混合控制作为位置/力外环，输出力矩指令给FOC电流内环：

力矩到电流的转换公式：

API说明

1. 数据结构

配置结构体

typedefstruct{ floatkp; /* 位置刚度 (N·m/rad) */ floatkd; /* 阻尼系数 (N·m·s/rad) */ floattau_ff; /* 前馈力矩 (N·m) */ floatq_des; /* 期望位置 (rad) */ floatdq_des; /* 期望速度 (rad/s) */ floattau_max; /* 最大输出力矩 (N·m) */ floattau_min; /* 最小输出力矩 (N·m) */ floatspeed_lpf_alpha; /* 速度低通滤波系数 (0-1) */ floatspeed_deadzone; /* 速度死区 (rad/s) */}mcl_hybrid_ctrl_cfg_t;

状态结构体

typedefstruct{ floatq_actual; /* 实际位置 (rad) - 输入 */ floatdq_actual; /* 实际速度 (rad/s) - 输入 */ floattau_output; /* 输出力矩 (N·m) - 输出 */ floatpos_error; /* 位置误差 (rad) - 输出 */ floatvel_error; /* 速度误差 (rad/s) - 输出 */ floatspeed_lpf; /* 滤波后速度 (rad/s) - 内部 */}mcl_hybrid_ctrl_state_t;

2. 核心函数

使用示例

1. 参数初始化

以下为bldc_foc示例中的初始化代码，适用于空载直驱电机：

voidmotor0_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);}

2. 控制循环

在ADC中断（PWM周期触发，20kHz）中执行控制算法：

voidisr_adc(void){ uint32_tstatus; mcl_user_value_tuser_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); }}

3. 用户交互

主循环中的模式选择和位置输入：

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); }}

参数调节指南

1. 参数含义与调节

位置刚度 kp

位置刚度决定了关节对位置偏差产生的恢复力矩：

阻尼系数 kd

阻尼系数决定了关节对速度的阻尼力矩：

推荐按临界阻尼设计：

力矩限幅

保护电机和驱动器，防止过流：

根据电机额定电流和转矩常数计算最大力矩：

速度滤波

速度信号通常噪声较大，建议使用低通滤波：

α 越小，滤波越强（推荐0.003-0.1）

死区用于消除静止时的小幅抖动

2. 不同应用场景的参数配置

空载直驱电机（测试用）

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);

3. 参数整定步骤

注意事项

电机转矩常数：需要根据实际电机参数设置，可从电机规格书获取或通过标定测量

编码器精度：位置和速度反馈精度直接影响控制效果

安全限幅：务必设置合理的力矩限幅，防止失控