[机器视觉]双目视觉在机械臂抓取中的应用(3D locate)

项目简述

实现功能

实习公司的项目,复现Cognex VisionPro 3D的大部分内容,涵盖眼在手外、眼在手上,包括相机标定、手眼标定、3D定位计算位移偏差。最后的位移偏差与Cognex的结果在1mm左右。

实施路线

用python实现原型验证算法,再移植成C++编译为dll,供C#调用。
python的库主要用到:cv2numpy
C++的库主要用到:OpenCVEigen

复现的VisionPro 3D函数

函数语句 函数功能
camCalRes = camCalibrator.Execute(pelRects, extractedFeatures, calHeights, calPoseTypes); 相机标定
heCalibs = heCalibrator.Execute(pelRects, intrinsics, extractedFeatures, robotPositions); 手眼标定
modelPoints = triangulator.Execute(cameraCalibs, pointsRaw2D, isPointValid, out is3DPointValid, out resRaw2D, out resPhys3D); 三角测量获得3D点坐标
Cog3DPoseEstimatorUsing2DPointsResult res = pParam.Models[model].Execute(camCalibs, features2D, weights2D); 通过2D点估计3D姿态
函数功能 主体类型 返回值类型
相机标定 Cog3DCameraCalibrator Cog3DCameraCalibrationResult
手眼标定 Cog3DHandEyeCalibrator List
<Cog3DHandEyeCalibrationResult>
三角测量获得3D点坐标 Cog3DTriangulator Cog3DVect3Collection
通过2D点估计3D姿态 List
<Cog3DPoseEstimatorUsing2DPoints>
Cog3DPoseEstimatorUsing2DPointsResult

封装成dll的函数接口

函数功能 函数名 所属dll
眼在手外的相机标定、手眼标定 void getMatsEth() CalibrateCpp.dll
眼在手上的相机标定、手眼标定 void getMatsEih() CalibrateCpp.dll
3D定位:左右相机的2D点转换为3D点 void calc_ImageToWorld() CalcImageToWorldCpp.dll
计算偏移 void calc_Excursion() CalcExcursionCpp.dll

函数接口:

  1. void getMatsEth(int* other_info, char* point3d_str, char* point2d_str, char* robot_str, double* mtx33_l, double* mtx33_r, double* mtx44_l, double* mtx44_r)
  2. void getMatsEih(int* other_info, char* point3d_str, char* point2d_str, char* robot_str, double* mtx33_l, double* mtx33_r, double* mtx44_l, double* mtx44_r)
  3. void calc_ImageToWorld(double* _mtx44_cam3dToPhy3d_l, double* _mtx33_camIntrin_l, double* _mtx44_cam3dToPhy3d_r, double* _mtx33_camIntrin_r, double* _Point_Cl, double* _Point_Cr, double* params)
  4. void calc_Excursion(int pointNum, double* _Point_Model_3D, double* _Point_Now_3D, double* _res_excursion)
函数名 输入 输出
void getMatsEth() other_info是长度为3的整型一维数组,3个元素分别代表标定图片数量、标定图片宽、标定图片高
point3d_str是字符串数组,为标定板角点在CalPlate3D下的坐标
point2d_str是字符串数组,为标定板角点在Raw2D下的坐标
robot_str是字符串数组,为机械手位姿所代表的4*4的矩阵
mtx33_l是浮点型数组,为左相机内参所代表的3*3的矩阵
mtx33_r是浮点型数组,为右相机内参所代表的3*3的矩阵
mtx44_l是浮点型数组,为左相机的手眼矩阵所代表的4*4的矩阵
mtx44_r是浮点型数组,为右相机的手眼矩阵所代表的4*4的矩阵
void getMatsEih() 同上 同上
void calc_ImageToWorld() _mtx44_cam3dToPhy3d_l是浮点型数组,为左相机的手眼矩阵所代表的4*4的矩阵
_mtx33_camIntrin_l是浮点型数组,为左相机内参所代表的3*3的矩阵
_mtx44_cam3dToPhy3d_r是浮点型数组,为右相机的手眼矩阵所代表的4*4的矩阵
_mtx33_camIntrin_r是浮点型数组,为右相机内参所代表的3*3的矩阵
_Point_Cl是长度为3的齐次形式的浮点型一位数组,为左相机的特征点在在Raw2D下的坐标
_Point_Cr是长度为3的齐次形式的浮点型一位数组,为右相机的特征点在在Raw2D下的坐标
params是长度为3的非齐次形式的浮点型一位数组,为特征点在在RobotBase3D下的坐标
CalcExcursionCpp.dll pointNum是整型值,为一组需计算偏移的点的个数
_Point_Model_3D是浮点型数组,为这组点在偏移前坐标系下的坐标值
_Point_Now_3D是浮点型数组,为这组点在偏移后坐标系下的坐标值
_res_excursion是长度为6的浮点型一维数组,分别是x、y、z、rx、ry、rz

原理概述

6个坐标系

坐标系 含义 备注
Raw2D 图像坐标系(像素单位)
Camera2D 图像坐标系(物理单位) 无畸变:去除光学畸变和像素比例的影响
Camera3D 相机坐标系 z轴即是相机的光轴,Z=1即为Camera2D
CalPlate3D/Phys3D 标定板坐标系
Hand3D/Tool 机械手坐标系/工具坐标系
RobotBase3D 基坐标系

总体步骤

  1. 相机标定
  2. 手眼标定
  3. 计算偏移

坐标系转换

  1. 左右相机通过特征提取,得到Raw2D下的特征点坐标
  2. 通过相机内参,将特征点转到Camera2D
  3. 未知高度作为未知数,将特征点转到Camera3D
  4. 通过手眼矩阵,将特征点转到Hand3D
  5. 通过示教器读取的机械手位姿,将特征点转到RobotBase3D

方程求解

最终共有6个方程,即左右相机各3个方程(x、y、z)
但6个方程中只有5个未知数,即RobotBase3D下的X、Y、Z、Camera3D下的左右相机的z
一般做法,用优化方法解超定方程即可

3D视觉基础知识

位姿

刚体在坐标系中用位姿描述
位姿 = 位置(position) + 姿态(orientation)
一般地,位置和姿态各用3个值表示,位置用x、y、z表示偏移,姿态用rx、ry、rz表示欧拉角旋转

标定板

工业常用标定板分两大类:实心圆阵列(Halcon)、棋盘格(VisionPro、OpenCV、Matlab)
非精确制造的标定板会导致不好的标定结果,比如激光/喷墨打印机打印的标定板

康耐视的棋盘格(Cognex checkerboard)包含标准的基准标识,能够做到不必在一张图片内拍摄整张棋盘格

相机标定

3D标定:在与图像像素相关的2D坐标系与物理世界相关的3D坐标系间建立数学联系
最初定义的物理世界坐标系是标定板坐标系

3D标定后的相机可以实现:
①Raw2D中的2D point ⇒ Phys3D中的3D ray
②Raw2D中的2D point + Phys3D中的3D plane ⇒ Phys3D中的3D point
③Phys3D中的3D point ⇒ Raw2D中的2D point

三角测量(Triangulation)

单个标定相机:Raw2D中的2D point ⇒ Phys3D中的3D ray
多个标定相机:Raw2D中的2D point ⇒ Phys3D中的3D point
(从不同位置观察同一物体,可在Phys3D中生成与物体同一特征相关的多个交叉直线,从而算出3D位姿)

注意:2D特征必须准确可靠(不会被3D影响,如透视收缩)

姿态估计

估计3D物体姿态至少需要3个不共线的点

手眼标定

输入:标定板图像、Hand3D位姿(示教器提供)

眼在手外:Camera3D中的3D point ⇒ RobotBase3D中的3D point
眼在手上:Hand3D中的3D point ⇒ Camera3D中的3D point

最终实现,Raw2D中的2D point ⇒ RobotBase3D中的3D point

3D严格变换(3D Rigid Transforms)

实现两个3D笛卡尔坐标系之间的映射。
只包含旋转平移,不包含比例、反射、错切。

维基百科讲解:旋转矩阵、欧拉角、四元数

标定实施

输入输出

在不同位姿下,用固定光学和机械参数的相机采集一组标定板的图像
输入:标定板图像、标定板的网格尺寸、标定板位姿
输出:内参、外参

内外参 变换关系 含义
内参(intrinsic) Raw2DfromCamera2D 去除光学畸变和像素比例的影响的非线性变换
外参(extrinsic) Camera3DfromPhys3D 两个坐标系之间的6个自由度的线性变换

眼在手外/眼在手上

眼在手外:相机固定相机拍被机械臂带着移动n个姿态的棋盘格
眼在手上:相机移动,机械臂带着移动n个姿态的相机拍固定的棋盘格

推荐单相机标定实施

图像数 实施
最少4张 以图像平面倾斜20°~30°
以相机光轴旋转90°
推荐9张 上述4张+互相平行高度不同
工业9张 1张基准+8张倾斜旋转

多相机标定

注意点:

  1. 固定两个相机间的相互位姿
  2. 固定所有相机的光学参数
  3. 多个相机需同时采集图像

本质上:每个相机有独立的Raw2D、Camera2D、Camera3D,有同一个Phys3D

推荐多相机标定实施

轴线

倾斜、旋转:轴线应当大体在相机光心的中心,且大体在工作空间的中心
变高:使用倾斜视角的轴线,并使标定板垂直于轴线

倾斜、旋转

标定板:倾斜20°、旋转90°

变高

标定板:彼此之间平行,改变高度

3D定位

3D视角项目架构

先粗定位,再精定位

寻找特征

关键:2D特征和3D特征的特征对(feature correspondence)

稳定的2D特征(能承受3D的不确定)

1.尖锐、平直的边缘
2.
3.具有旋转、缩放、平移不变性的独特特征

英文原文:
• Sharp, straight edges
• Circles
• Unique features that can be found regardless of rotation, scale or translation changes

应当避免的2D特征

1.圆角
2.倒圆边
3.部件其余部分的特出特征

英文原文:
• Round corners
• Round edges
• Features that extrude significantly towards the camera from the rest of the part

检查2D特征的准确性:5个姿态

1.原始姿态
2.视野左上角,z轴旋转20°
3.视野右上角,z轴旋转45°
4.视野左下角,z轴旋转67°
5.视野右下角,z轴旋转90°

相机标定

输入:correspondence pairs,即图像特征与其物理位置
输出:相机内参、相机外参、畸变系数

一般相机标定均采用张正友的标定方法
论文传送:Zhang z.A flexible new technique for canlera calibration[J].IEEE transactions on pattern analysis and machine intelligence,200.22(11):1330—1334.

论文解析与实现见我的另一篇博客,[机器视觉]详解相机标定

手眼标定

本质

手眼标定的本质:求解CX=XD,C与相机相关,D与机械臂相关
输入:C、D
输出:X

眼在手外/眼在手上

工业应用中,手(机械臂)眼(相机)有两种位置关系:

分类 相机关系 所求关系 详细描述
眼在手外 相机固定 Camera3D与RobotBase3D的关系 相机(眼)和机械臂(手)分离,眼的位置相对于手是固定的
眼在手上 相机移动 Camera3D与Hand3D的关系 把相机(眼)固定在机械臂(手)上面,眼随手移动

眼在手外

相机固定:Camera3D与RobotBase3D的关系固定
标定板固定在机械臂上:CalPlate3D与Hand3D的关系固定

结合上图,有公式:

标定时控制机械臂从位置1移动到位置2
坐标系变换关系为:Hand3D → RobotBase3D → Camera3D → CalPlate3D
可得位置1的公式:

合并,可得:

移动到位置2后:

由于cal和tool的相对位置是不变的,可通过此联立方程:

化为CX=XD的形式,可得:

其中,camHbase是待求解的,camHcal可通过相机外参获得,baseHtool可通过示教器读取

眼在手上

相机移动:CalPlate3D与RobotBase3D的关系固定
相机固定在机械臂上:Camera3D与Hand3D的关系固定

结合上图,有公式:

标定时控制机械臂从位置1移动到位置2
坐标系变换关系为:CalPlate3D → Camera3D → Hand3D → RobotBase3D
可得位置1的公式:

合并,可得:

移动到位置2后:

由于base和obj的相对位置是不变的,可通过此联立方程:

化为CX=XD的形式,可得:

其中,camHtool是待求解的,camHcal可通过相机外参获得,baseHtool可通过示教器读取

计算偏移

本质上是一个NPointToNPoint的问题
输入:一组点在A坐标系下的坐标、这组点在B坐标系下的坐标
输出:这组点从A坐标系变换到B坐标系的转换关系

一般做法是将其看作优化问题,将伪逆作为初始值,只考虑仿射变换中的旋转、平移

具体细节见我的另一篇博客,[机器视觉]欧式空间中的二维点变换关系

参考

参考内容 参考方面 备注
硕士论文《基于OpenCV的机器人双目手眼标定系统的研究与实现》 相机标定、手眼标定、3D定位的整体框架
Cognex VisionPro 3D Developer’s Guide 主体思路 标定流程是重点 P59
手眼标定注意点 P74 P75
手眼标定的两种方式 手眼标定公式推导
HALCON培训文档:三维定位方法 手眼标定思路