SO-101
课程总览/第 03 章 · 末端位姿
EN
第 03 章运动学约 15 分钟

末端位姿

配套页面:控制台上的「末端位姿」页签。

9 个小节 2 张图 3 段代码 位姿坐标系路径
末端位姿页

1. 位姿 = 位置 + 姿态#

位置说的是夹爪指尖那个点(TCP)在空间里的坐标;姿态说的是夹爪以什么角度朝过去。两者合起来才叫一个"位姿"。

它和关节角是两个层面的东西:

描述的是 谁在管
六个关节角 手臂怎么摆的 舵机,是唯一真相
末端位姿 末端在哪、朝哪 从关节角正解算出来的

关节角是底层状态,位姿是上层表达。你给一个位姿,SDK 用逆解把它翻译成关节角;反过来,关节角经过正解就得到位姿。这也解释了为什么"位姿对了"不代表"姿态唯一"—— 同一个位姿可能对应不同的手臂构型(肘上 / 肘下)。

2. 五个分量#

Pose 就是这五个数,单位是 mm 和度:

分量 是什么 说明
x 左右位置 基座系,向左为正
y 前后位置 向后为正;手臂伸出时是负值
z 高度 向上为正;桌面在 30.1 mm,不是 0
pitch 进近角 夹爪指向与水平面的夹角,90 = 竖直向下
roll 绕进近轴的转角 直接等于 wrist_roll 关节角

前三个定"在哪",后两个定"怎么朝向"。

3. 自由度:为什么只有 5 个数#

要把一个刚体在空间里完全定死,需要 6 个自由度:3 个位置 + 3 个姿态(roll / pitch / yaw)。这台臂的末端只有 5 个——少的那一个是 yaw。

分量 能不能控 为什么
x / y / z 能 由底座旋转、大臂、肘部共同决定
pitch 能 由 wrist_flex 给
roll 能 由 wrist_roll 直接给
yaw 不能 手臂指向某个点后,水平朝向就唯一确定了

换句话说:位置 3 个自由度 + 姿态 2 个自由度 = 5。这也是为什么 Pose 只有五个字段,而不是六个。

少一个 yaw 意味着什么#

手臂指向某个点之后,进近方向在水平面上的投影就定死了"从哪个方位靠近"。你没法在保持位置的同时,让夹爪换个水平朝向。

实际后果:

  • 想让夹爪换个水平朝向,只能改 x / y —— 位置跟着变
  • pitch=90(竖直向下)时,进近方向的水平投影退化成零,yaw 连"是什么"都谈不上
  • 有些位姿"看着能抓",实际是 5 自由度到不了的角度

求解器是怎么处理的#

逆解只解 4 个未知数:shoulder_pan / shoulder_lift / elbow_flex / wrist_flex,对 4 个残差(x, y, z, 姿态)。wrist_roll 不由求解器决定,直接取目标 roll。

这 4 个未知数怎么迭代、两个求解器("lm" 与 "riemann")差在哪儿、为什么 pitch = 90 是个敏感位置,都在 第 4 章:FK 与 IK 的第 4 节。

4. 这些数字相对哪儿:基座系#

界面里的 x / y / z 是 基座坐标系——URDF 里 base 这个 link 的坐标。原点是 base link 的原点,它在桌面下方 30.1 mm,不在桌面上。

轴 基座系指向
+X 向左
+Y 向后(所以手臂伸出时 y 是负的)
+Z 向上

零位时手臂沿 −Y 伸出去,TCP 落在 (20.4, −381.1, 256.8) mm。

三维视图里画了这组轴:白点是原点,红色 +X(左)、绿色 +Y(后)、蓝色 +Z(上)。转动视角时它们跟着转,可以随时确认"现在看到的这个方向对应哪个轴"。

桌面不在 z = 0。URDF 的 base 原点比桌面低 30.1 mm(table_z_mm 默认值),所以桌面是 z ≈ 30.1 mm。界面上那行"地面 = 桌面(基座系 z = 30.1 mm)"就是提醒这个。换句话说:z 填 0 意味着把末端放到桌面以下 30 mm,安全层会拦。

怎么确认桌面高度#

桌面高度不是永远 30.1 mm。不同桌子、垫板、底座安装高度都会变;z 是基座系,所以最可靠的办法是用真机 TCP 去“碰”一下桌面,而不是照抄文档里的数字。

确认桌面高度:① 示教把 TCP 贴到桌面,② 用当前末端填入,③ 保存

界面路径:动作区「示教」→ 遥测区「桌面高度」→「用当前末端」→「保存」。

  1. 连接真机,确认工作区清空、急停可触达,用手扶住机械臂。
  2. 点动作区的 「示教」(顶部出现「示教模式」横幅,力矩释放);扶着 TCP 慢慢下降,直到 TCP 刚好接触桌面。
  3. 保持 TCP 不动,在「遥测」区找到 「桌面高度」 写入框,点右侧 「用当前末端」(也就是“用当前末端填入”)——它会把当前 TCP 的 z 填进去。
  4. 点 「保存」;提示保存成功后,右侧「禁入线 z ≥ …」会按新桌面更新。
  5. 点 「完成示教」 按钮上力;之后坐标解释和安全层都以这张桌子的高度为准。

注意:

  • 是 TCP 碰桌面,不是夹爪外壳;下降要慢,接触即可,不要压桌面。
  • 示教时没有抱闸,必须一直扶住手臂;不要在手离开时保持示教状态。
  • 换桌子、加垫板、改底座安装高度,都要重新确认并保存;不要沿用上一次的值。
  • 这一步只是告诉安全层“桌面在哪”,不是关闭安全区;急停、过温、跟踪误差仍然生效。

TCP 是哪个点#

界面上的 X/Y/Z 是 TCP——gripper 连杆上的固定指尖接触点(不是两指中点),也不是夹爪的转动中心:

基座系坐标 (mm)
夹爪转动中心 (20.6, −277.5, 266.9)
TCP (20.4, −381.1, 256.8)
相距 104.1 mm

换夹爪或加装工具,这个偏移就要重新标定:

toml
[kinematics]
tcp_offset_mm = [-10.1, -0.2, -103.6]

pitch 和 roll 量的是什么#

分量 定义 实测
pitch 进近方向与水平面的夹角,90 = 竖直向下 wrist_flex=45° → pitch=45,进近 (0, −0.71, −0.71)
roll 绕进近轴的转角,直接等于 wrist_roll 关节角 wrist_roll=90° → roll=90,进近方向不变

pitch=90 时进近方向是 (0, 0, −1),此时手臂最高只到桌面上方约 80 mm。

一个容易忽略的细节:转动 wrist_roll 会让 TCP 动一点点——因为 TCP 相对腕部轴线有约 10 mm 的横向偏移(tcp_offset 的 x 分量是 −10.1 mm)。所以"腕滚不影响末端位置"只是近似。

脚本里也一样#

starm 这套 SDK 对外就是基座系,和界面完全一致:

python
with Arm.sim() as arm:
    arm.move_pose(y=-200, z=90, pitch=60)   # 基座系 mm
    print(arm.get_pose())                    # Pose(x=…, y=…, z=…, pitch=…, roll=…)

不需要记第二套坐标。唯一要记住的还是那句:桌面在 z ≈ 30.1 mm,不是 0。

互动实验

位姿读数器

基座系 · mm / °

把五个数填进去,看看它在基座系里到底是什么意思。注意 +Y 向后、−Y 向前,桌面在 z = 30.1 mm,不是 0。

离底座—mm
离桌面—mm
进近角—°
侧视图只画 Y–Z 平面;X 分量在右下角的俯视图里。红色箭头是进近方向,不是夹爪两指的方向。

5. 怎么读一组数#

界面上出现 (0, −260, 150, 65, 0) 时,逐项念:

分量 值 读作
x 0 在底座正中间,不偏左也不偏右
y −260 在底座前方 260 mm
z 150 离底座原点 150 mm,也就是离桌面约 120 mm
pitch 65 夹爪斜向下 65°(90° 才是竖直向下)
roll 0 夹爪绕自身轴没转

6. 两种路径:joint 和 line#

填好五个数、点「移动」之前,还有一个选项决定"怎么走过去"。它不改变终点,只改变中间的过程。

joint line
做法 只在起点算一次逆解,然后六个关节角各自插值 TCP 沿空间直线走,每个控制周期(30 Hz)重算一次逆解
TCP 轨迹 弧线 直线
速度 快 慢 15–20%

实测(从零位出发,只改路径):

目标 弦长 joint 偏离直线 joint 时长 line 偏离直线 line 时长
前方低位 163 mm 26.2 mm 5.00 s 0.0 mm 5.85 s
左侧 247 mm 41.1 mm 4.51 s 0.0 mm 5.44 s

joint 为什么会画弧:它插值的是关节角,而末端位置是这些角度的非线性函数。六个角各自平滑变化,末端自然走曲线。行程越长、越靠近工作空间边缘,弧得越厉害。

joint#

  • 优点:快,规划简单;大范围转移、回零最合适
  • 风险:弧线会扫过中间的空间。贴着桌面横移时,弧可能扎进桌面

line#

  • 优点:TCP 轨迹可预测,适合下探抓取、贴面平移、避障
  • 代价:每个周期都要解一次逆解(更吃 CPU),而且更慢——它要按路径上最陡的那一段关节速率配速,否则某个关节会顶到速度上限,把 TCP 拽离直线
  • 额外约束:求解时禁止"换构型"(allow_reseed=False)。否则中途换一个肘上 / 肘下解,手臂会突然横扫过去

一个内置的安全网#

line 在动作开始之前会沿直线采样 32 个点逐点试解。任何一点无解,就在动之前报错:

text
Unreachable: straight-line path leaves the workspace 43% of the way along it: ...

怎么选#

场景 用哪个
从 A 点大范围挪到 B 点、回零 joint
垂直下探去抓 / 放下 line
贴着桌面平移 line
只改 roll(夹爪自转) 都行

7. 在页面上动手#

打开「末端位姿」页签:

  1. 点「取当前」 —— 把输入框填成机械臂此刻的位姿
  2. 改一个分量,点「IK 预检」 —— 只算不动,看它够不够得着
  3. 点「移动」 —— 真的走过去
  4. 切 joint / line 路径再走一次 —— 同样的起点终点,joint 让 TCP 走弧线、line 走直线,轨迹完全不同。看第 6 节了解两者的取舍
  5. 把速度调下来再走一次 —— 速度滑块在「末端位姿」和「关节控制」两处都有,改一处另一处跟着变。它是"安全层允许的最大关节速度(默认 60 °/s)"的比例:0.4 就是 24 °/s,0.1 只剩 6 °/s。真机上先按 0.15 试,看着不慌了再加

两个值得做的实验:

  • 把 z 填 20(比桌面 30.1 还低)→ 安全层直接拒绝,SafetyError
  • 把 z 填 230、pitch 填 90,点「IK 预检」→ Unreachable,因为竖直向下时最高只到桌面上方 80 mm 左右

8. 常见误区#

说法 对不对 说明
"位姿就是关节角" ✗ 关节角是底层状态,位姿是它正解出来的结果
"z = 0 是桌面" ✗ 桌面在 30.1 mm;z=0 在桌面以下
"5 个数字就是 5 自由度,所以和 6 轴臂只差一点" ✗ 差的 yaw 恰恰是"任意朝向"的关键
"转腕滚不影响末端位置" ≈ TCP 有约 10 mm 横向偏移,会画小圆
"移动到了就是夹爪到了" ✗ 到的是 TCP(固定指尖接触点),比夹爪转动中心还往前 104 mm

参考#

  • 第 2 章:机械臂关节 —— 五个关节分别管什么
  • 代码里:starm/types.py 的 Pose、starm/kinematics.py 的 pose() / ik()