Opencv 机械臂手眼标定（九点标定法）

z13228604287 · 发表于 2022-11-18 14:00:23

本帖最后由 z13228604287 于 2022-11-18 19:19 编辑

QQ图片20221118135556.png

窗口程序集名	保留	保留	备注
九点标定			一般标定前，先进行畸变校正

子程序名	返回值类型	公开	备注
_启动子程序	整数型		本子程序在程序启动后最先执行

变量名	类型	静态	数组	备注
输入图像	多维矩阵类
内侧	多维矩阵类
标定板图坐标	点2f类		0	9个圆中心的坐标
机械臂坐标	点2f类		9	标定板9个圆中心的坐标想对应9个机械臂坐标
映射矩阵	多维矩阵类
A	双精度小数型
B	双精度小数型
C	双精度小数型
D	双精度小数型
E	双精度小数型
F	双精度小数型
i	整数型
计算机械坐标	点2f类

视觉_创建窗口 (“小白鼠”, #窗口_标准 )
' 1 首先我们需要准备一块，标定板。如果条件不足，可以使用白纸画上九个圆进行代替
输入图像＝视觉_图像解码 ( #标定板图1, #读图_彩色 )

如果真 (输入图像.空 ())

视觉_控制台输出 (“请先加载图片!”, )

视觉_控制台命令 (“pause”)

返回 (-1)
' 2 相机位置，机械手位置全部固定好，标定针固定在机械手上，固定好后不能够再移动。标定针的位置一定要与夹手或吸盘之内的工具同一位置高度。
' 3 将标定板放到相机下方，位置区域要与机械手工作的区域一样，包括高度必须尽量一致，这是标定准确度的关键。
' 4 调整好相机焦距，拍照，然后识别9个圆圆心的坐标并进行记录。
获取标定板坐标 (输入图像, 标定板图坐标, 20, 300)

如果真 (取数组成员数 (标定板图坐标) ＝ 0)

视觉_控制台输出 (“图像点位检测失败!”, )

视觉_控制台命令 (“pause”)

返回 (-1)
' 5 将机械手依次移动到9个圆的圆心位置，记下机械手坐标
' 这里输入机械臂相对与标定板圆心位置的对应坐标
机械臂坐标 [1].初始化 (1007, 729)
机械臂坐标 [2].初始化 (1301, 729)
机械臂坐标 [3].初始化 (1595, 729)
机械臂坐标 [4].初始化 (1007, 1023)
机械臂坐标 [5].初始化 (1301, 1022)
机械臂坐标 [6].初始化 (1596, 1022)
机械臂坐标 [7].初始化 (1006, 1317)
机械臂坐标 [8].初始化 (1301, 1317)
机械臂坐标 [9].初始化 (1596, 1316)
' 做完以上五步，我们会得到两个点集。一个为9个圆圆心坐标（标定板图坐标），一个为9个圆心对应的机械手坐标（机械臂坐标）。
映射矩阵＝视觉_计算仿射变换2D (标定板图坐标, 机械臂坐标, 内侧, 8, 3, 2000, 0.99, 10)
视觉_控制台输出 (“内侧:\n%mat\n”, 内侧)

如果真 (映射矩阵.空 () ＝假)

' 得出来的6个双精度小数型的参数，就是我们此次标定最终得到的标定参数了。

A ＝映射矩阵.取元素 (0, 0)

B ＝映射矩阵.取元素 (0, 1)

C ＝映射矩阵.取元素 (0, 2)

D ＝映射矩阵.取元素 (1, 0)

E ＝映射矩阵.取元素 (1, 1)

F ＝映射矩阵.取元素 (1, 2)

视觉_控制台输出 (“\n参： A:%D B:%D\nC:%D D:%D\nE:%D F:%D\n\n”, A, B, C, D, E, F)

' 后面我们把检测得到的图像坐标（x1，y1）代入，就可以得到与之相对应的机械手坐标（x2,y2）

' x2= (A * x1) + B * y1 + C)

' y2= (D * x1) + E * y1+ F)

计次循环首 (取数组成员数 (标定板图坐标), i)

计算机械坐标.左边＝ A × 标定板图坐标 [i].左边＋ B × 标定板图坐标 [i].顶边＋ C

计算机械坐标.顶边＝ D × 标定板图坐标 [i].左边＋ E × 标定板图坐标 [i].顶边＋ F

视觉_控制台输出 (“返回的机械臂坐标： %point2f\n”, 计算机械坐标)

计次循环尾 ()

视觉_显示图像 (“小白鼠”, 输入图像)
视觉_等待按键 (0)
返回 (0) ' 可以根据您的需要返回任意数值

子程序名	返回值类型	公开	备注
获取标定板坐标
参数名	类型	参考	可空	数组	备注
标定板图	多维矩阵类
标定板图坐标	点2f类
最小半径	双精度小数型
最大半径	双精度小数型

变量名	类型	静态	数组	备注
灰度图	多维矩阵类
二值化	多维矩阵类
轮廓集	点2i二维数组类
i	整数型
矩形	矩形2i类
面积	双精度小数型
圆心	点2f类
半径	小数型
轮廓	整数型
计数	整数型

视觉_颜色空间转换 (标定板图, 灰度图, #颜色_BGR转GRAY, 0)
视觉_高斯滤波 (灰度图, 灰度图, 尺寸2i (3, 3), 0, 0, 4)
视觉_阈值 (灰度图, 二值化, 0, 255, 位或 ( #阈值_二进制, #阈值_大津 ))
视觉_查找轮廓 (二值化, 轮廓集, , #检测_树, #逼近_连续, ) ' 查找轮廓

计次循环首 (轮廓集.尺寸 (), i)

矩形＝视觉_最大轮廓矩形V (轮廓集, i) ' //查找每个轮廓的外接矩形

面积＝视觉_轮廓面积V (轮廓集, i, 假)

视觉_最小包围圆形V (轮廓集, i, 圆心, 半径)

如果真 (矩形.宽度＞最小半径 × 2 且矩形.宽度＞最小半径 × 2 且矩形.宽度＜最大半径 × 2 且矩形.宽度＜最大半径 × 2 且矩形.宽度 × 1 ÷ 矩形.宽度 ≥ 0.8 且矩形.宽度 × 1 ÷ 矩形.宽度 ≤ 1.2 且面积 ÷ (3.1415926 × 半径 × 半径) ＞ 0.9)

视觉_圆f (标定板图, 圆心, 半径, 标量 (0, 255, 0), 5, 8, 0)

视觉_圆f (标定板图, 圆心, 3, 标量 (0, 255, 255), -1, 8, 0)

加入成员 (标定板图坐标, 圆心)

计次循环尾 ()
点位排序 (标定板图坐标, 1, 取数组成员数 (标定板图坐标))

计次循环首 (取数组成员数 (标定板图坐标), i)

视觉_放置文本f (标定板图, “C” ＋到文本 (i), 标定板图坐标 [i], 0, 3.5, 标量 (255, 0, 255), 6, 8, 假)

计次循环尾 ()

子程序名	返回值类型	公开	备注
点位排序			来源于 https://bbs.125.la/forum.php?mod ... F%E6%8E%92%E5%BA%8F
参数名	类型	参考	可空	数组	备注
标定板图坐标	点2f类
起始	整数型
数组长度	整数型

变量名	类型	静态	数组	备注
i	整数型
j	整数型
tmp	点2f类

i ＝起始
j ＝数组长度
tmp ＝标定板图坐标 [起始]

判断循环首 (i ＜ j)

判断循环首 (i ＜ j 且位置换算 (标定板图坐标 [j]) ≥ 位置换算 (tmp))

j ＝ j － 1

判断循环尾 ()

标定板图坐标 [i] ＝标定板图坐标 [j]

判断循环首 (i ＜ j 且位置换算 (标定板图坐标 [i]) ≤ 位置换算 (tmp))

i ＝ i ＋ 1

判断循环尾 ()

标定板图坐标 [j] ＝标定板图坐标 [i]

判断循环尾 ()
标定板图坐标 [i] ＝ tmp

如果真 (起始＜ i － 1)

点位排序 (标定板图坐标, 起始, i － 1)

如果真 (i ＋ 1 ＜数组长度)

点位排序 (标定板图坐标, i ＋ 1, 数组长度)

子程序名	返回值类型	公开	备注
位置换算	整数型
参数名	类型	参考	可空	数组	备注
点位	点2f类

返回 (点位.左边＋点位.顶边 × 3)

i支持库列表	支持库注释
OpenCV	(未知支持库)

.版本 2<br />
.支持库 OpenCV<br />
<br />
.程序集 九点标定, , , 一般标定前，先进行畸变校正<br />
<br />
.子程序 _启动子程序, 整数型, , 本子程序在程序启动后最先执行<br />
.局部变量 输入图像, 多维矩阵类<br />
.局部变量 内侧, 多维矩阵类<br />
.局部变量 标定板图坐标, 点2f类, , "0", 9个圆中心的坐标<br />
.局部变量 机械臂坐标, 点2f类, , "9", 标定板9个圆中心的坐标想对应9个机械臂坐标<br />
.局部变量 映射矩阵, 多维矩阵类<br />
.局部变量 A, 双精度小数型<br />
.局部变量 B, 双精度小数型<br />
.局部变量 C, 双精度小数型<br />
.局部变量 D, 双精度小数型<br />
.局部变量 E, 双精度小数型<br />
.局部变量 F, 双精度小数型<br />
.局部变量 i, 整数型<br />
.局部变量 计算机械坐标, 点2f类<br />
<br />
视觉_创建窗口 (“小白鼠”, #窗口_标准)<br />
' 1 首先我们需要准备一块，标定板。如果条件不足，可以使用白纸画上九个圆进行代替<br />
输入图像 ＝ 视觉_图像解码 (#标定板图1, #读图_彩色)<br />
.如果真 (输入图像.空 ())<br />
    视觉_控制台输出 (“请先加载图片!”, )<br />
    视觉_控制台命令 (“pause”)<br />
    返回 (-1)<br />
.如果真结束<br />
' 2 相机位置，机械手位置全部固定好，标定针固定在机械手上，固定好后不能够再移动。标定针的位置一定要与夹手或吸盘之内的工具同一位置高度。<br />
' 3 将标定板放到相机下方，位置区域要与机械手工作的区域一样，包括高度必须尽量一致，这是标定准确度的关键。<br />
' 4 调整好相机焦距，拍照，然后识别9个圆圆心的坐标并进行记录。<br />
获取标定板坐标 (输入图像, 标定板图坐标, 20, 300)<br />
.如果真 (取数组成员数 (标定板图坐标) ＝ 0)<br />
    视觉_控制台输出 (“图像点位检测失败!”, )<br />
    视觉_控制台命令 (“pause”)<br />
    返回 (-1)<br />
.如果真结束<br />
' 5 将机械手依次移动到9个圆的圆心位置，记下机械手坐标<br />
<br />
' 这里输入  机械臂  相对与  标定板圆心位置的对应 坐标<br />
机械臂坐标 [1].初始化 (1007, 729)<br />
机械臂坐标 [2].初始化 (1301, 729)<br />
机械臂坐标 [3].初始化 (1595, 729)<br />
机械臂坐标 [4].初始化 (1007, 1023)<br />
机械臂坐标 [5].初始化 (1301, 1022)<br />
机械臂坐标 [6].初始化 (1596, 1022)<br />
机械臂坐标 [7].初始化 (1006, 1317)<br />
机械臂坐标 [8].初始化 (1301, 1317)<br />
机械臂坐标 [9].初始化 (1596, 1316)<br />
' 做完以上五步，我们会得到两个点集。一个为9个圆圆心坐标（标定板图坐标），一个为9个圆心对应的机械手坐标（机械臂坐标）。<br />
映射矩阵 ＝ 视觉_计算仿射变换2D (标定板图坐标, 机械臂坐标, 内侧, 8, 3, 2000, 0.99, 10)<br />
视觉_控制台输出 (“内侧:\n%mat\n”, 内侧)<br />
.如果真 (映射矩阵.空 () ＝ 假)<br />
    ' 得出来的6个 双精度小数型的参数，就是我们此次标定最终得到的标定参数了。<br />
    A ＝ 映射矩阵.取元素 (0, 0)<br />
    B ＝ 映射矩阵.取元素 (0, 1)<br />
    C ＝ 映射矩阵.取元素 (0, 2)<br />
    D ＝ 映射矩阵.取元素 (1, 0)<br />
    E ＝ 映射矩阵.取元素 (1, 1)<br />
    F ＝ 映射矩阵.取元素 (1, 2)<br />
    视觉_控制台输出 (“\n参： A:%D  B:%D\nC:%D  D:%D\nE:%D  F:%D\n\n”, A, B, C, D, E, F)<br />
    ' 后面我们把检测得到的图像坐标（x1，y1）代入，就可以得到与之相对应的机械手坐标（x2,y2）<br />
    ' x2= (A * x1) + B * y1 + C)<br />
    ' y2= (D * x1) + E * y1+ F)<br />
<br />
    .计次循环首 (取数组成员数 (标定板图坐标), i)<br />
        计算机械坐标.左边 ＝ A × 标定板图坐标 <i>.左边 ＋ B × 标定板图坐标 <i>.顶边 ＋ C<br />
        计算机械坐标.顶边 ＝ D × 标定板图坐标 <i>.左边 ＋ E × 标定板图坐标 <i>.顶边 ＋ F<br />
        视觉_控制台输出 (“返回的机械臂坐标： %point2f\n”, 计算机械坐标)<br />
    .计次循环尾 ()<br />
.如果真结束<br />
<br />
视觉_显示图像 (“小白鼠”, 输入图像)<br />
视觉_等待按键 (0)<br />
<br />
返回 (0)  ' 可以根据您的需要返回任意数值<br />
<br />
.子程序 获取标定板坐标<br />
.参数 标定板图, 多维矩阵类<br />
.参数 标定板图坐标, 点2f类, 参考 数组<br />
.参数 最小半径, 双精度小数型<br />
.参数 最大半径, 双精度小数型<br />
.局部变量 灰度图, 多维矩阵类<br />
.局部变量 二值化, 多维矩阵类<br />
.局部变量 轮廓集, 点2i二维数组类<br />
.局部变量 i, 整数型<br />
.局部变量 矩形, 矩形2i类<br />
.局部变量 面积, 双精度小数型<br />
.局部变量 圆心, 点2f类<br />
.局部变量 半径, 小数型<br />
.局部变量 轮廓, 整数型<br />
.局部变量 计数, 整数型<br />
<br />
视觉_颜色空间转换 (标定板图, 灰度图, #颜色_BGR转GRAY, 0)<br />
视觉_高斯滤波 (灰度图, 灰度图, 尺寸2i (3, 3), 0, 0, 4)<br />
视觉_阈值 (灰度图, 二值化, 0, 255, 位或 (#阈值_二进制, #阈值_大津))<br />
视觉_查找轮廓 (二值化, 轮廓集, , #检测_树, #逼近_连续, )  ' 查找轮廓<br />
.计次循环首 (轮廓集.尺寸 (), i)<br />
    矩形 ＝ 视觉_最大轮廓矩形V (轮廓集, i)  ' //查找每个轮廓的外接矩形<br />
    面积 ＝ 视觉_轮廓面积V (轮廓集, i, 假)<br />
    视觉_最小包围圆形V (轮廓集, i, 圆心, 半径)<br />
    .如果真 (矩形.宽度 ＞ 最小半径 × 2 且 矩形.宽度 ＞ 最小半径 × 2 且 矩形.宽度 ＜ 最大半径 × 2 且 矩形.宽度 ＜ 最大半径 × 2 且 矩形.宽度 × 1 ÷ 矩形.宽度 ≥ 0.8 且 矩形.宽度 × 1 ÷ 矩形.宽度 ≤ 1.2 且 面积 ÷ (3.1415926 × 半径 × 半径) ＞ 0.9)<br />
        视觉_圆f (标定板图, 圆心, 半径, 标量 (0, 255, 0), 5, 8, 0)<br />
        视觉_圆f (标定板图, 圆心, 3, 标量 (0, 255, 255), -1, 8, 0)<br />
        加入成员 (标定板图坐标, 圆心)<br />
    .如果真结束<br />
<br />
.计次循环尾 ()<br />
点位排序 (标定板图坐标, 1, 取数组成员数 (标定板图坐标))<br />
.计次循环首 (取数组成员数 (标定板图坐标), i)<br />
    视觉_放置文本f (标定板图, “C” ＋ 到文本 (i), 标定板图坐标 <i>, 0, 3.5, 标量 (255, 0, 255), 6, 8, 假)<br />
.计次循环尾 ()<br />
<br />
.子程序 点位排序, , , 来源于 <a href="https://bbs.125.la/forum.php?mod=viewthread&tid=13662535&highlight=%E5%BF%AB%E9%80%9F%E6%8E%92%E5%BA%8F" target="_blank">https://bbs.125.la/forum.php?mod ... F%E6%8E%92%E5%BA%8F</a><br />
.参数 标定板图坐标, 点2f类, 参考 数组<br />
.参数 起始, 整数型<br />
.参数 数组长度, 整数型<br />
.局部变量 i, 整数型<br />
.局部变量 j, 整数型<br />
.局部变量 tmp, 点2f类<br />
<br />
i ＝ 起始<br />
j ＝ 数组长度<br />
tmp ＝ 标定板图坐标 [起始]<br />
.判断循环首 (i ＜ j)<br />
    .判断循环首 (i ＜ j 且 位置换算 (标定板图坐标 [j]) ≥ 位置换算 (tmp))<br />
        j ＝ j － 1<br />
    .判断循环尾 ()<br />
    标定板图坐标 <i> ＝ 标定板图坐标 [j]<br />
    .判断循环首 (i ＜ j 且 位置换算 (标定板图坐标 <i>) ≤ 位置换算 (tmp))<br />
        i ＝ i ＋ 1<br />
    .判断循环尾 ()<br />
    标定板图坐标 [j] ＝ 标定板图坐标 <i><br />
.判断循环尾 ()<br />
标定板图坐标 <i> ＝ tmp<br />
.如果真 (起始 ＜ i － 1)<br />
    点位排序 (标定板图坐标, 起始, i － 1)<br />
.如果真结束<br />
.如果真 (i ＋ 1 ＜ 数组长度)<br />
    点位排序 (标定板图坐标, i ＋ 1, 数组长度)<br />
.如果真结束<br />
<br />
<br />
.子程序 位置换算, 整数型<br />
.参数 点位, 点2f类<br />
<br />
返回 (点位.左边 ＋ 点位.顶边 × 3)