单轴导纳控制 (Single-Axis Admittance Control) 算法介绍
1. 算法介绍
导纳控制 (Admittance Control) 是一种力-位移映射的阻抗控制策略。它定义了外力 (Force) 与系统运动 (Velocity or Position) 之间的动态关系。
- 物理类比: 想象一个弹簧-阻尼-质量系统。当你施加一个力
F时，系统会产生一个加速度a，进而改变速度v和位置x。导纳控制就是模拟这样一个系统的动态响应。 - 核心思想: 用户施加的力
F被“允许” (admitted) 转化为机器人的运动x。力越大，期望的运动速度或加速度也越大。 - 应用场景:
- 人机协作 (HRI): 机器人被人类“引导”或“拖动”。
- 柔顺操作: 机器人在接触环境中能顺应外力，避免刚性碰撞。
- 康复机器人: 辅助患者进行运动训练。
- 虚拟固定装置: 创建一个“虚拟弹簧”，让机器人在受力时轻微偏移。
1.1 与阻抗控制的区别
- 阻抗控制 (Impedance Control): 给定期望的运动轨迹
x_des，计算所需的力F。F = f(x_des - x_actual)。是运动到力的映射。 - 导纳控制 (Admittance Control): 测量到的外力
F_ext，计算出期望的运动x_des。x_des = f(F_ext)。是力到运动的映射。
在单轴导纳控制中，我们只考虑一个自由度（如 X 轴方向）的力与运动关系。
2. 数学模型与公式
单轴导纳控制的核心是一个二阶动态系统，通常用质量-弹簧-阻尼模型表示：
2.1 连续时间域模型
系统的动态方程为：
M d²x(t)/dt² + B dx(t)/dt + K x(t) = F_ext(t)
X(s)/F_ext(s) = 1/(M s² + B s + K)
a_des(t) = (F_ext(t) - B v(t) - K x(t)) / M
v_des(t) = ∫ a_des(t) dt
x_des(t) = ∫ v_des(t) dt
v_des(t) = (F_ext(t) - B v(t)) / B_damping_only
x_des(t) = x_des(t-1) + v_des(t) * Δt
通过上面的数学模型，可以根据给定力的反馈，计算当前周期理论输出的加速度，根据加速度积分依次计算速度和位置，电机按照计算的速度运动即可实现恒力运动。
3. MATLAB 编程实现
以下是一个完整的单轴导纳控制 MATLAB 仿真脚本。它模拟了用户施加一个阶跃力，机器人根据导纳模型产生运动。
%% 单轴导纳控制算法演示
% 清理环境
clear; clc; close all;
%% 参数设置
M = 1.0; % 虚拟质量 (kg)
B = 5.0; % 虚拟阻尼 (N·s/m)
K = 10.0; % 虚拟刚度 (N/m)
% 时间参数
dt = 0.01; % 采样时间 (s)
t_final = 5; % 仿真总时间 (s)
t = 0:dt:t_final;
%% 初始化状态变量
n_steps = length(t);
position = zeros(1, n_steps); % 位置 x(t)
velocity = zeros(1, n_steps); % 速度 v(t)
acceleration = zeros(1, n_steps); % 加速度 a(t)
force_ext = zeros(1, n_steps); % 外力 F_ext(t)
% 初始条件 (假设从原点静止开始)
position(1) = 0;
velocity(1) = 0;
%% 模拟外力输入 (例如：在 t=1s 时施加一个 10N 的阶跃力)
force_idx = find(t >= 1, 1, 'first');
force_ext(force_idx:end) = 10; % 10 Newtons
%% 导纳控制主循环
for i = 2:n_steps
% === 核心导纳控制计算 ===
% 计算期望加速度 (基于当前测量的外力和当前状态)
acceleration(i) = (force_ext(i) - B * velocity(i-1) - K * position(i-1)) / M;
% 数值积分：从加速度得到速度 (欧拉法)
velocity(i) = velocity(i-1) + acceleration(i) * dt;
% 数值积分：从速度得到位置 (欧拉法)
position(i) = position(i-1) + velocity(i) * dt;
end
%% 绘图
figure('Position', [100, 100, 800, 600]);
% 子图1：外力和位置
subplot(3,1,1);
yyaxis left;
plot(t, force_ext, 'Color', [0.85, 0.325, 0.098], 'LineWidth', 2);
ylabel('外力 F_{ext} (N)', 'Color', [0.85, 0.325, 0.098]);
ylim([-1, 11]);
yyaxis right;
plot(t, position, 'Color', [0, 0.447, 0.741], 'LineWidth', 2);
ylabel('位置 x (m)', 'Color', [0, 0.447, 0.741]);
title('单轴导纳控制仿真');
grid on;
% 子图2：速度
subplot(3,1,2);
plot(t, velocity, 'Color', [0.929, 0.694, 0.125], 'LineWidth', 2);
ylabel('速度 v (m/s)');
grid on;
% 子图3：加速度
subplot(3,1,3);
plot(t, acceleration, 'Color', [0.466, 0.674, 0.188], 'LineWidth', 2);
xlabel('时间 t (s)');
ylabel('加速度 a (m/s²)');
grid on;
% 美化布局
linkaxes([gca; gca; gca], 'x'); % 链接x轴
4. 公式说明 (对应上面的线性格式)
M d²x(t)/dt² + B dx(t)/dt + K x(t) = F_ext(t)
这是连续时间下的二阶微分方程，描述了虚拟机械系统的动力学。X(s)/F_ext(s) = 1/(M s² + B s + K)
这是系统的传递函数，表示位置输出与外力输入的比值。a_des(t) = (F_ext(t) - B v(t) - K x(t)) / M
核心控制律。在离散控制中，我们用当前时刻的外力减去阻尼力和弹簧力，再除以质量，得到期望加速度。v_des(t) = ∫ a_des(t) dt
期望速度是期望加速度的积分。x_des(t) = ∫ v_des(t) dt
期望位置是期望速度的积分。v_des(t) = (F_ext(t) - B v(t)) / B_damping_only
一种简化形式，忽略刚度项K和质量项M，只保留阻尼。此时外力直接映射为期望速度。x_des(t) = x_des(t-1) + v_des(t) * Δt
在代码中，位置通过数值积分（欧拉法）更新。