3. 工业机器人运动学基础

在TwinCAT中集成工业机器人或运动平台时，控制器最终要回答两个问题：给定各关节角度，末端执行器在哪里？给定末端目标位姿，各关节应转到什么角度？前者是正运动学，后者是逆运动学。理解这两类问题的输入输出、坐标系约定以及关节空间与笛卡尔空间的差异，是后续在TwinCAT NC/PTP、Kinematic Transformation或自定义运动学功能块中正确配置参数的前提。

一、坐标系：描述位姿的参照基准

工业机器人通常涉及四类坐标系：基坐标系（Base）、关节坐标系（Joint）、工具坐标系（TCP）和工件坐标系（Work Object）。基坐标系固定在机器人安装底座上，是运动学计算的起点；关节坐标系描述每个旋转或移动关节的变量；工具坐标系定义在末端法兰或工具尖端，决定实际作业点；工件坐标系则把作业对象的位置纳入统一描述。TwinCAT中常通过AXIS_REF与坐标系变换功能块建立这些关系。

位姿由位置（x, y, z）和姿态（通常用欧拉角或四元数表示）共同构成，共6个自由度。姿态的旋转顺序必须明确约定，例如Z-Y-X欧拉角与Z-X-Z欧拉角结果完全不同。在TwinCAT中配置KinTransform或RobotKinematics时，若旋转顺序与机器人厂商定义不一致，即使位置正确，姿态也会出现偏差。

二、正运动学：从关节到末端

正运动学通过各关节变量和连杆参数，逐级计算末端位姿。以常见的6轴串联机器人为例，采用Denavit-Hartenberg（D-H）参数法，每个连杆用4个参数描述：连杆长度a、连杆扭转角α、关节偏距d、关节角θ。相邻坐标系之间的齐次变换矩阵为：

T_i = Rot_z(θ_i) · Trans_z(d_i) · Trans_x(a_i) · Rot_x(α_i)

将6个变换矩阵依次相乘，得到基坐标系到末端坐标系的变换矩阵T_0_6，其前三列表示姿态，第四列表示位置。在TwinCAT中，这一过程可由FB_KinematicsForward或自定义功能块实现，输入为6个关节角度，输出为笛卡尔位姿。

教学示例：假设某6轴机器人D-H参数为a2=0.4m，a3=0.05m，d1=0.3m，d4=0.35m，其余为0。当关节角为(0°, 0°, 90°, 0°, 0°, 0°)时，通过矩阵连乘可估算末端位置约为(0.05m, 0.35m, 0.7m)，姿态为绕Y轴90°。该结果属于示例计算，实际值需以机器人厂商参数为准。

三、逆运动学：从末端反推关节

逆运动学是正运动学的反问题，通常更复杂：可能存在多解、无解或奇异点。6轴串联机器人的逆解一般通过解析法或数值法求解。解析法利用几何关系分离关节变量，例如先由腕部中心位置求出前三个关节，再由姿态矩阵求出后三个关节。数值法（如雅可比迭代）适用于非标准构型，但计算量大且可能不收敛。

在TwinCAT中，逆运动学常由FB_KinematicsInverse或机器人厂商提供的库实现。配置时需注意：目标位姿是否可达、是否接近奇异点、关节限位是否满足。若目标点超出工作空间，功能块会返回错误码，此时应检查目标位置与机器人臂展的关系。

四、关节空间与笛卡尔空间

关节空间以各关节角度为坐标，笛卡尔空间以末端位姿为坐标。两者之间的映射关系正是运动学。在TwinCAT中，运动指令可在两种空间中执行：

• 关节空间运动：直接给定各关节目标角度，如MC_MoveAbsolute作用于各轴，路径简单、计算快，但末端轨迹不可控。
• 笛卡尔空间运动：给定末端目标位姿，由逆运动学解算各关节，如MC_MoveLinearAbsolute，末端走直线，但需实时逆解，计算量大。

选择哪种空间取决于工艺：焊接、涂胶等需要末端走直线或圆弧的场合用笛卡尔空间；快速定位、抓取等对末端路径无严格要求的场合用关节空间。TwinCAT的KinematicTransformation功能块可在两者之间切换，但切换时需保证位姿连续，避免跳变。

五、运动平台自由度与构型

运动平台按自由度可分为3自由度（如XYθ平台）、6自由度（如Stewart平台）等。自由度数目决定平台能实现的独立运动。Stewart平台由6个伸缩杆并联驱动，具有高刚度、高精度特点，但工作空间小、逆解复杂。TwinCAT中控制Stewart平台需使用并联运动学功能块，输入为平台位姿，输出为6个杆长。

构型方面，串联机器人工作空间大、控制简单，但刚度低、误差累积；并联平台刚度高、误差不累积，但工作空间小、运动学复杂。选择构型时需权衡负载、精度、速度和空间。在TwinCAT中，串联机器人常用RobotKinematics库，并联平台则需自定义或使用ParallelKinematics库。

在TwinCAT工程中落地时，建议先确认机器人或平台的D-H参数或厂商运动学模型，再选择对应的功能块。正运动学用于验证模型：给定一组关节角，观察末端位姿是否与示教器一致。逆运动学用于执行：给定目标位姿，检查解算出的关节角是否在限位内、是否接近奇异点。若使用笛卡尔空间指令，还需在MC_MoveLinearAbsolute中设置合理的速度、加速度和加加速度，避免逆解突变导致机械冲击。

对于运动平台，自由度与构型的匹配同样关键。3自由度平台无法实现空间6维位姿，若强行给定6维目标，逆解将无解。TwinCAT中可通过MC_ReadAxisInfo读取各轴状态，结合MC_ReadMotionState判断运动是否完成。实际调试时，先用关节空间低速验证各轴方向与限位，再切换到笛卡尔空间验证末端轨迹，最后逐步提高速度至工艺要求。