本文最后更新于83 天前，其中的信息可能已经过时，如有错误请发送邮件到2275012286@qq.com，或者在下方留言。

想了解更多技术分享请搜索“技术”标签，本人水平有限，内容可能不严谨或存在错误，如有发现错误请在评论区处留言，欢迎批评指正。

一、前言

1.什么是电机前馈力矩补偿

电机前馈力矩补偿是一种主动补偿控制策略，核心是在反馈控制的基础上，提前根据系统模型或已知扰动，计算出抵消干扰、跟踪指令所需的力矩，并直接叠加到控制器输出中，从而提升电机的动态响应速度和控制精度。

简单来说，传统的反馈控制（如 PID）是 “事后补救”—— 检测到实际输出与指令的偏差后，才调整输出力矩来纠正偏差；而前馈补偿是 “事前预判”—— 提前算出需要的力矩，主动施加，减少偏差的产生。

主要作用：

提升动态响应速度：前馈力矩提前提供了跟踪指令所需的力矩，无需等待反馈偏差积累，电机能更快地跟随指令变化，减少动态滞后。
提高控制精度：主动抵消已知扰动（如摩擦、重力、惯性），大幅降低稳态误差，尤其适合高精度定位、速度跟踪场景（如数控机床、伺服机械臂）。
降低反馈控制器负担：前馈承担了大部分 “确定性” 力矩需求，反馈控制器只需处理模型误差和未知扰动，可减小反馈增益，避免系统震荡。

2.关于MIT模式电机控制

MIT模式是麻省理工学院开发的一种电机混合闭环控制模式。它常通过 CAN 总线传输控制指令，能实现位置、速度、力矩的精细化协同控制，特别适配四足机器人等需力控或柔顺控制的场景。

核心控制逻辑与公式：

MIT 模式的控制核心是通过融合位置偏差调节、速度偏差调节和前馈补偿量，计算出电机控制所需的参考信号（力矩或电流），具体有两种核心计算公式，分别对应力矩和电流输出，具体如下：

灵活的控制模式切换：

通过调整位置增益Kp和速度增益Kd的参数，MIT 模式可切换为不同的专项控制模式，适配不同场景需求，具体如下：

电流模式：当Kp和Kd均为 0 时，刚度和阻尼都为 0，此时可通过前馈电流cff直接控制电机相电流，适合需要精准力矩输出的场景。
速度模式：当Kp为 0、Kd不为 0 时，刚度为 0 而阻尼生效，结合前馈电流可精准控制电机转速，避免转速波动。
位置模式：当Kp和Kd均不为 0 且速度指令为 0 时，可实现电机定点定位控制；若期望位置是连续变化的函数，期望速度作为其导数，还能实现位置和速度的同步跟踪，适合轨迹运动场景。

3.软件重力补偿应用

在电机控制（尤其是机器人关节、机械臂这类需要力控或位置精准保持的场景）中，重力补偿是一种抵消负载自身重力对电机输出力矩影响的控制策略，核心目的是让电机只需输出 “有效控制力矩”，而非额外承担克服重力的力矩，从而提升控制精度、响应速度和能效。

核心原理：

以机器人关节为例，关节电机驱动的连杆 / 负载会因自身重量产生一个重力矩，这个力矩的大小和方向会随关节角度变化而改变。

若没有重力补偿：电机需要同时输出 克服重力的力矩 + 实现运动 / 保持位置的控制力矩。
若加入重力补偿：预先计算出当前角度下的重力矩，让电机输出一个大小相等、方向相反的补偿力矩，抵消重力的影响，此时电机只需输出 “控制力矩” 即可。

数学表达：

对于单关节电机，重力补偿的核心是计算重力矩 τg，公式为：
在 MIT 模式电机控制中，重力补偿力矩通常会作为前馈项（cff 的一部分）加入控制公式：
此时，参考电流 cref 已经包含了抵消重力的分量，电机输出的力矩就能精准跟踪指令。

关系式实现方式：

模型法（常用）：基于负载的动力学模型，通过理论计算得到重力矩与关节角度的关系。适合已知负载参数（质量、质心位置）的场景。
辨识法：若负载参数未知，可通过实验辨识：控制电机在不同角度下保持静止，记录电机的输出力矩，拟合出重力矩与角度的函数关系，再将其作为补偿项。

二、演示

我将以RM英雄发射机构的pitch电机为例子，以辨识法的方法来获得角度和重力矩之间的关系，借用matlab软件（版本R2024b），使用正弦函数作为拟合函数。

实验操作：

matlab代码（点我）

τ(θ) 正弦拟合（多初始值+分区拟合版）
clear; clc; close all;

% ---------------------- 1. 输入实验数据 ----------------------
%注意，为了确保精度，最好多测点数据，从上往下和从下往上两个方向各测试一遍，角度一定不要回拨！不然摩擦力影响大！并且两个方向数据数量和角度间隔最好相同，如果输出的图像看到有些点的数据偏差很大，可以考虑移除这个数据。
theta = [-0.663959,-0.651751,-0.644503,-0.636492,-0.621614,-0.610933,-0.603304,-0.592240,-0.580796,-0.562486,-0.551423,-0.543412,-0.530441,-0.510223,-0.501450,-0.491531,-0.482758,-0.470169,-0.461013,-0.450713,-0.441176,-0.431640,-0.420959,-0.412566,-0.401503,-0.390821,-0.381285,-0.371748,-0.360685,-0.350386,-0.341992,-0.330548,-0.321393,-0.311093,-0.300031,-0.280575,-0.250439,-0.242046,-0.231365,-0.220684,-0.210383,-0.200847,-0.191310,-0.180629,-0.170711,-0.160029,-0.152781,-0.142099,-0.130655,-0.120737,-0.111200,-0.100519,-0.081445,-0.060845,-0.051309];%请填入自己的实验数据（角度）
tau   = [-0.466422,-0.564102,-0.354091,-0.539682,-0.373627,-0.485958,-0.305250,-0.349206,-0.310134,-0.578754,-0.485958,-0.368742,-0.544566,-0.373627,-0.315019,-0.603174,-0.564102,-0.573871,-0.378510,-0.490843,-0.598290,-0.363858,-0.388278,-0.612943,-0.603174,-0.417583,-0.485958,-0.363858,-0.671551,-0.412699,-0.622710,-0.398046,-0.495727,-0.661782,-0.407814,-0.671551,-0.437119,-0.647131,-0.451771,-0.583638,-0.612943,-0.656898,-0.671551,-0.695971,-0.427350,-0.432235,-0.710623,-0.681318,-0.735043,-0.466422,-0.700854,-0.456655,-0.446886,-0.676435,-0.705739];%请填入自己的实验数据（力矩）

% 数据长度检查
if length(theta) ~= length(tau)
    error('θ和τ数据长度必须一致！');
end

% ---------------------- 2. 定义正弦拟合模型 ----------------------
fit_fun = @(p, x) p(1)*sin(x - p(2)) + p(3);  % τ = (m/L)*sin(θ - θ0) + C

% ---------------------- 3. 多初始值生成（全局拟合） ----------------------
A_min = 0.3*max(tau); A_max = 1.7*max(tau);
theta0_min = -pi; theta0_max = pi; 
C_min = -0.2*mean(tau); C_max = 1.2*mean(tau);

A_list = linspace(A_min, A_max, 4);
theta0_list = linspace(theta0_min, theta0_max, 4);
C_list = linspace(C_min, C_max, 3);

init_guess = [];
for i = 1:length(A_list)
    for j = 1:length(theta0_list)
        for k = 1:length(C_list)
            init_guess = [init_guess; A_list(i), theta0_list(j), C_list(k)];
        end
    end
end
fprintf('生成 %d 组初始值（全局拟合）\n', size(init_guess,1));

% ---------------------- 4. 全局拟合（原蓝线） ----------------------
options = optimoptions('lsqcurvefit','Display','off');
options.MaxFunctionEvaluations = 20000;
options.MaxIterations = 2000;
options.TolFun = 1e-12;
options.TolX = 1e-12;

best_res = inf;
best_p = [];
for idx = 1:size(init_guess,1)
    p0 = init_guess(idx,:);
    try
        [p, res] = lsqcurvefit(fit_fun, p0, theta, tau, [], [], options);
        if res < best_res
            best_res = res;
            best_p = p;
        end
    catch
        fprintf('第%d组初始值（全局）拟合失败，跳过\n',idx);
        continue;
    end
end

% 全局拟合结果
m_over_L = best_p(1);
theta0 = best_p(2);
C = best_p(3);
tau_fit = fit_fun(best_p, theta);  % 全局拟合蓝线的数值
theta_smooth = linspace(min(theta),max(theta),500);
tau_smooth_global = fit_fun(best_p, theta_smooth);

% ---------------------- 5. 按蓝线划分上下部分数据 ----------------------
residual = tau - tau_fit;  % 残差：原始点 - 蓝线值
% 上部分：残差>0（点在蓝线上方）；下部分：残差<0（点在蓝线下方）
idx_upper = residual > 0;  % 上部分索引
idx_lower = residual < 0;  % 下部分索引

% 提取上下部分数据
theta_upper = theta(idx_upper);
tau_upper   = tau(idx_upper);
theta_lower = theta(idx_lower);
tau_lower   = tau(idx_lower);

fprintf('\n===== 数据划分结果 =====\n');
fprintf('上部分点数：%d 个\n', length(theta_upper));
fprintf('下部分点数：%d 个\n', length(theta_lower));

% ---------------------- 6. 上部分数据单独拟合 ----------------------
if ~isempty(theta_upper)
    fprintf('\n===== 上部分拟合 =====\n');
    best_res_upper = inf;
    best_p_upper = [];
    for idx = 1:size(init_guess,1)
        p0 = init_guess(idx,:);
        try
            [p, res] = lsqcurvefit(fit_fun, p0, theta_upper, tau_upper, [], [], options);
            if res < best_res_upper
                best_res_upper = res;
                best_p_upper = p;
            end
        catch
            fprintf('第%d组初始值（上部分）拟合失败，跳过\n',idx);
            continue;
        end
    end
    % 上部分拟合结果
    m_over_L_upper = best_p_upper(1);
    theta0_upper = best_p_upper(2);
    C_upper = best_p_upper(3);
    tau_smooth_upper = fit_fun(best_p_upper, theta_smooth);
    % 上部分R²
    tau_fit_upper = fit_fun(best_p_upper, theta_upper);
    SSE_upper = sum((tau_upper - tau_fit_upper).^2);
    SST_upper = sum((tau_upper - mean(tau_upper)).^2);
    R2_upper = 1 - SSE_upper/SST_upper;
    % 打印上部分结果
    fprintf('m/L（上）= %.8f\n', m_over_L_upper);
    fprintf('θ0（上，弧度）= %.8f （角度=%.4f°）\n', theta0_upper, rad2deg(theta0_upper));
    fprintf('偏置C（上）= %.8f\n', C_upper);
    fprintf('R²（上）= %.8f\n', R2_upper);
else
    fprintf('\n无上部数据点，跳过上部分拟合\n');
    tau_smooth_upper = nan(size(theta_smooth));
end

% ---------------------- 7. 下部分数据单独拟合 ----------------------
if ~isempty(theta_lower)
    fprintf('\n===== 下部分拟合 =====\n');
    best_res_lower = inf;
    best_p_lower = [];
    for idx = 1:size(init_guess,1)
        p0 = init_guess(idx,:);
        try
            [p, res] = lsqcurvefit(fit_fun, p0, theta_lower, tau_lower, [], [], options);
            if res < best_res_lower
                best_res_lower = res;
                best_p_lower = p;
            end
        catch
            fprintf('第%d组初始值（下部分）拟合失败，跳过\n',idx);
            continue;
        end
    end
    % 下部分拟合结果
    m_over_L_lower = best_p_lower(1);
    theta0_lower = best_p_lower(2);
    C_lower = best_p_lower(3);
    tau_smooth_lower = fit_fun(best_p_lower, theta_smooth);
    % 下部分R²
    tau_fit_lower = fit_fun(best_p_lower, theta_lower);
    SSE_lower = sum((tau_lower - tau_fit_lower).^2);
    SST_lower = sum((tau_lower - mean(tau_lower)).^2);
    R2_lower = 1 - SSE_lower/SST_lower;
    % 打印下部分结果
    fprintf('m/L（下）= %.8f\n', m_over_L_lower);
    fprintf('θ0（下，弧度）= %.8f （角度=%.4f°）\n', theta0_lower, rad2deg(theta0_lower));
    fprintf('偏置C（下）= %.8f\n', C_lower);
    fprintf('R²（下）= %.8f\n', R2_lower);
else
    fprintf('\n无下部数据点，跳过下部分拟合\n');
    tau_smooth_lower = nan(size(theta_smooth));
end

% ---------------------- 8. 可视化：分区拟合结果 ----------------------
figure('Color','white','Position',[100,100,1200,600]);

% 子图1：分区数据+全局/分区拟合曲线
subplot(1,2,1);
% 绘制原始点（上红，下绿）
plot(theta_upper, tau_upper, 'ro', 'MarkerSize',6,'MarkerFaceColor','r','DisplayName','上部分点'); hold on;
plot(theta_lower, tau_lower, 'go', 'MarkerSize',6,'MarkerFaceColor','g','DisplayName','下部分点');
% 绘制拟合曲线（全局蓝，上橙，下紫）
plot(theta_smooth, tau_smooth_global, 'b-','LineWidth',2,'DisplayName','全局拟合线');
plot(theta_smooth, tau_smooth_upper, 'o-','Color',[1 0.5 0],'LineWidth',2,'MarkerSize',1,'DisplayName','上部分拟合线');
plot(theta_smooth, tau_smooth_lower, 'm-','LineWidth',2,'DisplayName','下部分拟合线');
xlabel('θ (rad)'); ylabel('τ(θ)');
title('分区数据与拟合曲线');
grid on; legend('Location','best');
xlim([-0.67 0.00]);
ylim([-1.200 -0.800]);  % 按你的需求调整y轴范围

% 子图2：分区残差
subplot(1,2,2);
% 上部分残差（红）
if ~isempty(theta_upper)
    residual_upper = tau_upper - fit_fun(best_p_upper, theta_upper);
    plot(theta_upper, residual_upper, 'ro', 'MarkerSize',6,'MarkerFaceColor','r','DisplayName','上部分残差'); hold on;
end
% 下部分残差（绿）
if ~isempty(theta_lower)
    residual_lower = tau_lower - fit_fun(best_p_lower, theta_lower);
    plot(theta_lower, residual_lower, 'go', 'MarkerSize',6,'MarkerFaceColor','g','DisplayName','下部分残差'); hold on;
end
yline(0,'r--');
xlabel('θ (rad)'); ylabel('残差');
title('分区拟合残差分布');
grid on; legend('Location','best');
xlim([-0.67 0.00]);

% ---------------------- 9. 全局拟合结果（补充） ----------------------
% 全局R²
SSE = sum((tau - tau_fit).^2);
SST = sum((tau - mean(tau)).^2);
R2 = 1 - SSE/SST;
fprintf('\n===== 全局拟合结果 =====\n');
fprintf('m/L = %.8f\n', m_over_L);
fprintf('θ0（弧度）= %.8f （角度=%.4f°）\n', theta0, rad2deg(theta0));
fprintf('偏置C = %.8f\n', C);
fprintf('拟合优度R² = %.8f\n', R2);
fprintf('残差平方和SSE = %.8f\n', best_res);

运行后我们能得到以下图像：

实验分析：

我们看图发现，这里会出现两条拟函数线，并且对于数据点位，相同位置，但是不同运动方向下，电机力矩是不一样的，这是为什么呢？这里有一种猜测，pitch电机的外部发射结构的有摩擦力的影响。当发射机构上抬的时候，此时进行受力分析，电机要不仅要克服重力的作用，还要克服摩擦力，所以电机需要更大的力矩来抬升。当发射机构下降的时候，此时进行受力分析，重力和摩擦力反向，摩擦力抵消了一部分重力，所以电机需要的力矩会比较小。理论上，两种情况对应的就是这上下两个拟合函数，接下来我们来验证一下。

经过验证，我们发现确实如此！通过拟合函数预测出来的前馈力矩和实际电机力矩基本吻合，接下来我们只要把拟合函数的结果作为前馈项（cff 的一部分）加入控制公式，同时注意pitch 电机给力矩前馈要判断运动方向调用不同拟合函数来给前馈力矩。正弦函数拟合也会比常用的线性拟合的精度要高，精确的话这样操作似乎不只能软件上重力补偿，似乎都能软件上重力摩擦力全补偿了。