机械臂视觉抓取学习笔记

Ros机械臂视觉抓取（ROS机械臂视觉抓取_我不是“耀”神的博客-CSDN博客）（视觉机械臂自主抓取全流程_机械臂视觉抓取_knightsinghua的博客-CSDN博客）

Ros是机器人软件仿真平台，ur机械臂和二型连杆机械臂不一样。

Universal-Robots（优傲机器人），是丹麦的协作机械臂生产商。

Ur机械臂

二型连杆机械臂

串联机械臂

我们要实现的功能是相机传回图像处理后机械臂实现抓取与放置。

要抓取物件，我们得知道机械臂和物件的位置，然后规划机械臂轨迹去抓取，所以得进行坐标系转换。

整个抓取过程大致分为以下几步（含图中九步）：相机标定——→相机和机械臂进行手眼标定——→①读取摄像头信息——→②识别出目标物体并得出位姿——→③物体在相机中的二维坐标转化成相对于相机光心的三维坐标——→④再转换成相对于机械臂底座的世界坐标——→⑤计算机械臂末端执行器抓取物体时的位姿——→⑥通过已知的目标位置和末端执行器的位姿信息来计算机械臂上每个电机应转动的角度——→⑦根据有无障碍物、路径距离要求、时间要求规划出合适的路径——→⑧驱动机械臂各个关节一步一步到达合适的位置——→⑨末端执行器抓取物体——→投放到指定位置

一、相机标定（机器视觉（二）——摄像头标定_我不是“耀”神的博客-CSDN博客）(照相机标定_星空的力量的博客-CSDN博客)

摄像头这种精密仪器对光学器件的要求较高，由于摄像头内部与外部的一些原因，生成的物体图像往往会发生畸变，为了避免数据源造成的误差，需要针对摄像头的参数进行标定。

（一）.相机标定的原理

1.1 相机如何成像：

相机成像系统中，共包含四个坐标系：世界坐标系、相机坐标系、图像坐标系、像素坐标系。

1.1.1 世界坐标系：

世界坐标系（world coordinate），也称为测量坐标系，是一个三维直角坐标系，以其为基准可以描述相机和待测物体的空间位置。世界坐标系的位置可以根据实际情况自由确定。

1.1.2 相机坐标系：

相机坐标系（camera coordinate），也是一个三维直角坐标系，原点位于镜头光心处，x、y轴分别与相面的两边平行，z轴为镜头光轴，与像平面垂直

1.1.3 像素坐标系、图像坐标系：

像素坐标系（pixel coordinate）

如上图，像素坐标系是一个二维直角坐标系，反映了相机CCD/CMOS芯片中像素的排列情况。原点位于图像的左上角，x轴、y轴分别于像面的两边平行。像素坐标系中坐标轴的单位是像素（整数）。

像素坐标系不利于坐标变换，因此需要建立图像坐标系，其坐标轴的单位通常为毫米（mm），原点是相机光轴与相面的交点（称为主点），即图像的中心点，轴、轴分别与轴、轴平行。故两个坐标系实际是平移关系，即可以通过平移就可得到。

1.2 相机标定的目的：

求出相机的内、外参数，以及畸变参数。

标定相机后通常是想做两件事：一个是由于每个镜头的畸变程度各不相同，通过相机标定可以校正这种镜头畸变矫正畸变，生成矫正后的图像；另一个是根据获得的图像重构三维场景。

1.2 相机标定的总体原理：

摄像机标定（Camera calibraTIon）简单来说就是从世界坐标系换到图像坐标系的过程，也就是求最终的投影矩阵的过程。

（二）.相机标定步骤（张正友法解释三步骤详解张正友标定法_谜之_摄影爱好者的博客-CSDN博客）

1）、准备一个张正友标定法的棋盘格，棋盘格大小已知，用相机对其进行不同角度的拍摄，得到一组图像；

2）、对图像中的特征点如标定板角点进行检测，得到标定板角点的像素坐标值，根据已知的棋盘格大小和世界坐标系原点，计算得到标定板角点的物理坐标值；

3）、求解内参矩阵与外参矩阵。

根据物理坐标值和像素坐标值的关系，求出 H 矩阵，进而构造 V 矩阵，求解 B 矩阵，利用B矩阵求解相机内参矩阵 A ，最后求解每张图片对应的相机外参矩阵 :

4）、求解畸变参数。

构造 D 矩阵，计算径向畸变参数；

5）、利用L-M（Levenberg-Marquardt）算法对上述参数进行优化

（三）、实现

3.1 数据准备

对标定板在不同角度拍照，得到10张图片。得到如下类似的图片。

3.2 代码(Python+OpenCV)附录一

出错

cv2.error: OpenCV(4.6.0) D:\a\opencv-python\opencv-python\opencv\modules\imgproc\src\cornersubpix.cpp:58: error: (-215:Assertion failed) count >= 0 in function 'cv::cornerSubPix'

（cornersubpix.cpp:58: error: (-215:Assertion failed) count >= 0 in function 'cv::cornerSubPix'_知足的的亚伯圆的博客-CSDN博客）

原因1：检查棋盘格规格是几行几列的，内角点wh的个数，可能是数组大小超出了

原因2：images = glob.glob('D:/sensor*.png')
images = glob.glob('C:/Users/28635/Desktop/sensor/*.png')

多了一个斜杠在*前

原因3：数据类型改为32位或64位浮点型

出错corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)

这里我放弃了，用下面的matlab求畸变参数

3.3MATLAB（最详细、最完整的相机标定讲解_卍卐没想到的博客-CSDN博客）（Matlab相机标定方法及主要参数含义，坐标变换过程_matlab相机标定参数怎么用_神域炎阳的博客-CSDN博客）

利用matlab的cameraCalibrator 应用

Matlab里面的app下载不了（没有权限许可）

天哪！！！

经过我重新卸载程序再安装之后（里面的工具箱全选了）

然后点那个cameracalibrator

cameraParams.ImageSize	图像大小
cameraParams.RadialDistortion	径向畸变
cameraParams.TangentialDistortion	切向失真
cameraParams.WorldPoints	世界坐标
cameraParams.WorldUnits	世界单位
cameraParams.EstimateSkew	估计偏差
cameraParams.NumRadialDistortionCoefficients	径向畸变系数
cameraParams.EstimateTangentialDistortion	估计切向失真
cameraParams.ReprojectionErrors	投影误差
cameraParams.DetectedKeypoints	检测到关键点
cameraParams.RotationVectors	旋转矢量
K	K
cameraParams.NumPatterns	数字模式
cameraParams.Intrinsics	内参
cameraParams.PatternExtrinsics	外参
cameraParams.FocalLength	焦距
cameraParams.PrincipalPoint	主点
cameraParams.Skew	倾斜
cameraParams.MeanReprojectionError	投影错误
cameraParams.ReprojectedPoints	重投影点

建议选生成matlab脚本，这样在直接在命令行输出我们要的结果。

cameraParams.PatternExtrinsics 外参——1*1 rigidtform3d——cameraParams.PatternExtrinsics(1, 1).Translation位置 cameraParams.PatternExtrinsics(1, 1).R角度

3.4 halcon（了解这个也行，但是没用到）

二、手眼标定（机器人抓取（五）—— 手眼标定 hand eye calibration_hand_eye_calibration_zxxRobot的博客-CSDN博客）

区分

eye-to-hand 眼在手外：相机固定在机器人外的固定底座上。这种情况下，需要将标定板固定在机器人末端，求解的量为相机坐标系和机器人坐标系之间的位姿关系。

eye-in-hand 眼在手上：相机固定在机器人末端。这种情况下，需要将标定板固定在地面上，机器人坐标系和标定板坐标系的转换关系始终不变。求解的量为相机坐标和机器人末端坐标系的位姿关系。（我们做的是这个）

手眼标定步骤

事先准备：机械臂可以按照正常的位姿移动，并且能够正常的输出机械臂末端姿态；机械臂能够稳定搭载相机；相机能够正确稳定地检测到物体，并且能够正确输出位置参数。

通过机械臂上面的相机，对待测物体进行位姿检测，并且输出参数。

输出机械臂末端的位置参数。

重复2、3步骤，采集十组以上有效参数。

将采集的有效参数放入代码中进行计算。

输出手眼转换矩阵。

代码：见https://blog.csdn.net/hellohake/article/details/104808149

原文链接：https://blog.csdn.net/knightsinghua/article/details/125328920

注：手眼标定推导过程参考网址：

https://zhuanlan.zhihu.com/p/78855930

https://zhuanlan.zhihu.com/p/93183788

https://yongqi.blog.csdn.net/article/details/107421641

https://blog.csdn.net/zxxxiazai/article/details/107979149

https://blog.csdn.net/qq_34935373/article/details/103727968?utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-12.control&dist_request_id=&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-12.control

opencv4手眼标定（c语言编写）

手眼标定（一）：Opencv4实现手眼标定及手眼系统测试_opencv 手眼标定_jian_1996的博客-CSDN博客

1 程序环境

编译器：Visual Studio 2023；

Opencv版本： Opencv4.6.0版本；

2 原理

（1）主要使用Opencv的calibrateHandEye()函数，其中包括五种方法，相关文献见链接link。

（2）本代码主要根据Tsai两步法进行，利用公式AX=XB，先计算旋转矩阵R，再计算平移矩阵T。

（3）Tsai两步法的原理可参考博主一“手眼标定”： link；博主二“机械臂的手眼标定 opencv实现”：link.

3.bug

3.1（解决： error C2664: “cv::Mat::Mat(const cv::cuda::GpuMat &)”: 无法将参数 1 从“IplImage *”转换为“const cv::Mat &_“cv::mat::mat(cv::mat &&)”: 无法将参数 1 从“iplimage *”转_人工智能博士的博客-CSDN博客）

（1）通过张正友法标定相机的内参矩阵和畸变参数；(相机标定(一)——内参标定与程序实现_内参标定算法_white_Learner的博客-CSDN博客相机标定(一)——内参标定与程序实现)

（2）标定相机外参矩阵，用于图像坐标与世界坐标的转换；(相机标定(二)——图像坐标与世界坐标转换_opencv像素坐标转化为经纬度_white_Learner的博客-CSDN博客相机标定(二)——图像坐标与世界坐标转换)

（3）设置N个特征点(N > 3 N>3N>3)，计算其世界坐标，移动机械臂工作末端到特征点，记录末端坐标，获得N组数据；

（4）计算两组数据的R RR和t tt，其中特征点世界坐标为A组数据，末端坐标为B组数据；

备注：

计算两组数据的变换矩阵实际上为3D-3D的位姿估计问题，可用迭代最近点（Iterative Closest Point, ICP）求解，实现方法有两种：

利用线性代数的求解（主要是 SVD）（建议采用该方法）

利用非线性优化方式的求解（类似于 Bundle Adjustment）

Opencv标定（OpenCV手眼标定（calibrateHandeye()）_hellohake的博客-CSDN博客）

能跑

2. ① 把标定板固定在机械臂末端

     ② 使用相机拍摄不同机械臂姿态下的标定板图片13组

Translation vectors (millimeters):

0.0058461，0.0126577 ，0.4963047，-0.0456，0.1479，-0.0044，

0.0060335， -0.0266305，0.5001159 ，-0.0378，0.1507，-0.0065，

0.0049899 ， -0.0074784 ，0.4472318，-0.0470，0.1375，-0.0017，

0.0700471 ， -0.0079558 ，0.4462671，-0.0444，0.0528，-0.0014，

0.0576721， -0.0188012，0.4847139，-0.0299，0.0627，-0.0047，

0.0473181，0.0063320，0.5490494，-0.0338，0.0635，0.0016，

0.1651600 ，0.0057758，0.5358916，-0.0263，-0.0729，0.0025，

0.1607357，0.0267904， 0.5614160 ，-0.0141，-0.0671，0.0093，

0.1810469，-0.0137355，0.5122996 ，-0.0360，-0.0923，0.0001，

0.0284403 ，0.0033407 ，0.5161650，-0.0465，0.1737，-0.0000，

0.0137029 ，-0.0628223 ，0.4964629 ，-0.0468，0.1546，-0.0048，

0.0365659，0.0062588 ，0.5700691 ，-0.0402，0.1796，0.0028，

0.0108755 ，-0.0347908 ，0.4440656，-0.0386，0.1566，-0.0049

Rotation vectors:

-0.0456，0.1479，-0.0044，

-0.0378，0.1507，-0.0065，

-0.0470，0.1375，-0.0017，

-0.0444，0.0528，-0.0014，

-0.0299，0.0627，-0.0047，

-0.0338，0.0635，0.0016，

-0.0263，-0.0729，0.0025，

-0.0141，-0.0671，0.0093，

-0.0360，-0.0923，0.0001，

-0.0465，0.1737，-0.0000，

-0.0468，0.1546，-0.0048，

-0.0402，0.1796，0.0028，

-0.0386，0.1566，-0.0049，

机械臂角度

	1	1770	990	1904
	2	1845	900	1904
	3	1650	840	1904
	4	1650	840	1859
	5	1785	900	1859
	6	1920	1035	1859
	7	1920	1035	1799
	8	1920	1140	1799
	9	1890	945	1799
	10	1830	990	1919
	11	1830	765	1919
	12	1965	1050	1919
	13	1665	765	1919

     ③ 读取并记录对应相机拍摄标定板时机械臂位姿13组

MATLAB手眼标定(手眼标定学习总结：原理、Tsai方法和Matlab代码_手眼标定代码matlab_larry_dongy的博客-CSDN博客)

Matlab机械臂仿真

matlab中Link函数与SeriaLink函数学习笔记_seriallink函数_红烧肉不好吃的博客-CSDN博客 （从）

matlab机械臂建模运动学仿真+轨迹规划_matlab机械臂仿真_明月醉窗台的博客-CSDN博客

三、读取出摄像头信息并确定目标物体的位姿（K210实现单目测算距离—以21年电赛F题为例_k210双目测距_栈溢出工程师的博客-CSDN博客）但这里我们抓取用一个摄像机

这里我们的物品的姿态是固定的，所以姿态预估应该不用做，只用找到物块的三维坐标，让爪子运动到预设位置就可以实现抓取了

识别色块并且串口输出最大色块中心坐标（K210快速上手教程（色块、人脸、声源识别等）_k210教程_Smalld_cat的博客-CSDN博客）这里有代码

这个坐标是色块中心相对于相机的

求解实际距离的时候是用到了已知尺寸的物体来参照；那可不可以通过已知焦距的镜头和镜头距离物体的高度以及物体距离图像中心的距离来推算出物体的坐标。

四、转换成相对于机械臂底座的世界坐标（教程上面的），但是我们是eye in hand直接利用色块中心相对机械爪的坐标，然后使机械爪运动一个矢量可能会更方便一点

五、计算机械臂末端执行器抓取物体时的位姿

六、计算各个舵机旋转角度

七、驱动舵机实行抓取

八、直接利用固定工作组让物品放置在小车

K210与stm32通信（【K210】K210学习笔记五——串口通信_k210串口通信_欠羽的博客-CSDN博客）

K210学习里面目录库函数api手册有代码（K210学习笔记（一）——什么是K210？_k210芯片_纸箱里的猫咪的博客-CSDN博客）

Maixpy网站（MaixPy 常见问题 - Sipeed Wiki）