《机器人软件与具身智能：原理、架构与工程实践》 · 第一部分：机器人基础知识 · 第 03 篇

上一篇解决“目标相对于谁、在哪里”，本篇继续回答“关节应该怎样动”。从两段连杆的手算例子出发，理解正逆运动学、雅可比与奇异性。全文的数值模型只依赖 Python 和 NumPy，长度与限速均为教学设定。

假设视觉系统已经找到一个零件，软件也把目标点准确转换到了机械臂基座坐标系。接下来的指令是：“让工具尖端到达这里。”

问题仍没有结束。同一个位置，手臂可能向两个方向弯曲；位置到了，工具朝向却不对；目标虽然在可达范围内，靠近它时关节速度却突然变得很大。

这些现象有一条共同的线索：我们用末端的位置和朝向描述任务，机器人却通过关节运动实现任务。两者之间的映射既不是一一对应的，也不是在所有姿态下都同样好用。

图 1：从关节构型到末端目标，是机器人运动学要建立的联系。八界主体依据本次 URDF 姿态与实拍外观生成；周围的关节链、路径和运动扇面为技术概念表达，不代表八界实测工作空间或已验证轨迹，非实机拍摄。

1. 先分清：我们能改变什么，又要求末端做到什么

1.1 自由度看独立变量，不能只数关节外壳

把机器人当前的关节状态收集起来，得到一个向量 q，称为构型。对一条没有耦合约束的串联转动机械臂，可以用各关节角作为构型坐标；如果链上还有升降机构，q 中也可能包含一个以米计的位移。[1]

关键词是“独立”。例如，夹爪的两根手指按固定关系同步开合，外观上有两个运动部件，却不意味着有两个独立控制量。固定连接、机械耦合和闭链约束，都可能让“看见多少个关节”与“需要多少个独立变量”不同。

任务的表达方式是另一件事。把工具尖端移动到平面上一点，只需要指定 x、y；如果还要求工具保持某个平面朝向，就要再指定角度 φ。前者有两个任务变量，后者有三个。[2]

关节自由度足够，是实现一般任务的必要条件之一；它并不保证任意目标都可达，也不保证所有姿态都能朝任意方向运动。

1.2 本篇只用两个关节，把关系看清楚

建立一条平面二连杆机械臂，也叫 2R 机械臂，R 表示转动关节：

• 第一段长度 L1 = 0.40 m，第二段长度 L2 = 0.30 m。
• 基座固定在原点，x 轴向右、y 轴向上，逆时针为角度正方向。
• q1 是第一段相对基座 x 轴的角度；q2 是第二段相对第一段的角度。
• 工具点取第二段连杆的末端，没有额外工具偏移；工具朝向沿第二段连杆。

这些长度是为了便于手算选的，不是八界的机械臂参数。本篇也不计算电机力矩、负载与结构变形，那些属于动力学及实际执行问题。

2. 正运动学：把每一段位移放到同一个坐标系相加

正运动学，简称 FK，回答的是：已知关节构型 q，末端在哪里、朝向哪里？[3]

第一段的末端，也就是肘部位置，很容易写出来：

肘部 E = (L1 cos q1, L1 sin q1)

第二段相对第一段又转了 q2，所以它相对基座的方向是 q1 + q2。将两段位移都表达在基座坐标系中，再相加：

x = L1 cos q1 + L2 cos(q1 + q2)
y = L1 sin q1 + L2 sin(q1 + q2)
φ = q1 + q2

这三个式子里最容易错的地方，是把第二段写成 L2 cos q2、L2 sin q2。这样相当于把相对角误认成了绝对角，第一关节转动时，第二段就没有跟着它一起转。

图 2：橙色 q2 从第一段的延长线量起，因此第二段指向正上方。图中长度、角度和末端坐标由同一份示例代码计算。

取 q1 = 30°、q2 = 60°，代入可得：

x = 0.4 cos 30° + 0.3 cos 90° = 0.346410 m
y = 0.4 sin 30° + 0.3 sin 90° = 0.500000 m
φ = 90°

同一套机构、同一个基座参考系、同一组关节值，对应确定的末端位姿。这使 FK 成为很实用的核对工具：控制器说自己到了某组关节角，可以先把这些角度带回模型，检查末端应当出现在哪里。

2.1 与上一篇的齐次变换怎样接起来

如果沿用上一篇的列向量约定，把旋转与平移写成齐次变换，这个例子可以表达为：

T_base_tip = Rz(q1) × Tx(L1) × Rz(q2) × Tx(L2)

Rz 表示平面内的旋转，Tx 表示沿当前局部 x 轴的平移。右侧变换链把末端坐标表达转换到基座系；不能把乘法次序随意调换。

真实机构可能存在关节安装偏置、不同的旋转轴和工具偏移，因此不能把全部机械臂都压成上面这条简单链。但思想完全相同：沿着基座到工具的机构链，依次合成固定安装变换与关节运动变换。

3. 逆运动学：同一个目标，为什么可能有两种弯法

逆运动学，简称 IK，反过来问：给定期望的末端位置或位姿，哪些关节构型能够实现它？逆问题可能无解、有一个解，也可能有多个解。[3]

3.1 先从目标距离，算出肘部应该弯多少

暂时只要求末端到达 (x, y)，不限制朝向。将刚才的 x、y 表达式平方后相加：

x² + y² = L1² + L2² + 2 L1 L2 cos q2

c2 = cos q2
   = (x² + y² − L1² − L2²) / (2 L1 L2)

这里出现了第一道可达性判断：实数角度的余弦只能落在 [-1, 1] 内。如果 c2 超过这个范围，就没有对应的肘部角度。

进一步求解：

q2 = atan2( ±√(1 − c2²), c2 )
q1 = atan2(y, x)
     − atan2(L2 sin q2, L1 + L2 cos q2)

第一行中的正负号，产生两种弯曲分支。第二行可以直观地理解：先看“基座指向目标”的方向，再减去“第一段与基座到目标连线之间”的夹角。atan2 同时使用两个分量，能够区分象限；直接使用普通反正切 atan(y/x) 容易丢失方向信息。

继续使用上节的同一个目标 (0.346410…, 0.500000) m，得到：

解 A：q1 = 30.00°，q2 =  60.00°
解 B：q1 = 80.57°，q2 = −60.00°

图 3：两条机构链都到达同一个橙色目标点，肘部位置不同。短箭头表示第二段连杆的方向，提醒我们位置相同并不意味着位姿相同。

3.2 再加一个朝向要求，答案就可能消失

解 A 的末端朝向为 30° + 60° = 90°，解 B 为 80.57° − 60° = 20.57°。

如果任务要求“到达这个位置，并且工具保持水平向右，即 φ = 0°”，两组位置解都不满足。它们已经覆盖了这个目标在理想 2R 模型中的两种非退化位置分支，不能继续挑出第三种角度来满足这个额外要求。

这就是任务变量与机构自由度之间的具体矛盾：两个独立关节角，通常不能自由指定三个独立的 x、y、φ。某些位置与朝向组合恰好兼容，不意味着所有组合都兼容。

工程上常见的“位置误差已经很小，IK 还是失败”，就可能来自这里。接口求的是完整位姿，而调用者只检查了位置；求解器并没有忽略那个不满足的朝向条件。

4. 理想工作空间，只是可用解筛选的第一层

4.1 为什么可达区域是圆环，而不是圆盘

两段完全伸直，末端距离基座最远，为 L1 + L2 = 0.70 m；第二段折回时，最近距离为 |L1 − L2| = 0.10 m。

因此，在两个关节可自由转动、没有障碍、只要求位置的条件下：

0.10 m ≤ √(x² + y²) ≤ 0.70 m

中间的空洞来自两段长度不同：长的一段无法被短的一段完全抵消。0.80 m 处太远，0.05 m 处反而太近。[2][3]

圆环内部通常有两个位置分支；在内外边界，两分支合并为一个，角度按相差整圈等价处理。这个结论属于本篇长度不同的理想 2R 模型，不能直接套到有关节限位的实际机械臂上。

4.2 限位与碰撞，会删除不同的解

给刚才的例子加一个教学关节限位：第一关节只能在 [-90°, 60°] 之间转动。解 A 的 30° 合法，解 B 的 80.57° 超限。

再单独考虑另一种情况：不加上述限位，在平面中放一个圆形障碍，中心为 (0.200, 0.115) m，半径 0.045 m。这次解 A 的第一段会穿过障碍，而解 B 可以避开它。

如果同时施加这个限位和这个障碍，两组解就都被排除了。“位置在工作空间里”与“存在满足全部约束的构型”，回答的不是同一个问题。

图 4：下半图只检查零厚度连杆与教学圆形障碍的相交关系，用来说明解的筛选。真实机器人还要考虑壳体、工具、自碰撞及整条运动路径，不能把本图当作八界碰撞验证。

4.3 数值 IK 为什么还需要一个初始值

2R 模型结构简单，可以直接写出解析公式。更复杂的机构常用数值方法：从一个初始构型开始，计算当前末端误差，估计该如何调整关节，反复迭代。[4]

因此，初始值 seed 会影响求解过程和最后找到的分支。如果机器人当前接近解 B，用当前关节状态作为初值，通常比从一个相距很远的构型重新开始更合适；但这不保证一定收敛，也不保证找到所有解或全局最优解。

排查 IK 失败时，应该区分：目标本身不满足机构约束，还是当前初值、迭代次数、误差容限等条件下没有找到解。一次数值求解失败，不等于数学上已经证明目标不可达。

5. 雅可比：关节动一点，末端会往哪里动

FK 告诉我们一组关节角对应的位置。要让工具沿某个方向移动，还需要知道：当前姿态下，关节角的微小变化会怎样改变末端位置。

对 FK 中的 x、y 分别对两个关节角求偏导，得到位置雅可比 J(q)：

    [ −L1 sin q1 − L2 sin(q1+q2)    −L2 sin(q1+q2) ]
J = [                                               ]
    [  L1 cos q1 + L2 cos(q1+q2)     L2 cos(q1+q2) ]

Δp ≈ J(q) Δq
v  = J(q) q_dot

Δp 是一小段末端位移，Δq 是一小段关节角变化；v 是末端线速度，q_dot 是关节角速度。代码中角度必须使用弧度，这里 J 的单位是米/弧度。

这个矩阵的列有很直观的含义：第一列表示只有关节 1 以单位角速度运动时，末端产生什么速度；第二列对应关节 2。两个关节同时运动时，把各自贡献相加即可。[5]

为什么会出现正弦、余弦和连杆长度？转动关节让末端绕关节轴做瞬时圆周运动，线速度沿圆的切线，大小取决于角速度和到轴的距离。三维转动关节在位置雅可比中的一列也可以写成 轴方向 ×（末端位置 − 关节轴上一点），前提是这些向量表达在同一个参考系中。

在 q = (30°, 60°) 处，J 为：

J = [ −0.500000   −0.300000 ]  m/rad
    [  0.346410    0.000000 ]

如果关节速度是 (0.2, −0.1) rad/s：

关节 1 的贡献 = (−0.100000, 0.069282) m/s
关节 2 的贡献 = ( 0.030000, 0.000000) m/s
合成末端速度 = (−0.070000, 0.069282) m/s

图 5：绿色虚线把关节 2 的贡献平移到蓝色箭头末端，橙色箭头就是合速度。这里的坐标轴是速度，不是机器人的位置。

这里还有一个边界：v = J(q) q_dot 描述当前状态的瞬时关系，Δp ≈ J(q) Δq 则是小位移近似。关节转过很大角度后，J 本身也变了，不能拿起点处的 J 一次性预测整段运动。

这也解释了数值 IK 的基本思路：把当前位置误差暂时看成一小段期望位移，用 J 求一小步关节调整，再回到 FK 检查新的误差。线性化、更新、重新检查，缺少其中任何一步都容易产生误解。[4]

6. 奇异性：关节还在，但某些方向的运动能力退化了

6.1 伸直时，为什么瞬间不能向内缩

考虑 q1 = 0°、q2 = 0°，两段沿 x 轴完全伸直。此时：

J = [ 0.0   0.0 ]
    [ 0.7   0.3 ]

v_x = 0
v_y = 0.7 q1_dot + 0.3 q2_dot

两个关节的瞬时运动都只能给末端贡献竖直速度。无论怎样组合有限的关节速度，都不能在这一瞬间产生非零水平速度。

这不是说手臂永远缩不回来。它可以先弯曲，经过有限运动进入更靠近基座的位置；只是从完全伸直的状态出发，水平收缩在一阶速度关系中没有出现。用小角度展开也能看到：cos δ ≈ 1 − δ²/2，伸直附近的水平长度变化首先体现在二阶项中。

对本篇只控制 x、y 的任务：

det J = L1 L2 sin q2

当 q2 = 0 或 π，行列式为零，J 从通常的秩 2 降为秩 1。一般来说，奇异位形指相应雅可比的秩低于它能达到的最大秩；并不是看到非方阵就能判断奇异。[6]

需要同时记住任务定义：这里讨论的是二维位置雅可比。换成包含末端朝向的任务，就要换用相应的雅可比，不能照搬这个 2×2 矩阵的判据。

6.2 接近奇异时，没有“除零”，速度也可能已经过大

将第二关节稍微弯到 3°，此时 J 已经可逆。假设希望工具以 0.05 m/s 沿负 x 方向运动，y 方向保持不动：

q = (0°, 3°)
期望 v = (−0.05, 0) m/s

解 J q_dot = v，得到：
q1_dot = −2.3851 rad/s
q2_dot =  5.5697 rad/s

末端只要求每秒移动 5 厘米，第二关节却要接近每秒转 319°。原因不是目标“太远”，而是当前位置的运动方向很不利：两个关节要用较大的运动相互抵消竖直分量，只留下很小的水平分量。

如果教学设定规定每个关节最多 1 rad/s，这组速度要求已经不可能按原样满足。位置可达、J 可逆和速度可执行，是三个不同层次。

图 6：上图把关节速度圆映射为末端速度椭圆，角度为 3° 时已有明显退化趋势。[7] 上图使用二范数速度预算，下面的红虚线则表示每个关节单独限速，两者是不同约束。下图全部曲线由同一 2R 模型计算；阻尼解的速度较小，但不再精确实现原目标。

7. 阻尼为什么有效，它又牺牲了什么

直接求解 J q_dot = v，相当于坚持“必须完全实现期望速度”。接近奇异时，为了满足这个要求，关节速度可能非常大。

阻尼最小二乘法改变了问题：允许一定末端速度误差，同时惩罚过大的关节速度。用二范数表达这个折中：

最小化：‖J q_dot − v‖² + λ² ‖q_dot‖²

对关节速度求导并令其为零，得到：

(JᵀJ + λ²I) q_dot = Jᵀv

等价写法：q_dot = Jᵀ (JJᵀ + λ²I)⁻¹ v

λ > 0 是阻尼参数；本篇 J 只含位置对转动关节的导数，所以 λ 使用与 J 相应的 m/rad 尺度。实现时用线性方程求解器 solve，不需要显式计算矩阵的逆。Pinocchio 的官方逆运动学示例中也能看到类似的阻尼求解形式，但其完整空间误差与本篇二维例子并不相同。[8]

继续取刚才的 (0°, 3°) 和期望速度，设置 λ = 0.03 m/rad：

阻尼后的关节速度 ≈ (−0.1662, 0.3922) rad/s
对应的末端速度   ≈ (−0.00355, 0.00126) m/s
原期望末端速度    = (−0.05000, 0.00000) m/s

关节速度确实降下来了。但负 x 方向只实现了约 0.00355 m/s，远低于原来要求的 0.05 m/s，还出现了一小段 y 方向偏差。这个例子刻意把代价展示出来：阻尼抑制了对过大关节速度的需求，同时接受了跟踪误差，并没有让缺失的运动能力重新出现。

从奇异值看也很直观：设 J 为 U Σ Vᵀ，普通逆解在某个方向上按 1/σ 放大；阻尼解把这个系数改成 σ/(σ² + λ²)。当 σ 非常小时，后者不会无限增大，而会压低该方向的响应。

所以不能把阻尼当作万能修复：

• 它不是硬限速器；本例碰巧低于 1 rad/s，不代表任意目标都满足这个上限。
• 它不会自动处理障碍、自碰撞和关节限位。
• λ 太大可能造成明显偏差，太小仍可能放大接近奇异方向的速度需求。
• 如果任务误差混合米与弧度，或者关节变量混合转角与升降位移，需要先考虑权重与尺度，不能直接照抄这个 λ。

有些任务需要降低速度，有些可以换一个接近方向，还有些应该调整底盘、升降或手臂构型。选择哪种方式，取决于任务真正允许改变什么。

8. 用一段 NumPy 代码，把“算得出”变成“能复核”

下面两段 Python 代码按顺序保存到同一个文件即可运行。第一段实现 FK 与位置雅可比，第二段复算正常速度和接近奇异时的两种解。

import numpy as np

L1, L2 = 0.40, 0.30  # m，教学模型

def fk(q):
    q1, q2 = q  # rad
    return np.array([
        L1*np.cos(q1) + L2*np.cos(q1+q2),
        L1*np.sin(q1) + L2*np.sin(q1+q2),
    ])

def jacobian(q):
    q1, q2 = q
    return np.array([
        [-L1*np.sin(q1)-L2*np.sin(q1+q2),
         -L2*np.sin(q1+q2)],
        [ L1*np.cos(q1)+L2*np.cos(q1+q2),
          L2*np.cos(q1+q2)],
    ])

q = np.deg2rad([30.0, 60.0])
print("末端位置：", np.round(fk(q), 6))
print("末端速度：", np.round(jacobian(q) @ [0.2, -0.1], 6))

q_near = np.deg2rad([0.0, 3.0])
J = jacobian(q_near)
v = np.array([-0.05, 0.0])
lam = 0.03  # m/rad，只用于当前二维例子

qdot_exact = np.linalg.solve(J, v)
qdot_damped = J.T @ np.linalg.solve(J @ J.T + lam**2*np.eye(2), v)

print("精确速度解：", np.round(qdot_exact, 4))
print("阻尼速度解：", np.round(qdot_damped, 4))
print("阻尼后末端速度：", np.round(J @ qdot_damped, 5))
assert np.max(np.abs(qdot_exact)) > 1.0
assert not np.allclose(J @ qdot_damped, v)

对应输出为：

末端位置： [0.34641 0.5    ]
末端速度： [-0.07      0.069282]
精确速度解： [-2.3851  5.5697]
阻尼速度解： [-0.1662  0.3922]
阻尼后末端速度： [-0.00355  0.00126]

配套完整脚本 examples/03_kinematics.py 还包含解析 IK、两分支的 FK 回代、工作空间边界、圆形障碍筛选，以及用微小角度扰动检查雅可比的计算。

在本文运行环境中，对 132 组构型做检查，IK 回代的最大位置误差小于 3×10⁻¹⁶ m；解析雅可比与中心差分的最大元素差小于 2×10⁻¹⁰ m/rad。这些是数学实现的一致性检查，不是机器人实机定位精度；其他环境的浮点末位可能略有差异。

实现 IK 时还有一个需要保留的细节：浮点运算可能让本应等于 1 的余弦值变成 1 + 10⁻¹⁶，这时可以在很小的容差内修正边界。但不能把任意超出 [-1, 1] 的值都无条件截断，否则 0.80 m 这种真实不可达目标也会被伪装成边界上的“成功解”。

9. 放回八界：先选对机构链，再谈求解

平面 2R 模型帮助理解机制，实际八界需要从自己的 URDF/Xacro 读取机构关系。本文核对的本地模型中，机械臂包含 joint1 至 joint6；升降机构在机械臂上游；头部俯仰属于另一条分支；夹爪还存在主关节与 mimic 关系。

这意味着建立运动学任务时，至少要明确三件事。

第一，求哪个工具点，相对于哪个基座。 法兰原点、夹爪中心和真正接触物体的工具点可能不重合。上一篇的变换链负责统一表达，本篇的 FK/IK 则要沿选定的基座—工具链计算。选错工具点，即使求解器内部完全正确，也会到达错误的位置。

第二，哪些关节允许参与本次求解。 底盘固定时，只求机械臂是一种问题；允许升降参与，就有另一组变量和可达范围。头部转动不会改变机械臂末端在机身中的位置，因此不能把头部俯仰当成额外的手臂自由度。八界头部也没有独立的水平转头关节，水平朝向应与机身一致。

第三，使用真正的关节变换，而不是凭外观量一个长度。 关节 origin、轴方向、固定安装偏置、限位、mimic 关系都应进入模型。网格包围盒包含外壳，其长度不是关节中心之间的距离，不能拿它直接替换 FK 中的连杆参数。

以软件实现的组织方式为例，MoveIt 将机构关系及关节限制放入 RobotModel，把具体时刻的关节状态放入 RobotState；FK、IK 和雅可比计算都依赖这两类信息。[9] 这有助于区分“模型是什么”与“机器人现在是什么状态”，但不代表本篇示例已经调用了八界 SDK 或完成实机控制。

欢迎关注科沃斯八界 AtomGit，入口见文末参考链接 [10]。相关代码将逐步释放，实际开放内容与使用说明以仓库为准；本文的 NumPy 脚本是独立教学示例，不代表对应八界实现已经公开。

10. 得到一组关节角之后，还没有结束

IK 返回一个满足目标的构型，只确定了终点。机器人从当前构型到这个终点的途中，仍然可能碰到障碍、经过不利的运动方向，或超出速度与加速度限制。

把本篇的检查串起来，可以得到一个清晰的排查顺序：

1. 目标定义正确吗？ 检查参考系、单位、工具点，以及任务究竟要求位置还是完整位姿。
2. 构型满足要求吗？ 将候选关节值代回 FK，分别检查位置与朝向误差，再检查限位和碰撞。
3. 当前方向容易运动吗？ 查看相应雅可比的秩、奇异值，以及目标速度需要多大的关节速度。
4. 整段运动可执行吗？ 检查路径、时间安排与执行约束，而不只检查起终点。

前两篇建立了软件模块之间的联系与空间数据的含义。本篇再向下走了一层：目标经过坐标变换进入机器人自己的语言之后，还需要通过机构关系转换成关节构型，通过局部运动关系转换成速度，并在真实约束下形成可执行的运动。

---

参考链接

正文中的编号对应以下资料。通用概念参考一手教材与项目文档，数值演算及图 2—6 由本文教学模型独立计算。

• [1] Modern Robotics：2.2 Degrees of Freedom of a Robot：机构自由度与运动约束。
• [2] Modern Robotics：2.5 Task Space and Workspace：任务空间、工作空间及朝向要求。
• [3] Modern Robotics：Inverse Kinematics of Open Chains：正逆运动学关系、解析方法与平面 2R 逆解。
• [4] Modern Robotics：6.2 Numerical Inverse Kinematics：初始值、线性化、雅可比与迭代求解。
• [5] Modern Robotics：5.1.1 Space Jacobian：雅可比各列的运动学含义；正文采用的是二维位置雅可比，不等同于完整空间旋量雅可比。
• [6] Modern Robotics：5.3 Singularities：秩下降与瞬时运动能力。
• [7] Modern Robotics：5.4 Manipulability：关节速度球与末端速度椭球。
• [8] Pinocchio 官方示例：inverse-kinematics.py：带阻尼的数值求解实例。该文件中的 damp 直接加在 JJᵀ 上，对应本文公式的 λ²，不要把参数数值直接照搬。
• [9] MoveIt 官方文档：Robot Model and Robot State：机构模型、关节状态与运动学计算。
• [10] 科沃斯八界 AtomGit：相关代码将逐步释放，实际开放内容与使用说明以仓库为准。

本地配套材料：完整验算脚本、上一期：机器人如何描述空间。