双目视觉在机械臂抓取中的应用(3D locate)
共 8315字,需浏览 17分钟
·
2024-07-23 17:03
实现功能
实习公司的项目,复现Cognex VisionPro 3D的大部分内容,涵盖眼在手外、眼在手上,包括相机标定、手眼标定、3D定位计算位移偏差。最后的位移偏差与Cognex的结果在1mm左右。
实施路线
用python实现原型验证算法,再移植成C++编译为dll,供C#调用。
python的库主要用到:cv2、numpy
C++的库主要用到:OpenCV、Eigen
复现的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 |
函数接口:
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)
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)
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)
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 | 基坐标系 |
总体步骤
相机标定
手眼标定
计算偏移
坐标系转换
左右相机通过特征提取,得到Raw2D下的特征点坐标
通过相机内参,将特征点转到Camera2D下
把未知高度作为未知数,将特征点转到Camera3D下
通过手眼矩阵,将特征点转到Hand3D下
通过示教器读取的机械手位姿,将特征点转到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张倾斜旋转 |
多相机标定
注意点:
固定两个相机间的相互位姿
固定所有相机的光学参数
多个相机需同时采集图像
本质上:每个相机有独立的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培训文档:三维定位方法 | 手眼标定思路 |
End
声明:部分内容来源于网络,仅供读者学习、交流之目的。文章版权归原作者所有。如有不妥,请联系删除。