机械臂相连关节之间的耦合关系（末端J5-J6为例）

一、耦合是物理事实，无法消除

首先需要明确，J6的运动必然影响J5，这种影响来自物理结构本身，不是调参能消除的。想象J5是一个肘关节，J6是手腕关节，工具是手。

J5(肘) → 连杆 → J6(腕) → 工具(手)

当J6旋转时，工具（手）相对于J5（肘）的位置发生了改变，重力对J5轴的力矩取决于J5后面所有东西的质量中心在哪里。当J6转动，工具和J6连杆的质量中心变了，所以J5感受到的重力力矩必然变。这件事无论如何都会发生， 区别只在于，是否主动去补偿这个变化。

二、J5和J6的耦合力矩

2.1 物理拆解

• J5后面挂着两个质量团：连杆5（含J6电机本体）和工具组（J6输出端+清洁模组）
• J5的负载重力矩 = 连杆5的重力矩 + 工具组对J5轴的重力矩

对于J5的重力补偿，需要计算的是：

其中：

• m5,d5,ϕ5：连杆5的质量、质心距离、质心偏移角
• mtool,dtool,ϕtool：工具组的质量、质心距离、质心偏移角
• L5：J5到J6的连杆长度

可以看到，θ6直接出现在 τg5的表达式中，这就是耦合。

2.2 工程耦合

正确理解：不是消除J6对J5的影响，而是在计算J5的重力补偿时，把J6的角度作为输入考虑进去。

三种处理策略：

对于清洁模组（工具相对轻，J6行程小）：如果J6的总转动范围只有±15°，且工具重量<500g，那J6位置对J5的影响通常在10%~20%以内，静态近似（J6固定中位近似）是可行的，导纳外环会吸收这个小的误差。

但如果工具较重（>1kg）或J6行程大（>±30°），建议用联合补偿，把J6角度纳入J5的重力补偿计算。

三、J5调节时，J6的位置

3.1 推荐位置：J6的行程中点

原因：

1. 后续清洁控制中，J6大部分时间工作在中点附近，这是零空间优化的结果，J5做粗调，J6保持在中位并在中位附近做微调。
2. 如果标定和整定都在中位做，实际运行中的偏差最小。
3. J6偏离中位的幅度=标定误差+微调幅值，所以中位带来的系统偏差最小。

3.2 为什么不用任意位置

假如把J6停在了行程的+15°端，但实际清洁时J6大部分时间在-5°附近工作，那么：

• J5的重力补偿参数在+15°时是准的，在-5°时，工具组位置变了，J5感受到的重力矩就不准了
• J5会出现一个系统性偏置，导纳需要持续纠正它

所以整定位置与实际工作位置一致，如果实际清洁时J6只在中点±10°内运动，那就在中点整定。如果实际清洁时J6偏在某个位置（比如为了贴合曲面需要J6长期偏+5°），那就在那个位置整定。

3.3 实用建议

• 如果J6的行程范围很小（<20°），中位整定足够。
• 如果行程较大，建议在三个位置分别标定J5的重力补偿：中位、+极限×0.6、-极限×0.6，然后在线做线性插值。

四、不同角度的Kp/Kd

4.1 Kp/Kd是控制器增益，不是物理参数

Kp/Kd取决于系统的有效惯量 Jeff和期望的阻尼比 ζ：

当J5在不同角度时：

• 如果J5本身的连杆惯量远大于J6+工具的贡献，则 Jeff基本不变，Kp/Kd可以复用。
• 如果J6+工具的惯量占比显著，且J6位置改变导致J5的有效惯量变化>10%，则Kp/Kd随J6角度有一定变化。

4.2 场景分析

• J5驱动的是中等质量连杆（含J6电机），工具端质量很小（<1kg）。
• J6的旋转不会改变J5的有效惯量太多——因为J6轴线到J5轴线的距离固定，工具质量小，其惯量贡献的绝对值不大。
• 结论：Kp/Kd在J5的不同工作范围内基本可以复用， 不需要分区段调参。

4.3 验证方法

在J5的三个典型角度（0°、+30°、-30°）各做一次阶跃响应测试，观察：

• 响应曲线形状是否一致
• 超调量是否接近
• 稳态误差是否类似

如果三个角度的响应曲线几乎一致，说明Kp/Kd可以全域复用，如果某个角度下出现了明显的超调或振荡，说明有效惯量变了，此时可在该角度微调Kd，或直接接受（因为导纳外环会兜底）。

五、校验流程

Step 1: 确定J6的默认工作位置
        θ6_default = 行程中点（或实际清洁时的典型位置）

Step 2: 在θ6_default处，手动标定J5和J6的重力补偿
        - 让J5处于水平（最重载）姿态，调J5的τ_ff直到悬浮
        - 让J6也处于水平（相对于其连杆），调J6的τ_ff直到悬浮
        - 在不同θ5位置重复，得到重力补偿曲线

Step 3: 在θ6_default处，整定J5和J6的Kp/Kd
        - 悬空摆动实验
        - 阶跃响应测试

Step 4: 验证不同θ5下Kp/Kd是否可复用
        - θ5在0°、±30°各做一次阶跃测试
        - 响应一致→复用
        - 偏差大→微调

Step 5: 在控制循环中，实时计算τ_ff
        - 使用策略A（θ5+θ6联合重力补偿）或策略B（只θ5近似）
        - 摩擦补偿按各自关节的dq独立计算