机器人 / 系统课时

从相机目标生成并验证抓取位姿

从相机目标生成并验证抓取位姿

拿到相机中的三维目标点,还不能直接让机械臂抓取:必须把目标换算到基座系,并考虑工具安装偏置。本例使用四轴 SCARA 完成抓取位姿生成、逆解分支筛选和正运动学闭环,得到一组可验证的关节目标。

学习目标

  • 根据固定相机外参,将相机系目标位置与选定的抓取朝向转换到机器人基座系。
  • 利用 TCP 安装偏置反求法兰位姿,并筛选满足限位、接近当前关节位置的逆解。
  • 用位置和旋转误差验证运动学结果,区分仿真自洽与实机抓取精度。

定义相机、抓取点和机械臂模型

设 B 为机器人基座系、C 为固定相机光学系、F 为末端法兰系、G 为抓取 TCP 系;所有长度使用米、代码内部角度使用弧度。选择适合垂直拾取的四轴 SCARA:前两轴在水平面旋转,第三轴沿基座负 Z 方向伸出,第四轴调整工具偏航角。

输入 本例数值与含义
相机外参 BTC{}^B T_C 平移 (0.10,0.05,0.60)(0.10,0.05,0.60);旋转 Rx(180∘)R_x(180^\circ),相机光学 Z 轴向下
相机观测的目标点 CpG=(0.20,−0.10,0.35){}^C p_G=(0.20,-0.10,0.35)
目标抓取朝向 基座系绕 Z 轴旋转 30∘30^\circ;相机系取 CRG=Rx(180∘)Rz(30∘){}^C R_G=R_x(180^\circ)R_z(30^\circ)
工具安装变换 FTG{}^F T_G 无相对旋转;TCP 在法兰下方 0.100.10 m
机械臂几何 l1=l2=0.20l_1=l_2=0.20 m;第三轴零行程时法兰高度 z0=0.45z_0=0.45 m

单个三维点只能确定位置,不能推导完整抓取朝向。本例的 30∘30^\circ 是抓取策略给定的朝向,不是由该点计算出来的;实际项目还需由物体朝向、夹爪约束或抓取规划器给出旋转。

沿坐标链反求法兰目标

统一采用列向量与左乘约定:ATB{}^A T_B 表示把 B 系坐标变换到 A 系。先将目标 TCP 位姿从相机系映射到基座系,再消去工具相对法兰的固定偏置:

BTG=BTC CTG{}^B T_G={}^B T_C\,{}^C T_G
BTF=BTG(FTG)−1{}^B T_F={}^B T_G({}^F T_G)^{-1}

本例求得基座系 TCP 目标位置为 (0.30,0.15,0.25)(0.30,0.15,0.25) m、绕 Z 偏航 30∘30^\circ;因为 TCP 在法兰下方 0.100.10 m,法兰目标是 (0.30,0.15,0.35)(0.30,0.15,0.35) m。偏置应按刚体逆变换处理,不能在任意工具姿态下直接对世界 Z 坐标加减固定值。

固定相机到 SCARA 抓取位姿的坐标变换链

求解关节配置并筛选分支

对法兰目标 (x,y,z)=(0.30,0.15,0.35)(x,y,z)=(0.30,0.15,0.35),SCARA 的水平二维逆解先计算 c2=(x2+y2−l12−l22)/(2l1l2)=0.40625c_2=(x^2+y^2-l_1^2-l_2^2)/(2l_1l_2)=0.40625,再取 q2=±arccos⁡(c2)q_2=\pm\arccos(c_2)。对应的 q1q_1 根据两连杆三角关系求得;第三轴伸出量 d3=z0−z=0.100d_3=z_0-z=0.100 m;末端关节满足 q4=30∘−q1−q2q_4=30^\circ-q_1-q_2。

候选 q1q_1 q2q_2 d3d_3 q4q_4
A −6.45∘-6.45^\circ 66.03∘66.03^\circ 0.100 m −29.58∘-29.58^\circ
B 59.58∘59.58^\circ −66.03∘-66.03^\circ 0.100 m 36.45∘36.45^\circ

示例限位为 q1,q4∈[−180∘,180∘]q_1,q_4\in[-180^\circ,180^\circ]、q2∈[−135∘,135∘]q_2\in[-135^\circ,135^\circ]、d3∈[0,0.20]d_3\in[0,0.20] m,两组都合格。当前关节设为 (0∘,60∘,0.08 m,−20∘)(0^\circ,60^\circ,0.08\text{ m},-20^\circ);按角度变化 30∘30^\circ、直线变化 0.050.05 m 分别归一化后比较平方和,A 更接近当前姿态。真实控制前还须单独进行碰撞、奇异性、关节速度和轨迹连续性检查。

运行逆解与正运动学闭环

环境为 Python 3.10+ 和 NumPy;首次安装可运行 python -m pip install numpy。保存以下完整脚本为 grasp_check.py,运行 python grasp_check.py。这里的四轴解析模型只覆盖水平位置、垂直高度和偏航角,不能用于要求任意俯仰或横滚的六维抓取任务。

python
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 "失败")

预期输出:

text
基座系 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、第三轴伸出 d3=0.02d_3=0.02 m。预抓位姿也要单独求逆解、查限位和碰撞;经低速空载试运行确认后,才能沿受控路径下降到抓取点。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

从相机目标生成并验证抓取位姿

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通