MIT Drake 真实动力学模拟 Annin AR4 (MK5) 六轴机械臂（详细分册）

📌 本分册目标

站内的 AR4 真实 3D 轨迹实验室 是运动学演示：它在浏览器里做 6 轴 IK 解算，算出「关节角要转到多少」，然后用 Three.js 把机械臂画出来。它会告诉你几何上到得了，但不会告诉你真机转不转得动 —— 因为那里没有重力、没有惯性、没有电机扭矩上限。

本分册补上这一层：用 MIT Drake 把同一台 AR4 放进真实动力学里跑。URDF 的关节原点与官方 ar_macro.xacro 逐字段对齐，Drake 会真实计算重力矩、惯性耦合、科氏力与各轴扭矩，于是你能亲眼看到：同样一条轨迹，真机需要付出的跟踪滞后与扭矩饱和到底有多少。

你会亲手得到：一份末端轨迹 CSV、一组「最大跟踪误差 / 扭矩饱和占比」的量化结论，以及一段可直接离线播放的 3D 动画。这些结论直接决定真机该选多大的减速比、跑多快、末端能挂多重。

🧰 环境准备

• Python 3.9 ~ 3.11（Drake 的 wheel 只支持特定 Python 版本，越新的 Drake 要求越新的 Python）
• pydrake 与 numpy：python3 -m venv ~/drake-venv && source ~/drake-venv/bin/activate && pip install drake numpy
• 系统：Ubuntu 20.04 / 22.04 与 macOS 支持最好；Windows 请用 WSL2（Drake 无官方 Windows pip 包）
• Intel 版 macOS：官方较高版本已不再提供 x86_64 wheel，需装最后支持的版本 pip install drake==1.34.0
• 可选：浏览器（用于 Meshcat 的 3D 视图）

📦 第 1 步：四个文件，一个目录

• ⬇ ar4_arm.urdf 示意连杆模型：连杆外形用长方体近似，无需任何外部网格文件，Drake 可直接加载。关节原点 / 轴向 / 限位逐字段对应官方 xacro。先跑通这一个。
• ⬇ ar4_real.urdf 真实零件模型：18 个官方 STL 转出的 OBJ 网格，真机外观（银铝件 / 黑电机 / 蓝盖 / 白标）。用于第 5 步出 3D 动画。
• ⬇ arm_wave_drake_ar4.py 仿真主程序：建世界 + 1 kHz 数字控制器 + 增益自动整定 + 数据记录，支持回放网页导出的关节轨迹。
• ⬇ arm_meshcat_ar4.py 真实外观 3D：加载真实零件版 URDF，可在浏览器里看姿态，也可导出单文件动画 HTML；支持 --shape sine|circle|tri 在画板上画正弦曲线 / 圆形 / 三角形，并实时画出末端运动轨迹（见第 6 步）。

四个文件放在同一个目录下（脚本按自身位置找 URDF，不需要改路径）。若只想跑数值，前两个文件的下一个就够了。

🔧 第 2 步：读懂 URDF —— AR4 的「身份证」

URDF 用一棵树描述机器人：节点是 link（连杆），边是 joint（关节）。下表就是 ar4_arm.urdf 的全部关节参数，逐字段取自官方 annin_ar4_description/urdf/ar_macro.xacro（MK5）—— Drake 就是靠这些数算真实的动力学。

全臂自重合计约 5.1 kg，J2→J3 臂长 0.305 m、J4→J5 臂长 0.223 m，末端法兰到 tool 参考点 0.03 m。

<!-- ① 必须有一个固定关节把底座焊在世界原点，否则整条臂会自由落体 -->
<link name="world"/>
<joint name="world_joint" type="fixed">
  <parent link="world"/><child link="base"/>
  <origin xyz="0 0 0" rpy="0 0 0"/>
</joint>

<!-- ② 关节 = 父子关系 + 相对位置 + 轴向 + 限位 + 扭矩上限 + 阻尼 -->
<!--    注意 rpy="1.57079633 0 -1.57079633" 就是官方的 rpy="90 0 -90" -->
<joint name="J2" type="revolute">
  <parent link="link1"/><child link="link2"/>
  <origin xyz="0 0.06415 -0.07778" rpy="1.57079633 0 -1.57079633"/>
  <axis xyz="0 0 -1"/>
  <limit lower="-0.73303829" upper="1.57079633" effort="20" velocity="1.5"/>
  <dynamics damping="0.10"/>
</joint>

<!-- ③ 连杆 = 外观 + 碰撞 + 惯性（Drake 靠 mass 与 inertia 算真实受力） -->
<link name="link2">
  <visual><origin xyz="0 -0.15 0"/><geometry><box size="0.07 0.30 0.07"/></geometry></visual>
  <inertial>
    <origin xyz="0 -0.15 0"/>
    <mass value="1.10"/>
    <inertia ixx="0.008700" ixy="0" ixz="0" iyy="0.000898" iyz="0" izz="0.008700"/>
  </inertial>
</link>

▶️ 第 3 步：跑起来

[ar4] plant=6 dof · gravity=-9.81 m/s²
[ar4] 扭矩上限(N·m) = [20. 20. 15.  8.  8.  5.]
[ar4] 有效惯量(kg·m²) = [9.64e-02 2.16e-01 7.46e-02 5.37e-04 2.03e-03 9.08e-05]
[ar4] 整定 kp = [400. 400. 300. 160. 160. 100.]
[ar4] 整定 kd = [ 9.94 14.89  7.57  0.27  0.91  0.05]
t=7.0s 实测(rad):   -0.153   -0.012   -0.600    0.153    0.430    0.278  末端XYZ(m): 0.052 -0.308 0.616
[summary] 最大跟踪误差(rad): 0.000 0.000 0.000 0.000 0.000 0.000
[summary] 扭矩饱和占比(%):  0 2 0 0 0 6
[summary] 末端轨迹范围(m): x[-0.144, 0.052] y[-0.517, -0.308] z[0.216, 0.616]
[done] 轨迹已保存 -> ar4_trail.csv

PD 增益是怎么来的

不需要手调增益 —— 程序会先用动能法测出每个关节的有效转动惯量（令该关节速度为 1，则动能 T = ½I），再按下面的判据整定：

kp = min( effort / 0.05 ,  0.5 · (1.5/Δt)² · I )
     #  前项：0.05 rad 误差内电机不进入饱和（线性工作区）
     #  后项：1 kHz 数字控制的稳定带宽约束（ω·Δt < 1.5）
kd = min( 1.6 · sqrt( kp · I ) ,  I / (2Δt) )
     #  前项：略过阻尼，抑制摆动超调
     #  后项：微分时间常数 τ = I/kd 必须 ≥ 2Δt（腕部抖动的解药，见下节）

想手动覆盖，用 --kp / --kd；想看每次的力矩细节，加 --debug 会额外打印命令、误差、实际角速度与重力矩 —— 排查数值抖动靠的就是那一行角速度。

怎么读 summary

• 最大跟踪误差：真实动力学必然有滞后，关键看量级是否可接受（本模型实测：默认参数下收敛到 0，关掉重力前馈后 J2 约 0.016 rad ≈ 0.9°）。若是几十度，多半是增益或惯量设置有误。
• 扭矩饱和占比：某个关节饱和占比高，就说明这个动作在真机上最容易堵转丢步 —— 要么加大减速比，要么降低速度 / 幅值。
• 末端轨迹范围：与站内 3D 实验室画出的轨迹做对比，验证「解出来的角度」与「真跑出来的路径」差多少。
• ⚠️ 饱和高但误差小：最容易被骗过的一种情况，见下方专节。

⚠️ 腕部抖动：饱和 74%，误差却几乎为零

这是本教程在实际调试中真实踩到的坑，值得单独讲。早期版本的 kd 只由阻尼比决定，跑 14 s 摆动的结果是：J6 饱和占比 74%、J4 饱和 30%，但两者跟踪误差分别只有 0.073 / 0.006 rad —— 乍看「误差这么小，应该没问题」。

加 --debug 把实际角速度打出来，才看见真相：

   err :  -0.000  -0.000  -0.000   0.005  -0.000   0.022
   qd  :   0.440   0.035  -0.257 -10.435   0.484  47.229   ← J4 / J6 疯了
   u   :  -2.288  -5.854   0.025   5.609  -0.767  -4.982

J6 的实际角速度是 ±47 rad/s，而目标速度只有 0.22 rad/s，相差 200 倍 —— 它在正负之间高频抖动，平均值相互抵消，所以位置上看不出异常，力矩却一直顶在上限。

根因是微分时间常数 τ = I/kd。腕部三轴的惯量比 J2 小 2~3 个量级（J6 仅 9.08e-5 kg·m²），而 kd 按阻尼比公式算出 0.15，对应 τ = 0.6 ms —— 比控制周期 1 ms 还短，一步之内速度反馈就能把力矩打满，于是形成极限环。

规则很清楚：τ 大于控制周期的关节都正常，τ 小于控制周期的都在抖。加上 kd ≤ I/(2Δt) 这条约束后（J6 的 kd 从 0.15 降到 0.05）：

修正后 qd 与目标速度 cmd_dot 逐位吻合（-0.204 = 0.218·cos 3.5），抖动消失、真正跟上了。

三组对照实验（实测数据）

三行数据把「重力补偿前馈值不值」量化了：不加前馈，J2 会稳定偏差约 0.9°；而关掉重力场后连饱和都减半，说明 J2 的力矩份额主要来自扛自重。

🔁 第 4 步：回放网页导出的轨迹（本分册的重头戏）

这是把「网页运动学」和「真实动力学」接起来的关键一步。

🎬 第 5 步：真实外观 3D 与离线动画

• 看实时 3D：python3 arm_meshcat_ar4.py --hold --html none —— 浏览器打开终端打印的 http://localhost:7000，由 18 个官方零件组成的 AR4 姿态被真实动力学驱动。一定要加 --hold：否则脚本跑完就退出，meshcat 服务随之关闭，画面会一闪而过。
• 导出可离线播放的动画：python3 arm_meshcat_ar4.py --html arm_real_3d_ar4.html —— 生成的单个 HTML 文件自带全部几何与控制，双击即可播放，方便分享或存档（代价是要把 18 个网格内嵌成 base64，比较慢，所以加了 --html none 可跳过）。
• 只出数值、不开 3D：python3 arm_meshcat_ar4.py --no-meshcat —— 适合服务器或无图形界面环境。
• 回放网页轨迹：python3 arm_meshcat_ar4.py --q-traj q_traj.npy —— 用真实零件看那条轨迹在动力学下的表现。
• 在画板上画正弦 / 圆 / 三角形：python3 arm_meshcat_ar4.py --shape circle --hold --html none —— 场景里会实时画出末端运动轨迹，详见第 6 步。

⚠️ 两个必踩的坑（脚本已内建处理，这里说明原因）

坑一：官方 URDF 的碰撞体会把腕部关节「锁死」。AR4 官方 URDF 给每个连杆只配了一个粗略的长方体 collision，相邻连杆的盒子互相穿插 —— link4 的盒子覆盖 z∈[-0.22, 0]，link5 的盒子又落在同一区间。Drake 的接触求解器于是施加巨大的穿透修正力：实测 J4 饱和占比 89%、跟踪误差 0.430 rad（25°）完全跟不上；把 collision 剥离后立刻恢复到 0% / 0.007 rad。脚本默认自动剥离（源 URDF 不动，另存一份临时文件），要保留用 --keep-collisions。

坑二：连续积分器跑不动这个模型。腕部 J6 的有效惯量只有 9.08e-5 kg·m²，系统刚性极强，连续时间积分器被迫把步长压到极小 —— 实测跑了 4 分钟连 1 秒仿真都没到。脚本默认改用 0.001 s 离散半隐欧拉（正好等于 1 kHz 控制周期），8 秒仿真约 40 秒跑完。

🎯 第 6 步：在画板上画 正弦 / 圆形 / 三角形（运动轨迹可视化）

站内 3D 轨迹实验室 里的三种末端轨迹，现在可以直接在 Drake 里跑 —— 用的是同一块画板、同一组目标点，差别只在于：网页是瞬时的运动学解，这里是 1 kHz 扭矩控制下的真实动力学响应。

# 三种形状同时看：各起一个进程、端口分开，浏览器分别打开对照。
# 场景里会自动画出：蓝线 = 参考轨迹   黄线 = 末端实际轨迹   红球 = 当前目标点
python3 arm_meshcat_ar4.py --shape circle --port 7000 --hold --html none   # → localhost:7000
python3 arm_meshcat_ar4.py --shape sine   --port 7001 --hold --html none   # → localhost:7001
python3 arm_meshcat_ar4.py --shape tri    --port 7002 --hold --html none   # → localhost:7002

# 只跑一种：端口默认就是 7000，不用写 --port
python3 arm_meshcat_ar4.py --shape sine --hold --html none

# 导出可离线播放 / 转发分享的单文件动画
python3 arm_meshcat_ar4.py --shape sine --html arm_sine_3d.html

# 只算数值，并把解出的关节轨迹存下来，喂给数值脚本做定量分析
python3 arm_meshcat_ar4.py --shape tri --no-meshcat --save-q q_tri.npy
python3 arm_wave_drake_ar4.py --q-traj q_tri.npy

画板在 Drake 世界系里的位置

网页用 Y-up 显示，Drake 的世界系是 Z-up，中间差一次 Rx(-90°)。换算后网页坐标 (x, YC, ZP) 对应 Drake 的 (x, -ZP, YC)：画板竖直立在正前方 y = −0.174 m、板中心高 z = 0.40 m，板面 0.30 × 0.22 m。三种形状与网页的 targetAt() 逐字段一致，都是 5 s 一个周期的闭合路径，可以一直循环。

末端轨迹怎么变成关节角：用 Drake 的逆运动学

和网页一样，只解 J1 / J2 / J3 / J5，J4 与 J6 全程锁 0（末端姿态不变，笔尖始终垂直板面）。区别是这里用 Drake 自带的 InverseKinematics，约束直接写在 URDF 的真实运动学上。采样点数用 --shape-points 调（默认 240 点/圈）。

def shape_points(shape, n=240):
    """画板上的末端目标点，返回 n×3（Drake 世界系 Z-up）。
    与网页 arm_traj_3d.html 的 targetAt() 完全一致，且都是闭合周期路径。"""
    ts = np.linspace(0.0, 1.0, n, endpoint=False)
    ...
    pts[:, 1] = -BOARD_ZP            # 全部落在画板平面内
    return pts


def solve_ik_path(plant, frame, pts, seed=None):
    q = Q_SEED.copy()                # 与网页同一个初值
    for k, p in enumerate(pts):
        ik = InverseKinematics(plant)
        qv, prog = ik.q(), ik.prog()
        ik.AddPositionConstraint(frame, [0, 0, 0], plant.world_frame(),
                                 p - 2e-4, p + 2e-4)
        prog.AddQuadraticErrorCost(1e-3 * (W.T @ W), q, qv)   # 贴着上一个解
        prog.AddBoundingBoxConstraint(q[3], q[3], qv[3:4])    # J4 锁 0
        prog.AddBoundingBoxConstraint(q[5], q[5], qv[5:6])    # J6 锁 0
        prog.SetInitialGuess(qv, q)
        res = Solve(prog)
        if res.is_success():
            q = np.array(res.GetSolution())
        Q[k] = q
    return Q

把运动轨迹画进 3D 场景

# 开跑前：画板里放上参考轨迹（蓝线）与当前目标点（红球）
meshcat.SetLine("/board/ref", np.asfortranarray(ref_pts.T), line_width=3.0,
                rgba=Rgba(0.42, 0.55, 1.0, 0.85))
meshcat.SetObject("/board/target", Sphere(0.005), rgba=Rgba(1.0, 0.36, 0.45, 1.0))

# 主循环里：每 0.02 s 采一次 tool_link 的世界坐标，连成黄线
if t >= next_trail:
    next_trail += args.publish_period
    trail.append(plant.EvalBodyPoseInWorld(ctx, tool).translation())
    meshcat.SetLine("/trail", np.asfortranarray(np.array(trail).T),
                    line_width=3.0, rgba=Rgba(1.0, 0.84, 0.3, 0.95))
    meshcat.SetTransform("/board/target", RigidTransform(ref_now))

实测结果（macOS · Python 3.12 · Drake 1.34 · 8 s）

末端范围与画板设计逐位吻合（y 恒为 −0.174 m，说明末端始终贴着板面），IK 几何误差 ≤ 0.35 mm。三种轨迹的扭矩饱和都是 0%，这与第 3 步里关节空间摆动时 J2 会到 2~4% 形成鲜明对照 —— 原因很直接：沿画板走时末端速度只有约 0.07 m/s，比关节空间摆动温和得多。换句话说，同样一台臂，「画图」这种任务对电机是最友好的。

❓ 常见问题

🧭 与站内其他内容的关系

• AR4 真实 3D 轨迹实验室（运动学）：浏览器里看结构、玩 IK、导出 q_traj.npy —— 本分册的上游。
• MIT Drake 真实模拟 SmallRobotArm（项目 02 分册）：同样的方法论也能用在这台臂上，可对照着读。
• 做自己的机器人 · 项目库：AR4 的官方资料、URDF 描述文件与真机 BOM 入口都在这里。

💡 提示与避坑

• URDF 里的关节轴方向与符号请照抄官方，别自己「纠正」—— 改了会出现仿真能动、真机反向。
• 缺少 world 固定关节会让机械臂整体自由落体；惯量张量不满足三角不等式（ixx + iyy ≥ izz）Drake 会直接拒绝加载。
• 先跑示意模型 ar4_arm.urdf 把流程走通，再换 ar4_real.urdf 看外观 —— 网格只影响视觉，不影响控制结论。
• 关节阻尼（dynamics damping）别设太大，否则会掩盖控制器的真实表现。
• 本页模型为站内原创教学模型：关节原点 / 轴向 / 限位取自官方公开描述文件，质量与惯量为同量级估算，用于演示动力学与验证控制逻辑；真机标定请按实物替换参数。