2. 水下机械臂运动学基础
要让ROV机械臂“指哪打哪”，控制器必须先把“关节转了多少度”和“末端在什么位置”这两件事用数学语言联系起来，这就是运动学。它不涉及力、质量与惯性，只研究几何关系，却是轨迹规划、抓取对准、限位保护与手自动切换的共同基础。本节围绕坐标系定义、D-H参数、正运动学与逆运动学四条主线展开，并说明它们在水下作业中的实际意义。
一、坐标系定义：先把“参照物”说清楚
水下机械臂通常涉及四类坐标系，必须逐一定义并在全文保持一致：
- 基座坐标系 {B}：固连于机械臂安装法兰或ROV本体，原点取在肩关节轴线与基座安装面的交点，是整条运动链的起点。
- 连杆坐标系 {i}：每个关节对应一个，z轴沿关节轴线，x轴沿相邻两关节轴线的公垂线方向。
- 工具坐标系 {T}：固连于夹爪或作业工具，原点取在夹爪闭合中心（抓取点），z轴指向夹爪接近方向。
- 世界/目标坐标系 {W}：用于描述作业目标（如阀门手轮、采样篮、对接桩）的位置，通常由视觉或声呐给出。
方向约定必须明确：本文统一采用右手定则，关节角以绕z轴逆时针（从z轴正向看向原点）为正；ROV本体坐标系取x向前、y向左、z向下（水下常用“北东地”式约定）。若视觉系统输出的是“前左上”约定，必须先做一次坐标变换再送入逆解，否则会出现方向反号、越抓越偏的典型故障。
二、D-H参数：用四个数描述一段连杆
Denavit-Hartenberg（D-H）参数法用四个量描述相邻连杆坐标系的相对位姿，标准D-H的四个参数为：
- 连杆长度 ai：沿xi轴，从zi-1到zi的公垂距离；
- 连杆扭角 αi：绕xi轴，从zi-1转到zi的角度；
- 关节偏距 di：沿zi-1轴，从xi-1到xi的距离；
- 关节角 θi：绕zi-1轴，从xi-1转到xi的角度。
对转动关节，θi是变量，其余三个是常量；对移动关节则相反。相邻坐标系的齐次变换为：
T_i = Rot(z, θ_i) · Trans(z, d_i) · Trans(x, a_i) · Rot(x, α_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 ]以一个教学示例的3自由度水下机械臂（肩部俯仰、肘部俯仰、腕部俯仰，均为转动关节）为例，假设参数如下（示例值，非实测）：
| 连杆 i | ai (m) | αi (°) | di (m) | θi 范围 (°) |
|---|---|---|---|---|
| 1 | 0.10 | 90 | 0.15 | -90 ~ +90 |
| 2 | 0.45 | 0 | 0 | -30 ~ +120 |
| 3 | 0.35 | 0 | 0 | -120 ~ +120 |
参数表一旦确定，就相当于给机械臂建立了“身份证”。实际工程中，a与d应来自图纸或标定结果，α由装配关系决定；若装配存在偏差，可用激光跟踪或标定板反求修正，否则正逆解会系统性偏移。
三、正运动学：由关节角算末端位姿
正运动学是“已知各关节角，求末端在基座坐标系下的位姿”，本质是把各连杆变换依次相乘：
T_0T = T_1 · T_2 · T_3 · T_T
示例：θ1=30°, θ2=45°, θ3=-20°，工具偏移沿z_T 0.12 m
按上式逐级相乘，可得末端位置约为
x≈0.52 m, y≈0.30 m, z≈-0.41 m（示例计算结果，用于说明流程）正解永远有唯一结果，计算量小、实时性好，因此控制器内部大量使用正解：每周期根据编码器读数算出末端位姿，用于碰撞检测、工作空间监视和限位判断。水下作业中，正解还用于把“关节空间轨迹”实时映射为“末端空间轨迹”，供操作员在显示界面上观察夹爪是否偏离目标。
四、逆运动学：由目标位姿反求关节角
逆运动学是“已知末端目标位姿，求各关节角”，是抓取控制的核心。它比正解困难得多，原因有三：解可能不存在（目标超出工作空间）、可能多解（肘上/肘下构型）、也可能奇异（雅可比矩阵降秩，关节速度趋于无穷）。
对上述3自由度平面型机械臂，可用几何法求解。设末端在基座坐标系下的位置为 (px, py, pz)，先由第一关节确定偏航角：
θ1 = atan2(p_y, p_x)
再在竖直平面内，令 r = sqrt(p_x² + p_y²) - a1，s = p_z - d1
由余弦定理：
cosθ3 = (r² + s² - a2² - a3²) / (2·a2·a3)
θ3 = ±acos(cosθ3) // 正负对应肘上/肘下两组解
θ2 = atan2(s, r) - atan2(a3·sinθ3, a2 + a3·cosθ3)得到两组解后，需按“关节限位、避障、远离奇异、与上一周期解连续”四条准则择优。工程上常用“就近原则”：选择与当前关节角欧氏距离最小的一组，避免机械臂在两次解算之间突然翻转。
五、在水下作业中的实际意义
运动学不是纸面推导，它直接决定作业成败。典型场景包括：
- 抓取对准：视觉给出目标在 {W} 下的位姿，经手眼标定转换到 {B}，再逆解为关节角序列，夹爪才能沿接近方向合拢；
- 轨迹规划：在关节空间做插值可保证平滑，但末端路径不可控；在笛卡尔空间规划则需高频逆解，二者取舍取决于作业精度要求；
- 奇异规避：当机械臂接近伸直（θ3≈0）时雅可比降秩，微小的末端修正会引起关节角剧烈变化，控制器应限制该区域内的笛卡尔速度；
- 手自动切换：手动模式下操作员直接给关节速度，自动模式下由逆解给出角度目标，切换瞬间必须保证角度连续，否则会出现“跳臂”。
把上述链路落到实操，可以形成一条可复现的检查顺序：先确认D-H参数表与实物一致（必要时标定修正），再用正解验证几个已知关节角下的末端位置是否与卷尺测量吻合，然后在小范围内测试逆解——给定一个可达目标，观察两组解中控制器选了哪一组、切换时关节是否连续，最后在接近奇异位形处降低笛卡尔速度上限。若发现夹爪总是偏向同一侧，优先怀疑坐标系约定不一致或手眼标定矩阵符号错误，而不是反复调整增益。