4. 位姿表示与坐标系

在URScript中，机器人“在哪里、朝哪个方向”这件事，从来不是单一数值能说清的。它至少涉及两套描述体系：一套是关节空间里的六个角度，另一套是笛卡尔空间里的位置与姿态。理解这两套体系以及它们之间的桥梁——坐标系变换，是编写可靠运动程序的前提。

pose类型：笛卡尔位姿的六元组

URScript用pose类型表示笛卡尔空间中的一个位姿，它是一个包含6个浮点数的数组，顺序固定为[x, y, z, rx, ry, rz]。前三个量是位置，单位毫米；后三个量是姿态，单位弧度，采用旋转矢量（axis-angle）表示法：向量方向代表旋转轴，向量长度代表绕该轴旋转的角度。

例如：

p_home = p[300.0, 0.0, 400.0, 0.0, 3.14159, 0.0]

这表示工具中心点位于基坐标系x=300mm、y=0mm、z=400mm处，姿态为绕y轴旋转约180°。注意旋转矢量不是欧拉角，三个分量不能简单理解为“分别绕x、y、z转多少”。当旋转角度接近π时，旋转矢量的方向会变得敏感，这是姿态插值中需要留意的数值特性。

关节角度：另一种位姿描述

关节角度用joint类型或长度为6的数组表示，单位是弧度，顺序对应基座、肩部、肘部、腕部1、腕部2、腕部3。同一个末端位姿，通常存在多组关节解，因为肘部可以上翻或下翻、腕部可以翻转。URScript中get_inverse_kin(p)返回的是一组逆解，get_forward_kin(q)则把关节角正解为pose。

q_now = get_actual_joint_positions()
p_now = get_forward_kin(q_now)
q_target = get_inverse_kin(p_target, q_near)

第二个参数q_near非常关键：它告诉求解器“从哪组解附近找”，避免机器人突然翻肘或绕腕。实际编程中，应始终用当前关节角作为参考，而不是让求解器自由选择。

工具坐标系与基坐标系

基坐标系固定在机器人底座，原点在底座中心，z轴向上。工具坐标系（TCP）则定义在末端法兰所装工具的工作点上。程序中写的p[x,y,z,...]，默认是TCP在基坐标系下的位姿。如果TCP标定不准，所有绝对定位都会偏移。

URScript提供set_tcp(p)设置当前工具的TCP偏移，get_actual_tcp_pose()返回当前TCP在基坐标系下的位姿。工具坐标系与基坐标系之间的变换关系可以理解为：

p_base = p_flange_pose * p_tcp_offset

其中p_tcp_offset是工具相对法兰的固定变换。当工具更换时，只需更新这个偏移，程序中的目标点无需重写。

坐标系变换的实际操作

假设需要在工件坐标系下沿其x方向移动50mm，而工件坐标系相对基坐标系有一个已知位姿p_work。直接修改基坐标系下的x值是不对的，因为工件可能旋转过。正确做法是构造一个在工件坐标系下的位移pose，再通过pose_trans变换到基坐标系：

p_shift_in_work = p[50.0, 0.0, 0.0, 0.0, 0.0, 0.0]
p_target_base = pose_trans(p_work, p_shift_in_work)
movel(p_target_base)

pose_trans(p1, p2)的含义是：把p2当作在p1坐标系下的位姿，返回它在基坐标系下的等价位姿。与之对应的pose_inv(p)求逆变换。这两个函数是处理多坐标系编程的核心工具。

下面这张图展示了从关节角到笛卡尔位姿、再到工具坐标系与基坐标系变换的完整关系链，以及各环节对应的URScript函数。

姿态插值与旋转矢量注意事项

当用movel做直线运动时，UR控制器会在起点和终点的旋转矢量之间做插值。如果两个姿态的旋转矢量方向差异很大（例如一个接近+π，另一个接近-π），插值路径可能不是最短路径，导致工具绕远路旋转。工程上常用的规避方法是：在示教目标点时，尽量让相邻点的姿态变化平滑；必要时插入中间点，把大角度旋转拆成两段小角度旋转。

另外，get_actual_tcp_pose()返回的旋转矢量在角度接近0时，方向不稳定，但数值很小，对实际运动影响有限；真正需要警惕的是接近π的情况。如果程序中出现工具突然翻转，首先检查目标姿态的旋转矢量是否跨越了π边界。

一个完整的坐标变换示例

假设基坐标系下有一个工件坐标系p_work = p[400, 200, 100, 0, 0, 0]，工具TCP偏移p_tcp = p[0, 0, 150, 0, 0, 0]。现在要在工件坐标系原点上方50mm处、保持工具z轴与工件z轴对齐，执行一个移动。计算过程如下：

p_work = p[400.0, 200.0, 100.0, 0.0, 0.0, 0.0]
p_tcp  = p[0.0, 0.0, 150.0, 0.0, 0.0, 0.0]
set_tcp(p_tcp)

p_target_in_work = p[0.0, 0.0, 50.0, 0.0, 0.0, 0.0]
p_target_base = pose_trans(p_work, p_target_in_work)
movel(p_target_base)

这里p_target_base的结果是p[400, 200, 150, 0, 0, 0]。注意：movel的目标是TCP位姿，而TCP已经通过set_tcp偏移了150mm，所以实际法兰位置会比目标低150mm。如果忘记设置TCP，机器人会以法兰中心去到达这个点，工具尖端就会偏离50mm。这是初学者最常见的坐标系错误之一。

总结来说，关节角度和笛卡尔位姿是同一物理状态的两套坐标；工具坐标系和基坐标系之间的变换决定了程序中的数值到底对应空间中的哪个点。掌握pose_trans、pose_inv、get_forward_kin、get_inverse_kin这四个函数，并始终明确当前参考系，就能把多坐标系编程从“碰运气”变成可计算、可复现的工程操作。