概述

力位混合控制（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：

/* 輸出端等效剛度 = 電機端剛度 × 減速比2 */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. 參數整定步驟

注意事項

電機轉矩常數：需要根據實際電機參數設置，可從電機規格書獲取或通過標定測量

編碼器精度：位置和速度反饋精度直接影響控制效果

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