当前位置：机器人运动学
Chapter 2 · Robot Kinematics

机器人运动学：考试最容易出计算题的核心章

运动学题通常给关节变量、连杆长度、DH 参数或目标末端位置，要求算末端位姿、逆解、Jacobian、末端速度或关节力矩映射。这里按课件 Chapter 2 的题型来。

自由度齐次变换2R/PR 机构Jacobian
l1 l2 q1 q2 末端
2R 平面机械臂是正运动学、逆运动学、Jacobian 的标准题型。

1. 自由度与 Gruebler 公式

自由度题的本质是数“独立运动变量”。平面机构常用课件里的 Gruebler 公式：

$$F=3(n-1)-2l-h$$

其中 \(n\) 是构件总数，\(l\) 是低副数量，\(h\) 是高副数量。

这个公式可以这样理解：一个没有约束的平面刚体有 3 个自由度，分别是 \(x\) 平移、\(y\) 平移和绕平面外轴的转动。机构里通常把机架固定，所以只有 \(n-1\) 个活动构件，每个先按 3 个自由度计算。转动副和移动副这类低副会限制掉 2 个相对运动，只留下 1 个允许运动；高副一般限制掉 1 个运动。所以公式不是死记的符号，而是在“总自由度”里减去各运动副带来的约束数。

例 2.1 平面四杆机构自由度

仿 Chapter2

场景交代：画四根刚性构件围成闭环，其中一根为固定机架，其余三根为活动杆。四个连接点都是转动副，每个转动副只允许相邻构件相对转动，不允许相对平移。

题目：一个平面四杆机构含机架在内共有 \(n=4\) 个构件，4 个转动副。求自由度。

n = 4, l = 4, h = 0
平面四杆机构示意：机架也算构件，四个转动副提供约束。
图中元素：底部深色杆是固定机架，也要计入构件数；其余三根彩色杆是活动构件，四个白色圆圈是转动副。
建模顺序：
  1. 先画固定基座、活动关节和连杆，标清每个角度是相对哪个坐标轴或哪根杆量的。
  2. 写出几何关系或矩阵关系，再代入长度、角度、目标点或外力。
  3. 若是 Jacobian 题，速度用 \(J\dot q\)，末端力映射用 \(J^TF\)。
  4. 最后说明多解、无解、奇异或力矩正负号代表什么。

解析过程

先数构件，四杆机构包含机架，所以 \(n=4\)，真正活动的构件是 \(n-1=3\) 个。每个活动平面构件先按 3 个自由度看，因此未加约束前共有 \(3(4-1)=9\) 个自由度。

四个转动副都是低副，每个低副限制 2 个自由度，所以总共减去 \(2\times4=8\)。没有高副，所以 \(h=0\)。

$$F=3(n-1)-2l-h=3(4-1)-2(4)-0=1$$

所以四杆机构只有 1 个独立输入角。只要你指定一个杆的角度，其他杆的位置会被闭链几何约束决定，这就是自由度为 1 的物理意义。

易错点：机架也算构件；转动副和移动副都属于低副。

2. 齐次变换：把旋转和平移合在一起

考试中看到 \(T\) 矩阵，要立即读出它描述“一个坐标系相对另一个坐标系的位置和姿态”。

$$T=\begin{bmatrix}R&p\\0&1\end{bmatrix},\qquad p=\begin{bmatrix}x\\y\\z\end{bmatrix}$$
$$R_z(\theta)=\begin{bmatrix}\cos\theta&-\sin\theta&0\\\sin\theta&\cos\theta&0\\0&0&1\end{bmatrix}$$

齐次变换把两件事放进同一个矩阵：左上角的 \(R\) 负责描述坐标轴方向怎么旋转，右上角的 \(p\) 负责描述原点移动到哪里。最后一行 \([0,0,0,1]\) 看起来像是凑出来的，但它让旋转和平移都能用一次矩阵乘法完成，这样多个连杆的位姿就可以直接相乘：\(T_0^2=T_0^1T_1^2\)。在机器人正运动学里，每个关节/连杆都贡献一个小变换，末端位姿就是这些小变换按顺序连乘。

例 2.2 旋转后平移的齐次变换

矩阵计算

场景交代：画基坐标系 \(A\) 固定在平面上，再画坐标系 \(B\)：\(B\) 的坐标轴相对 \(A\) 绕 \(z\) 轴逆时针转过 \(30^\circ\)，\(B\) 的原点位于 \(A\) 坐标中的 \((1,2,0)\)。

题目：坐标系先绕 \(z\) 轴旋转 \(30^\circ\)，再相对基坐标平移 \(p=[1,2,0]^T\)。写出 \(T\)。

base frame p R
齐次变换示意：旋转矩阵决定坐标轴方向，平移向量决定原点位置。
图中元素：左下坐标架是基坐标系，右上旋转后的坐标架是局部坐标系；绿色虚线 \(p\) 表示局部原点相对基坐标的位置，蓝/橙轴方向由旋转矩阵 \(R\) 决定。
建模顺序：
  1. 先画固定基座、活动关节和连杆，标清每个角度是相对哪个坐标轴或哪根杆量的。
  2. 写出几何关系或矩阵关系，再代入长度、角度、目标点或外力。
  3. 若是 Jacobian 题，速度用 \(J\dot q\)，末端力映射用 \(J^TF\)。
  4. 最后说明多解、无解、奇异或力矩正负号代表什么。

解析过程

题目说“绕 \(z\) 轴旋转”，所以左上角用 \(R_z(30^\circ)\)。这个旋转矩阵的第一列表示新坐标系 \(x\) 轴在旧坐标系里的方向，第二列表示新 \(y\) 轴的方向；因此它不只是三角函数表，而是在记录坐标轴方向。

设 \(c=\cos30^\circ=\sqrt3/2\)，\(s=\sin30^\circ=1/2\)。平移 \(p=[1,2,0]^T\) 直接写在最后一列的前三个位置，表示新坐标系原点相对基坐标的位置。

$$T=\begin{bmatrix} c&-s&0&1\\ s&c&0&2\\ 0&0&1&0\\ 0&0&0&1 \end{bmatrix} =\begin{bmatrix} 0.866&-0.5&0&1\\ 0.5&0.866&0&2\\ 0&0&1&0\\ 0&0&0&1 \end{bmatrix}$$

如果用这个 \(T\) 去乘一个局部坐标点，它会先把点按 \(R\) 旋转到基坐标方向，再加上平移 \(p\)。这就是齐次矩阵在正运动学里能连续相乘的原因。

易错点：注意平移列 \(p\) 写在最后一列，不要和旋转矩阵相加。

例 2.2b 齐次变换求点坐标

基础变式

场景交代：画基坐标系 \(A\) 和局部坐标系 \(B\)。\(B\) 相对 \(A\) 先绕 \(z\) 轴逆时针转 \(90^\circ\)，再把原点放到 \(A\) 中的 \((1,2,0)\)；点固定在 \(B\) 坐标系的 \(x_B\) 轴正方向 1 m 处。

题目：坐标系变换 \(T=\begin{bmatrix}0&-1&0&1\\1&0&0&2\\0&0&1&0\\0&0&0&1\end{bmatrix}\)。局部坐标点 \(p_B=[1,0,0]^T\)，求它在基坐标中的位置。

x_A y_A x_B y_B origin p=(1,2,0) P pB=(1,0,0) pA=(1,3,0) rotate by R, then add translation p
齐次变换求点坐标示意：局部点先按局部坐标轴方向旋转，再加上局部原点相对基坐标的平移。
图中元素：左下角是基坐标系 \(A\)，右上角是局部坐标系 \(B\)；\(B\) 的原点在 \(A\) 中为 \((1,2,0)\)，且 \(x_B\) 轴因 \(90^\circ\) 旋转而竖直向上。点 \(p_B=[1,0,0]^T\) 位于 \(x_B\) 正方向 1 m 处，所以在基坐标中落到 \((1,3,0)\)。
建模顺序：
  1. 先画固定基座、活动关节和连杆，标清每个角度是相对哪个坐标轴或哪根杆量的。
  2. 写出几何关系或矩阵关系，再代入长度、角度、目标点或外力。
  3. 若是 Jacobian 题，速度用 \(J\dot q\)，末端力映射用 \(J^TF\)。
  4. 最后说明多解、无解、奇异或力矩正负号代表什么。

解析过程

齐次矩阵左上角是旋转 \(R\)，最后一列前三个数是平移 \(p\)。把局部点写成齐次坐标 \([1,0,0,1]^T\)，再左乘 \(T\)，就能同时完成旋转和平移。

$$p_A=Rp_B+p=\begin{bmatrix}0&-1&0\\1&0&0\\0&0&1\end{bmatrix}\begin{bmatrix}1\\0\\0\end{bmatrix}+\begin{bmatrix}1\\2\\0\end{bmatrix}=\begin{bmatrix}0\\1\\0\end{bmatrix}+\begin{bmatrix}1\\2\\0\end{bmatrix}=\begin{bmatrix}1\\3\\0\end{bmatrix}$$

物理上看，局部 \(x\) 轴经过 \(90^\circ\) 旋转后指向基坐标的 \(y\) 方向，所以点先变成 \([0,1,0]^T\)，再整体平移到 \([1,3,0]^T\)。

易错点：点坐标变换是 \(Rp+p_0\)，不是只把平移列读出来。

3. 姿态表示：旋转矩阵、欧拉角、固定角

Chapter2-Kinematics-2 里专门讲 Orientation and Rotation、Euler Angles、Fixed Angles。考试概念题常问：为什么姿态不能只用一个角？欧拉角和旋转矩阵是什么关系？

三维空间中的姿态本质上是在描述一个坐标架的三个轴分别指向哪里。一个角只能描述平面内绕固定轴的转动，无法描述三维物体的俯仰、偏航和滚转，所以三维姿态至少需要三个独立参数。旋转矩阵用 9 个数描述三个坐标轴方向，但这些数之间必须满足正交和单位长度约束；欧拉角用三个角度描述连续三次旋转，更直观，但在某些角度会出现万向节锁，也就是不同角的变化变得无法独立区分。

旋转矩阵

用 3 个正交单位轴组成，完整表示三维姿态。

$$R^TR=I,\qquad \det(R)=1$$

欧拉角 / 固定角

用 3 次绕指定轴的旋转表示姿态，例如 roll-pitch-yaw。

$$R=R_z(\psi)R_y(\theta)R_x(\phi)$$
考
欧拉角紧凑直观但可能奇异；旋转矩阵稳定但有 9 个元素和正交约束。

4. DH 参数与正运动学

课件的 planar rotary + prismatic 机构题要求建立 DH 表并求齐次变换。标准 DH 单步变换为：

$$A_i= \begin{bmatrix} \cos\theta_i&-\sin\theta_i\cos\alpha_i&\sin\theta_i\sin\alpha_i&a_i\cos\theta_i\\ \sin\theta_i&\cos\theta_i\cos\alpha_i&-\cos\theta_i\sin\alpha_i&a_i\sin\theta_i\\ 0&\sin\alpha_i&\cos\alpha_i&d_i\\ 0&0&0&1 \end{bmatrix}$$

DH 法的目的不是制造复杂矩阵，而是给每一节连杆一个统一的记账方式。四个参数分别描述相邻坐标系之间“沿哪里转、沿哪里移”：\(\theta_i\) 是绕上一坐标系 \(z\) 轴转多少，\(d_i\) 是沿 \(z\) 轴移多少，\(a_i\) 是沿 \(x\) 轴移多少，\(\alpha_i\) 是绕 \(x\) 轴扭多少。只要每一节都用同一套规则，整条机械臂的正运动学就变成把 \(A_1,A_2,\dots,A_n\) 顺序相乘。

如果把单步 DH 变换拆开看，它通常等价于四个动作的连续组合：先绕 \(z\) 轴转 \(\theta_i\)，再沿 \(z\) 轴平移 \(d_i\)，然后沿 \(x\) 轴平移 \(a_i\)，最后绕 \(x\) 轴转 \(\alpha_i\)。矩阵里的 \(a_i\cos\theta_i\) 和 \(a_i\sin\theta_i\) 就是在说：沿本地 \(x\) 方向走 \(a_i\) 之后，投影到基坐标的 \(x,y\) 分量会随 \(\theta_i\) 改变。这样看，DH 矩阵不是凭空来的，而是“旋转、平移、平移、旋转”四个几何动作压缩成一个矩阵。

考
平面机构很多时候不必完整套 DH，直接用几何法更快；但如果题目明确要求 DH 表，就按 \(\alpha,a,d,\theta\) 填表。

例 2.3 2R 平面机械臂正运动学

必会

场景交代：画平面串联 2R 机械臂，基座固定在原点，第一根杆从基座出发，第二根杆通过转动副接在第一根杆末端。\(q_1\) 从世界 \(x\) 轴量到第一杆，\(q_2\) 从第一杆量到第二杆。

题目：两连杆长度 \(l_1=1\), \(l_2=0.5\)，关节角 \(q_1=30^\circ\), \(q_2=60^\circ\)。求末端位置。

l1=1 l2=0.5 q1=30deg q2=60deg end (x,y)=(0.866,1.0) second link absolute angle: q1+q2=90deg
2R 正运动学计算示意：先由 \(q_1\) 确定第一杆方向，再把 \(q_2\) 加到第一杆方向上得到第二杆的绝对方向。
图中元素：基座在原点，第一杆长度 \(l_1=1\)、角度 \(q_1=30^\circ\)；第二杆长度 \(l_2=0.5\)，相对第一杆转 \(q_2=60^\circ\)，所以世界方向是 \(q_1+q_2=90^\circ\)。末端坐标由两段杆的 \(x,y\) 分量相加得到。
建模顺序：
  1. 先画固定基座、活动关节和连杆，标清每个角度是相对哪个坐标轴或哪根杆量的。
  2. 写出几何关系或矩阵关系，再代入长度、角度、目标点或外力。
  3. 若是 Jacobian 题，速度用 \(J\dot q\)，末端力映射用 \(J^TF\)。
  4. 最后说明多解、无解、奇异或力矩正负号代表什么。

解析过程

先画几何关系。第一根杆相对世界坐标的方向就是 \(q_1\)，所以它对末端 \(x,y\) 的贡献分别是 \(l_1\cos q_1\) 和 \(l_1\sin q_1\)。第二根杆的角度不是单独的 \(q_2\)，而是相对世界坐标的总角度 \(q_1+q_2\)，因为它安装在第一根杆末端。

$$x=l_1\cos q_1+l_2\cos(q_1+q_2),\qquad y=l_1\sin q_1+l_2\sin(q_1+q_2)$$

\(q_1+q_2=90^\circ\)。

$$x=1\cos30^\circ+0.5\cos90^\circ=0.866$$
$$y=1\sin30^\circ+0.5\sin90^\circ=1.0$$

末端位置为 \((0.866,1.000)\,\mathrm m\)。这个点的含义是：从基座出发，第一根杆把末端带到右上方，第二根杆此时竖直向上，所以只增加 \(y\) 坐标，不再增加 \(x\) 坐标。

易错点：第二根杆相对世界坐标的角度是 \(q_1+q_2\)，不是 \(q_2\)。

例 2.4 旋转-伸缩 PR 机构

仿课件

场景交代：画一个平面 PR 机构：第一个关节固定在原点并能绕 \(z\) 轴转动，第二个关节是沿当前杆方向伸缩的移动副。末端点始终位于转动方向射线上，距离原点为伸缩长度 \(L(t)\)。

题目：一个平面机构第 1 关节为转动 \(q_1=\theta\)，第 2 关节为沿第一杆方向伸缩，长度 \(L=1+\sin(2t)\)，且 \(\theta=2t\)。写末端轨迹。

theta L(t)
旋转-伸缩机构示意：角度决定方向，伸缩量决定半径。
图中元素：左下圆圈是固定转动关节，长杆方向由角度 \(\theta\) 决定；蓝色滑块表示沿当前杆方向伸缩的移动副，伸缩长度就是末端到基座的半径。
建模顺序：
  1. 先画固定基座、活动关节和连杆，标清每个角度是相对哪个坐标轴或哪根杆量的。
  2. 写出几何关系或矩阵关系，再代入长度、角度、目标点或外力。
  3. 若是 Jacobian 题，速度用 \(J\dot q\)，末端力映射用 \(J^TF\)。
  4. 最后说明多解、无解、奇异或力矩正负号代表什么。

解析过程

这个 PR 机构可以理解为一个“会转方向的伸缩杆”。转动关节决定末端射线的方向，移动关节决定沿这条射线走多远。所以末端点就是把长度 \(L\) 投影到 \(x\) 和 \(y\) 两个方向。

$$x(t)=L\cos\theta=(1+\sin2t)\cos2t$$
$$y(t)=L\sin\theta=(1+\sin2t)\sin2t$$

这正是课件中“旋转关节 + 伸缩关节”题的几何解法。这里 \(1+\sin2t\) 控制半径大小，\(2t\) 控制方向角，因此轨迹不是简单圆，而是半径随角度周期变化的平面曲线。

易错点：伸缩量是长度，不能当角度；角度是 \(2t\)。

5. 工作空间与逆运动学：目标位置反推关节角

工作空间是末端执行器所有可达位置的集合；灵巧工作空间还要求末端能以任意姿态到达。2R 平面机械臂常先判断目标点是否可达：

$$|l_1-l_2|\le r=\sqrt{x^2+y^2}\le l_1+l_2$$

2R 平面机械臂逆解常用余弦定理。先求 \(q_2\)，再求 \(q_1\)。

$$\cos q_2=\frac{x^2+y^2-l_1^2-l_2^2}{2l_1l_2}$$
$$q_1=\operatorname{atan2}(y,x)-\operatorname{atan2}(l_2\sin q_2,\;l_1+l_2\cos q_2)$$

可达性不等式来自三角形边长关系。两根杆完全伸直时末端最远能到 \(l_1+l_2\)，一长一短反向折叠时最近只能到 \(|l_1-l_2|\)。如果目标点距离基座的半径 \(r\) 不在这个范围内，逆运动学没有实数解。余弦定理求 \(q_2\) 时，其实是在看由 \(l_1,l_2,r\) 构成的三角形；求 \(q_1\) 时，则是先看目标点方向 \(\operatorname{atan2}(y,x)\)，再减去三角形内部造成的偏角。

例 2.5 2R 逆运动学

必会

场景交代：画基座在原点的 2R 平面机械臂，两根杆长度相同。目标点固定在平面坐标 \((1,1)\)，要求反推两根杆怎样转才能让第二杆末端正好落在该点。

题目：设 \(l_1=l_2=1\)，目标点 \((x,y)=(1,1)\)。求一组逆运动学解。

l1=1 l2=1 target (1,1) r=sqrt(2) solid and dashed elbows are two possible IK branches
2R 逆运动学示意：目标点先确定两杆组成的三角形，再由余弦定理反求关节角。
图中元素：基座固定在原点，两根杆长度都为 1；红色目标点是 \((1,1)\)。绿色实线表示一组解 \(q_1=0^\circ,q_2=90^\circ\)，灰色虚线表示另一组肘部在另一侧的解，说明逆运动学可能多解。
建模顺序：
  1. 先画固定基座、活动关节和连杆，标清每个角度是相对哪个坐标轴或哪根杆量的。
  2. 写出几何关系或矩阵关系，再代入长度、角度、目标点或外力。
  3. 若是 Jacobian 题，速度用 \(J\dot q\)，末端力映射用 \(J^TF\)。
  4. 最后说明多解、无解、奇异或力矩正负号代表什么。

解析过程

目标点到基座的距离是 \(\sqrt{1^2+1^2}=\sqrt2\)，在 \(0\) 到 \(2\) 的可达范围内，所以可以继续求解。先用余弦定理求两杆夹角 \(q_2\)。

$$\cos q_2=\frac{1^2+1^2-1^2-1^2}{2}=0$$

所以 \(q_2=90^\circ\) 或 \(-90^\circ\)。这两个角对应肘部在目标连线两侧的两种构型。取肘上解 \(q_2=90^\circ\)。

$$q_1=\operatorname{atan2}(1,1)-\operatorname{atan2}(1,1)=45^\circ-45^\circ=0^\circ$$

一组解为 \(q_1=0^\circ,q_2=90^\circ\)。另一组为 \(q_1=90^\circ,q_2=-90^\circ\)。

易错点：逆运动学通常有多解；答案要说明你取的是哪一支。

例 2.5b 2R 工作空间无解判断

基础判断

场景交代：画 2R 平面机械臂，基座固定在原点，第一杆长 1 m、第二杆长 0.5 m。把目标点画在 \(x\) 轴正方向 2 m 处；先比较目标半径与机械臂最大伸展长度。

题目：设 \(l_1=1\,\mathrm m,l_2=0.5\,\mathrm m\)，目标点为 \((2,0)\)。判断该点是否可达。

max reach = 1.5 m target (2,0) r = 2 m > l1 + l2 = 1.5 m reachable disk boundary
2R 工作空间无解示意：目标点到基座的距离超过最大伸展长度，所以逆运动学无实数解。
图中元素：左侧黑色圆圈是基座，绿色和蓝色两段杆完全伸直后总长为 \(l_1+l_2=1.5\,\mathrm m\)，虚线圆是最大可达边界；红色目标点在 \(x=2\,\mathrm m\) 处，位于虚线圆外。
建模顺序：
  1. 先画固定基座、活动关节和连杆，标清每个角度是相对哪个坐标轴或哪根杆量的。
  2. 写出几何关系或矩阵关系，再代入长度、角度、目标点或外力。
  3. 若是 Jacobian 题，速度用 \(J\dot q\)，末端力映射用 \(J^TF\)。
  4. 最后说明多解、无解、奇异或力矩正负号代表什么。

解析过程

先不要急着代逆运动学公式。2R 机械臂的末端最远只能到两杆完全伸直的位置，也就是 \(l_1+l_2=1.5\,\mathrm m\)。目标点到基座的距离为：

$$r=\sqrt{2^2+0^2}=2\,\mathrm m$$

因为 \(r=2>1.5\)，目标点在最大工作空间之外，所以没有实数逆运动学解。若硬代余弦定理，会得到 \(\cos q_2\) 超出 \([-1,1]\) 的范围，这正是“不可达”的代数信号。

易错点：逆运动学前先做可达性判断；\(\cos q_2\) 超界代表无实数解。

6. 并联机构与五杆机构逆运动学

课件用五杆机构，也就是 five-bar / 5-bar mechanism，说明并联机构的逆运动学。并联机构常把每条支链看成一个三角形：给定末端点，分别求左右驱动角。

$$\cos\alpha=\frac{a^2+b^2-c^2}{2ab}$$

串联机械臂的末端位姿是沿一条链从基座算到末端；并联机构则有多条支链同时约束同一个末端平台。五杆机构的逆运动学常比正运动学更容易，因为给定末端点后，左右每条支链都可以看成一个二连杆三角形。余弦定理给出的角通常有正负两支，对应不同装配模式，所以答案不能只写一个角就结束。

例 2.6 五杆机构一侧支链角度

仿 Chapter2

场景交代：画五杆并联机构的左半支链：左侧基座点 \(A\) 固定在原点，第一杆 \(AB\) 通过转动副接在 \(A\)，第二杆 \(BC\) 通过转动副接在 \(B\)，末端点 \(C\) 给定在 \((1,1)\)。

题目：左侧基点 \(A=(0,0)\)，末端点 \(C=(1,1)\)。左侧两杆 \(AB=BC=2\)。求第一杆 \(AB\) 相对 x 轴的可能角度。

C A D
五杆并联机构示意：左右支链都可看成三角形来做逆运动学。
图中元素：底部左右两个点是固定基座，左右两条支链同时连接到中间末端点 C；求逆运动学时可以把每侧单独看成一个三角形。
建模顺序：
  1. 先画固定基座、活动关节和连杆，标清每个角度是相对哪个坐标轴或哪根杆量的。
  2. 写出几何关系或矩阵关系，再代入长度、角度、目标点或外力。
  3. 若是 Jacobian 题，速度用 \(J\dot q\)，末端力映射用 \(J^TF\)。
  4. 最后说明多解、无解、奇异或力矩正负号代表什么。

解析过程

先把左支链单独拿出来看。基点 \(A\) 到末端 \(C\) 的连线是三角形的一条边，长度和方向都能从坐标直接算出。

$$r=\sqrt{1^2+1^2}=\sqrt2,\qquad \beta=\operatorname{atan2}(1,1)=45^\circ$$

现在三角形的三条边分别是 \(AB=2\)、\(BC=2\)、\(AC=\sqrt2\)。我们要求的是第一杆 \(AB\) 相对 \(AC\) 方向偏开的角 \(\gamma\)，所以用余弦定理。

$$\cos\gamma=\frac{AB^2+AC^2-BC^2}{2(AB)(AC)}=\frac{4+2-4}{4\sqrt2}=0.3536$$

\(\gamma=69.3^\circ\)。第一杆的绝对方向等于 \(AC\) 的方向 \(\beta\) 再加或减这个偏角，因此有两种可能装配方式：

$$\theta_A=\beta\pm\gamma=45^\circ\pm69.3^\circ$$
易错点：五杆机构常有多组装配模式，别只写一个角。

7. Jacobian：速度与力的桥

$$\dot x=J(q)\dot q,\qquad \tau=J^T(q)F$$

Jacobian 的第一层含义是“微小变化的放大器”。如果关节角发生很小变化 \(dq\)，末端位置会发生 \(dx\)，两者的线性近似就是 \(dx=Jdq\)；把时间除进去就得到速度关系 \(\dot x=J\dot q\)。第二层含义来自虚功原理：末端力 \(F\) 做的虚功 \(F^Tdx\) 必须等于关节力矩做的虚功 \(\tau^Tdq\)，再代入 \(dx=Jdq\)，就得到 \(\tau=J^TF\)。所以 \(J\) 不是孤立公式，它同时连接速度和力。

更具体地说，Jacobian 是把正运动学 \(x=f(q)\) 对关节变量求偏导得到的。平面 2R 里如果末端位置写成 \(x(q_1,q_2),y(q_1,q_2)\)，那么

$$J(q)=\begin{bmatrix}\frac{\partial x}{\partial q_1}&\frac{\partial x}{\partial q_2}\\[2pt]\frac{\partial y}{\partial q_1}&\frac{\partial y}{\partial q_2}\end{bmatrix}$$

再对时间求导时，用的是链式法则：\(\dot x=\frac{\partial x}{\partial q_1}\dot q_1+\frac{\partial x}{\partial q_2}\dot q_2\)，\(\dot y=\frac{\partial y}{\partial q_1}\dot q_1+\frac{\partial y}{\partial q_2}\dot q_2\)。这就是为什么 Jacobian 乘关节速度会得到末端速度。

对 2R 机械臂：

$$J= \begin{bmatrix} -l_1\sin q_1-l_2\sin(q_1+q_2)&-l_2\sin(q_1+q_2)\\ l_1\cos q_1+l_2\cos(q_1+q_2)&l_2\cos(q_1+q_2) \end{bmatrix}$$

例 2.7 Jacobian 速度计算

必会

场景交代：画 2R 平面机械臂，基座固定，第一杆沿世界 \(x\) 轴，第二杆从肘部竖直向上。两个关节同时转动，给出的 \(\dot q_1,\dot q_2\) 是绕各自转动副的角速度。

题目：2R 机械臂 \(l_1=l_2=1\)，\(q_1=0^\circ,q_2=90^\circ\)，关节速度 \(\dot q=[1,2]^T\,\mathrm{rad/s}\)。求末端速度。

l1, q1 = 0 deg l2, q2 = 90 deg qdot1 = 1 qdot2 = 2 xdot = [-3, 1]^T Jacobian columns = velocity contributions
Jacobian 速度计算示意：当前姿态下第一杆水平，第二杆竖直，两个关节角速度共同叠加成末端速度。
图中元素：左下方基座为第 1 转动关节，绿色第一杆沿 \(x\) 轴，蓝色第二杆竖直向上；橙色弧箭头表示 \(\dot q_1=1\)、\(\dot q_2=2\)，末端绿色箭头表示计算得到的 \(\dot x=[-3,1]^T\)。
建模顺序：
  1. 先画固定基座、活动关节和连杆，标清每个角度是相对哪个坐标轴或哪根杆量的。
  2. 写出几何关系或矩阵关系，再代入长度、角度、目标点或外力。
  3. 若是 Jacobian 题，速度用 \(J\dot q\)，末端力映射用 \(J^TF\)。
  4. 最后说明多解、无解、奇异或力矩正负号代表什么。

解析过程

先根据当前姿态计算 Jacobian。此时第一根杆沿 \(x\) 轴，第二根杆竖直向上，代入 \(q_1=0,q_1+q_2=90^\circ\) 后得到：

$$J=\begin{bmatrix}-1&-1\\1&0\end{bmatrix}$$

再用速度映射。第一列表示第 1 个关节以 \(1\,\mathrm{rad/s}\) 转动时末端速度贡献，第二列表示第 2 个关节的贡献；把两列按关节速度加权相加，就是末端速度。

$$\dot x=J\dot q=\begin{bmatrix}-1&-1\\1&0\end{bmatrix}\begin{bmatrix}1\\2\end{bmatrix}=\begin{bmatrix}-3\\1\end{bmatrix}\,\mathrm{m/s}$$
易错点：关节速度用 rad/s，不是度/秒；如果题目给度/秒先转弧度。

例 2.8 末端力映射到关节力矩

静力映射

场景交代：沿用上一题的 2R 姿态：第一杆水平向右，第二杆竖直向上，末端在肘部正上方。外力作用在末端点，方向竖直向下，要求计算两个转动关节为了平衡该力需要的力矩。

题目：上题姿态下，末端受力 \(F=[0,-10]^T\,\mathrm N\)。求关节力矩。

F at end tau1 tau2 tau = J^T F balance external wrench by joint torques
末端力到关节力矩映射示意：外力作用在末端点，通过 \(J^T\) 转换成各转动关节需要平衡的力矩。
图中元素：基座、肘关节和两根杆构成 2R 平面臂；红色箭头从末端竖直向下表示给定末端力，橙色弧箭头表示关节平衡力矩，公式 \(\tau=J^TF\) 说明力不是沿杆随便分配，而是由 Jacobian 转置映射。
建模顺序：
  1. 先画固定基座、活动关节和连杆，标清每个角度是相对哪个坐标轴或哪根杆量的。
  2. 写出几何关系或矩阵关系，再代入长度、角度、目标点或外力。
  3. 若是 Jacobian 题，速度用 \(J\dot q\)，末端力映射用 \(J^TF\)。
  4. 最后说明多解、无解、奇异或力矩正负号代表什么。

解析过程

力映射不用 \(J\)，而用 \(J^T\)。原因是末端力和关节力矩之间要保证虚功相等：末端力通过末端位移做功，关节力矩通过关节角位移做功。

$$\tau=J^TF=\begin{bmatrix}-1&1\\-1&0\end{bmatrix}\begin{bmatrix}0\\-10\end{bmatrix}=\begin{bmatrix}-10\\0\end{bmatrix}\,\mathrm{N\,m}$$

第 1 关节需要提供 \(-10\,\mathrm{N\,m}\) 来平衡该力，第 2 关节此姿态下对竖直力没有力矩贡献。

易错点：速度映射用 \(J\)，力映射用 \(J^T\)。

例 2.9 Jacobian 奇异位形判断

进阶

场景交代：画 2R 平面机械臂，重点比较两种共线构型：两杆完全伸直成一条线，以及第二杆反向折回与第一杆共线。此时末端在某些方向的瞬时运动自由度会退化。

题目：2R 平面机械臂的 Jacobian 行列式为 \(\det J=l_1l_2\sin q_2\)。判断何时奇异，并说明物理含义。

q2 = 0: fully stretched q2 = pi: folded collinear det J = l1 l2 sin(q2) = 0 collinear links lose one instantaneous task-space direction
2R Jacobian 奇异位形示意：两杆共线时 \(J\) 失去满秩，末端会丢失某个瞬时运动方向。
图中元素：左图是 \(q_2=0\) 的完全伸直构型，右图是 \(q_2=\pi\) 的完全折叠构型；两图中两根杆都在同一直线上，因此 \(\det J=l_1l_2\sin q_2=0\)。
建模顺序：
  1. 先画固定基座、活动关节和连杆，标清每个角度是相对哪个坐标轴或哪根杆量的。
  2. 写出几何关系或矩阵关系，再代入长度、角度、目标点或外力。
  3. 若是 Jacobian 题，速度用 \(J\dot q\)，末端力映射用 \(J^TF\)。
  4. 最后说明多解、无解、奇异或力矩正负号代表什么。

解析过程

奇异位形通常看 Jacobian 是否失去满秩。对 2R 平面机械臂，如果 \(l_1,l_2\) 都不为零，那么行列式是否为零只由 \(\sin q_2\) 决定。

$$\det J=l_1l_2\sin q_2=0\Rightarrow q_2=0\ \text{或}\ \pi$$

这两个情况分别对应两根杆完全伸直或完全折叠成一条直线。此时机械臂在某个末端方向上的瞬时速度能力丢失，某些末端速度无法由有限关节速度产生；反过来，在静力映射里某些方向的末端力会要求很大的关节力矩。考试里看到“伸直、折叠、Jacobian 行列式为零”，基本就是奇异位形信号。

易错点：奇异不是算错，而是机构在该姿态下丢失某个瞬时运动方向。

概念题与计算题自测

概念 1

场景交代：画一条 7 关节机械臂执行 6 自由度末端位姿任务，多出来的关节自由度可以在不改变末端位姿的情况下调整肘部或避障姿态。

为什么 7 自由度机械臂叫冗余机器人？

7 joints: extra posture choice 6D task same end pose, different elbow posture
冗余机械臂示意：关节自由度多于末端任务需要时，可在保持末端位姿不变的同时调整肘部姿态。
图中元素：左侧多段连杆表示 7 自由度机械臂，右侧蓝色框表示 6 自由度末端任务；橙色虚线姿态表示同一个末端位姿可以对应不同中间关节构型。
查看答案与解析

空间末端的完整位姿通常需要 6 个自由度描述，分别是 3 个位置自由度和 3 个姿态自由度。如果机械臂有 7 个关节自由度，就比完成末端任务所需的最小自由度多了一个。这个多出来的自由度不会让末端任务变得无法确定，反而给机器人留下选择余地：同一个末端位姿可以由不同关节姿态实现，因此可以用来避开奇异位形、绕开障碍、避开关节限位或让手术器械保持更安全的姿态。

计算 1

场景交代：画 2R 平面机械臂，基座固定在原点，两根杆通过转动副串联，且两根杆长度都为 1。第一杆相对世界 \(x\) 轴转到 \(45^\circ\) 方向；第二杆相对第一杆的转角为 \(0^\circ\)，所以第二杆与第一杆完全共线，整条机械臂等效为一根长度 2、方向 \(45^\circ\) 的杆。

2R 机械臂 \(l_1=1,l_2=1\)，\(q_1=45^\circ,q_2=0^\circ\)。求末端位置。

l1=1 l2=1 q1=45deg q2=0deg end (sqrt2, sqrt2) two links are collinear, total length = 2
2R 共线姿态求末端位置示意：当 \(q_2=0^\circ\) 时，第二杆没有相对第一杆再转角，两根杆在同一直线上。
图中元素：基座、肘关节和末端点排成一条 \(45^\circ\) 斜线；两根杆长度都是 1，所以等效为一根长度 2、方向 \(45^\circ\) 的杆。末端投影到 \(x,y\) 轴后得到 \(x=y=\sqrt2\)。
查看答案与解析

两节杆此时方向相同，因为第二关节相对第一杆的角度为 0，所以整条机械臂等效为长度 2、方向 \(45^\circ\) 的一根杆。

$$x=2\cos45^\circ=1.414,\quad y=2\sin45^\circ=1.414$$

结果说明末端位于第一象限，并且因为方向是 \(45^\circ\)，所以 \(x\) 和 \(y\) 坐标相等。

计算 2

场景交代：本题已经直接给出 Jacobian 矩阵和末端力向量，不需要假设具体连杆长度或姿态。画成矩阵映射即可：左边是末端力列向量 \(F=[3,4]^T\)，中间是 \(J^T\)，右边是关节力矩列向量 \(\tau\)；计算顺序是先把 \(J\) 转置，再与 \(F\) 相乘。

若 \(J=\left[\begin{smallmatrix}1&2\\0&1\end{smallmatrix}\right]\), \(F=[3,4]^T\)，求 \(\tau\)。

given force F=[3,4]^T transpose J^T [[1,0],[2,1]] joint torques tau=[3,10]^T This is a pure matrix mapping problem, not a geometric drawing problem.
已给 Jacobian 矩阵的静力映射示意：这里不需要猜测具体连杆姿态，只要按矩阵关系把末端力映射为关节力矩。
图中元素：左侧列向量是题目给出的末端力 \(F=[3,4]^T\)，中间蓝色框是 \(J^T\)，右侧列向量是要求的 \(\tau\)。箭头方向表示计算顺序：先转置 \(J\)，再乘 \(F\)。
查看答案与解析

末端力映射到关节力矩时使用 \(\tau=J^TF\)，这是由虚功相等得到的。先把 Jacobian 转置，再与末端力相乘。

$$\tau=J^TF=\begin{bmatrix}1&0\\2&1\end{bmatrix}\begin{bmatrix}3\\4\end{bmatrix}=\begin{bmatrix}3\\10\end{bmatrix}$$

第一个关节力矩为 3，第二个关节力矩为 10，说明在这个姿态下第二个关节对该末端力更敏感。

本章掌握度