第 3 章告诉我们方块在哪里。想让末端到方块上方,还要回答两个问题:这个坐标相对于哪里?两段连杆该分别转多少?这一章跟着 move(x, y, z) 从世界坐标走到实际关节运动。
可以先读示意图,跑完一次可达和不可达的移动,再回来看三角函数。
本章目标
你将区分世界坐标、机器人基座坐标和关节坐标;用两段连杆解释正运动学与逆运动学;运行可达和不可达的移动命令;根据反馈判断位置误差,而不是把“命令被接受”当成“已经到位”。
先固定坐标和单位
坐标系就是规定从哪里开始量、沿哪些方向量的一把三维尺。世界坐标系固定在场景中,机器人基座坐标系固定在机械臂底座上;两把尺可以有不同原点,所以同一个点会有不同数字。
课程 move(x, y, z) 接收世界坐标系中的绝对位置,距离单位是米。桌面为 z=0,z 轴向上;x、y 方向以场景定义为准,屏幕左右会随相机视角变化。x=-0.05 表示目标的世界 x 坐标为负 0.05 米,不表示“相对现在后退 5 厘米”。
基座位于世界坐标 (-0.4, 0, 0.32),水平两段连杆长分别为 0.35 与 0.30 米。连杆就是连接关节的那两段刚性手臂。关节角度用弧度表达:半圈是 π 弧度,也就是 180°;四分之一圈是 π/2。竖直滑动关节的位移仍用米,不能把它当成第三个角度。
关节坐标描述机器人怎样弯曲,末端坐标描述工具在哪里。上层可以要求工具去一个位置,底层再算出关节应怎样转动,两者并不冲突。
本场景基座坐标轴与世界坐标轴方向一致,水平换算只需减去基座平移:
x_base = x_world - (-0.4)
y_base = y_world - 0
若基座还发生旋转,就需要同时变换方向,不能只减一个偏移量。我们暂时选择轴方向一致的场景,是为了先看清这一步。
例如,世界目标 (-0.05, 0.10) 换成基座的水平坐标,是 (-0.05 + 0.4, 0.10) = (0.35, 0.10)。它表示目标在基座 x 方向 35 厘米、y 方向 10 厘米处。竖直滑动也有类似偏移:要求工具到世界高度 0.18 米时,理想滑动目标为 0.18 - 0.32 = -0.14 米,负号表示从基座高度向下移动。
纸上检查: 先不算关节角,世界点 (0, 0) 在基座的左侧还是右侧,水平距离是多少?答案是基座 x 正方向 0.4 米处。若算成 0 米,说明把世界原点与机器人基座当成了同一点;先修正这一步,再看后面的可达范围。
从关节到末端:正运动学
把两段连杆画在纸上,长度分别记为 l1、l2。第一段朝向是 q1,第二段相对于第一段又旋转 q2,所以第二段在基座中的朝向是 q1 + q2。cos 和 sin 分别帮助我们算一段倾斜连杆在 x、y 方向伸出了多远;把两段贡献相加:
x_base = l1 * cos(q1) + l2 * cos(q1 + q2)
y_base = l1 * sin(q1) + l2 * sin(q1 + q2)
这就是本章的水平正运动学:给定关节角度,计算末端位置。竖直高度由第三个滑动关节决定。注意不要把 cos(q2) 直接当作第二段的世界方向,也不要把角度制数值直接传给以弧度为单位的三角函数。
先用直角检验理解:q1=0、q2=π/2 时,第一段向 x 方向伸出 0.35 米,第二段向 y 方向伸出 0.30 米,所以基座坐标是 (0.35, 0.30),世界坐标是 (-0.05, 0.30)。这个可以画出来的例子,就是公式在做的事。
从末端到关节:逆运动学
逆运动学反过来问:末端要去某个位置,关节应该取什么值?对两连杆平面机构,可以由连杆和目标点形成的三角形得到:
c2 = (x_base² + y_base² - l1² - l2²) / (2 * l1 * l2)
q2 = ± arccos(c2)
q1 = atan2(y_base, x_base)
- atan2(l2 * sin(q2), l1 + l2 * cos(q2))
这里的 ± 提醒我们,同一个末端位置通常可能对应两种弯肘姿态。到达同一位置,不等于机器人形状相同。课程实现选择一个分支;更复杂系统还要结合关节限制、碰撞和当前姿态选择解。
公式中的 arccos 是已知余弦值反求角度;atan2(y, x) 根据 x、y 同时判断方向,能够区分目标位于哪个象限。课程代码会替你计算逆解,本章主要要理解它何时有解、为什么有时不止一个解。
如果 c2 超出 [-1, 1],理想几何下就没有实数解。等价地,不考虑关节限制时,目标到基座的水平距离必须位于两段长度差与长度和之间,也就是本场景的 0.05 至 0.65 米。这只是水平几何条件,还没有检查高度范围、关节限制和运动途中障碍。
实验一:执行一次可达移动
回到包含 lab/ 的实验根目录,并确认环境已经激活。下面命令的 --x、--y、--z 就是交给 move 的世界目标坐标:
python lab/run.py move --seed 7 --x -0.05 --y 0.10 --z 0.18 --out runs/move-018
这个点距基座的水平距离是 sqrt(0.35² + 0.10²),约 0.364 米,落在理想几何范围内。sqrt 表示开平方。再读返回结果:最外层 ok 是本次移动结果,实际位置在 observation.robot.tool_xyz,把它与请求的 [-0.05, 0.10, 0.18] 写在两列。
用三个分量差的平方和开方,计算位置误差:e = sqrt(dx² + dy² + dz²)。课程移动接口以小于 0.02 米作为到位阈值。记录实际误差和接口返回的 ok,才能知道工具报告是否与状态变化一致。
本机种子 7 的一条实际反馈为 [-0.05, 0.10002, 0.17893] 米。目标减去实际位置,三个差约为 0、-0.00002、0.00107 米,合成误差约 1.07 毫米。它小于 2 厘米阈值,所以 ok 为真,但这不表示误差恰好为零。自己的数值以实际输出为准。
也可以让 Python 计算自己那次移动的误差。把下面代码保存为根目录的 inspect_move.py 并执行 python inspect_move.py。它延续第 3 章的读日志方法,只读取本次唯一一条事件:
import json
import math
from pathlib import Path
line = Path("runs/move-018/events.jsonl").read_text().splitlines()[0]
event = json.loads(line)
requested = [event["arguments"][key] for key in ("x", "y", "z")]
actual = event["result"]["observation"]["robot"]["tool_xyz"]
error_m = math.dist(requested, actual)
print("请求:", requested, "实际:", actual)
print("位置误差(米):", round(error_m, 5))
print("小于2厘米:", error_m < 0.02, "工具返回:", event["result"]["ok"])
math.dist 就是上述距离公式;构造 requested 的那一行依次取出 x、y、z。种子 7 的参考输出约为 0.00107,两个检查值均为 True。若再打开汇总看到任务 success: false,这是合理的:本实验只让工具移动,没有抓取、搬运和释放方块。
先预测,再改一个参数: 仅把目标 z 从 0.18 改为 0.22,期望高度增加多少,水平位置是否应明显变化?写下答案后,另存第二次独立实验:
python lab/run.py move --seed 7 --x -0.05 --y 0.10 --z 0.22 --out runs/move-022
比较返回的水平位置和高度。预期高度约增加 0.04 m,也就是 4 厘米,x、y 应仍接近相同目标。可把检查程序中的目录改为 runs/move-022,再次计算各自的跟踪误差。不要以为第二条终端命令会自动接着上一条命令的场景执行;完整连续操作由同一次基线或 Agent 运行保存状态。
实验二:让不可达请求明确失败
先预测下面的结果,再运行:
python lab/run.py move --seed 7 --x 1.0 --y 0.0 --z 0.18 --out runs/move-unreachable
该点与基座的水平距离为 1.0 - (-0.4) = 1.4 米,远大于 0.65 米的连杆总长。接口应返回 ok: false 和 error: unreachable,命令以退出码 2 结束,并在拒绝时不修改仿真状态。这里的失败是有用的接口结果,保留 runs/move-unreachable 的记录继续分析即可。
在笔记中解释拒绝原因,列出上层可以选择的下一步:重新检查坐标系、确认单位、重新定位目标,或者明确报告无法到达。盲目重复同一个不可达坐标不会增加成功机会。
算出解还不等于完成运动
运动学只回答几何关系。实际执行还需要把目标变成控制信号,让状态随时间变化,再检查跟踪误差。即使起点和终点都可达,途中也可能碰到障碍;一个可达性检查不能代替路径规划。
还有一个容易忽略的区别:在关节空间平滑改变角度,不保证末端沿直线移动。两段连杆的三角函数关系会把关节变化映射为曲线。因此,不能凭终点正确就声称机器人执行了指定的直线路径。
本课程先学习末端位置接口。完整的“位姿”还包含朝向:同一个位置上的杯子可以杯口朝上,也可以倒扣。用户要求“把杯口朝上”时,需要同时控制朝向;当前三参数 move(x,y,z) 没有提供这项任意姿态控制能力。
验收与常见错误
保留两次可达移动的位置对照和一次不可达返回,附上基座换算、水平距离与误差计算。你应能说明哪个判断来自几何,哪个来自执行反馈,以及哪些问题仍不在本实验的验证范围里。
最常见的错误是把厘米当米、把相对位移当绝对坐标、忘记基座偏移、把关节角当末端位置,以及用相机画面的左右推断世界坐标正负。遇到奇怪动作,按这几个检查项逐一排查,比直接修改决策提示词更容易找到原因。
练习与一手参考
基础题:手算 q1=0、q2=0 时末端的世界水平位置,并解释为什么该姿态位于伸展边界。挑战题:选择一个几何可达点,分别计算两个逆解,再用正运动学代回,检查它们是否到达同一点。
基础题参考:两段都沿基座 x 正方向,基座坐标为 (0.65, 0),世界坐标为 (0.25, 0)。它已经用完两段连杆的总长度;若答案是 (0.65, 0),算对了连杆伸展,却漏了最后的坐标转换。
现在已经能发出一次有单位、有坐标系、能检查结果的移动。下一章把移动与抓取、释放组合起来,写清每项技能在什么时候可以开始、完成后应该检查什么。
- Kris Hauser:Robotic Systems,逆运动学:两连杆推导、多个解和可行性约束。
- MuJoCo 仿真循环:控制输入与状态推进之间的关系。
- 课程实验源码:教学模型尺寸、坐标约定与移动接口实现。