机械臂绘画的技术落地方案

一、整体逻辑流

1. 定义测试轨迹（在纸平面坐标系内，即一个固定于纸面的坐标系）。
2. 变换到机械臂基坐标系（Pinocchio 的运动学解算基于基坐标系）。
3. 生成末端工具位姿序列（不仅要有位置，还要有笔的姿态，通常是笔尖垂直纸面向下）。
4. 交给 Pinocchio 逆运动学求解，得到关节角度序列。
5. 发送关节指令给机械臂执行。

二、标定（纸面坐标系与机械臂基坐标系）

纸必须相对于机器人基座固定，并且要知道它在哪。

• 将一张白纸平放在一个刚性平板上，平板放置在机械臂前方、工作空间内。
• 利用末端探针/笔尖和机器人已有的运动学，手动操作或编写一段简单的定位程序，让笔尖触碰纸面上标记好的三个不共线的点（例如纸面左上角、左下角、右下角）。
• 记录这三点在机械臂基坐标系下的坐标 P1,P2,P3。
• 利用这三个点建立纸面坐标系 {Paper}：

• 这样就得到了一个变换矩阵 T_paperbase，它把纸面上的二维点 (x, y, 0) 变换到基坐标系下。

三、轨迹规划：定义要绘制的形状

在纸面坐标系里定义轨迹，轨迹最终需要转成一系列离散的末端位姿点，每个点间隔约 1~2mm，点数根据需要的平滑度确定。

1. 正方形（边长 L = 100mm）

在纸面坐标系中，四个角点（按顺序）：

• A=(0,0)
• B=(L,0)
• C=(L,L)
• D=(0,L)

需要离散化每条边，在每条边上均匀采样 N 个点（含端点），或者用插值函数，最终的离散路径点列表就是：

[(0,0), (Δx,0), (2Δx,0), ..., (L,0), (L,Δx), ..., (L,L), (L,L-Δx), ..., (0,L), (0,L-Δx), ..., (0,0)]

2. 圆形（半径 R = 50mm）

圆心在纸面坐标系的 (cx, cy) 处，比如 (50, 50)，圆的参数方程：

• x(t)=cx+R⋅cos⁡(θ)
• y(t)=cy+R⋅sin⁡(θ)

让 θ 从 0 到 2π 均匀步进（例如 360 个点），得到一系列点。

3. 直线（用于测试重复定位或回差）

在纸面上从 (x1, y1) 到 (x2, y2) 的线段，等距采样。

对于每个二维路径点 (x_paper, y_paper)，要赋予完整的 6D 位姿，才能调用逆运动学。

四、为每个路径点生成末端工具位姿（Pinocchio 需要的格式）

Pinocchio 的逆运动学通常需要一个目标位姿，表达为 4×4 齐次变换矩阵，或者位置+四元数。

对于一个路径点，其末端工具的位姿构建如下：

1. 位置： 将纸面点 (x_paper, y_paper, 0) 通过变换矩阵映射到基坐标系：

取的 (P_x, P_y, P_z) 是末端工具坐标系原点的位置，因为笔尖是工具末端，所以这实际是笔尖的位置。

1. 姿态： 笔要垂直于纸面，即工具坐标系的 Z 轴应指向纸内，纸面法向量 w 就是笔应该对齐的方向。
   • 设工具坐标系 Z 轴指向纸内，则 Z 方向 = −w。
   • 工具坐标系 X 轴可以选择为纸面坐标系的 X 轴方向u（即纸的水平方向）。
   • Y 轴由右手法则确定，可构造一个旋转矩阵 R_basetool(需保证是正交归一矩阵)：

   

   或者直接用纸面坐标系的 X, Y 轴作为工具 X, Y，Z 为纸面法向向内。

2. 组装变换矩阵： 把 R 和 P_base 装成 4×4 矩阵 T：

这就是 Pinocchio 逆运动学所需的末端工具目标位姿。

注意：笔是安装在末端模组上的，笔尖到末端法兰有一个固定的偏移变换 Ttoolflange，应当先用笔尖标定出这个工具偏置，然后在轨迹规划时，把笔尖位姿 T 转换为法兰位姿送给 IK。或者，更简单地，在 Pinocchio 的 URDF 中，把笔作为一个附加的虚拟连杆，其末端就是笔尖，IK 解算时直接给笔尖坐标系的目标位姿即可。

五、具体实施步骤

阶段1：准备

1. 在 URDF 中添加一个笔工具连杆，固连在 J6 之后，其原点在笔尖，Z 轴指向纸面。
2. 固定好纸板，手动操作机械臂，用笔尖轻触纸面上预先画好的三个标记点，记录下这三个点在基坐标系下的坐标（通过正运动学读笔尖位置）。
3. 根据三点计算 T_paper_base。

阶段2：轨迹生成脚本（Python 伪代码逻辑）

# 定义纸面轨迹 (正方形)
L = 0.1  # 100mm
paper_points = []
# 离散化四条边，step 为步长 (如 0.002m)
for p in 离散正方形点(L, step=0.002):
    # p 是 [x_paper, y_paper]
    # 构造纸面坐标系下的齐次位姿 (笔尖)
    T_paper_tip = np.eye(4)
    T_paper_tip[:3, 3] = [p[0], p[1], 0.0]  # 位置
    # 姿态：笔尖Z轴指向纸面法向的反方向 (即向下压)，X轴沿纸面X
    R_paper_tip = 由纸面X轴、Y轴和-Z方向构成的矩阵
    T_paper_tip[:3, :3] = R_paper_tip
    # 变换到基座标系
    T_base_tip = T_paper_base @ T_paper_tip
    # 调用 Pinocchio IK，传入 T_base_tip
    q_sol = pinocchio.inverse_kinematics(model, data, T_base_tip, q_init)
    joint_trajectory.append(q_sol)
    q_init = q_sol  # 用上一解作为初值，保证连续性

阶段3：执行

• 将 joint_trajectory 按时间戳通过位置接口发送给机械臂，移动速度要慢且均匀（比如 20mm/s），避免笔被拖动跳跃。
• 可让笔尖在纸上轻轻接触，无需用力，只需画出线。

六、误差分析

• 正方形边长 vs 100mm：若偏小，可能是基座标系的尺度不准（连杆长度参数偏小或关节减速比有误），调整 URDF 中的连杆长度 a 或考虑关节传动比。
• 正方形的直角不直：说明关节轴不垂直，可能存在轴线偏斜（需修正 DH 参数中的 alpha）。
• 圆不圆，呈椭圆：通常因 X、Y 轴灵敏度不一致，或某关节存在反向间隙。可检查关节回差。
• 起点和终点不闭合：重复定位误差大，传动存在间隙或零位漂移。

这些观察可以明确调整 DH 参数表的哪一项，例如，若 100mm 边长变成了 99mm，且误差是线性的，可以在 Pinocchio 中按比例修正连杆长度，再重新测试。