概述

力位混合控制（Hybrid Force-Position Control）是一種結(jié)合力控制和位置控制的阻抗控制方法，廣泛應(yīng)用于機(jī)器人關(guān)節(jié)控制、柔順裝配、人機(jī)交互等場景。本庫實(shí)現(xiàn)了基于PD+前饋的阻抗控制算法，集成于HPM MCL電機(jī)控制庫中。

控制原理

1. 阻抗控制模型

力位混合控制的核心思想是讓關(guān)節(jié)表現(xiàn)出期望的機(jī)械阻抗特性，即彈簧-阻尼系統(tǒng)特性。控制律如下：

其中：

2. 物理意義

該控制律模擬了一個(gè)虛擬的彈簧-阻尼系統(tǒng)：

3. 臨界阻尼條件

為獲得無超調(diào)的快速響應(yīng)，通常采用臨界阻尼設(shè)計(jì)：

其中 J 為關(guān)節(jié)等效轉(zhuǎn)動(dòng)慣量。對(duì)于未知慣量的系統(tǒng)，可簡化為：

系統(tǒng)架構(gòu)

力位混合控制作為位置/力外環(huán)，輸出力矩指令給FOC電流內(nèi)環(huán)：

力矩到電流的轉(zhuǎn)換公式：

API說明

1. 數(shù)據(jù)結(jié)構(gòu)

配置結(jié)構(gòu)體

typedefstruct{ floatkp; /* 位置剛度 (N·m/rad) */ floatkd; /* 阻尼系數(shù) (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; /* 速度低通濾波系數(shù) (0-1) */ floatspeed_deadzone; /* 速度死區(qū) (rad/s) */}mcl_hybrid_ctrl_cfg_t;

狀態(tài)結(jié)構(gòu)體

typedefstruct{ floatq_actual; /* 實(shí)際位置 (rad) - 輸入 */ floatdq_actual; /* 實(shí)際速度 (rad/s) - 輸入 */ floattau_output; /* 輸出力矩 (N·m) - 輸出 */ floatpos_error; /* 位置誤差 (rad) - 輸出 */ floatvel_error; /* 速度誤差 (rad/s) - 輸出 */ floatspeed_lpf; /* 濾波后速度 (rad/s) - 內(nèi)部 */}mcl_hybrid_ctrl_state_t;

2. 核心函數(shù)

使用示例

1. 參數(shù)初始化

以下為bldc_foc示例中的初始化代碼，適用于空載直驅(qū)電機(jī)：

voidmotor0_hybrid_ctrl_init(void){ /* 初始化配置結(jié)構(gòu)體 */ mcl_hybrid_ctrl_init(&motor0.hybrid_ctrl_cfg); /* 清零狀態(tài)結(jié)構(gòu)體 */ memset(&motor0.hybrid_ctrl_state,0,sizeof(motor0.hybrid_ctrl_state)); /* 設(shè)置PD參數(shù) - 空載直驅(qū)電機(jī)參數(shù)較小 */ mcl_hybrid_ctrl_set_kp(&motor0.hybrid_ctrl_cfg,0.06f); mcl_hybrid_ctrl_set_kd(&motor0.hybrid_ctrl_cfg,0.001429f); /* 設(shè)置初始期望位置和速度 */ mcl_hybrid_ctrl_set_position(&motor0.hybrid_ctrl_cfg,0.0f); mcl_hybrid_ctrl_set_velocity(&motor0.hybrid_ctrl_cfg,0.0f); /* 設(shè)置力矩限幅，防止過流 */ mcl_hybrid_ctrl_set_limits(&motor0.hybrid_ctrl_cfg, -0.5f,0.5f); /* 設(shè)置速度濾波：alpha=0.003（強(qiáng)濾波），死區(qū)=0.1 rad/s */ mcl_hybrid_ctrl_set_speed_filter(&motor0.hybrid_ctrl_cfg,0.003f,0.1f);}

2. 控制循環(huán)

在ADC中斷（PWM周期觸發(fā)，20kHz）中執(zhí)行控制算法：

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); /* 編碼器數(shù)據(jù)處理 */ 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：執(zhí)行力位混合控制算法 */ mcl_hybrid_ctrl_step(&motor0.hybrid_ctrl_cfg, &motor0.hybrid_ctrl_state); /* 步驟3：力矩轉(zhuǎn)電流，發(fā)送給FOC電流環(huán) */ user_current.enable =true; /* kt = 0.053 N·m/A（電機(jī)轉(zhuǎn)矩常數(shù)） */ user_current.value = motor0.hybrid_ctrl_state.tau_output /0.053f; hpm_mcl_loop_set_current_q(&motor0.loop, user_current); } /* 執(zhí)行FOC電流環(huán) */ hpm_mcl_loop(&motor0.loop); }}

3. 用戶交互

主循環(huán)中的模式選擇和位置輸入：

if(user_mode ==2) { /* 初始化力位混合控制 */ motor0_hybrid_ctrl_init(); hybrid_ctrl_mode =true; /* 禁用速度環(huán)，由力位混合控制接管 */ 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) { /* 讀取用戶輸入的目標(biāo)位置（度） */ position =atoi(input_data); /* 角度轉(zhuǎn)弧度：deg * (π/180) ≈ deg * 0.01745 */ mcl_hybrid_ctrl_set_position(&motor0.hybrid_ctrl_cfg, (float)position *0.00157079632f); /* 實(shí)際為 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); }}

參數(shù)調(diào)節(jié)指南

1. 參數(shù)含義與調(diào)節(jié)

位置剛度 kp

位置剛度決定了關(guān)節(jié)對(duì)位置偏差產(chǎn)生的恢復(fù)力矩：

阻尼系數(shù) kd

阻尼系數(shù)決定了關(guān)節(jié)對(duì)速度的阻尼力矩：

推薦按臨界阻尼設(shè)計(jì)：

力矩限幅

保護(hù)電機(jī)和驅(qū)動(dòng)器，防止過流：

根據(jù)電機(jī)額定電流和轉(zhuǎn)矩常數(shù)計(jì)算最大力矩：

速度濾波

速度信號(hào)通常噪聲較大，建議使用低通濾波：

α 越小，濾波越強(qiáng)（推薦0.003-0.1）

死區(qū)用于消除靜止時(shí)的小幅抖動(dòng)

2. 不同應(yīng)用場景的參數(shù)配置

空載直驅(qū)電機(jī)（測(cè)試用）

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

特點(diǎn)：慣量小，無減速器，參數(shù)需要較小以避免過激響應(yīng)。

帶減速器的關(guān)節(jié)電機(jī)

假設(shè)減速比 N=100N=100，電機(jī)端轉(zhuǎn)矩常數(shù) kt=0.1kt=0.1 N·m/A，最大電流3A：

/* 輸出端等效剛度 = 電機(jī)端剛度 × 減速比2 */mcl_hybrid_ctrl_set_kp(&cfg,50.0f);mcl_hybrid_ctrl_set_kd(&cfg,14.0f); /* ≈ 2*sqrt(50) *//* 輸出端最大力矩 = 電機(jī)端力矩 × 減速比 */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);

柔順人機(jī)交互

需要較低剛度，允許人手推動(dòng)：

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

高精度定位

需要較高剛度和強(qiáng)阻尼：

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. 參數(shù)整定步驟

注意事項(xiàng)

電機(jī)轉(zhuǎn)矩常數(shù)：需要根據(jù)實(shí)際電機(jī)參數(shù)設(shè)置，可從電機(jī)規(guī)格書獲取或通過標(biāo)定測(cè)量

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

安全限幅：務(wù)必設(shè)置合理的力矩限幅，防止失控