机器人 / 系统课时
用正逆解闭环检测末端位姿误差
用正逆解闭环检测末端位姿误差
逆运动学求出关节角后,不能仅凭求解器返回成功就认定末端已经到达目标。将这组关节角重新送入正运动学,分别计算位置误差和姿态误差,才能检查解是否满足精度要求。本课用一个可运行的三关节模型建立闭环校验,并演示如何识别超过阈值的结果。
学习目标
- 建立目标位姿、逆解关节角和正解回算位姿之间的闭环检查流程。
- 使用欧氏距离和四元数旋转角分别量化位置误差与姿态误差。
- 编写自动校验程序,并区分数值求解误差与模型本身的误差。
统一坐标系与误差判定条件
假设逆运动学的目标是机器人基座坐标系 B 下的 TCP 位姿,记为目标位置 和目标姿态 。求解器输出关节配置 ,正运动学再由 计算出回算位置 和回算姿态 。
这四个量必须使用同一基座坐标系、同一 TCP 定义和同一工具安装偏置。否则比较到的可能是坐标定义差异,而不是运动学求解误差。
闭环检查按以下顺序执行:
- 记录目标 TCP 位置和姿态,以及目标使用的基座坐标系。
- 执行逆运动学,取得一组候选关节角,并确认求解器没有报告不可达或数值失败。
- 将候选关节角完整代入正运动学,得到回算 TCP 位姿。
- 分别计算位置误差和姿态误差,与各自阈值比较。
- 两项同时合格才报告 PASS;任何一项超限均报告 FAIL。

例如,可以为一个定位任务设置以下验收条件。这些数值是教学用阈值,并非所有机器人的通用标准。
| 检查项 | 示例阈值 | 判断方式 |
|---|---|---|
| TCP 位置误差 | 1 mm | 小于或等于阈值 |
| TCP 姿态误差 | 0.1° | 小于或等于阈值 |
| 综合结果 | 两项均通过 | 逻辑 AND |
如果位置误差为 0.4 mm、姿态误差为 0.15°,即使位置合格,整体仍然 FAIL。需要分别保留两项误差,不能用一个加权总分替代独立的精度约束。
分别计算位置误差和姿态误差
位置误差使用同一坐标系下两个 TCP 位置之间的欧氏距离。设目标位置为 ,回算位置为 ,则:
如果坐标单位是米,比较毫米阈值之前应乘以 1000。例如回算位置相对目标的偏差为 m,那么位置误差为 0.0005 m,即 0.5 mm。
姿态误差不能直接将三个欧拉角逐项相减再求模。欧拉角具有周期性和旋转顺序依赖,例如同一轴上的 179° 与 -179° 实际只相差 2°,直接相减却会得到 358°。
对于归一化的目标四元数 和回算四元数 ,可用内积计算两姿态之间的最短旋转角:
计算结果以弧度表示,乘以 后转换为角度。内积取绝对值,是因为 与 表示相同旋转;将结果限制在有效范围内,则可以避免浮点舍入造成反余弦定义域错误。
实际使用时还要注意:
- 两个四元数必须先归一化,并使用一致的分量顺序,例如都采用
(w, x, y, z)。 - 如果求解器只提供旋转矩阵,应先通过可靠的矩阵转换得到四元数,不能直接把矩阵元素当作四元数分量。
- 位置单位、姿态角度单位和阈值单位必须分别统一,不能将米直接与毫米比较,也不能将弧度直接与角度比较。
用 Python 验证一个三关节逆解
使用平面三旋转关节机械臂作为最小可复现实例。三段连杆长度依次为 1.0 m、0.8 m、0.2 m,末端位于 XY 平面,姿态仅绕 Z 轴旋转。目标 TCP 位置为 (1.2, 0.8, 0.0) m,目标偏航角为 30°。
逆解先根据目标朝向减去最后一段连杆的偏移,求出腕心,再用余弦定理求前两个关节角,最后让第三关节补足末端朝向。此处只选择第二关节角非负的一组解析解,目的是验证闭环,不负责枚举所有逆解。
下面程序适用于 Python 3.8 及以上版本,仅使用标准库,可以保存为 ik_fk_check.py 直接运行。
import math
L1, L2, L3 = 1.0, 0.8, 0.2
TARGET_POS = (1.2, 0.8, 0.0)
TARGET_YAW = math.radians(30.0)
TARGET_Q = (
math.cos(TARGET_YAW / 2), 0.0, 0.0,
math.sin(TARGET_YAW / 2)
)
def fk(q):
# 三个关节均绕Z轴旋转,TCP位于XY平面。
a = q[0] + q[1]
yaw = a + q[2]
p = (
L1 * math.cos(q[0]) + L2 * math.cos(a) + L3 * math.cos(yaw),
L1 * math.sin(q[0]) + L2 * math.sin(a) + L3 * math.sin(yaw),
0.0
)
r = (math.cos(yaw / 2), 0.0, 0.0, math.sin(yaw / 2))
return p, r
def ik_xy_yaw(x, y, yaw):
# 回退工具末端偏置,得到前两连杆需要到达的腕心。
wx = x - L3 * math.cos(yaw)
wy = y - L3 * math.sin(yaw)
c2 = (wx * wx + wy * wy - L1 * L1 - L2 * L2) / (2 * L1 * L2)
if c2 < -1.0 - 1e-10 or c2 > 1.0 + 1e-10:
raise ValueError("目标腕心不可达")
c2 = max(-1.0, min(1.0, c2))
q2 = math.acos(c2)
q1 = math.atan2(wy, wx) - math.atan2(
L2 * math.sin(q2), L1 + L2 * math.cos(q2)
)
return (q1, q2, yaw - q1 - q2)
def check(label, q):
p, r = fk(q)
# 单位四元数使用(w,x,y,z)顺序,取绝对内积处理双重表示。
ep = math.dist(p, TARGET_POS)
dot = abs(sum(a * b for a, b in zip(r, TARGET_Q)))
er = math.degrees(2 * math.acos(min(1.0, max(0.0, dot))))
passed = ep <= 0.001 and er <= 0.1
status = "PASS" if passed else "FAIL"
print(f"{label}: position={ep * 1000:.3f} mm, orientation={er:.3f} deg, {status}")
if __name__ == "__main__":
solution = ik_xy_yaw(1.2, 0.8, TARGET_YAW)
check("闭环", solution)
# 人为给第三关节增加0.2°,检查姿态超限是否被识别。
q_bad = (solution[0], solution[1], solution[2] + math.radians(0.2))
check("扰动", q_bad)运行命令:
python ik_fk_check.py预期输出:
闭环: position=0.000 mm, orientation=0.000 deg, PASS
扰动: position=0.698 mm, orientation=0.200 deg, FAIL第一行表示解析逆解重新代入正运动学后,位置与姿态均满足阈值。显示的 0.000 是保留三位小数后的结果,并不代表浮点运算绝对没有误差。
第二行故意给第三关节增加 0.2°。由于末端连杆长度为 0.2 m,位置偏差约为 0.698 mm,仍小于 1 mm;但姿态偏差为 0.2°,超过 0.1°,因此综合结果为 FAIL。
这个例子同时说明了为何要分别检查位置与姿态:单看位置误差,第二组配置会被错误地接受。
按误差特征定位闭环失败
真实六轴机器人通常需要对逆运动学求出的每组候选解分别进行正解回算。建议至少记录目标位置、目标四元数、六个关节角、回算位置、回算四元数、两项误差和最终状态,便于在失败时复现问题。
可以按照以下顺序排查:
- 先检查坐标系。 如果三个位置分量存在稳定偏差,确认目标是否在基座系下、是否已经将相机系目标转换到基座系,以及 FK 和 IK 是否使用同一个 TCP。把法兰位置与工具尖端位置直接比较,会将安装偏置误判为运动学误差。
- 再检查角度和长度单位。 若误差非常大,确认关节角输入是弧度还是角度,连杆长度使用米还是毫米。例如将 90° 直接作为弧度传入三角函数,结果就不会对应原本的关节姿态。
- 检查姿态表示。 如果位置接近目标而姿态差异明显,应检查四元数分量顺序、是否归一化、欧拉角旋转顺序,以及 TCP 朝向是否包含工具安装旋转。
- 检查数值求解的停止条件。 若迭代逆解在误差尚未合格时就返回成功,需要分别设置平移和旋转收敛阈值,而不是仅依赖迭代次数或单一综合误差。
- 检查候选解是否满足约束。 正逆解闭环 PASS 只证明这组角度可重现目标位姿,不代表它满足关节限位、避障或运动过程的连续性要求。关节越界和自碰撞需要单独验证。
例如一个迭代求解器返回的位置误差为 0.8 mm、姿态误差为 0.3°,并报告已经收敛。若验收阈值仍为 1 mm 和 0.1°,这组结果必须判为 FAIL。可以继续迭代、调整姿态误差权重或改用另一组初始关节角,但不能因为求解器标记成功就跳过最终检查。
闭环通过仍可能存在模型误差
正逆解闭环只能证明 IK 与 FK 使用的数学模型在该目标处相互一致,不能独立证明模型与真实机器人的几何尺寸、关节零位和工具安装关系完全一致。
例如程序中将最后一段工具长度设置为 0.20 m,实际工具长度却是 0.25 m。由于 IK 和 FK 都使用相同的错误参数,闭环仍可能得到几乎为零的位置误差并报告 PASS。然而实际 TCP 将沿工具方向偏离模型预测位置 0.05 m,也就是 50 mm。
因此在真实机器人上,建议采用两层验收:
- 数值闭环验收: 对工作空间内不同位置、不同姿态和多组逆解执行 IK→FK 检查,统计最大位置误差、最大姿态误差和失败率。例如抽取 1000 个可行目标,要求每个有效解都满足约定阈值。
- 独立几何验收: 使用经过校准的测量系统或独立基准数据,对实际 TCP 位姿与模型预测位姿进行比较,检查工具长度、关节零位偏差和基座标定误差。不要用同一套可能有误的 FK 结果充当真实位置的测量值。
还应覆盖接近工作空间边界、关节限位和奇异位形附近的样本,因为这些位置更容易暴露数值敏感性或求解失败。对于无法求解的目标,应明确记录不可达或求解失败,而不是把空结果当成零误差。
只有数值闭环合格,并且独立测量确认模型精度满足实际任务要求,才能将该运动学模型用于需要精确定位的机器人任务。