SO-101
课程总览/第 04 章 · FK 与 IK
EN
第 04 章运动学约 18 分钟

FK 与 IK

配套页面:关节控制(看 FK 的方向)+ 末端位姿(看 IK 的方向)

9 个小节 1 张图 11 段代码 正解逆解数值迭代
FK 与 IK

1. 一条双向链,但只有一边是"读"#

text
舵机编码器
    │  读数(角度)
    ▼
 关节角 ──── FK 正解 ────▶ 位姿(X / Y / Z / pitch / roll)
    ▲                          │
    └──────── IK 逆解 ─────────┘

这两条箭头看着对称,其实性质完全不同:

  • FK 是"算":给定六个关节角,照着连杆几何一路乘下去,一定有唯一结果,而且瞬间出结果。
  • IK 是"求":给定一个位姿,反推关节角,是迭代逼近出来的——可能失败,也可能不止一个解。

还有更关键的一点:和位姿相关的硬件量只有关节角(电压、温度、电流也会读,但它们不是位姿)。位姿从来不是传感器直接给你的,它是 FK 算出来的。

2. 硬件到底给了什么#

舵机内部是磁编码器,上报的是一个计数。lerobot 按标定文件把这个计数换算成角度,starm 拿到的就是这六个角度:

python
arm.get_joints()
# {'shoulder_pan': 4.9, 'shoulder_lift': -5.0, ..., 'gripper': 30.0}

而位姿,是拿这六个角度现算的:

python
def get_pose(self) -> Pose:
    return self.kinematics.pose(self.get_joints())   # arm.py

没有"位姿传感器"这个东西。 这条推论很有用:位姿的误差不可能来自"测位姿测不准",它只可能来自两个地方——关节角不准(标定误差),或者连杆几何不准(URDF / TCP 偏移)。

3. FK:关节角 → 位姿#

怎么算#

整条手臂是串联的,每个关节贡献一个 4×4 变换:

text
T_关节 = 固定安装变换(URDF 里的 origin) × 绕该轴转 θ

从底座开始一路乘到夹爪,最后再加一个固定的 TCP 偏移:

text
T_基座→TCP = T_肩 × T_大臂 × T_肘 × T_腕俯仰 × T_腕滚 × T_TCP偏移

代码里就是这一件事(starm/kinematics.py):

python
def fk(self, joints):
    frames = {"base": np.eye(4)}
    for name in JOINT_NAMES:
        joint = self._joints[name]
        angle = np.radians(joints.get(name, 0.0))
        frames[joint.child] = frames[joint.parent] @ joint.fixed @ _rot_z(angle)
    return frames

零位实测#

六个关节全 0° 时,每个连杆原点在基座系里的位置(mm):

连杆 x y z
base 0.0 0.0 0.0
shoulder 20.8 −23.1 94.9
upper_arm 2.5 −53.5 149.1
lower_arm 2.5 −81.5 261.7
wrist 2.5 −216.4 266.9
gripper 20.6 −277.5 266.9
jaw 39.4 −300.9 287.1
TCP 20.4 −381.1 256.8

注意 TCP 在 y 方向上比 jaw 还远 80 mm —— 这就是那段约 104 mm 的 TCP 偏移。

每个关节对末端的影响有多大#

单独把某个关节转 10°,TCP 移动的距离(其余关节不动):

关节 +10° TCP 移动
shoulder_pan 62.4 mm
shoulder_lift 60.1 mm
elbow_flex 52.2 mm
wrist_flex 28.8 mm
wrist_roll 1.8 mm

前三行好理解:它们是大臂长、离末端远的关节。wrist_roll 只移动 1.8 mm,是因为绕进近轴自转几乎不改变 TCP 位置——那 1.8 mm 全部来自 TCP 相对腕轴的横向偏移。

这张表也解释了为什么标定误差在肩部比在腕部更致命。

4. IK:位姿 → 关节角#

为什么不是解析解#

这台臂是 5 自由度,几何上还有偏移量和限位,写解析式既丑又容易在边界处出问题。 SDK 用的是数值迭代:Levenberg–Marquardt,从当前姿态出发一步步逼近。

解的是四个未知数#

text
未知数:shoulder_pan, shoulder_lift, elbow_flex, wrist_flex
残差  :x, y, z, 进近方向的竖直分量

wrist_roll 不参与求解,直接取目标里的 roll。这就是"5 个位姿分量 → 5 个关节角"的分法:

位姿分量 由谁负责
x, y, z 前四个关节迭代求解
pitch 同上(体现在进近分量里)
roll wrist_roll 直接赋值

一个细节:残差不用 pitch 角#

如果直接拿 pitch 这个角度当残差,pitch = 90°(竖直向下)会是奇异点——角度在那里对姿态变化不敏感,雅可比会退化。代码换成进近向量的 z 分量:

python
target_approach_z = -np.sin(np.radians(target.pitch))
# 残差 = [位置误差(3), w × (approach_z - target_approach_z)]

方向向量的分量在整个球面上都光滑,包括"竖直向下"那一点。所以 pitch=90 能正常求解,只是能到达的高度受限(约桌面上方 80 mm)。

两种求解器#

SDK 提供了两个逆解实现,默认用 "lm",可以在配置里改,也可以每次调用单独指定:

求解器 姿态残差怎么算 适合
"lm"(默认) 进近向量的竖直分量 approach_z 冷启动、离目标很远时最稳
"riemann" SO(3) 上的测地旋转误差(绕 pitch 轴转过的角度) 要求"每度代价一致",比如竖直向下抓取

为什么会有第二种:approach_z 这个残差在 pitch = 90° 附近梯度趋近于零——竖直分量对 pitch 不再敏感,求解器只能靠重新播种硬撑过去。"riemann" 把姿态误差写在 SO(3) 上:误差的长度就是"要让夹爪转过的角度",在任何 pitch 下都有梯度,而且不随手臂指向改变 "每度"的代价。

两者解的是同一组四个未知数,接口完全一样:

python
arm.ik(pose)                        # 用默认(配置里的 ik_solver)
arm.ik(pose, solver="riemann")      # 这一次用测地版

"riemann" 失败时会用同一个种子退回 "lm",所以选它不会比默认更差。

界面上也能直接切:「连接」页有个 IK 求解器卡片,下拉即切,不用断开重连, IK 预检和之后的每一次移动都会立刻用新求解器(改的是同一个配置对象)。

什么时候需要切:默认的 lm 在大多数位姿上一试就中;如果你在竖直向下的抓取上反复吃 Unreachable,切到 riemann 往往能解出来——它对 pitch = 90 不敏感。

种子:从当前姿态出发#

求解器总是先拿当前关节角当起点。这有两个好处:

  1. 增量动作(直线路径、拖滑块)会保持原来的构型,不会突然跳到另一种手臂姿态
  2. 收敛快

如果起点收敛不了,还会试几个固定姿态兜底;都不行才抛 Unreachable。

失败时长什么样#

python
arm.move_pose(y=-300, z=230, pitch=90)
# Unreachable: no solution for Pose(x=0.0, y=-300.0, z=230.0, pitch=90.0, roll=0.0):
# closest attempt was 1.4 mm and 43.5 deg away; wrist_flex is at its upper limit (95.0 deg).
# Pointing straight down costs reach height: at pitch 90 the arm only reaches about 80 mm
# above the table

报错里三件事:差多远、卡在哪个关节、为什么。这不是"运气不好",是几何上到不了。

互动实验

IK 求解器

浏览器内 LM 迭代

给一个目标位姿,看看逆解怎么从当前姿态一步步逼近。残差用的是“位置误差 + 进近方向竖直分量”,和 SDK 是同一套参数化;这里是一个简化实现,用于观察行为,不是控制器的替身。

目标 TCP解出的姿态桌面
等待求解拖动目标,或点「求解」
「竖直向下高处」就是第 4 章里的失败例子:pitch = 90 时手臂最高只到桌面上方约 80 mm。这里没有安全层,所以它只报“几何上够不着”,不会真的动。

5. 两个方向并不对称#

FK 正解 IK 逆解
输入 → 输出 6 个关节角 → 1 个位姿 1 个位姿 → 6 个关节角
一定成功吗 一定 可能失败(Unreachable)
唯一吗 唯一 可能多解(本机实际基本唯一)
怎么算 直接连乘 迭代逼近,最多 100 次
快慢 微秒级 毫秒级
有副作用吗 无,纯计算 求解本身无副作用,但下发前还要过安全层

IK 预检通过 只代表"几何上够得着",不代表"能执行"——限位、工作空间、速度限制都还在后面。

6. 走一次完整的往返#

python
target = Pose(x=0.0, y=-260.0, z=150.0, pitch=65.0, roll=0.0)

joints = k.ik(target, seed=zero)     # IK:位姿 → 关节角
back   = k.pose(joints)              # FK:关节角 → 位姿

实测:

步骤 结果
目标位姿 (0, −260, 150), pitch 65
IK 解出的关节角 pan 4.9° / lift −5.0° / elbow −9.9° / wrist_flex 80.0° / wrist_roll 0°
FK 回到的位姿 (0.00, −260.00, 150.00), pitch 65.00
位置误差 0.005 mm(5 µm)

数值解不是"精确"的,但误差在微米量级,对一台 3D 打印的臂来说远小于机械误差。

7. 在页面上看这两个方向#

控制台的两个页签其实是同一对映射的两个方向:

页签 你在动什么 背后发生什么
关节控制 拖六个关节滑块 FK:后端用新关节角算出位姿,遥测和三维模型跟着变
末端位姿 填五个位姿数字 IK:点「IK 预检」先解一遍关节角给你看,不动机器人

一个练习:在「关节控制」把 wrist_flex 拖到 90°,切到「末端位姿」看遥测里的 pitch 是不是也变成 90 —— 这就是 FK 在跑。再在「末端位姿」填 z = 230,点「IK 预检」,得到 Unreachable —— 这就是 IK 在失败。

8. 常见误区#

说法 对不对 说明
"位姿是传感器直接读出来的" ✗ 和位姿相关的硬件量只有关节角;位姿是 FK 算的
"IK 就是 FK 的反函数,两者对称" ✗ FK 一定有解且唯一;IK 可能无解、要靠迭代
"IK 解出来就一定能动" ✗ 还要过安全层:限位、工作空间、速度、急停
"IK 一定有多解" ≈ 理论上可能,但本机受 5 自由度和限位约束,实测基本唯一
"IK 失败是代码有 bug" ✗ 多半是几何上真的够不着

参考#