前言

1. 机械臂视觉抓取流程

A. 图像采集 ：摄像头拍摄工作台的画面。
B. 图像处理 ：去除图像畸变，检测物体在图像中的位置，以及物体的其他图像特征。
C. 位姿估计 ：根据图像特征得到物体在工作台/摄像头坐标系下的位姿。
D. 空间变换 ：将物体在工作台坐标系下的位姿转换为机械臂坐标系下的位姿。
E. 机械臂运动规划 ：驱动机械臂执行一段轨迹，将物体抓取到特定的地方，并保证末端平滑运动。

另外，在实现机械臂视觉抓取之前，需要做一些前置工作：

F. 相机标定 ：通过相机标定得到相机的内部参数，包括相机畸变系数与内参矩阵。

畸变系数用于去除图像畸变 (步骤B)，内参矩阵用于物体的位姿估计 (步骤C)。
G.手眼标定 ：手眼标定是为了明确机械臂基坐标系与相机坐标系/工作台坐标系之间的空间变换。

得到的变换矩阵用于空间变换 (步骤D)。

2. 刚体空间变换

刚体空间变换包含两个核心要素： 物体位姿描述 以及 物体在不同坐标系下的变换 。

步骤C、D、E、F、G都依赖刚体空间变换的知识。

3. 机械臂运动学

因此想要实现控制机械臂末端移动到指定的位置，就需要借助 机械臂运动学 与 机械臂轨迹规划 的知识。

机械臂运动学是机械臂轨迹规划的前置知识点。

机械臂正向运动学 ：已知机械臂各关节角度，求解此时机械臂末端的位姿。
机械臂逆向运动学 ：已知机械臂末端位姿，求解此时机械臂各关节的角度。

如果是自己设计机械臂或者设计机械臂末端，则需要自己推导运动学公式。

如果是购买机械臂成品，则可以直接调用机械臂厂家提供的API。

4. 运动规划

运动规划包含 轨迹规划 和 路径规划 两个板块。

4.1 轨迹规划

如果是自己开发机械臂的运动控制器，需要自己实现轨迹规划。让机械臂末端/关节完成从A点到B点的平滑运动。

如果是使用成品的运动控制器或者成品机械臂，一般情况下不需要自己写基础的轨迹规划部分。可以在PC端规划更高级的运动曲线，然后分段发送给运动控制器。

还可以添加时间最短/加加速度最小等其他轨迹规划的约束，实现轨迹最优化。

轨迹规划主要分为 关节空间 与 笛卡尔空间 下的轨迹规划。

关节空间下的轨迹规划，每个关节都是相对独立的，是对关节的角度进行规划，不关心末端的轨迹。

笛卡尔空间下的轨迹规划，指的是对工具坐标系在机械臂基坐标系下的轨迹进行规划，让机械臂末端从位姿A到位姿B实现平滑的过渡。

4.2 路径规划

如果机械臂的实际工况比较复杂，需要根据机械臂本体、周围环境与工件的情况，规划出一条无碰撞的轨迹。这时就需要做机械臂的路径规划，需要在PC端/工控机端实现。

5. 轨迹跟踪

轨迹规划出来之后，到实际的执行层面，则称之为轨迹跟踪。

如果是 闭环控制 ，可以设计运动控制器，让机械臂末端跟踪预设的轨迹。当发生扰动时，还可以尽快恢复轨迹的执行。

如果是 开环控制 ，则可以按照特定的时间间隔发送控制指令，例如位置 / 位置+速度 / 位置+速度+加速度。

6. 相机标定

相机镜头都是有畸变的，因此要先做一下相机标定。相机标定后，得到相机的 内参矩阵 与 畸变系数 。

利用相机标定得到的畸变系数，去除图像的畸变。畸变去除后，则畸变系数为0，重新计算得到新的相机内参。

7. 图像处理

图像处理的目的是提取物体的特征信息，用于物体的位姿估计。

通过 透视变换 ，对工作台的图像进行梯形校正，得到工作台的正视图。
通过 目标检测 算法 (例如YoloV5)，可以得到物体在画面中的矩形框。
通过 图像分割 算法 (例如SegNet)，可以得到每个像素点对应的类型。
通过 实例分割 算法 (例如Mask-RCNN)，可以得到物体的轮廓。

进一步，还可以提取轮廓图二值化图像的外接矩形。
通过 关键点匹配 算法，可以得到模板图像中的像素点在当前画面中的像素位置。

以及模板图像四个角点在采集图像中的像素坐标。

上述方法都还是平面图像中的图像处理，还可以通过深度摄像头获得工作台的点云数据。

通过 点云分割 ，去除环境点云，得到物体的点云数据。

8. 位姿估计

通过图像信息，获取工件在工作台上/相机坐标系下的位姿。

对于平面抓取的简单情况，可以根据物体在图像中像素的中心，提取物体在工作台上的坐标值。

使用物体二值化图像的最小外接矩形，则可以提取得到物体在工作台上的偏航角。
可以根据关键点模板匹配得到2D像素点，以及该点在物体坐标系下的3D坐标。

使用 PNP算法 来估计物体的位姿。
使用深度学习模型，输入工作台图像，直接获取物体的6DoF的位姿估计。

如果是点云数据：

对物体上表面进行拟合，提取上表面中心点三维坐标以及法向量。
使用在工作台上采集的物体点云数据与模板点云数据，进行点云配准得到物体位姿。

点云配准 是3D点与3D点之间的空间变换，常用的配准算法有 ICP 、 CPD 、 ECPD 、 FilterReg 。

9. 手眼标定

前面已经获得了工件在工作台上的位姿，那么怎么让机械臂知道要到哪里抓取呢？

这时就需要明确机械臂基坐标系到相机坐标系，或者机械臂基坐标系到工作台坐标系之间的变换关系。

这里可以使用一种简单，高精度的实用算法 九点标定法 。在工作台上选取9个点，可以得到9点分别在工作台坐标系下的3D坐标。控制机械臂的末端依次到达工作台，得到机械臂到达9点时末端位置的3D坐标。由此就得到了9个3D点对，使用ICP算法可以求解得到机械臂坐标系到工作台坐标系之间的空间变换。

另外还可以使用经典的 TSAI手眼标定法 , 它同时适用于眼在手上与眼在手外两种情况。

10. 视觉抓取

在完成手眼标定之后，就可以将工件在工作台上的位姿，转换为工件在机械臂基坐标系下的位姿了。

对于平面抓取这种简单的情况来讲，可以使用门式轨迹规划。

操作步骤如下：

到达物体正上方
夹爪落下，夹取物体
抬起来
移动到目标位置的正上方 (中间的轨迹可以是直线，也可以是任意轨迹)
下移
释放夹爪，物体掉落。

再复杂一点，需要考虑物块此时的姿态，调整工具的姿态。同时要根据夹爪类型，机械臂自由度这些条件来制定抓取策略。

如果是夹爪+3D点云的情况要更复杂一些，需要根据点云信息，推断出合适的下爪点，寻找一条无碰撞轨迹。