【机器人学习】小白学习第04讲：逆运动学、工作空间与冗余

你学你的我点份烤冷面

5人浏览 · 2026-10-31 13:07:03

你学你的我点份烤冷面 · 2026-10-31 13:07:03 发布

【机器人学习】小白学习第04讲：逆运动学、工作空间与运动学冗余

核心一句话：正运动学是"给角度算位置"，逆运动学是"给位置反算角度"——因为是反着算，所以可能多解、可能无解，也可能有无穷多解。

本讲目标：会判断逆解有没有解、有几个解；会手算三连杆平面臂的解析逆解；明白工作空间、冗余、标定分别是怎么回事。

一、为什么需要这个

小讲 1：正运动学顺着算、逆运动学反着算都难在哪

正运动学顺着算，逆运动学反着算

想象你在教一个蒙着眼睛的人用手指去碰桌上的杯子。

正运动学（上一讲）：你知道他肩、肘、腕各转了多少度，让你算指尖在哪。——照着算就行，答案唯一。
逆运动学（本讲）：你告诉他"指尖要到这个点、朝这个方向"，让他反过来算出每个关节要转多少度。

工程任务几乎全是"反着算"

现实里的任务全是第二种。工人示教的是"焊枪走这条焊缝"，激光切割要的是"焦点沿这条曲线"；而电机只听得懂"转多少弧度"。逆运动学就是把任务翻译成电机指令的必经之路。

停下来想一想

为什么反着算就难？因为四个麻烦同时存在：方程一般非线性（含 sin、cos），不一定有封闭解；同一位姿可能有多个解；冗余的臂有无穷多解；给定位姿还可能根本到不了。正运动学是一条直通的路，逆运动学是个岔路口。

停下来想一想

停下来想一想：工人示教的是"焊枪走这条焊缝"，而电机只听得懂"转多少弧度"——这两句话分别属于关节空间还是操作空间？把前者翻译成后者靠的是什么？

二、核心概念

小讲 2：关节空间/操作空间、多解无解、工作空间、精度与冗余

关节空间 vs 操作空间

关节空间（Joint Space）：由 n 个关节变量 q 张成的空间；转动关节 qᵢ = ϑᵢ，移动关节 qᵢ = dᵢ。

操作空间（Operational Space）：任务所在的空间，末端位姿 x_e = [ p_e ; φ_e ]，维数 m ≤ n。

正运动学就是从关节空间走到操作空间的桥：x_e = k(q)。

逆运动学问题

已知末端位置与姿态，反求关节变量 q。就是把 x_e = k(q) 反过来解。

它的重要性在于：只有它才能把"在操作空间里下达的运动指令"翻译成"关节空间里的运动"，机械臂才动得起来。

多解 / 无解 / 无穷多解

多解：非零 DH 参数越多，解越多。无关节限位时，六自由度臂最多可有 16 组解。
无解：目标点落在可达工作空间之外。
无穷多解：运动学冗余时。

多解必须靠准则挑选，比如最短行程或避障；最典型的例子就是下面要手算的 elbow-up / elbow-down。

可达 vs 灵巧工作空间

可达工作空间：末端原点能以至少一个姿态到达的全部点。

灵巧工作空间：末端原点能以任意姿态到达的点，是可达工作空间的子集。

所以：只有当给定位姿属于灵巧工作空间时，解的存在性才被保证。

图 1 工作空间

图 1 工作空间。左：不带限位的二连杆臂，可达区域是半径介于 |a₁ − a₂| 与 a₁ + a₂ 之间的圆环，两条边界分别由"完全伸直"和"完全折叠"扫出。右：灵巧工作空间是可达工作空间的子集；只有在它里面，才能保证给任意姿态都拿得到解。实际的工业机器人还有关节限位，会把这个区域再切掉一块。

精度与重复精度

精度（Accuracy）：指令位姿与实际到达位姿的偏差。臂展 1.5 m 级的工业机器人典型为 0.2–1 mm。

重复精度（Repeatability）：反复回到同一个已示教点的能力，典型 0.02–0.2 mm，一般比精度好一个数量级。

"每次都焊在同一个地方"容易，"按图纸坐标精确到位"难——后者要靠标定。

运动学冗余

自由度数多于描述任务所需变量数时，就是冗余。

固有冗余：m < n，如四连杆平面臂 n = 4, m = 3。
功能性冗余：m = n，但只有 r < m 个分量与任务有关。例如激光切割时绕进刀方向的旋转无关紧要，六轴臂变成 r = 5，冗余 1 个自由度。

人手有七自由度：手掌位姿固定后，手肘还能自由挪动去避开障碍，这就是冗余的价值。

停下来想一想

停下来想一想：末端能以"至少一个姿态"到达的全部点叫什么？能以"任意姿态"到达的点又叫什么？要保证给任意姿态都有解，目标位姿必须落在哪一个里面？

三、关键公式

小讲 3：逆运动学的紧凑写法与三连杆平面臂的代数解、几何解

1. 正/逆运动学的紧凑写法

$x_{e} = \begin{bmatrix} p_{e} \\ \varphi_{e} \end{bmatrix} = k(q) q = [ q_{1} ... q_{n} ]^{\top}$

符号	含义	直觉
x_e	末端位姿向量（m×1）	任务书：末端要到这儿、朝这个方向
q	关节变量向量（n×1）	电机指令：每个关节动多少
k(·)	正运动学函数，一般非线性	两者之间的翻译软件

特意提醒：严格说 (2.82) 形式下正运动学也不是唯一确定的，因为欧拉角本身不唯一——这是后面第 06 讲"表示奇异"的伏笔。

2. 三连杆平面臂的代数逆解

核心思路：先把第三根连杆砍掉。因为 φ 已给定，可以先退一步求出腕点 W（Frame 2 原点，只依赖前两个角）：

$\begin{aligned} \varphi = \vartheta_{1} + \vartheta_{2} + \vartheta_{3} \\ p_{Wx} = p_{x} - a_{3} \cos \varphi = a_{1} c_{1} + a_{2} c_{12} \\ p_{Wy} = p_{y} - a_{3} \sin \varphi = a_{1} s_{1} + a_{2} s_{12} \end{aligned}$

两式平方相加，消掉 ϑ₁，得到只含 ϑ₂ 的一行：

$\begin{aligned} p_{Wx}^{2} + p_{Wy}^{2} = a_{1}^{2} + a_{2}^{2} + 2 a_{1} a_{2} c_{2} \\ \Rightarrow c_{2} = ( p_{Wx}^{2} + p_{Wy}^{2} - a_{1}^{2} - a_{2}^{2} ) / ( 2 a_{1} a_{2} ) \\ \Rightarrow s_{2} = \pm \sqrt{ 1 - c_{2}^{2} } \\ \Rightarrow \vartheta_{2} = Atan2( s_{2}, c_{2} ) \end{aligned}$

提示

**正负号就是"二选一"：**s₂ 取正号 → elbow-down（肘下）；取负号 → elbow-up（肘上）。两者都是合格答案，没有谁算错，要靠工程准则挑一个。

解的存在条件就是−1 ≤ c₂ ≤ 1，等价于 |a₁ − a₂| ≤ √(p_Wx² + p_Wy²) ≤ a₁ + a₂，也就是"三角形必须能围起来"——目标点必须在可达工作空间内。

有了 ϑ₂ 再代回去，解关于 s₁、c₁ 的二元一次方程组：

$\begin{aligned} s_{1} = ( (a_{1} + a_{2} c_{2}) p_{Wy} - a_{2} s_{2} p_{Wx} ) / ( p_{Wx}^{2} + p_{Wy}^{2} ) \\ c_{1} = ( (a_{1} + a_{2} c_{2}) p_{Wx} + a_{2} s_{2} p_{Wy} ) / ( p_{Wx}^{2} + p_{Wy}^{2} ) \\ \vartheta_{1} = Atan2( s_{1}, c_{1} ) \\ \vartheta_{3} = \varphi - \vartheta_{1} - \vartheta_{2} \end{aligned}$

3. 几何法（余弦定理版）

$\begin{aligned} \alpha = Atan2( p_{Wy}, p_{Wx} ) \\ \beta = \cos^{-1}( ( p_{Wx}^{2} + p_{Wy}^{2} + a_{1}^{2} - a_{2}^{2} ) / ( 2 a_{1} \sqrt{p_{Wx}^{2} + p_{Wy}^{2}} ) ) \\ \vartheta_{1} = \alpha \pm \beta \vartheta_{2} < 0\text{（肘上）取} + \text{号，}\vartheta_{2} > 0\text{（肘下）取} - \text{号} \end{aligned}$

几何法的优点是每一步都看得见：α 是从基座看向目标点的方位角，β 是从这条方向往回修正的一个角，两个候选解关于弦 OW 对称。代数法好写成代码，几何法好画图理解，结果完全一致。

补充：当 s₂ = 0（即 ϑ₂ = 0 或 π，两连杆共线）时，指出这是运动学奇异——第 06 讲会详细讲。

小讲 4：球形腕为什么能解耦，以及运动学标定怎么算

4. 带球形腕的臂：为什么会"解耦"

$p_{W} = p_{e} - d_{6} a_{e}$

a_e 是末端坐标系的 z 轴单位向量（进刀方向），所以 p_W 就是腕心，即三个腕关节轴的交点。于是逆解被拆成四步：

由 p_e 与 a_e 算出腕心 p_W；
用前三轴解位置子问题（就是上面那套方法，平面臂、球坐标臂或拟人臂）；
算 R⁰₃，再求 R³₆ = (R⁰₃)ᵀ R_e；
用 ZYZ 欧拉角公式解姿态子问题，得到 ϑ₄, ϑ₅, ϑ₆。

点明了前提：六自由度结构有封闭解，当且仅当"三个连续转动轴交于一点"（球形腕）或"三个连续转动轴平行"。工业界几乎全用球形腕，一半是为了结构紧凑，一半就是为了逆解算得出来。

5. 运动学标定

为什么理论模型和实际机器人对不上？因为 DH 参数只是名义值，加工公差、装配误差让真实杆长、偏距、零点都差一点点。把这些偏差找出来的技术叫运动学标定：

$KaTeX parse error: Undefined control sequence: \Deltax at position 20: …gin{aligned} \̲D̲e̲l̲t̲a̲x̲ ̲= \partial k/ \…$

符号	含义
ζ	把 a、α、d、ϑ 全部装成的 4n×1 参数向量
Φ	标定矩阵，是位姿对各参数的偏导数（本质也是雅可比）
Δx	实测位姿 x_m 与名义位姿 x_n 之差，也就是精度误差

直觉：在许多个位姿上精确测量末端实际位姿，把"末端差了多少"反向分摊给"每个 DH 参数错了多少"。一组姿态只有 m 个方程却有 4n 个未知数，所以必须测 l 个姿态凑够方程（取 l·m ≫ 4n），再用最小二乘求解。它本质是非线性参数估计，要迭代到 Δζ 收敛为止。标定之后不仅能得到更准的几何参数，还能顺带得到关节传感器读数的修正量。

另有一种用户做的"标定"：用增量式编码器时上电先把机械结构摆到参考位姿 HOME，给位置传感器赋初值——那叫回零，不是运动学标定。

停下来想一想

停下来想一想：求三连杆平面臂逆解时，为什么可以先"砍掉"第三根连杆、先求腕点 W？s₂ 取正号和取负号分别对应哪两种肘部姿态？

图 2 三连杆平面臂逆解的两种肘部姿态

图 2 三连杆平面臂逆解的两种肘部姿态。以 O 和 W 为圆心、以 a₁、a₂ 为半径画两个圆，交点就是两个候选的肘位置，它们关于弦 OW 对称。第三连杆从 W 出发，方向由 φ 唯一确定，但 ϑ₃ = φ − ϑ₁ − ϑ₂ 在两种解下不同：本例肘下解 ϑ₃ = −35.1°，肘上解 ϑ₃ = +14.8°。

小讲 5：从图上读肘上/肘下两解与可达、灵巧工作空间

停下来想一想

停下来想一想：图 1 里两个候选肘位置关于哪条线对称？图 2 中标着①的点为什么求不出逆解？

五、例题 / 动手

小讲 6：三连杆平面臂逆解的六步手算与动手练习

例题：三连杆平面臂逆运动学完整手算

已知：a₁ = 0.30 m，a₂ = 0.32 m，a₃ = 0.20 m。要求末端到位p = [ 0.60 , 0.46 ]ᵀ m，姿态角φ = 30°。

第 1 步：砍掉第三连杆，求腕点 W

$\begin{aligned} p_{Wx} = 0.60 - 0.20 \cdot \cos30° = 0.60 - 0.1732 = 0.4268 \\ p_{Wy} = 0.46 - 0.20 \cdot \sin30° = 0.46 - 0.1000 = 0.3600 \\ d = \sqrt{p_{Wx}^{2} + p_{Wy}^{2}} = 0.5584 m \end{aligned}$

第 2 步：检查有没有解a₁ + a₂ = 0.62 ≥ 0.5584 ✓，|a₁ − a₂| = 0.02 ≤ 0.5584 ✓ → 有解。

第 3 步：算 ϑ₂

$\begin{aligned} c_{2} = ( 0.5584^{2} - 0.30^{2} - 0.32^{2} ) / ( 2 \cdot 0.30 \cdot 0.32 ) \\ = ( 0.3118 - 0.0900 - 0.1024 ) / 0.192 = 0.6216 \\ s_{2} = \pm \sqrt{ 1 - 0.6216^{2} } = \pm 0.7833 \\ \vartheta_{2} = Atan2( s_{2} , c_{2} ) = \pm 51.6° \end{aligned}$

第 4 步：几何法算 ϑ₁

$\begin{aligned} \alpha = Atan2( 0.3600 , 0.4268 ) = 40.2° \\ \beta = \cos^{-1}( ( 0.3118 + 0.0900 - 0.1024 ) / ( 2 \cdot 0.30 \cdot 0.5584 ) ) \\ = \cos^{-1}( 0.8935 ) = 26.7° \\ \text{肘下解：}\vartheta_{1} = \alpha - \beta = 13.5° \text{（配} \vartheta_{2} = +51.6°\text{）} \\ \text{肘上解：}\vartheta_{1} = \alpha + \beta = 66.8° \text{（配} \vartheta_{2} = -51.6°\text{）} \end{aligned}$

第 5 步：算 ϑ₃

$\begin{aligned} \text{肘下解：}\vartheta_{3} = 30° - 13.5° - 51.6° = -35.1° \\ \text{肘上解：}\vartheta_{3} = 30° - 66.8° - (-51.6°) = +14.8° \end{aligned}$

第 6 步：代回正运动学验算（不能省）

$\begin{aligned} \text{肘下解：}x = 0.30 \cdot \cos13.5° + 0.32 \cdot \cos65.1° + 0.20 \cdot \cos30° = 0.4266 + 0.1732 = 0.5998 \\ y = 0.30 \cdot \sin13.5° + 0.32 \cdot \sin65.1° + 0.20 \cdot \sin30° = 0.3602 + 0.1000 = 0.4602 \end{aligned}$

提示

结论：同一个位姿 [ 0.60, 0.46, 30° ] 有两组完全合格的解，都回到同一个点。工程上按最短行程挑——若当前姿态靠近 (0°, 90°, −30°)，选肘下那组；若中间有障碍物挡住肘下的肘部位置，就改选肘上那组。这就是"多解取舍准则"。

动手练习

把上面的流程写成十几行 Python，核心只有三处：atan2(sqrt(1-c2*c2), c2)得 ϑ₂；atan2(pWy, pWx)得 α；再用 α±β 得两组 ϑ₁。
把目标点改成 p = [ 1.05 , 0.46 ]ᵀ 试试，观察 c₂ 会算出什么，为什么会无解。
习题 2.17：a₁ = 0.5，a₂ = 0.3，a₃ = 0.2，且 ϑ₁ ∈ [−π/3, π/3]、ϑ₂ ∈ [−2π/3, 2π/3]、ϑ₃ ∈ [−π/2, π/2]，画出这个三连杆平面臂的工作空间。建议用网格扫描：遍历关节角，把末端点画成散点。