📚 课程目录第3章 XPlanar坐标系与运动学基础

3. XPlanar坐标系与运动学基础

XPlanar 的路径规划之所以不能只靠“把动子从 A 点挪到 B 点”来理解，根本原因在于：动子既在定子平面上被电磁力驱动，又可能同时发生悬浮高度变化、绕自身轴的旋转以及倾斜。要让规划器、控制器和上位程序对“动子现在在哪里、朝向如何、下一步往哪走”达成一致，就必须先建立统一的坐标系与运动学描述。本节围绕三类坐标系和六自由度变换展开，目标是让你能读懂 XPlanar 的位置数据，并能把一条期望轨迹正确表达成规划器可用的坐标与姿态序列。

三类坐标系的定义与职责

XPlanar 系统中通常涉及三个层次的坐标系，它们各自承担不同职责，不能混用。

全局坐标系（Global / World Frame）：以整个 XPlanar 定子拼接平面为参照的绝对坐标系，通常记为 {W}。它的原点一般取在系统标定后确定的参考点，X、Y 轴位于定子平面内，Z 轴垂直向上。全局坐标系用于描述多个定子模块拼接后的整体布局、动子在不同定子之间的迁移路径，以及上位系统对“整条产线”的统一调度。当系统由多块定子拼接时，各定子的局部坐标必须通过标定统一到全局坐标系下，否则动子跨定子移动时会出现位置跳变。

定子坐标系（Stator Frame）：以单块定子模块为参照的局部坐标系，记为 {S}。每块定子有自己独立的原点与轴向，用于描述该定子内线圈阵列产生的力场分布、动子在该定子上的可达范围以及边缘约束。定子坐标系是电磁力计算和位置检测的直接参照，规划器在单块定子内运动时通常先在定子坐标系中求解，再变换到全局坐标系。

动子坐标系（Mover Frame）：固连在动子本体上的坐标系，记为 {M}。原点通常取动子的几何中心或标定参考点，随动子一起平移和旋转。动子坐标系用于描述动子自身的姿态、负载安装方向以及抓取/放置时工具的相对位置。当动子旋转时，动子坐标系相对全局坐标系发生旋转，而工具在动子坐标系中的位置保持不变。

记忆要点：全局坐标系回答“在整个平面上在哪”，定子坐标系回答“在哪块定子上、离边缘多远”，动子坐标系回答“动子自己朝哪、工具装在哪”。

六自由度运动描述

XPlanar 动子具有六个自由度，可记为 (x, y, z, rx, ry, rz)。其中 x、y 为平面内平移，z 为悬浮高度，rx、ry 为绕 X、Y 轴的倾斜（俯仰与横滚），rz 为绕 Z 轴的偏航旋转。工程上常把前三个称为平动自由度，后三个称为转动自由度。

需要特别注意的是 z 与 rx、ry 的耦合关系。动子悬浮高度由多个线圈共同决定，当动子发生倾斜时，不同测量点的高度不同，控制器需要区分“整体升降”和“倾斜导致的局部高度差”。因此在描述姿态时，z 通常指动子参考点的高度，而 rx、ry 描述相对水平面的倾斜角。若把倾斜误当作高度变化处理，会导致悬浮不稳或碰撞。

rz 旋转对路径规划影响最直接：动子旋转后，其外形轮廓在全局坐标系中的投影发生变化，原本可通过的通道可能变窄。规划器必须结合动子外形尺寸和当前 rz 判断是否与相邻动子或定子边缘干涉。

坐标变换：从动子到全局

设动子在全局坐标系中的位姿为 (x, y, z, rx, ry, rz)，动子坐标系中某点 p_M 要变换到全局坐标系，标准做法是先旋转再平移。以最常见的平面内 rz 旋转为例，忽略倾斜时：

p_W = R_z(rz) · p_M + T
其中 T = [x, y, z]^T
R_z(rz) = [ cos(rz)  -sin(rz)  0
            sin(rz)   cos(rz)  0
            0         0        1 ]

若同时考虑 rx、ry，则旋转矩阵为 R = R_z(rz)·R_y(ry)·R_x(rx)，具体顺序必须与控制器约定一致。XPlanar 中通常采用先绕自身轴旋转、再平移的约定，工程实现时应以 TwinCAT 中实际使用的变换顺序为准，不可自行假设。

反向变换用于把全局坐标系中的目标点转换到动子坐标系，例如抓取工件时已知工件在全局坐标系中的位置，需要求出动子应到达的位姿。反向变换为：

p_M = R^T · (p_W - T)

由于旋转矩阵是正交矩阵，其逆等于转置，这也是工程上常用 R^T 而非求逆的原因。

教学示例：一次带旋转的搬运位姿计算

假设某动子当前在全局坐标系中的位姿为 x=420.0 mm、y=180.0 mm、z=1.2 mm、rz=30°，动子上工具安装点在动子坐标系中为 p_M=(60.0, 0.0, 0.0) mm。求该工具安装点在全局坐标系中的位置（示例数据，用于说明计算方法，非实测结果）。

cos30° ≈ 0.8660, sin30° = 0.5000
R_z(30°)·p_M = [0.8660×60 - 0.5×0, 0.5×60 + 0.8660×0, 0]
             = [51.96, 30.00, 0]
p_W = [51.96 + 420.0, 30.00 + 180.0, 0 + 1.2]
    = [471.96, 210.00, 1.20] mm

可以看到，动子仅旋转 30°，工具点就偏离了动子中心约 60 mm 的旋转投影。若规划器忽略 rz 而直接把工具点当作动子中心处理，抓取位置将产生约 52 mm 的横向误差，足以导致抓取失败。这也说明为什么路径规划必须把姿态与位置一起规划，而不能只规划平移轨迹。

工程实现中的关键约定

在 TwinCAT 中配置 XPlanar 时，坐标与姿态数据通常以数组或结构体形式传递。以下为示例性的位姿结构定义，用于说明字段含义，实际类型名以工程为准：

TYPE ST_XPlanarPose :
STRUCT
    x  : LREAL;   // mm，全局坐标系
    y  : LREAL;   // mm，全局坐标系
    z  : LREAL;   // mm，悬浮高度
    rx : LREAL;   // deg，绕 X 轴倾斜
    ry : LREAL;   // deg，绕 Y 轴倾斜
    rz : LREAL;   // deg，绕 Z 轴偏航
END_STRUCT
END_TYPE

规划轨迹时，通常需要给出位姿序列而非单点。例如从 P1 到 P2 的搬运，若要求动子在途中完成 90° 旋转，则应在轨迹中插入若干中间位姿，使 rz 平滑过渡。若只在终点设置 rz，控制器可能在最后阶段突然旋转，造成工件甩动或与相邻动子干涉。

常见错误：把定子坐标系下的坐标直接当作全局坐标使用。多定子拼接时，各定子原点不同，必须先通过标定矩阵变换到全局坐标系，否则动子跨定子时位置会突变。

结果判读与验证方法

完成坐标配置后，可通过以下方式判断坐标系与变换是否正确：

让动子移动到全局坐标系中一个已知点，观察实际停止位置与目标位置的偏差，偏差应在标定精度范围内。
给定 rz=90°，检查工具点在全局坐标系中的投影是否与理论计算一致，用于验证旋转方向与旋转顺序。
让动子从定子 S1 迁移到 S2，观察过渡区域是否出现位置跳变或速度突变，判断全局坐标系标定是否统一。
检查 z 与 rx、ry 的耦合：人为设置小角度倾斜，观察高度读数是否被误判为整体升降。

只有坐标系定义一致、变换顺序明确、姿态与位置同步规划，后续的路径插补、避障和抓取放置才能建立在可靠的运动学基础之上。