点击下方卡片，关注「3D视觉工坊」公众号
选择星标，干货第一时间送达
来源：3D视觉工坊
3D视觉硬件汇总，包括结构光3D相机(支持室内外，焊接，机械臂抓取等场景)，相位偏折术、散斑、机械臂抓取和焊接软件，手持扫描仪(支持定位、重定位、稠密建图)、四足狗和小车定位和导航，底盘。点击->查看详情：
传统机器人工具中心点标定和手眼标定分开进行，会产生较大的累积误差。提出一种基于 TCP 标定与手眼标定协同标定方法。采用九点法及平面 TCP 标定法获取手眼标定初始矩阵及 TCP 初始偏移量，并建立基坐标系下空间任意点的坐标方程。通过李代数线性降维方法获取 TCP 值、旋转角、相机高度及手眼矩阵四个误差补偿量，并得到手眼标定与 TCP 标定的精确值。仿真与实验结果表明，采用的协同标定算法的平均标定位置误差比分离标定法降低了约 ，提升了机器人的标定精度。通过实验验证表明，本算法减小了位置误差与姿态误差，提升了机器人的标定精度，可以满足更多的工作场景需求。
1 平面 TCP 标定
平面标定法用一块固定在基座上的平板进行标定，且在整个标定过程中标定板不发生移动，相比于机械臂点触标定针，这种机械臂末端与标定板接触方法更加快捷。该方法分为两步骤：
1）保持法兰姿态不变，只有位置移动，控制末端带有工具的机械臂多次接触标定平面，当二者恰好接触时，记录每次接触时法兰中心点在基坐标系下坐标 ，因为在机械臂移动过程中，法兰的姿态没有发生改变，所以将记录的坐标点进行最小二乘法平面拟合，求得的平面方程为 ，且拟合出的平面与标定平面平行，记录平面法向量 ，再用最小二乘法可得：
再用 QR 分解求出 。
2）在操作面板上控制机械臂并改变其位姿，以不同的姿态接触标定平面。
将式（2）展开得到：
其中： 为末端坐标系到基坐标系的 旋转矩阵； 为末端坐标系到基坐标系 向量； 为工具坐标系到末端坐标系 旋转矩阵； 为要求的 TCP 坐标； 为工具坐标系到基坐标系 旋转矩阵； 为工具坐标系到基坐标系 平移向量。
重复以上步骤，任取其中两组位姿数据，列出等式：
因为所有的点都在同一平面上，所以两点之间构成平面向量，这些向量与法向量相乘为零，即
至此可将问题就可转化为 ，令 ，，。测出 组位姿数据写成方程组，再应用最小二乘法可求出 。
2 九点标定法原理
九点标定法的核心是点与点之间进行的仿射变换，通过相机识别棋盘格角点的像素坐标与世界坐标得出转移矩阵，九点标定具有高精度、易操作等优点，受到广泛应用。
为获得转移矩阵，首先将相机固定在一定高度支架上，确保机械臂的运动不会受相机的影响。标定板固定在机械臂基座上并保证在相机视野内，保证标定过程中标定板不发生移动。其次控制机械臂依次点触选中的九个棋盘格角点记录法兰坐标（法兰坐标由机械臂示教器上获得）与像素坐标可得：
其中： 为机械臂基坐标系下二维坐标； 为像素坐标系到机械臂基坐标系 二维旋转矩阵； 为像素坐标系到机械臂基坐标系 二维平移矩阵； 为像素坐标系下坐标； 为相机坐标系到基坐标系下转移矩阵在 平面上的分量；用最小二乘拟合求出转移矩阵。根据九个点数据可列出以下方程：
通过记录九个点的像素坐标和与之相对应的法兰中心点坐标进行仿射变换，求出 平面旋转矩阵 与平移矩阵 。因为相机光轴方向与 轴平行且方向相反，所以可将转移矩阵维数补充得：
其中： 为相机在机械臂基坐标系中高度；即可求出所需的旋转矩阵与平移矩阵。
3 两阶段线性降维标定原理
上文所介绍的标定方法在工业机器人标定时通常分开进行，这将导致二者之间没有耦合，本节设计了一种 TCP 与手眼矩阵协同标定算法，分为两个阶段：1）利用平面 TCP 标定法与九点法计算 TCP 标定初值和手眼矩阵初值；2）联列等式运用李代数计算出补偿量，将补偿量与初值相加得到手眼矩阵与 TCP 精确值。图1：算法流程图 为算法流程图。
工具末端点经机械臂到基坐标系下表示为 ，而工具末端点经相机到基坐标系下表示为 ，二者在本质上表示的都是空间中的同一点。可得到：
其中： 为相机拍照高度； 为相机内参； 为第 次的末端坐标系到基坐标系的旋转矩阵； 为第 次相机坐标系到基坐标系旋转矩阵。
将式（11）中相机高度 ，已求出的 TCP 值 ，手眼标定转移矩阵 与 这四个量转化为标称值和补偿值：
再根据手眼标定初值 、平面 TCP 标定初值 和相机拍照高度 ，可将式（11）改写为：
在机械臂的运动过程中会产生旋转角的误差，假设误差为 ，令 ，则有：
其中：，。
因为 为微小量，所以有：，，，将其带入式（14）得：
根据李代数定义：
根据李代数运算法则，令 ，则有：
这时未知量由 变成 ，将式（13）与式（17）联列，并舍去含有高阶微小量 项，得到：
将式（18）改写成 的形式，其中：
至此求出 中的补偿量，再将其给到标定值，便得到 TCP 值与手眼矩阵的精确值。
4 实验部分
利用 iARM6 协作机械臂和海康单目相机做实验对本文算法进行验证。实验设备见图2：实验设备照片。
实验过程，首先控制机械臂保持姿态不变，缓慢移动机械臂末端 TCP 点触棋盘格的角点图3：TCP点触标定板照片，重复此步骤，在控制面板上读取每次对应的法兰位置并记录，拟合标定面方程图4：相同位姿TCP点触标定板照片。通过程序改变机械臂姿态，同样缓慢移动机械臂末端 TCP 多次点触棋盘格的角点图5：不同位姿TCP点触标定板照片，记录每次对应的法兰位置与位姿。
将上述步骤获取的数据写入 Matlab 软件中，进行算法的运算得到 TCP 标定的初值为：
根据九点法计算出初值 ，首先选定九个拍照记录点的像素坐标，再缓慢移动机械臂依次点触这九个点，并记录对应点在基坐标系下坐标。
将上文算法与实验所获得的数据写入到 Matlab 软件，求出手眼标定的旋转矩阵初值与平移矩阵初值分别为：
获得手眼标定和 TCP 标定值后，进行数据采集，验证本文算法的有效性。为了方便采集 TCP 点像素坐标，假设探针的尖端两条边缘线交叉，控制机械臂使探针尖端出现在相机视野内并拍照，重复且每次控制机械臂位姿不同任取九个不同位姿，获取每张照片中探针尖端像素坐标和与其对应的法兰位姿，将已经获得的手眼矩阵初值、TCP 初值和机械臂不同位姿下 TCP 像素坐标带入式（18）求补偿值，见 表1：协同算法补偿值 。
定义标定误差为测量值和真实值之间的欧氏距离，因为位置误差是在 三个维度上，姿态的误差存在于 三个维度。位置误差的表示方式为 ，姿态的误差表示为 ，如图6：协同标定位置误差分布图、图7：协同标定姿态误差分布图所示，可知测量 35 次位置误差平均值为 2.698mm，位置误差最大值为 5.482mm，位置误差最小值为 0.438mm。旋转误差平均值为 ，旋转误差最大值为 ，旋转误差最小值为 。
利用标称值与补偿值求出精确值后，还需对本文方法进行验证，选择一个高精度的棋盘格利用相机识别棋盘格的角点像素坐标，并用协同标定算法计算出角点在基坐标系坐标 。另外在分离标定下计算出角点在基坐标系的坐标 。在此基础上与 Matlab 工具箱中提供的 Wengert 手眼标定方法进行位置误差和姿态误差两方面比较。协同标定、分离标定、Wengert 手眼标定 3 种优化方法 10 次重复性实验的旋转误差和位置误差，结果见表2：实测实验旋转误差和平移误差结果，协同标定旋转误差和位置误差分别记为 ；分离标定旋转误差和位置误差分别记为 ；Wengert 手眼标定方法旋转误差和位置误差分别记为 。
最终实验结果表明，单独校准方法仅产生单个矩阵参数，该参数无法考虑校准过程中产生的累积误差。因此，最终精度可能不足以满足工业机器人的运行需求。由表2：实测实验旋转误差和平移误差结果可以看出，协同标定平均位置误差 2.688mm，平均姿态误差 ，分离标定平均位置误差 3.301mm，平均姿态误差 ，Wengert 方法标定平均位置误差 4.559mm，平均姿态误差 ，可见协同标定算法精度优于其他两种方法。协同标定算法可以保证旋转精度，减小标定的平移误差，且协同标定算法优于分离标定算法与 Wengert 手眼标定方法，可实现高精度操作。从本文的校准原理和过程可知，机器人手臂的绝对定位精度越高，算法最终表现越好；该算法还利用摄像头测量 TCP 像素坐标，因此对摄像头的精度有一定要求。手眼校准的精度用于适应硬件的精度，从而提升系统整体精度。本文所采用的相机在拍摄照片时图像会产生畸变，对此畸变未做考虑。因此，未来可进一步在提升硬件精度和图像畸变处理方向开展工作。
5 结语
本文提出基于李代数原理手眼标定与 TCP 标定的线性协同算法，该方法是空间中一点经末端和机械臂本体在基坐标系下表示方式与这一点经相机到基坐标系下表示方式联列等式，利用李代数求出 TCP 值、旋转角、相机高度及手眼矩阵四个误差补偿量从而得到精确值。此方法减少了手眼标定与 TCP 标定分开标定的误差累积，节约了标定所用时间，提高了精确度的同时性能更加稳定，为机器人从事高精度作业提供了一种新思路。
本文仅做学术分享，如有侵权，请联系删文。
。
添加微信：cv3d001，备注：姓名+方向+单位，邀请入群。
