运动学系统
运动学 (Kinematics) 是机器人控制的数学基础 — 它描述关节空间与笛卡尔空间之间的映射关系, 不涉及力 / 力矩。Darra 内置运动学引擎支持 11 种运动学模型, 覆盖串联、并联、混合三大类。
运动学模型总览
| 序号 | 类型 | 名称 | 典型轴数 | 逆解方式 | 典型应用 |
|---|---|---|---|---|---|
| 1 | 串联 | 6 轴关节臂 (球腕) | 6 | 闭解 + 多组解 | 搬运、焊接、装配 |
| 2 | 串联 | 6 轴关节臂 (偏置腕) | 6 | 数值迭代 | 弧焊、激光切割 |
| 3 | 串联 | SCARA | 4 | 闭解 | 高速取放、锁螺丝 |
| 4 | 并联 | Delta 3 轴 | 3 | 闭解 (IK) / 数值 (FK) | 高速分拣、包装 |
| 5 | 并联 | Delta 4 轴 (1 旋转) | 4 | 闭解 + 旋转独立 | 带旋转分拣 |
| 6 | 并联 | Stewart 平台 | 6 | 闭解 (IK) / 数值 (FK) | 运动模拟、精密定位 |
| 7 | 串联 | 7 轴冗余臂 | 7 | DLS 数值解 | 狭小空间避障 |
| 8 | 串联 | 龙门 / 桁架 | 3 | 直接映射 (线性) | 大范围搬运 |
| 9 | 混合 | 人形机器人上半身 | 7~8 | 数值解 | 服务 / 协作场景 |
| 10 | 并联 | 双四杆平行四边形 | 5 | 闭解 + 连杆约束 | 高速码垛 |
| 11 | 串联 | 协作机器人 (偏置腕) | 6~7 | 数值解 + 力约束 | 人机协作、力控 |
选型指导
- 串联 (Serial): 工作空间大、灵活性高; 刚性低、末端误差累积。适合大范围、复杂姿态作业。
- 并联 (Parallel): 刚性高、末端精度高、速度快; 工作空间小。适合高速分拣、精密定位。
- 混合 (Hybrid): 串联 + 并联组合, 兼顾工作空间和刚性。多见于协作 / 特殊工艺场景。
正运动学 (Forward Kinematics)
正运动学 FK 输入关节角度 q = [q₁, q₂, ..., qₙ], 输出末端位姿 T = [R|t] (4×4 齐次变换矩阵)。
齐次变换链
对 n 轴串联机器人, FK 是每个关节齐次变换矩阵的连乘:
T_0ⁿ = T_0¹(q₁) · T_1²(q₂) · ... · T_{n-1}ⁿ(qₙ)
其中:
T_{i-1}ⁱ = Rot(z, θᵢ) · Trans(z, dᵢ) · Trans(x, aᵢ) · Rot(x, αᵢ)
Darra 使用修正 DH (Modified DH) 约定 (Craig 1986), 链乘顺序与标准 DH 不同 — 变换附在连杆远端, 便于参数辨识和标定。
示例: 6 轴球腕 FK
FUNCTION FwdKin6Axis : ROBOT_POSE
VAR_INPUT
Q : ARRAY[1..6] OF LREAL; // 关节角 [rad]
DH : DH_PARAMS_6; // DH 参数表
END_VAR
VAR
T01, T12, T23, T34, T45, T56 : MATRIX_4X4;
T06 : MATRIX_4X4;
END_VAR
// 逐关节变换
T01 := DHTransform(A := DH.a1, Alpha := DH.alpha1, D := DH.d1, Theta := Q[1] + DH.theta_offset1);
T12 := DHTransform(A := DH.a2, Alpha := DH.alpha2, D := DH.d2, Theta := Q[2] + DH.theta_offset2);
T23 := DHTransform(A := DH.a3, Alpha := DH.alpha3, D := DH.d3, Theta := Q[3] + DH.theta_offset3);
T34 := DHTransform(A := DH.a4, Alpha := DH.alpha4, D := DH.d4, Theta := Q[4] + DH.theta_offset4);
T45 := DHTransform(A := DH.a5, Alpha := DH.alpha5, D := DH.d5, Theta := Q[5] + DH.theta_offset5);
T56 := DHTransform(A := DH.a6, Alpha := DH.alpha6, D := DH.d6, Theta := Q[6] + DH.theta_offset6);
// 齐次变换链
T06 := T01 * T12 * T23 * T34 * T45 * T56;
// 提取位置 + 旋转 (RPY / 四元数)
FwdKin6Axis.X := T06[1,4];
FwdKin6Axis.Y := T06[2,4];
FwdKin6Axis.Z := T06[3,4];
FwdKin6Axis.A := ATAN2(T06[2,1], T06[1,1]); // Rz
FwdKin6Axis.B := ATAN2(-T06[3,1], SQRT(T06[3,2]² + T06[3,3]²)); // Ry
FwdKin6Axis.C := ATAN2(T06[3,2], T06[3,3]); // Rx
系统级接口:
VAR
FK_Result : ROBOT_POSE;
END_VAR
// 调用 Darra 内置 FK
FK_Result := RobotFK(Robot := Robot1, Joints := CurrentJoints, Coord := COORD_WORLD);
逆运动学 (Inverse Kinematics)
逆运动学 IK 输入末端位姿 T, 输出关节角度 q。这是运动学中最核心、最复杂的问题。
闭解 (Analytical / Closed-form)
对满足 Pieper 准则 (三个相邻关节轴交于一点) 的机器人 — 即球腕 6 轴臂 — 存在解析闭解。球腕结构使 J4/J5/J6 可独立解耦, 前 3 轴决定腕心位置, 后 3 轴决定姿态。
多组解: 闭解 IK 通常返回 2~8 组关节角解, 系统按以下优先级选出最优解:
| 优先级 | 判据 | 说明 |
|---|---|---|
| 1 | 关节限位内 | 超出限位的解直接排除 |
| 2 | 最小关节行程 | 距当前位姿关节移动最小 (加权) |
| 3 | 避奇异 | 排除操纵度 < 阈值 的解 |
| 4 | 避碰撞 | 排除自碰撞 / 环境碰撞的解 |
| 5 | 姿态偏好 | 保持手腕在约定侧 (左/右/翻转/非翻转) |
示例 — 指定解的选择策略:
VAR
Solutions : ARRAY[1..8] OF JOINT_POSITION;
BestSol : JOINT_POSITION;
Count : INT;
END_VAR
// 计算所有解
Count := IK_6Axis(Pose := TargetPose, DH := RobotDH, Solutions := Solutions);
// 选择最优解 (加权最小行程 + 姿态偏好)
BestSol := SelectBestSolution(
Solutions := Solutions,
Count := Count,
Current := CurrentJoints,
PreferFlip := FALSE, // 非翻转手腕优先
AvoidSing := TRUE
);
数值解 (Numerical IK)
对非球腕 / 偏置腕 / 7 轴冗余机器人, 使用数值迭代法。Darra 默认采用阻尼最小二乘 (DLS, Damped Least Squares):
qₖ₊₁ = qₖ + J^T(qₖ) · (J(qₖ) · J^T(qₖ) + λ² · I)⁻¹ · e
其中 e = TargetPose - CurrentPose 是笛卡尔误差, λ 是阻尼系数。DLS 在奇异点附近自动加阻尼, 保证数值稳定性。
收敛判据 (任一满足即停止):
| 判据 | 阈值 | 说明 |
|---|---|---|
| 位置误差 | < 0.01 mm | TCP 位置偏差 |
| 姿态误差 | < 0.01° | TCP 姿态偏差 |
| 迭代次数 | > 200 | 防死循环, 返回次优解 |
| 停滞检测 | < 1e-6 变化 | 连续 5 次迭代改进量不足 |
VAR
IK_Result : JOINT_POSITION;
Success : BOOL;
IterCount : INT;
END_VAR
// 调用数值 IK
IK_Result := RobotIK_Num(
Robot := Robot1,
Pose := TargetPose,
Success => Success,
IterCnt => IterCount
);
7 轴冗余 IK
7 轴机器人有一个冗余自由度 — 肘部可在保持 TCP 不变的情况下绕肩-腕轴旋转。Darra 用肘角参数化解决:
q = IK_7Axis(Pose, ElbowAngle)
冗余 IK 流程:
- 用户指定肘角
θ_e ∈ [-180°, 180°](或自动模式) - 在肘角固定下, 前 3 轴有唯一闭解
- 后 3 轴 (偏置腕) 用数值解
- 若当前肘角无解, 系统在肘角 ±10° 范围内搜索
自动模式: 系统每周期评估候选肘角集, 选择使可操作度最大 + 关节限位裕度最大的肘角。
// 自动肘角
MoveJ(Target := TargetPose, RedundancyMode := REDUNDANCY_AUTO);
// 指定肘角 (示教时锁定肘部位置)
MoveJ(Target := TargetPose, RedundancyMode := REDUNDANCY_MANUAL, ElbowAngle := 15.0);
DH 参数表
Darra 使用修正 DH (Modified DH) 约定, 每个关节由 4 个参数描述:
| 参数 | 符号 | 单位 | 物理意义 |
|---|---|---|---|
| αᵢ | alpha | deg | 连杆扭转角 (Zᵢ₋₁ → Zᵢ 绕 Xᵢ) |
| aᵢ | a | mm | 连杆长度 (Zᵢ₋₁ → Zᵢ 沿 Xᵢ) |
| dᵢ | d | mm | 连杆偏距 (Xᵢ₋₁ → Xᵢ 沿 Zᵢ₋₁) |
| θᵢ | theta | deg | 关节角 (Xᵢ₋₁ → Xᵢ 绕 Zᵢ₋₁) |
示例: 典型 6 轴工业臂 DH 表
| Joint | α (deg) | a (mm) | d (mm) | θ_offset (deg) |
|---|---|---|---|---|
| J1 | 0 | 0 | 350 | 0 |
| J2 | -90 | 50 | 0 | -90 |
| J3 | 0 | 600 | 0 | 0 |
| J4 | -90 | 150 | 650 | 0 |
| J5 | 90 | 0 | 0 | 0 |
| J6 | -90 | 0 | 100 | 0 |
SCARA DH 表 (4 轴)
| Joint | α (deg) | a (mm) | d (mm) | θ_offset (deg) |
|---|---|---|---|---|
| J1 | 0 | 300 | 400 | 0 |
| J2 | 0 | 250 | 0 | 0 |
| J3 (Z) | 180 | 0 | var | 0 |
| J4 | 0 | 0 | 0 | 0 |
Delta 参数
Delta 由静平台半径 R、动平台半径 r、主动臂长度 L₁、从动臂长度 L₂ 四个结构参数定义:
| 参数 | 典型值 | 说明 |
|---|---|---|
| R | 200 mm | 静平台铰点分布圆半径 |
| r | 50 mm | 动平台铰点分布圆半径 |
| L₁ | 300 mm | 主动臂长度 |
| L₂ | 800 mm | 从动臂 (平行四边形) 长度 |
Delta IK 有闭解: 给定动平台中心 (x, y, z), 可直接解出 3 个主动臂角。FK 则需解 3 个球面方程交会, 用 Newton-Raphson 迭代。
工作空间分析
工作空间 (Workspace) 是机器人末端可达的所有位姿集合。Darra 提供三种工作空间分析方式。
1. 可达工作空间 (Reachable WS)
仅考虑位置, 不考虑姿态。生成方法:
- 关节空间蒙特卡洛采样: 在关节限位内均匀随机采样 N=100,000 组
- 对每组 FK → 得到末端位置
- 在 3D 空间生成点云 (图 1)
- 用 alpha-shape 计算凸包 / 凹包
3D 视口: 工具栏 → 工作空间 → 勾选 "可达空间"。
2. 灵巧工作空间 (Dexterous WS)
末端能以所有姿态到达的位姿集合 — 比可达空间小得多。生成方法:
- 对每个位置点, 在姿态球上均匀采样 ~500 个姿态
- 对每个姿态跑 IK
- 如果全部姿态都有解 → 该点属于灵巧空间
- 否则标记为 "部分灵巧" (附姿态自由度缺失方向)
典型比例: 灵巧空间体积 ≈ 可达空间体积的 15% ~ 30% (球腕 6 轴)。
3. 工作空间云图
| 可视化类型 | 颜色 | 含义 |
|---|---|---|
| 可达空间 | 蓝色点云 | 所有可达位置 |
| 灵巧空间 | 绿色点云 | 全姿态可达位置 |
| 部分灵巧 | 黄 → 红色渐变 | 姿态自由度受限程度 (红 = 最受限) |
| 奇异边界 | 红色半透明壳 | 奇异位姿表面 |
| 不可达 | 透明 / 空白 | 完全不可达 |
// SCL 中查询工作空间
VAR
wsVolume : LREAL; // 可达空间体积 [m³]
dexVolume : LREAL; // 灵巧空间体积 [m³]
wsRatio : LREAL; // 灵巧比 = dexVolume / wsVolume
END_VAR
GetWorkspaceVolume(Robot := Robot1, Volume => wsVolume);
GetDexterousVolume(Robot := Robot1, Volume => dexVolume);
wsRatio := dexVolume / wsVolume;
IF wsRatio < 0.1 THEN
Log.Warn('Dexterous workspace very small (%.1f%%), check tool orientation', wsRatio * 100);
END_IF;
奇异点处理
详见 奇异点处理 专页, 此处补充运动学角度的关键约束。
雅可比矩阵奇异性
奇异点的数学本质是雅可比矩阵秩亏:
det(J · J^T) = 0 (或可操作度 manip = 0)
此时 J⁻¹ 不存在 → IK 无唯一解 → 某关节速度趋无穷。
运动学层面的处理
- 闭解 IK 的多组解筛选: 奇异点附近的多组解可能收缩为 1 组 → 选解时优先选可操作度最大的解。
- 数值 IK 的 DLS: 阻尼项 λ 保证 J·J^T + λ²·I 可逆 → 即使奇异也有平滑的次优解。
- 路径级干预: 检测到路径将经过奇异 → 自动插入过渡姿态或提示切换 MoveJ。
冗余机器人的奇异规避
7 轴机器人的冗余自由度可直接用于实时避奇异:
minimize: -w₁ · manip(q) + w₂ · ||q - q_mid||²
其中 q_mid 是关节空间中心点, w₁ / w₂ 是权重
系统在每周期评估 10~20 个候选肘角 → 选使可操作度最大的肘角 → IK 求解。
运动学参数标定
运动学参数标定 (Kinematic Calibration) 是提高机器人绝对精度的关键步骤。出厂 DH 参数存在制造公差 (±0.11 mm / ±0.05°), 标定可将 TCP 绝对误差从 15 mm 降到 < 0.3 mm。
标定流程
Step 1: 数据采集
└─ 机器人走到 N ≥ 30 个不同的关节位姿
└─ 用外部测量设备 (激光跟踪仪 / 摄影测量 / 球杆仪) 记录末端真值
Step 2: 误差建模
└─ 建立 DH 参数误差向量 δ = [Δα, Δa, Δd, Δθ] (共 4n 个参数)
└─ 末端位姿误差 = FK(q, DH_nominal + δ) - FK(q, DH_nominal)
Step 3: 参数辨识
└─ 最小二乘: min_δ ||FK(q, DH_nominal + δ) - P_measured||²
└─ Levenberg-Marquardt 迭代求解
└─ 可辨识性分析: 剔除不可辨识的参数组合 (如 J1 α 与重力方向耦合)
Step 4: 验证
└─ 留 N/3 个测量点做交叉验证
└─ 最大残差 < 0.3 mm 视为合格
IDE 内置标定工具
Darra IDE 提供半自动标定向导:
- 数据采集 — 连接激光跟踪仪 (Leica AT960 / API Radian) 或使用球杆仪 (Ball-bar, 低成本替代方案)
- 自动采样 — 系统自动生成 40~60 个最优测量位姿 (D-optimal 设计), 确保参数可辨识性
- 参数辨识 — 一键运行, 输出修正 DH 表 + 残差报告
- 验证 — 随机 10 个测试位姿, 对比标定前后误差
// 加载标定后的 DH 参数
VAR
CalibDH : DH_PARAMS_6;
END_VAR
LoadCalibratedDH(Robot := Robot1, ParamSet := "2026-03-15", DH => CalibDH);
ApplyDH(Robot := Robot1, DH := CalibDH);
可辨识性分析
DH 参数并非全部可独立辨识。典型的不可辨识组合:
| 不可辨识参数 | 原因 | 处理 |
|---|---|---|
| J1 α₁ | 与重力方向耦合 | 固定为名义值 |
| J6 d₆ | 与 TCP 偏距耦合 | 由 TCP 标定独立处理 |
| 共线关节的 θ_offset | 相邻轴平行时 a 和 α 不独立 | 固定一个, 只辨识另一个 |
标定精度对比
| 方法 | 设备成本 | 精度 | 操作难度 | 适用场景 |
|---|---|---|---|---|
| 激光跟踪仪 | 高 (¥30~80万) | ±0.02 mm | 中 | 高精度产线, 首次标定 |
| 摄影测量 | 中 (¥5~15万) | ±0.05 mm | 低 | 定期复标 |
| 球杆仪 | 低 (¥1~3万) | ±0.1 mm | 低 | 快速校验, 日常维护 |
| 无外部测量 | 零 | ±0.5 mm | 低 | 末端反馈 (2 点 / 4 点法) |
无外部测量标定 (自标定)
不使用外部测量设备, 利用末端接触约束进行标定:
- 2 点法: 机器人以不同姿态接触同一定位锥 → 约束 TCP 位置不变 → 辨识 DH 参数
- 4 点法: 同上, 用 4 个不同定位锥 → 增加约束方程
// 自标定流程
VAR
CalibResult : BOOL;
END_VAR
// Step 1: 手动示教机器人以不同姿态接触标定锥
TeachTouchPoint(Robot := Robot1, PointID := 1, Pose := "Pose_A");
TeachTouchPoint(Robot := Robot1, PointID := 1, Pose := "Pose_B");
TeachTouchPoint(Robot := Robot1, PointID := 1, Pose := "Pose_C");
// Step 2: 运行自标定
CalibResult := SelfCalibrate(Robot := Robot1, Points := 1, TouchCount := 3);
// Step 3: 应用标定结果
IF CalibResult THEN
Log.Info('Self-calibration succeeded, accuracy improved to %.2f mm', GetCalibError(Robot1));
ELSE
Log.Error('Self-calibration failed: insufficient pose diversity');
END_IF;
自标定精度: 通常可达 ±0.5 mm (取决于示教姿态的多样性), 适合日常复检, 首次标定仍建议使用外部测量设备。
性能指标
| 指标 | 6 轴球腕 | 6 轴偏置腕 | SCARA | Delta | 7 轴冗余 |
|---|---|---|---|---|---|
| FK 计算时间 | < 2 μs | < 2 μs | < 1 μs | < 1 μs | < 3 μs |
| IK 计算时间 (闭解) | < 20 μs | — | < 5 μs | < 5 μs | — |
| IK 计算时间 (数值) | < 50 μs | < 80 μs | — | < 100 μs (FK) | < 150 μs |
| 平均 IK 成功率 | > 99.9% | > 99% | > 99.9% | > 99.9% | > 99% |
| 多组解数量 | 2~8 | 1~2 | 2~4 | 2 | 1 + 肘角连续 |
最佳实践
- 优先使用闭解: 球腕 6 轴选闭解 IK, 更快更稳。非球腕 / 偏置腕才用数值解。
- DH 参数精度: 标定前用名义 DH 验证 FK (让机器人走到已知点), 确认 DH 表正确。
- 工作空间规划: 工件放在灵巧空间内, 避免放在可达空间边缘 (奇异+精度差)。
- 冗余肘角习惯: 7 轴机器人示教时锁定肘角 (RedundancyMode := REDUNDANCY_MANUAL), 避免运行时肘部突然摆动。
- 标定周期: 新装机必标; 之后每 6 个月复标; 碰撞事故后必须重标。
- 自标定常态化: 每周跑一次自标定 (2 点法, 5 个姿态), 及早发现参数漂移。
相关文档
- 奇异点处理
- MoveJ / 关节运动
- MoveL / 直线运动
- 工具坐标系 (TCP)
- 机器人诊断
- 模型导入 — 自定义机器人 DH 配置