FK 与 IK
配套页面:关节控制(看 FK 的方向)+ 末端位姿(看 IK 的方向)
1. 一条双向链,但只有一边是"读"#
舵机编码器
│ 读数(角度)
▼
关节角 ──── FK 正解 ────▶ 位姿(X / Y / Z / pitch / roll)
▲ │
└──────── IK 逆解 ─────────┘
这两条箭头看着对称,其实性质完全不同:
- FK 是"算":给定六个关节角,照着连杆几何一路乘下去,一定有唯一结果,而且瞬间出结果。
- IK 是"求":给定一个位姿,反推关节角,是迭代逼近出来的——可能失败,也可能不止一个解。
还有更关键的一点:和位姿相关的硬件量只有关节角(电压、温度、电流也会读,但它们不是位姿)。位姿从来不是传感器直接给你的,它是 FK 算出来的。
2. 硬件到底给了什么#
舵机内部是磁编码器,上报的是一个计数。lerobot 按标定文件把这个计数换算成角度,starm
拿到的就是这六个角度:
arm.get_joints()
# {'shoulder_pan': 4.9, 'shoulder_lift': -5.0, ..., 'gripper': 30.0}
而位姿,是拿这六个角度现算的:
def get_pose(self) -> Pose:
return self.kinematics.pose(self.get_joints()) # arm.py
没有"位姿传感器"这个东西。 这条推论很有用:位姿的误差不可能来自"测位姿测不准",它只可能来自两个地方——关节角不准(标定误差),或者连杆几何不准(URDF / TCP 偏移)。
3. FK:关节角 → 位姿#
怎么算#
整条手臂是串联的,每个关节贡献一个 4×4 变换:
T_关节 = 固定安装变换(URDF 里的 origin) × 绕该轴转 θ
从底座开始一路乘到夹爪,最后再加一个固定的 TCP 偏移:
T_基座→TCP = T_肩 × T_大臂 × T_肘 × T_腕俯仰 × T_腕滚 × T_TCP偏移
代码里就是这一件事(starm/kinematics.py):
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,从当前姿态出发一步步逼近。
解的是四个未知数#
未知数: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 分量:
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 下都有梯度,而且不随手臂指向改变
"每度"的代价。
两者解的是同一组四个未知数,接口完全一样:
arm.ik(pose) # 用默认(配置里的 ik_solver)
arm.ik(pose, solver="riemann") # 这一次用测地版
"riemann" 失败时会用同一个种子退回 "lm",所以选它不会比默认更差。
界面上也能直接切:「连接」页有个 IK 求解器卡片,下拉即切,不用断开重连, IK 预检和之后的每一次移动都会立刻用新求解器(改的是同一个配置对象)。
什么时候需要切:默认的
lm在大多数位姿上一试就中;如果你在竖直向下的抓取上反复吃Unreachable,切到riemann往往能解出来——它对pitch = 90不敏感。
种子:从当前姿态出发#
求解器总是先拿当前关节角当起点。这有两个好处:
- 增量动作(直线路径、拖滑块)会保持原来的构型,不会突然跳到另一种手臂姿态
- 收敛快
如果起点收敛不了,还会试几个固定姿态兜底;都不行才抛 Unreachable。
失败时长什么样#
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
报错里三件事:差多远、卡在哪个关节、为什么。这不是"运气不好",是几何上到不了。
5. 两个方向并不对称#
| FK 正解 | IK 逆解 | |
|---|---|---|
| 输入 → 输出 | 6 个关节角 → 1 个位姿 | 1 个位姿 → 6 个关节角 |
| 一定成功吗 | 一定 | 可能失败(Unreachable) |
| 唯一吗 | 唯一 | 可能多解(本机实际基本唯一) |
| 怎么算 | 直接连乘 | 迭代逼近,最多 100 次 |
| 快慢 | 微秒级 | 毫秒级 |
| 有副作用吗 | 无,纯计算 | 求解本身无副作用,但下发前还要过安全层 |
IK 预检通过 只代表"几何上够得着",不代表"能执行"——限位、工作空间、速度限制都还在后面。
6. 走一次完整的往返#
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" | ✗ | 多半是几何上真的够不着 |
参考#
- 第 2 章:机械臂关节 —— 每个关节的轴和范围
- 第 3 章:末端位姿 —— 五个分量、自由度、坐标系
- 代码:
starm/kinematics.py的fk()/pose()/ik()