网站头部修改,机关网站建设创新,wordpress元器件,网站建设中是因为没有ftp上传吗Eye-in-hand和Eye-to-hand问题求解和实验
2018年12月07日 00:00:40 百川木易 阅读数 3018
2018/12/5 By Yang Yang#xff08;yangyangipp.ac.cn#xff09;
本文所有源码和仿真场景文件全部公开#xff0c;点击Gitee仓库链接。 文章目录 问题描述Eye-in-hand问题求解公式…Eye-in-hand和Eye-to-hand问题求解和实验
2018年12月07日 00:00:40 百川木易 阅读数 3018
2018/12/5 By Yang Yangyangyangipp.ac.cn
本文所有源码和仿真场景文件全部公开点击Gitee仓库链接。 文章目录 问题描述Eye-in-hand问题求解公式推导 Eye-in-hand变换矩阵求解方程仿真实验 搭建手眼系统仿真环境 生成棋盘图生成仿真棋盘标定板编写手眼系统场景文件编写基于RobWorkStudio的标定图像采集模拟程序编写基于OpenCV的标定计算程序 棋盘角点和位姿检测标定计算标定结果比较Eye-to-Hand问题求解公式推导 Eye-to-Hand变换矩阵求解方程问题描述
机器人和摄像机的手眼标定问题分为两类构型
eye-to-hand摄像机固定与机器人基坐标系相对位置不变。eye-in-hand摄像机安装在机器人末端随着机器人一起移动。
本文主要介绍eye-in-hand构型的标定方法和仿真实验。对于eye-to-hand构型手眼系统其标定方法与eye-in-hand构型的手眼系统没有本质区别。
Eye-in-hand问题求解公式推导
Eye-in-hand变换矩阵 weHi∗ecHwgH∗gcHi
ewHi∗ceHgwH∗cgHi weHj∗ecHwgH∗gcHj
ewHj∗ceHgwH∗cgHj
其中
weHi
ewHi 和 weHjewHj 表示机器人末端endTCP坐标系相对于机器人基座坐标系的齐次变换矩阵分别对应第 ii 次和第 j
j 次样本。ecHceH 表示摄像机camera坐标系相对于机器人末端endTCP坐标系的齐次变换矩阵这是手眼标定问题的求解目标。wgHgwH 表示棋盘图grid相对于世界坐标系也就是机器人基坐标系的齐次变换矩阵由于在整个标定过程中棋盘图的位置不能变动因此 wgHgwH 是一个常量矩阵。gcHicgHi 和 gcHjcgHj 表示摄像机坐标系相对于棋盘图坐标系的齐次变换矩阵。注意采用OpenCV的solvePnP函数得到的是棋盘图相对于摄像机坐标系的齐次矩阵即 cgHigcHi 和 cgHj gcHj因此代入以上两个公式时需要进行求逆计算。 齐次变换矩阵求逆方法 对于原齐次矩阵 ABT[ABR0ApB01] BAT[BAR0ApB01]其逆矩阵为 BAT[ABRT0−ABRT∗ApB01] ABT[BART0−BART∗ApB01] 求解方程 联立上述的两个公式 weHi∗ecH∗(gcHi)−1wgH ewHi∗ceH∗(cgHi)−1gwH weHj∗ecH∗(gcHj)−1wgH ewHj∗ceH∗(cgHj)−1gwH 消除常量矩阵 wgH gwH weHi∗ecH∗(gcHi)−1weHj∗ecH∗(gcHj)−1 ewHi∗ceH∗(cgHi)−1ewHj∗ceH∗(cgHj)−1 上式两边同时左乘 (weHj)−1 (ewHj)−1 得 (weHj)−1∗weHi∗ecH∗(gcHi)−1ecH∗(gcHj)−1 (ewHj)−1∗ewHi∗ceH∗(cgHi)−1ceH∗(cgHj)−1 上式两边同时右乘 gcHi cgHi 得 (weHj)−1∗weHi∗ecHecH∗(gcHj)−1∗gcHi (ewHj)−1∗ewHi∗ceHceH∗(cgHj)−1∗cgHi 考虑到(gcHj)−1(cgHj) (cgHj)−1(gcHj) 和 (gcHi)(cgHi)−1 (cgHi)(gcHi)−1因此上式可以转换为 (weHj)−1∗weHi∗ecHecH∗cgHj∗(cgHi)−1 (ewHj)−1∗ewHi∗ceHceH∗gcHj∗(gcHi)−1 令XecH XceHA(weHj)−1∗weHiA(ewHj)−1∗ewHi 以及 BcgHj∗(cgHi)−1 BgcHj∗(gcHi)−1则上式可以化简为 AXXB AXXB 以上方程为手眼标定的最终求解方程具体求解方式可以参考Tasi and Lenz1989的论文A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration。 仿真实验 以下描述通过仿真实验的方式进行手眼标定测试和验证的方法。大致流程如下 手眼系统仿真场景搭建。标定样本采集。包括摄像机图像采集以及对应的机器人关节构型采集。图像处理提取棋盘图角点并计算棋盘相对于摄像机坐标系的位姿。采用基于Tasi-lenz的公式进行计算。仿真实验的主要环境配置和使用到的工具有 操作系统Windows 7 64bit图像处理工具MatlabOpenCV机器人和摄像机仿真环境RobWork 需先安装QT、Boost等依赖库虚拟棋盘图编辑AC3D用于生成带贴图的棋盘标定板加载到手眼仿真场景中搭建手眼系统仿真环境 生成棋盘图 利用如下MATLAB代码ChessboardGenerator生成一个边长为30mm7行10列的棋盘图图像最终保存为ppm格式的图片文件。之所以选择生成7行10列的棋盘图是因为这样能够方便的利用OpenCV进行棋盘角点检测并且保证每张图像的棋盘图坐标系原点都是一致的角点检测顺序也不会发生改变。不应当使用正方形布置的棋盘图例如10行10列这样会导致OpenCV无法统一棋盘原点给下一步的手眼标定工作带来困难。 以下源码参考用Matlab编写的棋盘格图像生成程序 clc; close all;GridSize 30; %length of the square
m 7; %number of row
n 10; % number of col
margin 10; % white boarder size
I ones(m*GridSize2*margin,n*GridSize2*margin)*255;%the first grid is black
for i 1:mif mod(i,2)1for j 1:nif mod(j,2)1I(1(i-1)*GridSizemargin:i*GridSizemargin,...1(j-1)*GridSizemargin:j*GridSizemargin) 0;endendelsefor j 1:nif mod(j,2)0I(1(i-1)*GridSizemargin:i*GridSizemargin,...1(j-1)*GridSizemargin:j*GridSizemargin) 0;endendend
end
imshow(I);imwrite(I,chessboard.ppm);1234567891011121314151617181920212223242526272829最终生成棋盘图如下所示。 生成仿真棋盘标定板 处理步骤 首先利用ChessboardGenerator生成棋盘图格式为BMP。棋盘图的长边为10个方块短边为7个方块每个方块的尺寸为30个像素单位另外棋盘图还有10个像素单位的白边。因此整个图像的分辨率为320x230。打开AC3D软件新建一个Rect矩形通过MoveTo调整矩形的中心点坐标。通过, 调整矩形的尺寸。选中矩形对象在object-Texture设置纹理为步骤1中的ppm图像。图像看起来比较模糊测试提高生成图像分辨率能够获得更清晰的虚拟Chessboard Marker将矩形保存成chessboard.ac格式文件必要时用文本编辑器打开该.ac格式文件如有必要需修改纹理图像的路径为相对路径。编写手眼系统场景文件 RobWork是一个由南丹麦大学SDURobtics团队开发的一款用于机器人运动学、动力学计算和仿真的代码库其包含了一种用于机器人可视化仿真和摄像机仿真的软件RobWorkStudio。RobWork提供了一种基于XML文件的机器人场景构建方法该场景文件可以用于机器人的碰撞检测、路径规划、视觉传感器仿真等并且可以在RobWorStudio中进行场景的可视化。 将上一步生成的chessboard.ac文件添加到场景XML文件中并且在UR机器人末端设置一个虚拟摄像机最终得到的机器人运行场景如下图。 安装在UR末端的摄像机的定义方式可以参考RobWork官网上的教程本例中摄像机的定义方式如下如下 Frame nameHand-Eye refframeUR.TCPRPY0 -15 0/RPYPos0.1 0 0/Pos
/FrameFrame nameHand-Eye-Sim refframeHand-Eye typeMovableRPY0 0 180/RPYPos0 0 0/PosProperty nameCameraNameHand-Eye/PropertyProperty nameCamera31.0482 960 720/Property
/Frame 1234567891011编写基于RobWorkStudio的标定图像采集模拟程序 需编写一个RobWorkStudio的插件用于采集摄像机图片以及与图像对应的机器人TCP相对于机器人基坐标系的齐次变换矩阵并保存到本地硬盘上。 源码的运行要求安装RobWork安装方法参考官网链接。 编译本教程提供源码时需注意修改加载的机器人场景文件的路径。 编写基于OpenCV的标定计算程序 棋盘角点和位姿检测 int cornerRow 6;int cornerColum 9;double patternLength 30.0; //mmvectorrw::math::Transform3Ddouble chessboardPose;vectorPoint3d objectPoints;int pointNumber cornerRow * cornerColum;for (int i 0; i cornerRow; i){for (int j 0; j cornerColum; j){Point3d p;p.x patternLength * j;p.y patternLength * i;p.z 0;objectPoints.push_back(p);}}int imageResolutionX 960;int imageResolutionY 720;double fovy 31.0482 * 3.14159265 / 180;double focalLength (imageResolutionY / 2.0) / (tan(fovy / 2.0));Mat cameraMatrix Mat(3, 3, CV_64FC1, 0.0f);cameraMatrix.atdouble(0, 0) focalLength;cameraMatrix.atdouble(0, 2) imageResolutionX / 2.0f;cameraMatrix.atdouble(1, 1) focalLength;cameraMatrix.atdouble(1, 2) imageResolutionY / 2.0f;cameraMatrix.atdouble(2, 2) 1.0f;Mat distortionCoff(0, 0, 0, 0);cout \nCamera Matrix: \n cameraMatrix endl;//namedWindow(Display Image, WINDOW_AUTOSIZE);int imageNumber 25;for (int i 0; i imageNumber; i){char imageName[150];sprintf(imageName, ..\\..\\data\\test-data-01\\image%05d.bmp, i);Mat imageColored imread(imageName, 1);if (!imageColored.data){printf(No image data \n);return -1;}Mat image;cv::cvtColor(imageColored, image, CV_BGR2GRAY);Size patternSize(cornerColum, cornerRow);vectorPoint2f corners;bool patternfound findChessboardCorners(image, patternSize, corners, CALIB_CB_ADAPTIVE_THRESH);if (patternfound){printf(found chessboard of image%05d!\n, i);cornerSubPix(image, corners, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS CV_TERMCRIT_ITER, 30, 0.1));}Mat rvec; //reference to camera frameMat tvec; //reference to camera framesolvePnP(objectPoints, corners, cameraMatrix, distortionCoff, rvec, tvec);Mat chessbaordRotationMatrix; //reference to camera frameRodrigues(rvec, chessbaordRotationMatrix);rw::math::Transform3Ddouble t;t(0, 0) chessbaordRotationMatrix.atdouble(0, 0);t(0, 1) chessbaordRotationMatrix.atdouble(0, 1);t(0, 2) chessbaordRotationMatrix.atdouble(0, 2);t(0, 3) tvec.atdouble(0) / 1000; //mm to mt(1, 0) chessbaordRotationMatrix.atdouble(1, 0);t(1, 1) chessbaordRotationMatrix.atdouble(1, 1);t(1, 2) chessbaordRotationMatrix.atdouble(1, 2);t(1, 3) tvec.atdouble(1) / 1000; //mm to mt(2, 0) chessbaordRotationMatrix.atdouble(2, 0);t(2, 1) chessbaordRotationMatrix.atdouble(2, 1);t(2, 2) chessbaordRotationMatrix.atdouble(2, 2);t(2, 3) tvec.atdouble(2) / 1000; //mm to m//printTransformation(t);chessboardPose.push_back(t);}123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990其中一张棋盘图的角点检测结果如下图 标定计算 标定计算程序主要参考了经典手眼标定算法之Tsai-Lenz的OpenCV实现主要源码如下
cv::Mat skew(cv::Mat m)
{Mat t(3, 3, CV_64FC1);t.atdouble(0, 0) 0;t.atdouble(0, 1) - m.atdouble(2, 0);t.atdouble(0, 2) m.atdouble(1, 0);t.atdouble(1, 0) m.atdouble(2, 0);t.atdouble(1, 1) 0;t.atdouble(1, 2) - m.atdouble(0, 0);t.atdouble(2, 0) - m.atdouble(1, 0);t.atdouble(2, 1) m.atdouble(0, 0);t.atdouble(2, 2) 0;return t;
}void Tsai_HandEye(Mat Hcg, vectorMat Hgij, vectorMat Hcij)
{CV_Assert(Hgij.size() Hcij.size());int nStatus Hgij.size();Mat Rgij(3, 3, CV_64FC1);Mat Rcij(3, 3, CV_64FC1);Mat rgij(3, 1, CV_64FC1);Mat rcij(3, 1, CV_64FC1);double theta_gij;double theta_cij;Mat rngij(3, 1, CV_64FC1);Mat rncij(3, 1, CV_64FC1);Mat Pgij(3, 1, CV_64FC1);Mat Pcij(3, 1, CV_64FC1);Mat tempA(3, 3, CV_64FC1);Mat tempb(3, 1, CV_64FC1);Mat A;Mat b;Mat pinA;Mat Pcg_prime(3, 1, CV_64FC1);Mat Pcg(3, 1, CV_64FC1);Mat PcgTrs(1, 3, CV_64FC1);Mat Rcg(3, 3, CV_64FC1);Mat eyeM Mat::eye(3, 3, CV_64FC1);Mat Tgij(3, 1, CV_64FC1);Mat Tcij(3, 1, CV_64FC1);Mat tempAA(3, 3, CV_64FC1);Mat tempbb(3, 1, CV_64FC1);Mat AA;Mat bb;Mat pinAA;Mat Tcg(3, 1, CV_64FC1);for (int i 0; i nStatus; i){Hgij[i](Rect(0, 0, 3, 3)).copyTo(Rgij);Hcij[i](Rect(0, 0, 3, 3)).copyTo(Rcij);Rodrigues(Rgij, rgij);Rodrigues(Rcij, rcij);theta_gij norm(rgij);theta_cij norm(rcij);rngij rgij / theta_gij;rncij rcij / theta_cij;Pgij 2 * sin(theta_gij / 2)*rngij;Pcij 2 * sin(theta_cij / 2)*rncij;tempA skew(Pgij Pcij);tempb Pcij - Pgij;A.push_back(tempA);b.push_back(tempb);}//Compute rotationinvert(A, pinA, DECOMP_SVD);Pcg_prime pinA * b;Pcg 2 * Pcg_prime / sqrt(1 norm(Pcg_prime) * norm(Pcg_prime));PcgTrs Pcg.t();Rcg (1 - norm(Pcg) * norm(Pcg) / 2) * eyeM 0.5 * (Pcg * PcgTrs sqrt(4 - norm(Pcg)*norm(Pcg))*skew(Pcg));//Computer Translation for (int i 0; i nStatus; i){Hgij[i](Rect(0, 0, 3, 3)).copyTo(Rgij);Hcij[i](Rect(0, 0, 3, 3)).copyTo(Rcij);Hgij[i](Rect(3, 0, 1, 3)).copyTo(Tgij);Hcij[i](Rect(3, 0, 1, 3)).copyTo(Tcij);tempAA Rgij - eyeM;tempbb Rcg * Tcij - Tgij;AA.push_back(tempAA);bb.push_back(tempbb);}invert(AA, pinAA, DECOMP_SVD);Tcg pinAA * bb;Rcg.copyTo(Hcg(Rect(0, 0, 3, 3)));Tcg.copyTo(Hcg(Rect(3, 0, 1, 3)));Hcg.atdouble(3, 0) 0.0;Hcg.atdouble(3, 1) 0.0;Hcg.atdouble(3, 2) 0.0;Hcg.atdouble(3, 3) 1.0;
}123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121标定结果比较 在自定义的手眼标定场景文件中摄像机相对于机器人TCP的关系如下 Frame nameHand-Eye refframeUR.TCPRPY0 -15 0/RPYPos0.1 0 0/Pos
/Frame1234转换成4 × × 4的齐次变换矩阵如下 ecH⎡⎣⎢⎢⎢⎢0.96592600.25881900100−0.25881900.96592600.1001⎤⎦⎥⎥⎥⎥ ceH⎣⎢⎢⎡0.96592600.25881900100−0.25881900.96592600.1001⎦⎥⎥⎤ 通过Tasi-Lenz方法计算得出的不考虑摄像机参数误差摄像机相对于机器人TCP的关系如下 ecH⎡⎣⎢⎢⎢⎢0.9659861360843613−0.00031871107048437940.258593664493272700.00038437325583683220.9999999054504882−0.00020336227483621180−0.25859357522955960.00029584162685441490.965986167254855500.10012491158556857.92132967571962e−053.021520822331239e−051⎤⎦⎥⎥⎥⎥ ceH⎣⎢⎢⎡0.9659861360843613−0.00031871107048437940.258593664493272700.00038437325583683220.9999999054504882−0.00020336227483621180−0.25859357522955960.00029584162685441490.965986167254855500.10012491158556857.92132967571962e−053.021520822331239e−051⎦⎥⎥⎤ 通过对比以上两组齐次变换矩阵结果可以两者是十分接近的证明手眼标定结果是正确的。 Eye-to-Hand问题求解公式推导 Eye-to-Hand变换矩阵 wcH∗cgHiweHi∗egH cwH∗gcHiewHi∗geH wcH∗cgHjweHj∗egH cwH∗gcHjewHj∗geH 其中 wcHcwH表示摄像机camera坐标系相对于机器人基坐标系也是世界坐标world的齐次变换矩阵。这是Eye-to-hand问题的求解目标。cgHigcHi和cgHigcHi表示棋盘图grid相对于摄像机坐标系的齐次变换矩阵分别对应第 ii 次和第 jj 次样本。weHiewHi 和 weHjewHj 表示机器人末端endTCP坐标系相对于机器人基座坐标系的齐次变换矩阵分别对应第 ii 次和第 jj 次样本。egHgeH 表示棋盘图grid相对于机器人末端TCP的齐次变换矩阵由于在整个标定过程中棋盘图固定连接在机器人末端因此 egH geH 是一个常量矩阵。求解方程 改写以上公式 (weHi)−1∗wcH∗cgHiegH (ewHi)−1∗cwH∗gcHigeH (weHj)−1∗wcH∗cgHjegH(ewHj)−1∗cwH∗gcHjgeH 消除常量egHgeH (weHi)−1∗wcH∗cgHi(weHj)−1∗wcH∗cgHj(ewHi)−1∗cwH∗gcHi(ewHj)−1∗cwH∗gcHj 上式两边同时左乘weHjewHj得 weHj∗(weHi)−1∗wcH∗cgHiwcH∗cgHjewHj∗(ewHi)−1∗cwH∗gcHicwH∗gcHj 上式两边同时右乘(cgHi)−1(gcHi)−1 weHj∗(weHi)−1∗wcHwcH∗cgHj∗(cgHi)−1ewHj∗(ewHi)−1∗cwHcwH∗gcHj∗(gcHi)−1 令XwcHXcwHAweHj∗(weHi)−1AewHj∗(ewHi)−1BcgHj∗(cgHi)−1BgcHj∗(gcHi)−1可得 AXXB AXXB 以上方程是Eye-to-Hand标定的最终求解方程具体求解方式可以参考Tasi and Lenz1989的论文A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration。 具体的eye-to-hand求解仿真可以参考eye-in-hand标定求解仿真进行这里不再赘述
