EXP-04 · 第 2 章 · 综合性 · 4 学时

逆运动学基础与 IK 解算

这一章把上一章倒过来：给定指尖要到的位置和姿态，反推五个关节角该是多少。听起来只是把公式反过来用，实际上难得多，答案可能有两组，也可能根本没有。下面同样拆成七级台阶：前四级用余弦定理把答案解出来，让你真的懂；第五、六级换成数值迭代，让程序什么情况都能算；第七级上真机，看看解出来的角到底能不能让指尖到位。

⭐⭐⭐ 较难 4 学时 atan2 · 余弦定理 · 雅可比

00先认识一下这个名字

上一章你学会了：知道关节转了多少，把指尖在哪算出来。这一章要反过来回答另一个问题：我想让指尖到某个地方，五个关节各该转多少？

这个学问叫 Inverse Kinematics，中文译作「逆运动学」，平时叫 IK。名字里的「逆」，是说它的方向和 FK 相反。FK 是摆好姿势看手在哪，IK 是手要放这、姿势该怎么摆。

真机上你天天在做这件事。想让夹爪去抓桌上的杯子，你脑子里想的是那个杯子的位置，不是五个舵机的角度。把「杯子在哪」翻译成「五个角各多少」，就是 IK 干的活。

为什么 IK 比 FK 难

FK 是一路乘下去，答案唯一。IK 不一样，它有三个麻烦，这一章会一个一个处理：

麻烦	说人话	在第几级遇到
可能有两组答案	手肘朝上或者朝下，都能摸到同一个点	第 3 级
可能根本没有答案	那个点太远，手臂够不着	第 2 级
某些姿势下算不动	手臂完全伸直时，怎么转弯都只会朝一个方向	第 6 级

要动用的数学

和 FK 用的不太一样。FK 一路是乘法和三角函数，IK 更像在做几何题：

数学	在这儿干什么用	第几级
atan2	已知一个点的横竖两个分量，反算它的方向角	第 1 级
余弦定理	三角形三条边都知道，反算夹角	第 2 级
反三角函数	arccos、arcsin，正弦余弦反过来求角度	第 2、3 级
雅可比矩阵	关节动一点点，指尖跟着动多少，用来指路	第 5 级

要注意的是，这一章几乎不出现矩阵连乘。解析法的核心是几何，把手臂拆成三角形，用余弦定理直接算。矩阵要到第 5 级讲雅可比时才回来。

01问题与路线图

先把要解决的问题说具体。教学模型的尺寸你还记得：大臂 0.126 米、小臂 0.126 米、工具段 0.151 米，J2 离地 0.189 米。

现在的问题是：我想让指尖落在 (0.157, 0, 0.342)，而且工具段要水平朝前。五个关节角分别是多少？

这组数据的答案你其实见过，上一章默认姿态就是它：q2 = −50°、q3 = 105°、q4 = 35°。但那是已知角去算位置。现在要求的是反方向：只看目标，把角解出来。

七级台阶

还是同样的思路：先把几何解打通，让你真的懂；再用数值法让程序不挑情况。

图 1-1｜七级台阶怎么排队。第 1~3 级把手臂拆成三角形，用几何算出答案，第 4 级串成完整链路，走到这儿你就能自己反解。第 5、6 级讲雅可比和数值迭代，几何法失效时靠它兜底，程序里两套都实现了。第 7 级上真机验证解出来的角准不准。

几个会反复出现的词

词	说的是什么
解析法	用公式直接算出来，一次到位。快，但只对特定构型有效
数值法	先猜一个答案，再一点点往目标挪。慢，但什么情况都能试
多解	同一个目标，有两组甚至更多组关节角都能到
奇异位形	手臂伸直或完全折叠时，某个方向上动不了
可达性	目标点离底座的距离，要在手臂能伸到的范围内

先约定两件事

第一件，角度还是从竖直的 Z 轴量起，跟上一章一致。零位时整条臂笔直竖着，q=0 表示和竖直线重合。

第二件，这一章为了讲得清楚，先把三维问题压到二维平面里。因为 J2、J3、J4 三根轴都沿 Y 方向，它们只能让手臂在 X-Z 竖直平面里动。所以做法永远是：先用 J1 转到目标朝向，之后就在这个平面里解两杆问题。俯视图定方向，侧视图定形状。

⚠️ 但有一个"降维损失"要提前知道把 5 关节压成 4 自由度（x, y, z, pitch）是要付代价的：J5 的解恒为 0。因为 J1 与 J5 共轴，在"末端俯仰已由 J2/J3/J4 定死"的前提下，J5 再怎么转都不能改变工具的朝向和位置——它是冗余的，所以解析解里直接取 0。后果是：这台机器不能只靠 IK 把夹爪绕自身轴摆成任意角度，需要在 solution_to_dict() 之外单独给 q5 赋值。想实现"夹爪换个朝向再抓"，得在解完之后手工设 q5，而不是指望 solve_analytical_ik 帮你解出来。

🔍 限位到底是多少，两处说法要分清解析器 config.py 的 jointLimits 写的是 ±π（±180°），这是 URDF 名义行程；真正执行的是 arm_controller.py 的 JOINT_LIMITS_RAD = ±2.356（±135°），来自 270° 舵机的物理行程。所以：解算器认为合法的角，控制器仍可能拒收。写代码时两边都要过，别只信一边。

02台阶一：先转底座，对准方向

IK 的第一步最简单。目标点在水平面上的哪个方向，J1 就转到哪。

俯视图上看：目标的 x 和 y 两个坐标，决定了它相对于底座的方向。用 atan2(y, x) 一算就出来。这个函数和 atan(y/x) 的区别是它能分清四个象限，右边偏上是正的，左边偏下也是对的，不会算反。

1q1 = atan2(y, x)　目标在水平面上的方向角

2剩下的问题就变成二维的了：在这个方向上，距离多远、抬多高

动画拖一下，看 J1 怎么对准目标

目标 x0.157

目标 y0.000

算出来的 q1 = 0.00°

水平距离 r = 0.157 m

拖 y 滑块，看红色箭头跟着转。q1 只负责对准方向，不管抬多高、伸多远。

这一步的答案是唯一的，因为它不涉及手臂的形状，只涉及朝向。真正的难题在后面：对准方向之后，手臂该怎么弯，才能让指尖正好落在那个距离和高度上。

顺带说一句，代码里 q1 的公式还多减了一个小项 asin(Y_TCP/r)，用来补偿真机上指尖那一丁点横向偏移。教学模型没有这个偏移，可以完全忽略；等你上真机时会看到它为什么需要。

03台阶二：两杆怎么弯，余弦定理上场

方向对准了，问题就变成：两节杆，从起点出发，怎么弯才能让末端落到指定的一点？这是初中就学过的几何题。

把起点、中间那个关节、末端连成一个三角形。三条边你都知道：两条是杆长（0.126 和 0.126），第三条是起点到目标的直线距离 d。三条边都知道，求夹角，这就是余弦定理：

cos(夹角) = (d² − L1² − L2²) / (2 · L1 · L2)

算出来的余弦值，反手用 arccos 一按，夹角就出来了。

手算一个例子

假设对准方向之后，目标离起点的直线距离是 d = 0.2154 米。代进去：

步骤	算式	结果
算 d²	0.2154²	0.046400
算 L1² + L2²	0.12614² × 2	0.031823
算 2·L1·L2	2 × 0.12614 × 0.12614	0.031823
代余弦定理	(0.046400 − 0.031823) ÷ 0.031823	0.457994
反算角度	arccos(0.457994)	62.74°

动画拖滑块改目标距离，看三角怎么变

目标距离 d0.215

cos(夹角) = 0.458

夹角 = 62.7°

拖到最右边（0.252 m）时两杆完全伸直，夹角是 0。往回拖，夹角变大。超过 0.252 就够不着了。

有一个边界要记住：两杆加起来最远能伸 0.2523 米。超过这个距离，余弦定理算出来的值会小于 −1，arccos 没有答案，这就相当于够不着。

⚠️ 这里藏着一个真问题：源码把越界值"夹"掉了，而不是报错看 ik_analytical.py 第 125-127 行：

cosQ3rel = (distSq - L1² - L2²) / (2·L1·L2)
cosQ3rel = max(-1.0, min(1.0, cosQ3rel))   # ← 超界值被静默夹到 ±1
q3rel_mag = math.acos(cosQ3rel)

后果是：目标离得太远时，余弦值本该 < −1，却被夹成 −1，arccos(−1) = 180°，程序照样返回一组"解"，一声不吭。它的 q3 停在 ±0.17° 附近（整条臂几乎伸直），既不报 "Out of reach"，也不返回错误。
实测复现（本章第 09 节就是让你自己跑出来）：(0.25, 0, 0.15)、(0.22, 0.03, 0.12)、(0.18, 0.05, 0.10) 三个点位全部超界，回代 FK 后误差 18.4 / 22.9 / 21.7 mm；而工作空间内的 (0.20, −0.03, 0.18) 误差是 0.00 mm。
这就是"求解成功 ≠ 解准确"。注意源码里其实有一个可达性检查（dist > maxReach + 1e-5 就返回 "Out of reach"），但它拦不住这种情况——因为 maxReach 只算到 J4（L1+L2），而 dist 是到 J4 的距离，判据本身没写错；真正漏掉的是：夹紧发生在更后面，把一次本该失败的求解粉饰成了一次成功。

🔍 工程上该怎么防两条防线缺一不可：① 求解前按 L1+L2 = 0.2523 m 先算一遍可达性，超界就直接拒绝，别把选择权交给 arccos；② 求解后永远做一次 FK 回代、比对误差（这正是下一节"闭环验证"的由来）。只看 error is None 就认为解对了，是本课程最容易踩的坑。

04台阶三：两组解，选一个

刚才算出来 62.74°。但仔细想想，这个角可以是正的，也可以是负的：

+62.74°：肘部朝上弯，手臂呈「∧」形
−62.74°：肘部朝下弯，手臂呈「∨」形

两组都让末端落到同一个点。这就是 IK 的第一个特点：答案不唯一。

把两组都算出来，验一遍

还是 d = 0.2154 米那个位置，目标是让末端落在 (0.080, 0.200)：

	解 1（肘上）	解 2（肘下）
大臂倾角	−9.57°	+53.17°
肘部相对角 q3	+62.74°	−62.74°
小臂倾角	+53.17°	−9.57°
末端落在（回代 FK）	(0.0800, 0.2000)	(0.0800, 0.2000)

两组解算出来的末端位置完全一样，都是目标点。这不是误差，是真实存在的两个姿势。

动画同一个目标点，两种姿势都摸得到

q3 角度+62.7°

大臂倾角 = −9.6°

小臂倾角 = +53.2°

末端落在 = (0.080, 0.200)

只有 q3 = ±62.7° 这两种情况，末端才正好落在黄色目标点上。其它角度都落不到那儿，试试看。

实际怎么选：看哪种姿势不会撞到东西、不会超出关节限位、离当前姿势更近（转过去更省时间）。源码里默认优先肘上，因为这样工作空间更大，不容易碰到桌面。

05台阶四：完整反解，一串起来就通了

前面三步解决了"在平面里怎么把末端送到一个点"。但真机上的目标不是"末端的位置"，而是"指尖的位置 + 工具的朝向"。所以还差一步换算：

1目标：指尖在 (0.157, 0.342)，工具段水平朝前

2倒推 J4：指尖减去工具那段 0.151 × (sin90°, cos90°)，得到 (0.0067, 0.3424)

3对两杆逆解：相对 J2（高度 0.189）算出 (0.0067, 0.1534)，距离 0.1535

4补 q4：工具的目标俯仰角减去小臂的倾角

手算一遍

步骤	算式	结果
① 倒推 J4	(0.1573, 0.3424) − 0.1506 × (1, 0)	(0.0067, 0.3424)
② 相对 J2	z 减去 0.1890	(0.0067, 0.1534)
③ 距离	√(0.0067² + 0.1534²)	0.1535
④ 余弦定理求 q3	(0.1535² − 0.0318) ÷ 0.0318	cos = −0.2591 → ±105.02°
⑤ 大臂倾角	atan2 再减去中间那个偏角	−50.02° / +55.00°
⑥ 补 q4	90° − 小臂倾角	35.00° / 140.02°

两组解都能让指尖落在目标点上，回代 FK 验证误差为零：

解 1：q2 = −50.024°   q3 = +105.019°   q4 = +35.004°
解 2：q2 = +54.996°   q3 = −105.019°   q4 = +140.024°

注意解 1 完全是上一章默认姿态那组数：q2 = −50°、q3 = 105°、q4 = 35°。这说明反解真的把正解倒回来了。两组都回代 FK，指尖都落在 (0.157, 0.342)，误差为零。

走到这里你就真的会了：一支笔、一张纸、一个计算器，就能从"指尖要到哪"算出"五个角该多少"。下面两级是为了让程序也能算，而且不挑情况。

06台阶五：雅可比，抖一下看反应

前面用的余弦定理，只对"两节杆"这种结构好使。如果手臂有七个关节、或者形状不规则呢？那就没法硬套公式了。这时候得换个思路：先猜一个姿势，看看差多远，然后往差得少的方向挪一点，反复几次。

要"往差得少的方向挪"，得先知道一件事：每个关节动一点点，指尖会跟着动多少？这张换算表就是雅可比矩阵。

1J 的每一列，回答"这个关节单独动一下，指尖往哪动、动多远"

2所以指尖的变化量 = J × 关节的变化量

3反过来：想让指尖动到某处，关节该动多少 = 用 J 反算

动画只动 J3 一点点，看指尖跳多远

q390°

指尖位移 / 关节位移 = 0.126 m/rad

这条"换算比例"就是雅可比那一项的大小。它随姿势变，不是常数。

有个细节值得注意：这个比例不是固定的。同样是 J3 转 1°，手臂伸直时指尖几乎不动，手臂直角时指尖动得最多。等到 q3 接近 0°，比例掉到零——这就引出了下一级的问题。

07台阶六：数值迭代与奇异

有了雅可比，就能用"猜 + 修"的办法解 IK：

猜一组关节角 q
重复：
    算出当前指尖在哪  →  和目标差多少 e
    用雅可比反算：关节该挪多少 Δq
    q = q + Δq
    差得够小了就停

这就是数值法。它的好处是通用：不管几个关节、什么形状，只要雅可比能算，就能试。代价是要迭代很多次，比解析法慢几十上百倍。

但有一种姿势会卡住

还记得台阶五里那个"比例"吗？它正比于 sin(q3)。当 q3 接近 0° 或 180°，手臂完全伸直或完全折叠，这个比例趋近于零：

q3	\|sin q3\|	状态
0°	0.0000	奇异，完全伸直
3°	0.0523	接近奇异
10°	0.1736	正常
90°	1.0000	最灵活
180°	0.0000	奇异，完全折叠

这时候迭代会要一个"无穷大的关节角位移"去补一点点指尖误差，程序直接崩或者疯狂抖动。工程上的解法是给它加一个阻尼项（就是源码里的 DLS 阻尼最小二乘），让"无穷大"变成"很大但有限"，慢慢磨过去。

源码里两套都有：ik_analytical.py 是解析法，一次算出答案，快但不通用；数值法在 ik-numerical.js 里用 DLS 迭代。真机上默认走解析法，遇到解不出来才切数值法。

08台阶七：上真机，解出来的角真的准吗

解出一组角，代回 FK 算指尖位置，如果和目标是同一个点，说明数学没错。但真机上还有另一层：这组角发给舵机之后，指尖真的会到那儿吗？

先做闭环测试

目标点 → IK → 一组关节角 → FK → 算出来的指尖位置
                                      ↓
                              和目标点比，差多少？

这一步在源码里是 test_fk_ik.py，用内置的几个点位跑一遍。如果差得远，说明模型或参数有问题。

差得远的时候，先分清是哪一种

这是这一章最容易走错的地方。误差有两个来源，处理方式完全不同：

误差类型	表现	怎么处理
模型误差	同一组角，算出来的位置和实际差一截，而且每次差得差不多	改尺寸参数，重新标定
求解误差	解出来的角代回 FK 就对不上目标	算法问题，看是不是多解选错、或者迭代没收敛

判断方法很简单：先只比"解出来的角代回 FK"和"目标"。这一步只有数学，不涉及真机。如果这里就差了，跟装配无关；如果这里对得上，真机上却差，那才是装配和标定的问题。

往真机下发

解出来的角要换成 PWM 才能发给舵机，换算关系在上一章讲过：PWM = rad ÷ 0.002356 + 1500。源码里 solution_to_pwm() 做这件事，然后通过串口发出去。

把它和别的可能的错因分清楚，是本章唯一需要背下来的一张表：

现象	该怀疑谁	怎么验证
解出来的角代回 FK，就和目标差了十几毫米，但仍返回"成功"	求解器 bug（本章那个夹紧问题）	只跑纯数学、不接真机；换一个工作空间内的点做对照，误差立刻归零
代回 FK 完全对得上，但真机指尖差几厘米	机械 / 标定：零位、TCP、装配公差	卷尺量、四点法复验（见标定专题第 2 部分）
真机动作方向反了、或某个关节明显超范围	PWM↔弧度方向 / 限位	发单个关节 `s 1 1520` 看是不是只动 1 号；查 `SERVO_CONFIG` 的 `direction`
同一个点在两次运行里误差不同	没有闭环或没有回代 FK	把 FK 回代误差打进日志，每次都看

想上真机？先走 --direct

本章的最高配是"跳过 ROS2、直连串口下发"，用来把算法误差和架构误差彻底分开：interactive_ik_test.py --direct 会直接打开串口（和 EXP-02 完全同一条链路），你输入的 x/y/z/pitch 在本地解完 IK 立刻变成 PWM 发下去——中间没有节点、没有话题、没有 launch。它跑通了却和 ROS2 路线结果不同，问题就一定出在架构层（话题没接上、参数没加载、TF 不同步），而不是算法。

⚠️ 用 --direct 前必须做的事和 EXP-02 一模一样的清场：先停掉 bringup / arm_controller，否则两边抢 /dev/stm32。而且这条路径绕过了 arm_controller 的软限位检查（JOINT_LIMITS_RAD = ±2.356），防越界只能靠你自己看 solution_to_pwm() 打出来的 5 路 PWM 有没有贴到 500/2500。

这一章不接实机也能做，只要闭环测试跑通就算达标。想上真机验证的，按实操步骤里的命令来。

09动手验一遍

先用 3D 模型手动试

下面这个就是上一章的模型，它现在能拖关节角。你可以这样验证 IK：先在纸上用余弦定理算出一组角，再把它拖到模型上，看指尖是不是落在预期的位置。

互动 9-1｜用 IK 算出来的角，拖进这个模型验证。比如目标 (0.157, 0.342)，解是 q2=−50°、q3≈105°、q4=35°，拖过去读数应该对得上。

再跑闭环测试

进入 ros2_ws/src/xflz_eai_car1/arm/kinematics/kinematics/
运行 python3 test_fk_ik.py
看每个点位：IK 解出的角代回 FK，误差多大？
如果某点误差大，先算清楚是"解不对"还是"模型不对"

做完了自检

能说清 IK 和 FK 的方向差别，以及 IK 为什么会多解
能用余弦定理手算出两杆的夹角
能解释"手臂伸直时算不动"的原因（sin q3 → 0）
能分清模型误差和求解误差
闭环测试能跑通，误差记在报告里

10回头看看走过的这七步

台阶	用了什么	得到了什么
一、先转底座	atan2	q1，方向定了
二、两杆怎么弯	余弦定理	肘部相对角，两组取正负
三、两组解选一个	多解判断	肘上 / 肘下，都到得了
四、完整反解	倒推 J4 + 补 q4	五个角全算出来，FK 回代误差为零
五、雅可比	换算比例	知道每个关节动一点，指尖动多少
六、数值法与奇异	迭代 + 阻尼	几何法失效时也能算；知道哪儿会卡
七、上真机	闭环测试	解出来的角真的能让指尖到位

如果只能记住一句，记住台阶四：IK 的完整链路是"先倒推 J4，再解两杆，最后补工具角"。前面那些几何公式，都是为了这一步服务的。

几道题

1. 目标距离正好等于 0.2523 米时，q3 是多少？为什么这时候只有一个解？

2. 如果把 J1 转到 90°，同一个目标点的 q2、q3、q4 会不会变？为什么？

3. 两组解里，一组 q3 = 62.7°，另一组 q3 = −62.7°。它们在物理上分别是什么姿势？

4. 闭环测试里，如果所有点位的误差都是同一个方向、同一个大小，你最该怀疑什么？

5. 数值法迭代到一个奇异位形会怎样？加阻尼之后为什么能绕过？

这一章把 IK 解出来了，但解出来不等于抓得到。下一章开始做固定抓取：给定一个已知位置的物体，怎么让夹爪对准它、张开、合上、抬起来。那是个完整的动作序列，IK 只是其中的一环。