工业机器人手眼标定（眼在手）方程AX=XB采用Tsai两步法进行求解的过程并分析

参考博客：
https://blog.csdn.net/yunlinwang/article/details/51622143

该博客内容基本把Tsai求解该方程的过程已经阐述清楚了。只是没有写一个反对称矩阵，虽然简单，还是给出可以直接运行的程序吧！！！下面这个程序在配置好Opencv环境之后就可以直接运行。

Rodrigues(Rgij, rgij);该函数是根据旋转矩阵求旋转向量，即罗德里格斯公式
theta_gij = norm(rgij);该函数是求向量的2范数，也就是求向量的模
skew(Pgij + Pcij);（自定义），求一个向量的反对称矩阵。
具体原理可以参考上面博客。以下只给出直接运行的代码。

需要注意的是：该方法求解该方程只适合A和B是位姿矩阵的形式，旋转矩阵加平移向量。旋转矩阵部分满足旋转矩阵的约束条件，向量单位化且两两正交

#include<iostream>
#include <vector>
#include <functional>
#include <algorithm>
#include <string>
#include<map>
#include <opencv2/opencv.hpp>
using namespace cv;
using namespace std;
#define PRINT_INT(x, y) x#y
Mat skew( Mat res )
{Mat result = (Mat_<double>(3, 3) << 0, -res.at<double>(2), res.at<double>(1),res.at<double>(2), 0, -res.at<double>(0),-res.at<double>(1), res.at<double>(0), 0);return result;
}void Tsai_HandEye(Mat Hcg, vector<Mat> Hgij, vector<Mat> 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;cout << Rcg << endl;Rcg.copyTo(Hcg(Rect(0, 0, 3, 3)));Tcg.copyTo(Hcg(Rect(3, 0, 1, 3)));Hcg.at<double>(3, 0) = 0.0;Hcg.at<double>(3, 1) = 0.0;Hcg.at<double>(3, 2) = 0.0;Hcg.at<double>(3, 3) = 1.0;}void testMat(Mat res)
{res.at<double>(0, 2) = 199;
}int main(int argc,char *agrv[])
{vector<Mat> res1, res2;Mat a1 = (Mat_<double>(4, 4) <<0.9397,         0,    0.3420, 20.56,0.2418 ,   0.7071, - 0.6645, 10.26,-0.2418,    0.7071,    0.6645, 5.23,0, 0, 0, 1);Mat b1 = (Mat_<double>(4, 4) <<0.6964, - 0.7071,    0.1228, 30.56,0.6964 ,   0.7071  ,  0.1228, 10.26,-0.1736,         0 ,   0.9848, 5.20,0, 0, 0, 1);res1.push_back(a1);res2.push_back(b1);Mat result = (Mat_<double>(4, 4));Tsai_HandEye(result, res1, res2);cout << result << endl;
}

工业机器人手眼标定（眼在手）方程AX=XB采用Tsai两步法进行求解的过程并分析相关推荐

Halcon：手眼标定——眼在手外与眼在手上
为什么需要九点标定? 为了得到机械和相机的关系,就好比人的手和眼的关系.我们用手将一个物体放到空间的一个位置,用眼看到这个物体,这也存在两个坐标系,一个是手所在的运动空间的坐标系,一个是视网膜上成像的 ...
3D相机机器人手眼标定(眼在手上)全过程
3D相机机器人手眼标定(眼在手上)全过程简述目前在机器人高层规划中,机器人越来越依赖于摄像头的反馈信息,比如自动打磨,焊接,喷涂的智能规划,或者一些分拣,码垛的规划. 在项目开始前, 第一步要做的 ...
Tsai两步法求手眼标定矩阵
Tsai两步法求手眼标定矩阵 Tsai方法介绍术语概念齐次变换矩阵和坐标系的定义旋转轴和旋转角度引理的证明和解释 AX=XB构造引理1 引理2 引理3 引理4 引理5 引理6 --得到公式( ...
opencv4.2中求解手眼标定方程AX=XB
opencv函数: void cv::calibrateHandEye (InputArrayOfArrays R_gripper2base, InputArrayOfArrays t_gripper ...
机器人抓取平台搭建记录（六）：手眼标定--眼在手上--Kinova Gen2 JACO2 七自由度机械臂--Realsense D435
吐槽一,手眼标定两个多月前就做好了,这篇文章也是两个多月前就写了,但当时不知出了什么差错,在我还没有写完想暂时保存一下草稿的时候,写的内容竟然没了小半.于是就拖到现在,拖到忘记了当时写作的思路,操作细 ...
手眼标定----眼在手上(eye-in-hand)
@TOC 标定之前你可以先拿个怡宝盖子进行测试. 第一步:打开halcon,采集,然后生成代码,显示十字架第二步:把怡宝盖子放在十字架中心位置,在图像处理,找到怡宝盖子第三步:找到怡宝盖子然后进行 ...
【相机标定】传统相机标定技术-Tsai两步法
Tsai两步标定法 Tsai说:在对摄像机进行标定时如果考虑过多的非线性畸变会引入过多的非线性参数,这样往往不仅不能提高标定精度,反而会引起解的不稳定,与线性模型标定类似,只是正交了,理想成像平面到实 ...
手眼标定—眼在手上（eye-in-hand）基本原理
基本概念相机固定在机械臂末端,机械臂移动相机也随之移动.主要标定求解相机坐标系和机械臂末端坐标系之间的转换矩阵. 涉及坐标系机械臂基坐标系(base).机械臂末端坐标系(gripper).相机坐标 ...
手眼标定AX=XB求解方法（文献总结）
本人在看论文时碰到了手眼标定问题,看了几篇论文,顺便来总结一下关于公式AX=XB的解决方法和论文的内容. 在手眼标定问题中,AX=XB公式的求解方法很关键,手眼即机器人末端和相机,两者有两种组合形式: ...

工业机器人手眼标定（眼在手）方程AX=XB采用Tsai两步法进行求解的过程并分析

工业机器人手眼标定（眼在手）方程AX=XB采用Tsai两步法进行求解的过程并分析相关推荐

最新文章

热门文章