机器人 / 系统课时
从相机目标生成并验证抓取位姿
从相机目标生成并验证抓取位姿
拿到相机中的三维目标点,还不能直接让机械臂抓取:必须把目标换算到基座系,并考虑工具安装偏置。本例使用四轴 SCARA 完成抓取位姿生成、逆解分支筛选和正运动学闭环,得到一组可验证的关节目标。
学习目标
- 根据固定相机外参,将相机系目标位置与选定的抓取朝向转换到机器人基座系。
- 利用 TCP 安装偏置反求法兰位姿,并筛选满足限位、接近当前关节位置的逆解。
- 用位置和旋转误差验证运动学结果,区分仿真自洽与实机抓取精度。
定义相机、抓取点和机械臂模型
设 B 为机器人基座系、C 为固定相机光学系、F 为末端法兰系、G 为抓取 TCP 系;所有长度使用米、代码内部角度使用弧度。选择适合垂直拾取的四轴 SCARA:前两轴在水平面旋转,第三轴沿基座负 Z 方向伸出,第四轴调整工具偏航角。
| 输入 | 本例数值与含义 |
|---|---|
| 相机外参 | 平移 ;旋转 ,相机光学 Z 轴向下 |
| 相机观测的目标点 | |
| 目标抓取朝向 | 基座系绕 Z 轴旋转 ;相机系取 |
| 工具安装变换 | 无相对旋转;TCP 在法兰下方 m |
| 机械臂几何 | m;第三轴零行程时法兰高度 m |
单个三维点只能确定位置,不能推导完整抓取朝向。本例的 是抓取策略给定的朝向,不是由该点计算出来的;实际项目还需由物体朝向、夹爪约束或抓取规划器给出旋转。
沿坐标链反求法兰目标
统一采用列向量与左乘约定: 表示把 B 系坐标变换到 A 系。先将目标 TCP 位姿从相机系映射到基座系,再消去工具相对法兰的固定偏置:
本例求得基座系 TCP 目标位置为 m、绕 Z 偏航 ;因为 TCP 在法兰下方 m,法兰目标是 m。偏置应按刚体逆变换处理,不能在任意工具姿态下直接对世界 Z 坐标加减固定值。

求解关节配置并筛选分支
对法兰目标 ,SCARA 的水平二维逆解先计算 ,再取 。对应的 根据两连杆三角关系求得;第三轴伸出量 m;末端关节满足 。
| 候选 | ||||
|---|---|---|---|---|
| A | 0.100 m | |||
| B | 0.100 m |
示例限位为 、、 m,两组都合格。当前关节设为 ;按角度变化 、直线变化 m 分别归一化后比较平方和,A 更接近当前姿态。真实控制前还须单独进行碰撞、奇异性、关节速度和轨迹连续性检查。
运行逆解与正运动学闭环
环境为 Python 3.10+ 和 NumPy;首次安装可运行 python -m pip install numpy。保存以下完整脚本为 grasp_check.py,运行 python grasp_check.py。这里的四轴解析模型只覆盖水平位置、垂直高度和偏航角,不能用于要求任意俯仰或横滚的六维抓取任务。
import math
import numpy as np
def make_T(R, xyz):
out = np.eye(4)
out[:3, :3] = R
out[:3, 3] = xyz
return out
def rz(a):
c, s = math.cos(a), math.sin(a)
return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
def inverse(T):
out = np.eye(4)
R = T[:3, :3]
out[:3, :3] = R.T
out[:3, 3] = -R.T @ T[:3, 3]
return out
def wrap(a):
return (a + math.pi) % (2 * math.pi) - math.pi
# 相机光学 z 轴向下,基座 z 轴向上。
R_flip = np.diag([1.0, -1.0, -1.0])
T_BC = make_T(R_flip, [0.10, 0.05, 0.60])
T_CG = make_T(R_flip @ rz(math.radians(30)), [0.20, -0.10, 0.35])
T_FG = make_T(np.eye(3), [0, 0, -0.10])
T_BG = T_BC @ T_CG
T_BF = T_BG @ inverse(T_FG)
L1 = L2 = 0.20
home_z = 0.45
x, y, z = T_BF[:3, 3]
c2 = (x*x + y*y - L1*L1 - L2*L2) / (2 * L1 * L2)
if abs(c2) > 1 + 1e-10:
raise ValueError("目标超出水平工作空间")
c2 = float(np.clip(c2, -1, 1))
d3 = home_z - z
yaw = math.atan2(T_BF[1, 0], T_BF[0, 0])
q_ref = np.array([0, math.radians(60), 0.08, math.radians(-20)])
limits = [
(-math.pi, math.pi),
(math.radians(-135), math.radians(135)),
(0, 0.20),
(-math.pi, math.pi),
]
# 分别求肘部两种构型,剔除越限解。
candidates = []
for sign in (1, -1):
q2 = sign * math.acos(c2)
q1 = math.atan2(y, x) - math.atan2(L2 * math.sin(q2), L1 + L2 * math.cos(q2))
q4 = wrap(yaw - q1 - q2)
q = np.array([wrap(q1), wrap(q2), d3, q4])
if all(lo <= v <= hi for v, (lo, hi) in zip(q, limits)):
candidates.append(q)
if not candidates:
raise RuntimeError("逆解不满足关节限位")
def score(q):
delta = np.array([wrap(q[0] - q_ref[0]), wrap(q[1] - q_ref[1]),
q[2] - q_ref[2], wrap(q[3] - q_ref[3])])
# 不同类型关节分别归一化,避免角度和米数直接相加。
scale = np.array([math.radians(30), math.radians(30), 0.05, math.radians(30)])
return float(np.sum((delta / scale) ** 2))
q = min(candidates, key=score)
q1, q2, d3, q4 = q
# 正解回算 TCP,再比较两个完整的刚体位姿。
T_BF_actual = make_T(
rz(q1 + q2 + q4),
[L1 * math.cos(q1) + L2 * math.cos(q1 + q2),
L1 * math.sin(q1) + L2 * math.sin(q1 + q2),
home_z - d3],
)
T_BG_actual = T_BF_actual @ T_FG
position_mm = 1000 * np.linalg.norm(T_BG_actual[:3, 3] - T_BG[:3, 3])
relative_R = T_BG[:3, :3].T @ T_BG_actual[:3, :3]
angle_deg = math.degrees(math.acos(float(np.clip((np.trace(relative_R) - 1) / 2, -1, 1))))
print("基座系 TCP 目标 (m):", np.round(T_BG[:3, 3], 3))
print("法兰目标 (m):", np.round(T_BF[:3, 3], 3))
print("满足限位的逆解数:", len(candidates))
print(f"选中关节: q1={math.degrees(q1):.2f}°, q2={math.degrees(q2):.2f}°, "
f"d3={d3:.3f} m, q4={math.degrees(q4):.2f}°")
print(f"闭环误差: {position_mm:.4f} mm, {angle_deg:.4f}°")
print("运动学验证:", "通过" if position_mm <= 1.0 and angle_deg <= 1.0 else "失败")预期输出:
基座系 TCP 目标 (m): [0.3 0.15 0.25]
法兰目标 (m): [0.3 0.15 0.35]
满足限位的逆解数: 2
选中关节: q1=-6.45°, q2=66.03°, d3=0.100 m, q4=-29.58°
闭环误差: 0.0000 mm, 0.0000°
运动学验证: 通过这里的位置误差采用两点欧氏距离,姿态误差采用相对旋转矩阵的旋转角。以 1 mm 与 1° 为示例阈值,两项都达标才输出“通过”;由于数据由同一解析模型生成,接近零的结果是正常现象。
从闭环通过到允许抓取
闭环通过只说明 IK 和 FK 对同一运动学模型自洽,不能证明相机标定、TCP 实际安装或机械臂零位准确。实机仍需用标定块或已知位置的工件测量独立误差;例如把五个不同位置的实际 TCP 落点与测量基准比较,示例验收要求位置不超过 3 mm、朝向不超过 2°,具体阈值应以夹持公差为准。
正式抓取还应先生成高于目标 0.08 m 的预抓 TCP 位姿,即本例基座系 Z 为 0.33 m,对应法兰 Z 为 0.43 m、第三轴伸出 m。预抓位姿也要单独求逆解、查限位和碰撞;经低速空载试运行确认后,才能沿受控路径下降到抓取点。