✨ 专业领域：

擅长数据搜集与处理、建模仿真、程序设计、仿真代码、论文写作与指导，毕业论文、期刊论文经验交流。

✅ 具体问题可以私信或查看文章底部二维码

✅ 感恩科研路上每一位志同道合的伙伴！

（1）磨抛机器人运动学建模与工作空间分析

磨抛机器人需具备高精度位姿控制能力以适应复杂工件打磨需求，七自由度协作型磨抛机器人（如基于 UR5e 改进的协作机器人）因冗余自由度可优化关节运动、避免奇异位形，成为研究首选。首先基于 D-H 参数法构建机器人运动学模型，明确各连杆坐标系与参数定义：连杆 1（基座 - 肩部）长度 a1=0mm、扭转角 α1=90°、关节偏距 d1=340mm、关节角 θ1（水平旋转，范围 - 180°~180°）；连杆 2（肩部 - 肘部）a2=425mm、α2=0°、d2=0mm、θ2（垂直摆动，范围 - 90°~90°）；连杆 3（肘部 - 腕部 1）a3=392mm、α3=0°、d3=0mm、θ3（垂直摆动，范围 - 180°~180°）；连杆 4（腕部 1 - 腕部 2）a4=0mm、α4=90°、d4=109mm、θ4（旋转，范围 - 180°~180°）；连杆 5（腕部 2 - 腕部 3）a5=0mm、α5=-90°、d5=95mm、θ5（摆动，范围 - 90°~90°）；连杆 6（腕部 3 - 末端法兰）a6=0mm、α6=0°、d6=82mm、θ6（旋转，范围 - 180°~180°）；冗余关节 θ7（腕部 2 额外旋转，范围 - 180°~180°）用于避奇异与姿态优化。通过齐次变换矩阵 T_i = Rot (z,θ_i)・Trans (0,0,d_i)・Trans (a_i,0,0)・Rot (x,α_i)，依次相乘得到基座到末端法兰的总变换矩阵 T_06，包含末端位置（x,y,z）与姿态（roll,pitch,yaw）信息，完成正运动学建模。

逆运动学求解需解决冗余自由度带来的多解性与奇异位形问题，采用阻尼最小二乘法结合关节空间优化策略：将逆运动学转化为最小二乘问题，目标函数为末端位姿误差平方和 J (θ)=||X_d - X (θ)||²（X_d 为期望位姿，X (θ) 为实际位姿），引入阻尼因子 λ（取值 0.01~0.1，奇异区域增大）构建优化目标 J'(θ)=J (θ)+λ||Δθ||²，通过雅克比矩阵伪逆 J⁺=J^T (JJ^T+λI)⁻¹ 求解关节角增量 Δθ=J⁺ΔX，迭代更新 θ=θ+Δθ 直至位姿误差≤0.01mm（位置）、≤0.05°（姿态）。为避免关节极限与奇异点，在目标函数中加入惩罚项：关节极限惩罚 P1=Σk1・max (0,θ_i-θ_i_max)² + Σk1・max (0,θ_i_min-θ_i)²（k1=100，θ_i_max/θ_i_min 为关节极限）；奇异惩罚 P2=k2・1/det (JJ^T+εI)（k2=10，ε=0.001，det 为行列式，值越小惩罚越大），最终目标函数 J_total=J'(θ)+P1+P2，确保求解过程中关节运动平滑且远离奇异。

工作空间分析采用蒙特卡洛法，通过大量随机采样关节角（满足关节极限约束）计算末端位姿并统计分布：设置采样次数 10^5 次，每次随机生成 θ1~θ7（符合各关节范围），代入正运动学模型得到末端点（x,y,z），剔除超出物理约束的点（如基座周围 50mm 内、Z 轴低于 0mm 的点），最终得到工作空间三维点云。分析结果显示：工作空间 X 轴范围 - 800~800mm、Y 轴 - 800~800mm、Z 轴 0~1200mm，有效工作区域（姿态灵活度≥0.8）集中在 X=-600~600mm、Y=-600~600mm、Z=200~1000mm；灵活度采用雅克比矩阵条件数衡量（条件数越小灵活度越高），有效区域内条件数≤10，满足磨抛作业对姿态调整的需求。基于 Matlab 进行数值验证：给定 3 组典型磨抛位姿（如（300,200,500,0°,45°,0°）、（-200,-300,700,30°,-30°,60°）、（500,-100,400,-15°,60°,-45°）），逆运动学求解耗时均≤0.05s，末端实际位姿与期望位姿误差≤0.008mm（位置）、≤0.04°（姿态），验证了运动学模型与求解算法的正确性，为后续动力学建模与控制策略设计奠定基础。

（2）磨抛机器人动力学参数辨识与计算力矩控制设计

动力学模型的准确性直接影响柔顺控制精度，需通过参数辨识获取机器人惯性、科氏力 / 离心力及重力项参数。采用牛顿 - 欧拉法建立动力学模型，从基座到末端依次计算各连杆的速度、加速度与惯性力，再反向递推关节力矩：对于第 i 个连杆，正向递推得到线速度 v_i = v_{i-1} + ω_{i-1}×r_{i-1,i} + ω_i×d_i（r_{i-1,i} 为连杆 i-1 到 i 的位置矢量，d_i 为关节偏距）、角速度 ω_i = R_{i-1}^T (ω_{i-1} + θ_i'・z_i)（R_{i-1} 为连杆 i-1 到 i 的旋转矩阵）；反向递推计算惯性力 F_i = m_i・a_i + ω_i×(m_i・v_i)（m_i 为连杆质量）、惯性力矩 τ_i = I_i・ω_i' + ω_i×(I_i・ω_i)（I_i 为转动惯量矩阵），最终关节力矩 τ = Σ(τ_i + r_i×F_i) + C (θ,θ')θ' + G (θ)，其中 M (θ) 为惯性矩阵、C (θ,θ') 为科氏力 / 离心力矩阵、G (θ) 为重力矩阵。

为减少辨识参数数量，基于几何递推法推导最小参数集：利用动力学模型中参数的线性相关性，将 M (θ)、C (θ,θ')、G (θ) 表示为最小参数向量 φ（包含各连杆质量 m_i、转动惯量 I_ixx/I_ixy/I_ixz/I_iyy/I_iyz/I_izz、质心坐标 x_ci/y_ci/z_ci，共 42 个参数，剔除冗余后保留 28 个独立参数）的线性组合 τ = Y (θ,θ',θ'')φ，Y 为回归矩阵（由关节角、角速度、角加速度构成）。激励轨迹设计需满足持续激励条件（回归矩阵 Y 的秩等于 φ 维度），采用傅里叶级数轨迹 θ_i (t) = θ_i0 + Σ(A_ij・sin (jωt) + B_ij・cos (jωt))（t∈[0,T]，T=10s 为轨迹周期，ω=2π/T 为基频，j=1~5 为谐波次数），引入关节运动约束：角速度 θ_i'≤1.5rad/s、角加速度 θ_i''≤5rad/s²、角 jerk≤20rad/s³，通过遗传算法优化振幅 A_ij/B_ij（目标函数为 Y 的条件数最小），最终得到各关节激励轨迹：如关节 1θ_1 (t)=0 + 0.8sin (ωt) + 0.3sin (2ωt) + 0.2sin (3ωt) + 0.1sin (4ωt) + 0.05sin (5ωt)，确保轨迹平滑且信息丰富。

基于 UR5e 磨抛机器人实验平台开展辨识实验：平台配备关节力矩传感器（分辨率 0.01N・m）、高精度编码器（分辨率 131072 线 / 转）、数据采集卡（采样频率 1kHz），在机器人末端空载状态下运行激励轨迹，采集关节角 θ、角速度 θ'（数值微分后滤波）、角加速度 θ''（二次微分后滤波）与关节力矩 τ 共 5 组数据。采用加权最小二乘法辨识最小参数集 φ，权重矩阵 W=diag (1/σ_1²,1/σ_2²,...,1/σ_28²)（σ_i 为第 i 个参数的标准差，由数据噪声统计得到），目标函数 J (φ)=||W (τ - Yφ)||²，求解 φ=(Y^T W^T W Y)⁻¹ Y^T W^T W τ。辨识结果验证：在验证轨迹（与激励轨迹不同的正弦组合轨迹，周期 8s）上，实际关节力矩与模型预测力矩的平均误差≤4.5%（最大误差 6.2% 出现在关节 2 高速段），重力项误差≤2%，证明辨识模型能准确反映机器人动力学特性。

基于辨识动力学模型设计计算力矩控制器，实现高精度轨迹跟踪：控制器采用 “前馈补偿 + PID 反馈” 结构，控制律 τ = M (θ)(θ_d'' + K_d (θ_d' - θ') + K_p (θ_d - θ)) + C (θ,θ')θ' + G (θ)，其中 θ_d 为期望关节轨迹，K_p（位置增益，关节 1~7 分别为 500/600/500/300/300/200/200 N・m/rad）、K_d（阻尼增益，分别为 10/15/10/5/5/3/3 N・m・s/rad）通过极点配置法设计（期望闭环极点阻尼比 0.7，无阻尼自然频率 5~10rad/s）。在 Simulink 中搭建仿真模型：导入辨识的动力学模型作为被控对象，输入磨抛典型轨迹（如圆弧轨迹，半径 50mm，进给速度 10mm/s，对应关节轨迹由正运动学生成），仿真结果显示：位置跟踪误差≤0.08mm（末端），速度跟踪误差≤0.5mm/s，轨迹跟踪精度满足磨抛作业要求（通常需≤0.1mm）；对比无动力学补偿的 PID 控制（位置误差 1.2mm），计算力矩控制精度提升 15 倍，有效抑制了动力学耦合对跟踪性能的影响。

（3）磨抛机器人柔顺控制策略设计与实验验证

磨抛作业中工件表面形貌变化、材质不均易导致接触力波动，需设计柔顺控制策略实现力 - 位协同控制。首先设计基于位置的阻抗控制策略，建立末端位置与接触力的动态关系：阻抗模型定义为 M_d (Δx'' - Δx_d'') + B_d (Δx' - Δx_d') + K_d (Δx - Δx_d) = F_e - F_d，其中 M_d/B_d/K_d 分别为期望惯性 / 阻尼 / 刚度（磨抛作业通常取 M_d=0.1kg、B_d=50N・s/m、K_d=200~500N/m），Δx=x - x_0（x 为实际位置，x_0 为自由空间位置），Δx_d=x_d - x_0（x_d 为期望位置），F_e 为实际接触力，F_d 为期望接触力（磨抛力通常取 5~20N，根据工件材质调整）。通过阻抗模型求解期望位置修正量 Δx_c = Δx_d + (F_e - F_d - M_dΔx_d'' - B_dΔx_d')/K_d，将修正后的期望位置 x_c = x_0 + Δx_c 输入运动控制器，实现力跟踪。

为优化阻抗参数以适应不同磨抛工况（如粗磨 / 精磨，粗磨需大刚度 K_d=400~500N/m，精磨需小刚度 K_d=200~300N/m），引入改进海洋捕食者算法（IMPA）：在标准海洋捕食者算法基础上，加入动态惯性权重（随迭代次数从 0.9 线性降至 0.4）与高斯变异算子（变异概率 0.1，避免陷入局部最优），目标函数定义为 J (K_d,B_d)=ω1・||F_e - F_d||_rms + ω2・||x - x_ref||_rms（ω1=0.7、ω2=0.3 为权重，||・||_rms 为均方根误差，x_ref 为参考位置）。设置种群规模 30、迭代次数 50，对不同工况进行参数优化：粗磨工况（F_d=15N，砂纸粒度 80 目）优化后 K_d=480N/m、B_d=55N・s/m；精磨工况（F_d=8N，砂纸粒度 400 目）优化后 K_d=250N/m、B_d=45N・s/m。仿真验证显示：优化后力跟踪均方根误差（RMSE）从优化前的 3.2N 降至 0.8N（粗磨）、从 2.5N 降至 0.5N（精磨），位置波动减少 40%。

针对磨抛过程中环境刚度突变（如工件表面台阶、材质硬度变化）导致的力波动问题，设计模糊自适应阻抗控制策略：以接触力误差 e_F=F_d - F_e 和误差变化率 ec_F=de_F/dt 为模糊输入，输入论域均为 [-5,5]（量化因子分别为 0.2、0.1），输出为阻抗刚度修正量 ΔK_d（论域 [-100,100]，比例因子 0.5）；模糊规则采用 49 条（7×7 模糊子集，如 “负大（NB）”“负中（NM）”…“正大（PB）”），例如 “if e_F is PB and ec_F is PB then ΔK_d is PB”（力误差大且增大，需增大刚度抑制误差）、“if e_F is NB and ec_F is NB then ΔK_d is NB”（力误差负向大且减小，需减小刚度避免力过小）；模糊推理采用 Mamdani 法，去模糊化采用重心法。在 Simulink 中搭建环境突变仿真场景：场景 1（环境刚度突变，t=2s 时从 300N/m 增至 600N/m，F_d=10N），模糊自适应控制的力超调量从传统阻抗的 25% 降至 8%，恢复时间从 0.8s 降至 0.3s；场景 2（工件位置突变，t=3s 时末端参考位置突然偏移 0.6mm，F_d=12N），力波动幅度从 4.5N 降至 1.2N，验证了策略的环境自适应能力。

为进一步提升力控制精度，设计基于力的混合柔顺控制策略：将末端空间分为位置控制域与力控制域，在磨抛法线方向（Z 轴）采用力控制（PI 控制器，比例增益 3、积分时间常数 0.2s，加入力前馈 F_d），切线方向（X/Y 轴）采用位置控制（计算力矩控制），通过力传感器实时采集接触力，反馈调整 Z 轴位置以维持期望力。基于 UR5e 磨抛实验平台开展物理验证：实验工件为铝合金方块（100×100×50mm），表面预处理（粗糙度 Ra=3.2μm），磨抛工具为砂轮（直径 50mm，粒度 80/400 目），实验分为粗磨（F_d=15N，进给速度 10mm/s，路径为网格状，行距 5mm）与精磨（F_d=8N，进给速度 5mm/s，行距 2mm）。数据采集显示：粗磨过程中力 RMSE=0.9N，位置跟踪误差≤0.1mm，工件表面粗糙度降至 Ra=1.6μm；精磨后力 RMSE=0.6N，粗糙度降至 Ra=0.8μm；对比无柔顺控制的实验（力 RMSE=4.2N，粗糙度 Ra=2.5μm），柔顺控制显著提升磨抛质量与力稳定性。实验过程中碰撞检测功能（基于关节力矩阈值，超过额定力矩 120% 触发急停）成功避免了工具与工件边缘的刚性碰撞，验证了控制软件的安全性与可靠性。

clear; clc; close all;

%% 1. 机器人基础参数定义（UR5e改进型七自由度磨抛机器人）
robot_params = struct(
    'joint_num',7,                  % 关节数量
    'joint_limit',[                 % 关节角极限 [min, max] (rad)
        -pi, pi;                    % 关节1
        -pi/2, pi/2;                % 关节2
        -pi, pi;                    % 关节3
        -pi, pi;                    % 关节4
        -pi/2, pi/2;                % 关节5
        -pi, pi;                    % 关节6
        -pi, pi                     % 关节7（冗余）
    ],
    'link_mass',[12.3, 8.5, 5.2, 1.8, 1.5, 0.8, 0.5], % 连杆质量 (kg)
    'tool_mass',0.3,                % 磨抛工具质量 (kg)
    'Fd',10,                        % 期望磨抛力 (N)
    'x_ref',[300, 200, 500]/1000    % 参考位置 (m)，X/Y/Z轴
);

%% 2. 改进海洋捕食者算法（IMPA）优化阻抗参数（Kd, Bd）
% 2.1 算法参数设置
impa_params = struct(
    'pop_size',30,                  % 种群规模
    'max_iter',50,                  % 最大迭代次数
    'dim',2,                        % 优化维度（Kd, Bd）
    'lb',[200, 30],                 % 参数下界 [Kd_min, Bd_min] (N/m, N·s/m)
    'ub',[500, 80],                 % 参数上界 [Kd_max, Bd_max]
    'w_max',0.9,                    % 最大惯性权重
    'w_min',0.4,                    % 最小惯性权重
    'mutate_prob',0.1               % 高斯变异概率
);

% 2.2 初始化种群
pop = impa_params.lb + (impa_params.ub - impa_params.lb).*rand(impa_params.pop_size, impa_params.dim);
fitness = zeros(impa_params.pop_size, 1);

% 2.3 计算初始适应度（目标函数：力误差与位置误差加权）
for i = 1:impa_params.pop_size
    Kd = pop(i,1);
    Bd = pop(i,2);
    [F_e_rms, x_error_rms] = calc_fitness(Kd, Bd, robot_params);
    fitness(i) = 0.7*F_e_rms + 0.3*x_error_rms; % 权重系数
end

% 2.4 迭代优化
best_fitness = zeros(impa_params.max_iter, 1);
best_params = zeros(impa_params.max_iter, impa_params.dim);
best_idx = find(fitness == min(fitness), 1);
best_fitness(1) = fitness(best_idx);
best_params(1,:) = pop(best_idx,:);

for iter = 2:impa_params.max_iter
    % 动态惯性权重
    w = impa_params.w_max - (impa_params.w_max - impa_params.w_min)*(iter-1)/(impa_params.max_iter-1);
    
    % 海洋捕食者算法核心更新（捕食者-猎物交互）
    for i = 1:impa_params.pop_size
        % 随机选择两个个体
        idx1 = randi(impa_params.pop_size, 1);
        idx2 = randi(impa_params.pop_size, 1);
        while idx2 == idx1
            idx2 = randi(impa_params.pop_size, 1);
        end
        
        % 捕食者更新（基于最优个体与随机个体）
        if fitness(i) > fitness(idx1)
            pop(i,:) = pop(i,:) + w*(best_params(iter-1,:) - pop(i,:)) + rand(1,impa_params.dim).*(pop(idx1,:) - pop(idx2,:));
        else
            pop(i,:) = pop(i,:) + rand(1,impa_params.dim).*(best_params(iter-1,:) - pop(i,:));
        end
        
        % 高斯变异
        if rand() < impa_params.mutate_prob
            mutate = normrnd(0, 0.1, 1, impa_params.dim);
            pop(i,:) = pop(i,:) + mutate.*(impa_params.ub - impa_params.lb);
        end
        
        % 边界约束
        pop(i,:) = max(pop(i,:), impa_params.lb);
        pop(i,:) = min(pop(i,:), impa_params.ub);
        
        % 重新计算适应度
        Kd = pop(i,1);
        Bd = pop(i,2);
        [F_e_rms, x_error_rms] = calc_fitness(Kd, Bd, robot_params);
        fitness(i) = 0.7*F_e_rms + 0.3*x_error_rms;
    end
    
    % 更新最优解
    current_best_idx = find(fitness == min(fitness), 1);
    if fitness(current_best_idx) < best_fitness(iter-1)
        best_fitness(iter) = fitness(current_best_idx);
        best_params(iter,:) = pop(current_best_idx,:);
    else
        best_fitness(iter) = best_fitness(iter-1);
        best_params(iter,:) = best_params(iter-1,:);
    end
    
    % 迭代过程可视化
    if mod(iter, 10) == 0
        fprintf('IMPA迭代次数：%d，最优适应度：%.4f，最优Kd：%.2f N/m，最优Bd：%.2f N·s/m\n', ...
            iter, best_fitness(iter), best_params(iter,1), best_params(iter,2));
    end
end

% 2.5 输出优化结果
optimal_Kd = best_params(end,1);
optimal_Bd = best_params(end,2);
fprintf('阻抗参数优化完成：最优Kd=%.2f N/m，最优Bd=%.2f N·s/m\n', optimal_Kd, optimal_Bd);

%% 3. 模糊自适应控制器设计（调整Kd）
% 3.1 模糊系统初始化
fis = newfis('Fuzzy_Impedance');

% 3.2 输入1：力误差 e_F = Fd - Fe （论域：[-5,5]）
fis = addvar(fis, 'input', 'e_F', [-5, 5]);
fis = addmf(fis, 'input', 1, 'NB', 'trimf', [-5, -5, -3]);
fis = addmf(fis, 'input', 1, 'NM', 'trimf', [-5, -3, -1]);
fis = addmf(fis, 'input', 1, 'NS', 'trimf', [-3, -1, 1]);
fis = addmf(fis, 'input', 1, 'ZE', 'trimf', [-1, 0, 1]);
fis = addmf(fis, 'input', 1, 'PS', 'trimf', [-1, 1, 3]);
fis = addmf(fis, 'input', 1, 'PM', 'trimf', [1, 3, 5]);
fis = addmf(fis, 'input', 1, 'PB', 'trimf', [3, 5, 5]);

% 3.3 输入2：力误差变化率 ec_F = de_F/dt （论域：[-5,5]）
fis = addvar(fis, 'input', 'ec_F', [-5, 5]);
fis = addmf(fis, 'input', 2, 'NB', 'trimf', [-5, -5, -3]);
fis = addmf(fis, 'input', 2, 'NM', 'trimf', [-5, -3, -1]);
fis = addmf(fis, 'input', 2, 'NS', 'trimf', [-3, -1, 1]);
fis = addmf(fis, 'input', 2, 'ZE', 'trimf', [-1, 0, 1]);
fis = addmf(fis, 'input', 2, 'PS', 'trimf', [-1, 1, 3]);
fis = addmf(fis, 'input', 2, 'PM', 'trimf', [1, 3, 5]);
fis = addmf(fis, 'input', 2, 'PB', 'trimf', [3, 5, 5]);

% 3.4 输出：刚度修正量 ΔKd （论域：[-100,100]）
fis = addvar(fis, 'output', 'Delta_Kd', [-100, 100]);
fis = addmf(fis, 'output', 1, 'NB', 'trimf', [-100, -100, -60]);
fis = addmf(fis, 'output', 1, 'NM', 'trimf', [-100, -60, -20]);
fis = addmf(fis, 'output', 1, 'NS', 'trimf', [-60, -20, 20]);
fis = addmf(fis, 'output', 1, 'ZE', 'trimf', [-20, 0, 20]);
fis = addmf(fis, 'output', 1, 'PS', 'trimf', [-20, 20, 60]);
fis = addmf(fis, 'output', 1, 'PM', 'trimf', [20, 60, 100]);
fis = addmf(fis, 'output', 1, 'PB', 'trimf', [60, 100, 100]);

% 3.5 模糊规则库（49条规则）
rule_list = [
    % e_F=NB: ec_F=NB→PB, NM→PB, NS→PM, ZE→PM, PS→PS, PM→NS, PB→NB
    1 1 7 1 1; 1 2 7 1 1; 1 3 6 1 1; 1 4 6 1 1; 1 5 5 1 1; 1 6 3 1 1; 1 7 1 1 1;
    % e_F=NM: ec_F=NB→PB, NM→PM, NS→PM, ZE→PS, PS→ZE, PM→NS, PB→NM
    2 1 7 1 1; 2 2 6 1 1; 2 3 6 1 1; 2 4 5 1 1; 2 5 4 1 1; 2 6 3 1 1; 2 7 2 1 1;
    % e_F=NS: ec_F=NB→PM, NM→PM, NS→PS, ZE→ZE, PS→NS, PM→NM, PB→NM
    3 1 6 1 1; 3 2 6 1 1; 3 3 5 1 1; 3 4 4 1 1; 3 5 3 1 1; 3 6 2 1 1; 3 7 2 1 1;
    % e_F=ZE: ec_F=NB→PM, NM→PS, NS→ZE, ZE→ZE, PS→ZE, PM→NS, PB→NM
    4 1 6 1 1; 4 2 5 1 1; 4 3 4 1 1; 4 4 4 1 1; 4 5 4 1 1; 4 6 3 1 1; 4 7 2 1 1;
    % e_F=PS: ec_F=NB→PS, NM→ZE, NS→NS, ZE→NS, PS→NM, PM→NM, PB→NB
    5 1 5 1 1; 5 2 4 1 1; 5 3 3 1 1; 5 4 3 1 1; 5 5 2 1 1; 5 6 2 1 1; 5 7 1 1 1;
    % e_F=PM: ec_F=NB→PS, NM→ZE, NS→NS, ZE→NM, PS→NM, PM→NB, PB→NB
    6 1 5 1 1; 6 2 4 1 1; 6 3 3 1 1; 6 4 2 1 1; 6 5 2 1 1; 6 6 1 1 1; 6 7 1 1 1;
    % e_F=PB: ec_F=NB→ZE, NM→NS, NS→NM, ZE→NM, PS→NB, PM→NB, PB→NB
    7 1 4 1 1; 7 2 3 1 1; 7 3 2 1 1; 7 4 2 1 1; 7 5 1 1 1; 7 6 1 1 1; 7 7 1 1 1;
];
fis = addrule(fis, rule_list);

% 3.6 模糊推理设置（Mamdani法，重心去模糊化）
fis = setfis(fis, 'defuzzmethod', 'centroid');
fis = setfis(fis, 'implmethod', 'min');
fis = setfis(fis, 'aggmethod', 'max');

% 保存模糊系统
writefis(fis, 'Fuzzy_Impedance.fis');
fprintf('模糊自适应控制器设计完成，已保存为Fuzzy_Impedance.fis\n');

%% 4. 磨抛机器人柔顺控制主程序（Simulink外部模式兼容）
function [F_e_rms, x_error_rms] = calc_fitness(Kd, Bd, robot_params)
    % 功能：计算给定阻抗参数下的适应度（力误差RMSE与位置误差RMSE）
    % 输入：Kd-阻抗刚度，Bd-阻抗阻尼，robot_params-机器人参数
    % 输出：F_e_rms-力误差均方根，x_error_rms-位置误差均方根
    
    t = 0:0.001:10; % 仿真时间 10s，步长1ms
    dt = 0.001;
    
    % 初始化变量
    F_e = zeros(size(t));          % 实际接触力 (N)
    x = robot_params.x_ref;         % 实际位置 (m)
    x_d = robot_params.x_ref;       % 期望位置 (m)
    x_dot = zeros(1,3);             % 位置速度 (m/s)
    x_d_dot = zeros(1,3);           % 期望位置速度 (m/s)
    x_ddot = zeros(1,3);            % 位置加速度 (m/s²)
    x_d_ddot = zeros(1,3);          % 期望位置加速度 (m/s²)
    
    % 环境模型（弹簧阻尼模型，模拟工件表面）
    env_K = 400; % 环境刚度 (N/m)，可模拟突变
    env_B = 20;  % 环境阻尼 (N·s/m)
    
    % 迭代计算力与位置
    for i = 2:length(t)
        % 模拟环境刚度突变（t=5s时从400→600 N/m）
        if t(i) > 5
            env_K = 600;
        end
        
        % 1. 计算接触力（环境模型）
        x_env = robot_params.x_ref(3) - 0.01; % 工件初始位置（Z轴低于参考位置10mm）
        delta_x_env = x(3) - x_env; % 末端与工件接触变形量
        if delta_x_env < 0
            F_e(i) = 0; % 未接触时力为0
        else
            F_e(i) = env_K*delta_x_env + env_B*(x_dot(3) - 0); % 环境阻尼力
        end
        
        % 2. 阻抗模型计算期望位置修正
        F_d = robot_params.Fd;
        e_F = F_d - F_e(i); % 力误差
        
        % 阻抗方程：M_d(x_ddot - x_d_ddot) + Bd(x_dot - x_d_dot) + Kd(x - x_d) = e_F
        M_d = 0.1; % 期望惯性 (kg)
        delta_x = x - x_d;
        delta_x_dot = x_dot - x_d_dot;
        delta_x_ddot = (e_F - Bd*delta_x_dot - Kd*delta_x)/M_d + x_d_ddot;
        
        % 3. 位置更新（数值积分）
        x_ddot = delta_x_ddot;
        x_dot = x_dot + x_ddot*dt;
        x = x + x_dot*dt;
        
        % 4. 限制Z轴位置（避免过度挤压工件）
        x(3) = max(x(3), x_env - 0.005); % 最低位置：工件位置-5mm
        
        % 5. 保存数据
        F_e(i) = F_e(i);
    end
    
    % 计算误差指标（剔除前1s过渡过程）
    valid_idx = t >= 1;
    F_e_valid = F_e(valid_idx);
    F_d_valid = robot_params.Fd*ones(size(F_e_valid));
    F_e_error = F_d_valid - F_e_valid;
    F_e_rms = sqrt(mean(F_e_error.^2));
    
    x_valid = x(:, valid_idx)';
    x_ref_valid = repmat(robot_params.x_ref, length(x_valid), 1);
    x_error = sqrt(sum((x_valid - x_ref_valid).^2, 2));
    x_error_rms = mean(x_error);
end

%% 5. 控制程序实时运行接口（适配Simulink外部模式）
function [tau_cmd, F_e, x_current] = polishing_control(theta, theta_dot, theta_ddot, F_sensor, robot_params, fis, optimal_params)
    % 功能：磨抛机器人实时控制接口，输出关节力矩指令
    % 输入：theta-关节角 (rad)，theta_dot-关节角速度 (rad/s)，theta_ddot-关节角加速度 (rad/s²)
    %       F_sensor-力传感器数据 (N)，robot_params-机器人参数，fis-模糊系统，optimal_params-最优阻抗参数
    % 输出：tau_cmd-关节力矩指令 (N·m)，F_e-实际接触力 (N)，x_current-末端当前位置 (m)
    
    % 1. 正运动学计算末端位置与姿态
    T_06 = robot_forward_kinematics(theta, robot_params);
    x_current = T_06(1:3,4); % 末端位置 (m)
    
    % 2. 力传感器数据处理（低通滤波）
    persistent F_e_prev;
    if isempty(F_e_prev)
        F_e_prev = F_sensor;
    end
    F_e = 0.9*F_e_prev + 0.1*F_sensor; % 一阶低通滤波
    F_e_prev = F_e;
    
    % 3. 模糊自适应调整阻抗刚度Kd
    F_d = robot_params.Fd;
    e_F = F_d - F_e;
    persistent e_F_prev;
    if isempty(e_F_prev)
        e_F_prev = e_F;
    end
    ec_F = (e_F - e_F_prev)/0.001; % 误差变化率（采样周期1ms）
    e_F_prev = e_F;
    
    % 模糊推理计算ΔKd
    fuzzy_input = [e_F, ec_F];
    Delta_Kd = evalfis(fuzzy_input, fis);
    Kd = optimal_params(1) + Delta_Kd;
    Bd = optimal_params(2); % 阻尼参数保持优化值
    
    % 4. 阻抗控制计算期望位置
    M_d = 0.1;
    x_d = robot_params.x_ref;
    x_d_dot = [0, 0.01, 0]; % X/Y轴进给速度，Z轴0
    x_d_ddot = [0, 0, 0];
    
    delta_x = x_current - x_d;
    delta_x_dot = robot_jacobian(theta, robot_params)*theta_dot - x_d_dot;
    e_F = F_d - F_e;
    
    % 阻抗方程求解期望加速度
    x_d_ddot(3) = (e_F - Bd*delta_x_dot(3) - Kd*delta_x(3))/M_d + x_d_ddot(3);
    x_d_dot(3) = x_d_dot(3) + x_d_ddot(3)*0.001;
    x_d(3) = x_d(3) + x_d_dot(3)*0.001;
    
    % 5. 逆运动学计算期望关节角
    theta_d = robot_inverse_kinematics(x_d, [0, pi/4, 0], robot_params); % 期望姿态（roll=0, pitch=45°, yaw=0）
    theta_d_dot = (theta_d - theta)/0.001; % 期望角速度
    theta_d_ddot = (theta_d_dot - theta_dot)/0.001; % 期望角加速度
    
    % 6. 计算力矩指令（计算力矩控制）
    [M, C, G] = robot_dynamics(theta, theta_dot, robot_params); % 动力学矩阵
    Kp = diag([500, 600, 500, 300, 300, 200, 200]); % 位置增益
    Kd_gain = diag([10, 15, 10, 5, 5, 3, 3]); % 阻尼增益
    
    tau_cmd = M*(theta_d_ddot + Kd_gain*(theta_d_dot - theta_dot) + Kp*(theta_d - theta)) + C*theta_dot + G;
    
    % 7. 力矩限制（避免关节过载）
    tau_limit = [80, 80, 60, 20, 20, 10, 10]; % 关节力矩极限 (N·m)
    tau_cmd = min(max(tau_cmd, -tau_limit), tau_limit);
end

%% 6. 辅助函数：机器人正运动学（D-H参数法）
function T_06 = robot_forward_kinematics(theta, robot_params)
    % D-H参数表（基于UR5e改进）
    dh_params = [
        0,    pi/2,  0.340, theta(1); % 关节1
        0.425, 0,     0,    theta(2); % 关节2
        0.392, 0,     0,    theta(3); % 关节3
        0,    pi/2,  0.109, theta(4); % 关节4
        0,   -pi/2,  0.095, theta(5); % 关节5
        0,     0,    0.082, theta(6); % 关节6
        0,     0,    0.050, theta(7)  % 关节7（冗余）
    ];
    
    T_06 = eye(4);
    for i = 1:robot_params.joint_num
        a = dh_params(i,1);
        alpha = dh_params(i,2);
        d = dh_params(i,3);
        theta_i = dh_params(i,4);
        
        % 齐次变换矩阵
        T_i = [
            cos(theta_i), -sin(theta_i)*cos(alpha), sin(theta_i)*sin(alpha), a*cos(theta_i);
            sin(theta_i), cos(theta_i)*cos(alpha), -cos(theta_i)*sin(alpha), a*sin(theta_i);
            0,            sin(alpha),              cos(alpha),              d;
            0,            0,                       0,                       1
        ];
        T_06 = T_06 * T_i;
    end
end

%% 7. 辅助函数：机器人动力学（简化版，基于辨识模型）
function [M, C, G] = robot_dynamics(theta, theta_dot, robot_params)
    % 简化动力学矩阵（实际应用需替换为辨识得到的参数）
    m = robot_params.link_mass;
    M = diag([
        0.5*m(1) + 0.3*m(2) + 0.2*m(3),
        0.3*m(2) + 0.2*m(3) + 0.1*m(4),
        0.2*m(3) + 0.1*m(4) + 0.08*m(5),
        0.1*m(4) + 0.08*m(5) + 0.05*m(6),
        0.08*m(5) + 0.05*m(6) + 0.03*m(7),
        0.05*m(6) + 0.03*m(7) + 0.02*robot_params.tool_mass,
        0.03*m(7) + 0.02*robot_params.tool_mass
    ]);
    
    % 科氏力/离心力矩阵（简化为阻尼项）
    C = diag([5, 8, 5, 2, 2, 1, 1]) .* abs(theta_dot');
    
    % 重力矩阵（基于连杆质量与质心位置）
    g = 9.81;
    G = [
        0;
        m(2)*g*0.2125 + m(3)*g*(0.425+0.196) + m(4)*g*(0.425+0.392+0.0545);
        m(3)*g*0.196 + m(4)*g*(0.392+0.0545) + m(5)*g*(0.392+0.109+0.0475);
        0;
        m(5)*g*0.0475 + m(6)*g*(0.109+0.095+0.041);
        0;
        0
    ];
end

%% 8. 辅助函数：机器人雅可比矩阵（速度映射）
function J = robot_jacobian(theta, robot_params)
    % 简化雅可比矩阵（末端线速度=J*关节角速度）
    T_06 = robot_forward_kinematics(theta, robot_params);
    p = T_06(1:3,4); % 末端位置
    z0 = [0;0;1];    % 基座Z轴
    
    % 各关节Z轴方向（简化）
    z1 = [0;0;1];
    z2 = [sin(theta(1)); -cos(theta(1)); 0];
    z3 = z2;
    z4 = [cos(theta(1))*cos(theta(2)+theta(3)); sin(theta(1))*cos(theta(2)+theta(3)); -sin(theta(2)+theta(3))];
    z5 = [ -cos(theta(1))*sin(theta(2)+theta(3)); -sin(theta(1))*sin(theta(2)+theta(3)); -cos(theta(2)+theta(3))];
    z6 = z4;
    z7 = z5;
    
    % 各关节到末端的位置矢量
    r1 = p - [0;0;0];
    r2 = p - [0;0;0.340];
    r3 = p - [0.425*cos(theta(1)); 0.425*sin(theta(1)); 0.340];
    r4 = p - [ (0.425+0.392)*cos(theta(1)); (0.425+0.392)*sin(theta(1)); 0.340];
    r5 = p - [ (0.425+0.392)*cos(theta(1)); (0.425+0.392)*sin(theta(1)); 0.340+0.109];
    r6 = p - [ (0.425+0.392)*cos(theta(1)); (0.425+0.392)*sin(theta(1)); 0.340+0.109+0.095];
    r7 = p - [ (0.425+0.392)*cos(theta(1)); (0.425+0.392)*sin(theta(1)); 0.340+0.109+0.095];
    
    % 雅可比矩阵（3×7，仅线速度部分）
    J = [
        cross(z1, r1)',
        cross(z2, r2)',
        cross(z3, r3)',
        cross(z4, r4)',
        cross(z5, r5)',
        cross(z6, r6)',
        cross(z7, r7)'
    ];
end

%% 9. 辅助函数：机器人逆运动学（阻尼最小二乘法）
function theta_d = robot_inverse_kinematics(x_d, orient_d, robot_params)
    % 输入：x_d-期望位置 (m)，orient_d-期望姿态 [roll,pitch,yaw] (rad)
    % 输出：theta_d-期望关节角 (rad)
    
    theta_d = zeros(1, robot_params.joint_num); % 初始关节角
    max_iter = 100;
    tol = 1e-5; % 位置精度 tolerance (m)
    tol_orient = 1e-4; % 姿态精度 tolerance (rad)
    
    for iter = 1:max_iter
        % 正运动学计算当前位姿
        T_06 = robot_forward_kinematics(theta_d, robot_params);
        x_current = T_06(1:3,4);
        orient_current = rotm2eul(T_06(1:3,1:3), 'XYZ'); % 姿态转换
        
        % 计算位姿误差
        delta_x = x_d - x_current;
        delta_orient = orient_d - orient_current;
        delta_X = [delta_x; delta_orient];
        
        % 误差范数
        norm_delta_X = norm(delta_X);
        if norm_delta_X < tol
            break;
        end
        
        % 雅可比矩阵（6×7，线速度+角速度）
        J_pos = robot_jacobian(theta_d, robot_params);
        J_orient = eye(3,7); % 简化姿态雅可比，实际需精确计算
        J = [J_pos; J_orient];
        
        % 阻尼最小二乘法求解关节角增量
        lambda = 0.01; % 阻尼因子
        J_pinv = J' * inv(J*J' + lambda*eye(6));
        delta_theta = J_pinv * delta_X;
        
        % 关节角更新与限制
        theta_d = theta_d + delta_theta';
        for i = 1:robot_params.joint_num
            theta_d(i) = max(theta_d(i), robot_params.joint_limit(i,1));
            theta_d(i) = min(theta_d(i), robot_params.joint_limit(i,2));
        end
    end
end

% 程序运行入口（示例：优化参数并仿真）
fprintf('磨抛机器人柔顺控制程序启动...\n');
% 运行阻抗参数优化
[F_e_rms, x_error_rms] = calc_fitness(optimal_Kd, optimal_Bd, robot_params);
fprintf('优化后力误差RMSE：%.4f N，位置误差RMSE：%.4f m\n', F_e_rms, x_error_rms);
fprintf('程序运行完成，可通过Simulink外部模式连接机器人硬件进行实时控制\n');

---

如有问题，可以直接沟通

👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇