机械臂模拟器：运动学与 ROS 规划

控件

模拟器提供关节滑块、可拖动的工具目标、坐标系叠加和连杆尺寸控件，并且底盘居中摆放，让完整的 360° 可达工作空间都能看见。正解模式根据你设定的关节角计算工具位姿。逆解模式跟踪一个由你拖动的目标——实时求解关节角，并把不可达或存在歧义的目标报告出来，而不是悄悄吸附到某个结果上。第三种抓取放置模式让你先摆好取货点和目的地，然后让机械臂跑完整个任务：移到物体、抓取、搬运、放到投放区——这正是每一个真实机械臂都被造来做的那件标准工作。

原理

每个关节都在坐标系之间贡献一个变换。把这些变换从底盘一路乘到工具，就得到正解位姿。逆运动学则是反向推算，可能无解、有一个解，或者有多个解。

在奇异点附近，工具的一点点移动可能要求关节做出极大的动作。规划器还必须满足位置、速度、加速度、扭矩和碰撞方面的限制。仅仅在几何上够得到，并不意味着一条轨迹就是安全的。

算法

1. 定义连杆长度、关节轴、初始变换和关节限位。
2. 针对当前关节状态计算机器人正解位姿。
3. 把当前工具位姿与请求的目标作比较。
4. 用解析或数值求解器在限位内减小位姿误差。
5. 剔除会碰撞的或奇异的候选解，并选择连续的那个解。
6. 对路径做时间参数化，并把同步的设定点发给关节控制器。

源代码

平面二连杆臂的正向模型很紧凑：

from math import cos, sin

def forward_kinematics(theta_1, theta_2, link_1, link_2):
    elbow = (link_1 * cos(theta_1), link_1 * sin(theta_1))
    tool = (
        elbow[0] + link_2 * cos(theta_1 + theta_2),
        elbow[1] + link_2 * sin(theta_1 + theta_2),
    )
    return elbow, tool

对实物机械臂来说，在给执行器上电之前，要在一个有边界、考虑了碰撞的环境里验证每一条规划出来的轨迹。

假设与硬件差异

这条臂是平面的，有两个或三个转动关节，连杆完全刚性，速度响应瞬时，没有重力、动力学和碰撞——你可以从肘上摆到肘下而没有任何扭矩限制。真实的舵机有位置和速度限制、减速箱背隙、受电流限制的扭矩以及随负载变化的下垂，而六轴机械臂还必须处理朝向、奇异点以及关节限位附近的工作。这个实验台就是让你在加入那些让这些位姿难以保持的动力学之前，先弄明白为什么环带内的目标可能有两个解、为什么边缘上的目标是奇异的。

电路图

把大电流的执行器电源与计算电源分开，同时保持所选硬件所需的参考信号和安全信号。

引导实验

拖动目标是建立直觉最快的方式，对那些在纸面上证明起来枯燥、 在硬件上发现起来又昂贵的性质尤其如此。

1. 找出工作空间的形状

把目标慢慢向外拖，直到求解器报告不可达，然后沿着这条边界完整地 描一圈。现在再把目标朝底盘方向往里拖。

连杆长度不等时，你会找到两条边界：一条是 L1 + L2 处的外圆， 另一条是 |L1 − L2| 处的内圆。工作空间是一个环带而不是圆盘， 中间有一个机械臂在物理上够不到的洞。

最后一行是一个真正有用的设计事实：等长连杆能彻底消除这个死区。 如果你的机械臂需要在靠近自身底盘的地方工作，这就是选择等长连杆的理由。

2. 观察两个解如何合并

在工作空间中部附近的某个目标上，在肘上分支和肘下分支之间来回切换： 那是两个明显不同的机械臂位姿，却到达同一个点。

现在把这个目标朝边界方向往外拖。两个位姿逐渐收敛，在完全伸展时 它们变成同一个位姿。那就是一个奇异点，也是机械臂失去沿某个方向 移动工具能力的地方——无论关节怎么动都做不到。

3. 体会奇异点的代价

让目标沿着一条经过接近完全伸展处的直线慢慢、稳定地移动。 看关节角度，而不要看机械臂本身。

工具以恒定速率移动，关节却不。靠近边界时，工具的一点移动就要求 关节做出巨大的动作，于是一条平滑的笛卡尔路径变成了剧烈的关节运动。 在硬件上，这就是一个舵机猛地撞到自己的限位。

把规划路径保持在最大臂展的约 95% 以内，这个问题就不再出现。

4. 跨过分支边界，看那次翻转

把求解器设成永远选择肘上分支，然后拖着目标穿过一个该分支会撞上 关节限位、从而迫使求解器切换的区域。机械臂会翻过一次幅度大、 速度快、完全没有计划的运动。

这就是为什么正确的默认选择是最小运动量——挑选离当前位姿最近的 那个分支——而不是固定某个分支。在真实硬件上，一次悄无声息的中途分支 切换既吓人又可能造成破坏。

你应该观察到什么

硬件落地

几何是可以精确迁移的。其他一切都不行，而这道差距正是第一台机械臂 的建造所要花时间的地方：

有两个数字能说明问题。190 mm 臂展上肩关节 4 度的间隙，换算到夹爪端超过 13 mm——比数学引入的任何误差都大。而在 95 mm 的连杆上把长度量成 105 mm，就是一个 10% 的系统性误差，任何整定都去不掉。

实际结论是：硬件精度来自标定和一致的接近方向，而不是更好的运动学。 永远从同一个方向接近目标，背隙就变成一个可以减掉的、可重复的偏移。 把每种失败情况都处理好的完整求解器，见二连杆臂的逆运动学。