2. MOTOMAN机器人硬件与运动学基础

要让MOTOMAN机器人“看见并抓住”目标，第一步不是写视觉算法，而是把机器人的机械本体、控制接口和运动学模型这三件事对齐。视觉给出的是相机坐标系下的目标位姿，机器人执行的是各关节角度；两者之间必须通过一套明确的坐标系约定和运动学关系连接起来。本节围绕这条链路展开：先认识本体结构与控制柜接口，再建立坐标系与DH参数，最后落到正逆运动学的求解与验证。

一、本体结构与控制柜接口

以常见的6轴垂直多关节MOTOMAN（如GP系列、MH系列）为例，本体由基座、S轴（回转）、L轴（下臂）、U轴（上臂）、R轴（腕部回转）、B轴（腕部摆动）、T轴（腕部扭转）依次串联。S、L、U三轴决定手腕参考点（腕心）的位置，R、B、T三轴决定末端工具的姿态，这种“前三轴定位、后三轴定向”的分工是后续逆解能解析求解的结构基础。各轴由伺服电机经减速机驱动，编码器提供关节角反馈，部分机型在腕部还带有力矩传感器接口。

控制柜（如YRC1000、DX200）是机器人与外部世界的交汇点。工程上最常打交道的接口包括：

• 示教器接口：用于手动示教、点位记录、程序编辑与报警查看，是标定和调试的第一入口。
• I/O接口：通用数字输入/输出用于抓取信号、夹具开合、安全门等；专用I/O常用于伺服电源、启动、暂停等时序控制。
• 通信接口：以太网口支持与上位机（视觉系统）通信，常见方式有TCP/IP套接字、EtherNet/IP、PROFINET等；视觉引导通常由上位机把目标位姿通过通信下发，或由控制柜主动请求。
• 编码器/伺服接口：连接本体各轴，传输位置与速度指令及反馈。

理解接口的意义在于：视觉引导系统最终要落到“谁把目标位姿传给谁、以什么坐标和单位传”。通常约定位置单位为毫米、姿态用欧拉角或四元数，且必须明确是相对哪个坐标系（基座、法兰还是工具）。

二、坐标系定义

MOTOMAN工程中至少涉及以下坐标系，必须逐一明确并保持一致：

• 基坐标系（Base）：固定在本体安装面，是机器人运动的绝对参考，逆解结果通常相对它给出。
• 关节坐标系：每个轴绕自身轴线转动，正方向由右手定则确定，是伺服实际控制的量。
• 法兰坐标系（Flange）：位于R/B/T三轴交点（腕心）或法兰安装面，是工具安装的基准。
• 工具坐标系（TCP）：定义在夹爪或吸盘的作用点，抓取时真正要对准目标的是TCP而非法兰。
• 用户坐标系：为方便现场作业在工件或工装上建立的局部坐标系，视觉目标常在此描述。
• 相机坐标系：视觉输出的目标位姿所在坐标系，需通过手眼标定转换到机器人可用的坐标系。

方向与正负号约定必须统一：例如规定绕Z轴逆时针为正、角度单位为度或弧度、欧拉角旋转顺序为Z-Y-X。若中途切换参照系而不声明，逆解结果会出现系统性偏差，这是现场最常见的“抓偏”原因之一。

三、DH参数与正运动学

标准DH（Denavit-Hartenberg）用四个参数描述相邻连杆关系：连杆长度a、连杆扭角α、关节偏距d、关节角θ。对转动关节，θ为变量，其余为常量。相邻两坐标系变换矩阵为：

T_i = RotZ(θ_i) · TransZ(d_i) · TransX(a_i) · RotX(α_i)

展开为4×4齐次矩阵：
[ cosθ  -sinθcosα   sinθsinα   a·cosθ ]
[ sinθ   cosθcosα  -cosθsinα   a·sinθ ]
[  0        sinα       cosα        d   ]
[  0         0          0          1   ]

正运动学就是把6个这样的矩阵依次相乘：T_0_6 = T_1·T_2·T_3·T_4·T_5·T_6，得到末端相对基座的位姿。以某6轴MOTOMAN的示例DH参数（单位：mm、度，仅作教学示例）为例：

给定一组关节角，例如θ=[0°, 30°, -20°, 0°, 45°, 0°]，按上式逐级相乘即可算出TCP位置与姿态。实际工程中不必手算，可用控制柜的FORWARD函数或上位机库（如ROS的KDL、Python的roboticstoolbox）验证。正运动学是“已知关节求位姿”，用于验证模型、生成轨迹和检查可达性。

四、逆运动学求解与验证

逆运动学是“已知目标位姿求关节角”，是视觉抓取的核心。6轴串联机器人通常存在多解（最多8组），需要按关节限位、避障、姿态连续性筛选。解析法思路是：先由腕心位置解出前三个关节，再由姿态解出后三个关节。以腕心位置p_w为例，先求θ1：

θ1 = atan2(p_wy, p_wx)  或  θ1 = θ1 + π（对应另一组解）

再由几何关系解θ2、θ3：
设水平距离 r = sqrt(p_wx² + p_wy²) - a1
D = (r² + p_wz² - a2² - a3²) / (2·a2·a3)
θ3 = atan2(±sqrt(1-D²), D)
θ2 = atan2(p_wz, r) - atan2(a3·sinθ3, a2 + a3·cosθ3)

后三轴由目标姿态矩阵R与前三轴旋转矩阵的乘积解出，常用Z-Y-X欧拉角反解得到θ4、θ5、θ6。得到多组解后，按以下顺序筛选：①各关节是否在限位内；②是否与工件/夹具干涉；③与当前关节角的差值是否最小（保证运动平滑）；④是否满足奇异位形回避（如θ5接近0时腕部奇异）。

下面用一张信号链与坐标系关系图，把从视觉目标到关节角的完整数据流和坐标变换关系串起来，便于对照理解各环节的输入输出与验证点。

把这张图与前面的公式对照，可以看到视觉引导的完整闭环：相机给出目标位姿→手眼标定转换到基坐标系→补偿TCP偏移→逆运动学求关节角→下发控制柜→正运动学回代验证。任何一个环节的坐标系或参数约定出错，都会在最后的误差判据上暴露出来。实际调试时，建议先用示教器手动把TCP移到已知点，记录关节角并回代正运动学，确认模型无误后再接入视觉数据，这样能把“模型问题”和“标定问题”分开排查。