手眼标定

　　在基于位置的视觉控制中，手眼标定起着关键性作用，手眼标定是解决相机与机器人之间位置关系的理论，手眼关系一般有两种形式，一种是相机固定在机器人执行器末端随机器人运动，称为Eye-in-Hand,另一种为相机固定在机器人附近的地面或设备上，不随机器人运动，称为Eye-to-Hand.无论是怎样的形式，最终解决的问题都是同样的，即看到目标物在什么位置，告诉机器人去什么位置执行加工动作。Eye-in-Hand的形式中，相机随机器人运动视野比较宽不会受到机械臂的阻挡，但是存在运动过量可能看不到目标物体，导致目标丢失。Eye-to-Hand的形式中，相机不随机器人运动，固定在一处，能够看到在视野中目标，但是在机械臂移动的过程中容易受到阻挡。两种形式各有利弊，下面是每种形式求取手眼关系的过程。

　　在上图Eye-in-Hand关系图中，机械臂末端带着相机从{C1}移动到{C2}的过程中相机和机械臂末端执行器的固定关系保持不变即：

[{}^{C1}{T_{H1}} = {}^{C2}{T_{H2}}]

　　同时带有Marker的目标物在机器人世界坐标系中的关系也不变。

[{}^B{T_O} = {}^{ m{B}}{T_{H1}} ullet {}^{C1}T_{H1}^{ - 1} ullet {}^{C1}{T_O} = {}^{ m{B}}{T_{H2}} ullet {}^{C2}T_{H2}^{ - 1} ullet {}^{C2}{T_O}]

[({}^{ m{B}}T_{H2}^{ - 1} ullet {}^{ m{B}}{T_{H1}}) ullet {}^{C1}T_{H1}^{ - 1} = {}^{C2}T_{H2}^{ - 1} ullet ({}^{C2}{T_O} ullet {}^{C1}T_O^{ - 1})]

　　写作：$AX = XB$，求解X即可得出手眼之间的关系。

　　在上图Eye-to-Hand关系图中对于机器人末端执行器夹着Marker从{O1}移动到和{O2}执行任务时，执行器末端和Marker之间的位置关系保持不变，因此在两个位置存在如下的转换关系：

[{}^{O1}{T_{H1}} = {}^CT_{O1}^{ - 1} ullet {}^C{T_B} ullet {}^{ m{B}}{T_{H1}}]

[{}^{O2}{T_{H2}} = {}^CT_{O2}^{ - 1} ullet {}^C{T_B} ullet {}^{ m{B}}{T_{H2}}]

[{}^CT_{O1}^{ - 1} ullet {}^C{T_B} ullet {}^{ m{B}}{T_{H1}} = {}^CT_{O2}^{ - 1} ullet {}^C{T_B} ullet {}^{ m{B}}{T_{H2}}]

[({}^C{T_{O2}} ullet {}^CT_{O1}^{ - 1}) ullet {}^C{T_B} = {}^C{T_B} ullet ({}^{ m{B}}{T_{H2}} ullet {}^{ m{B}}T_{H1}^{ - 1})]

　　写作：$AX = XB$，求解X即可得出手眼之间的关系。

注意在求取的过程中多采集几组A，B的数据结果才会准确，不然无法求解。

  1 //向量转反对称矩阵
  2 
  3 cv::Mat Calculate::skew(cv::Mat vec)
  4 {
  5 
  6     cv::Mat skewM(3, 3, CV_64FC1);
  7 
  8     double vx = vec.at<double>(0, 0);
  9 
 10     double vy = vec.at<double>(1, 0);
 11 
 12     double vz = vec.at<double>(2, 0);
 13 
 14     skewM.at<double>(0, 0) = 0.0; skewM.at<double>(0, 1) = -vz; skewM.at<double>(0, 2) = vy;
 15 
 16     skewM.at<double>(1, 0) = vz; skewM.at<double>(1, 1) = 0.0; skewM.at<double>(1, 2) = -vx;
 17 
 18     skewM.at<double>(2, 0) = -vy; skewM.at<double>(2, 1) = vx; skewM.at<double>(2, 2) = 0.0;
 19 
 20     return skewM;
 21 
 22 }
 23 Solve AX=XB, A-RT_Tcpij, B-RT_Camij, B2A
 24 
 25 Tsai_HandEye(cv::Mat Hcg, vector<cv::Mat> Hgij, vector<cv::Mat> Hcij)
 26 
 27 {
 28 
 29     CV_Assert(Hgij.size() == Hcij.size());
 30 
 31     int nStatus = Hgij.size();
 32 
 33     cv::Mat Rgij(3, 3, CV_64FC1);
 34 
 35     cv::Mat Rcij(3, 3, CV_64FC1);
 36 
 37     cv::Mat rgij(3, 1, CV_64FC1);
 38 
 39     cv::Mat rcij(3, 1, CV_64FC1);
 40 
 41     double theta_gij;
 42 
 43     double theta_cij;
 44 
 45     cv::Mat rngij(3, 1, CV_64FC1);
 46 
 47     cv::Mat rncij(3, 1, CV_64FC1);
 48 
 49     cv::Mat Pgij(3, 1, CV_64FC1);
 50 
 51     cv::Mat Pcij(3, 1, CV_64FC1);
 52 
 53     cv::Mat tempA(3, 3, CV_64FC1);
 54 
 55     cv::Mat tempb(3, 1, CV_64FC1);
 56 
 57     cv::Mat A;
 58 
 59     cv::Mat b;
 60 
 61     cv::Mat pinA;
 62 
 63     cv::Mat Pcg_prime(3, 1, CV_64FC1);
 64 
 65     cv::Mat Pcg(3, 1, CV_64FC1);
 66 
 67     cv::Mat PcgTrs(1, 3, CV_64FC1);
 68 
 69     cv::Mat Rcg(3, 3, CV_64FC1);
 70 
 71     cv::Mat eyeM = cv::Mat::eye(3, 3, CV_64FC1);
 72 
 73     cv::Mat Tgij(3, 1, CV_64FC1);
 74 
 75     cv::Mat Tcij(3, 1, CV_64FC1);
 76 
 77     cv::Mat tempAA(3, 3, CV_64FC1);
 78 
 79     cv::Mat tempbb(3, 1, CV_64FC1);
 80 
 81     cv::Mat AA;
 82 
 83     cv::Mat bb;
 84 
 85     cv::Mat pinAA;
 86 
 87     cv::Mat Tcg(3, 1, CV_64FC1);
 88 
 89     for (int i = 0; i < nStatus; i++)
 90 
 91     {
 92 
 93         Hgij[i](cv::Rect(0, 0, 3, 3)).copyTo(Rgij);
 94 
 95         Hcij[i](cv::Rect(0, 0, 3, 3)).copyTo(Rcij);
 96 
 97         Rodrigues(Rgij, rgij);
 98 
 99         Rodrigues(Rcij, rcij);
100 
101         theta_gij = norm(rgij);
102 
103         theta_cij = norm(rcij);
104 
105         rngij = rgij / theta_gij;
106 
107         rncij = rcij / theta_cij;
108 
109         Pgij = 2 * sin(theta_gij / 2)*rngij;
110 
111         Pcij = 2 * sin(theta_cij / 2)*rncij;
112 
113         tempA = skew(Pgij + Pcij);
114 
115         tempb = Pcij - Pgij;
116 
117         A.push_back(tempA);
118 
119         b.push_back(tempb);
120 
121     }
122 
123     //Compute rotation
124 
125     invert(A, pinA, cv::DECOMP_SVD);
126 
127     Pcg_prime = pinA * b;
128 
129     Pcg = 2 * Pcg_prime / sqrt(1 + norm(Pcg_prime) * norm(Pcg_prime));
130 
131     PcgTrs = Pcg.t();
132 
133     Rcg = (1 - norm(Pcg) * norm(Pcg) / 2) * eyeM + 0.5 * (Pcg * PcgTrs + sqrt(4 - norm(Pcg)*norm(Pcg))*skew(Pcg));
134 
135     //Compute Translation
136 
137     for (int i = 0; i < nStatus; i++)
138 
139     {
140 
141         Hgij[i](cv::Rect(0, 0, 3, 3)).copyTo(Rgij);
142 
143         Hcij[i](cv::Rect(0, 0, 3, 3)).copyTo(Rcij);
144 
145         Hgij[i](cv::Rect(3, 0, 1, 3)).copyTo(Tgij);
146 
147         Hcij[i](cv::Rect(3, 0, 1, 3)).copyTo(Tcij);
148 
149         tempAA = Rgij - eyeM;
150 
151         tempbb = Rcg * Tcij - Tgij;
152 
153         AA.push_back(tempAA);
154 
155         bb.push_back(tempbb);
156 
157     }
158 
159     invert(AA, pinAA, cv::DECOMP_SVD);
160 
161     Tcg = pinAA * bb;
162 
163     Rcg.copyTo(Hcg(cv::Rect(0, 0, 3, 3)));
164 
165     Tcg.copyTo(Hcg(cv::Rect(3, 0, 1, 3)));
166 
167     Hcg.at<double>(3, 0) = 0.0;
168 
169     Hcg.at<double>(3, 1) = 0.0;
170 
171     Hcg.at<double>(3, 2) = 0.0;
172 
173     Hcg.at<double>(3, 3) = 1.0;
174 
175 
176 }