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

古城微笑少年丶 2023-03-05 03:25 73阅读 0赞

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

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dfd2VpeGlhb3Rhbw_size_16_color_FFFFFF_t_70_pic_center]  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dfd2VpeGlhb3Rhbw_size_16_color_FFFFFF_t_70_pic_center 1]

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

1.  Rodrigues(Rgij, rgij);该函数是根据旋转矩阵求旋转向量，即罗德里格斯公式
2.  theta\_gij = norm(rgij);该函数是求向量的2范数，也就是求向量的模
3.  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 rotation
    	invert(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;
    }

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dfd2VpeGlhb3Rhbw_size_16_color_FFFFFF_t_70_pic_center]: /images/20230208/7f1e0a300be141a284a08088a57ef2df.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dfd2VpeGlhb3Rhbw_size_16_color_FFFFFF_t_70_pic_center 1]: /images/20230208/d96f1cd22a234ae4bf0a4bd85e662d52.png