手眼标定算法---Navy算法（Robot sensor calibration: solving AX=XB on the Euclidean group） - 好文

本文主要介绍Frank C. Park and Bryan J. Martin在文献Robot sensor calibration: solving
AX=XB on the Euclidean group
<http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=326576&url=http://ieeexplore.ieee.org/iel4/70/7747/00326576.pdf?arnumber=326576>
中提出的手眼标定算法，该算法也被称为Navy手眼标定算法，该算法的主要创新点为利用李群理论的知识来求解手眼标定经典方程。该算法基于OpenCV的C++版本程序
<http://download.csdn.net/detail/yunlinwang/9545212>可去CSDN资源下载，MATLAB版本
<http://download.csdn.net/detail/yunlinwang/9530209>作者为苏黎世理工的Christian
Wengert，也可在此处下载。转自：https://blog.csdn.net/YunlinWang/article/details/51871520
<https://blog.csdn.net/YunlinWang/article/details/51871520>

正如Tsai等在文献中指出的，手眼标定问题其实就是求解AX=XB方程问题。其中AA为机器人末端连杆坐标架在机器人-摄像机系统移动前后的转换关系，B为摄像机坐标架在移动前后的相对关系。Tsai指出要唯一确定手眼矩阵的各分量，至少需要旋转轴不平行的两组运动。由于在观测中一般存在噪声，因此在实际测量中一般需要多组运动来求解该方程。

假设有多组观测值{(A1,B1),(A2,B2),⋯,(Ak,Bk)}，求解AX=XB方程可以转化为如下最小化问题



其中，d表示在欧式群上的距离测度。利用李群理论知识可以将上述最小化问题转化为最小二乘拟合问题，从而可以得到简单而又明确的解。下面介绍李群中的基本知识。

<>李群基本知识

描述刚体运动可以用欧式群表示，由如下形式的矩阵表示：

其中，θ∈SO(3),b∈ℜ3，SO(3)表示旋转矩阵组成的群。

每个李群都有其对应的李代数，李群与李代数之间的转换可以由指数映射和对数映射
完成。由于本人数学知识比较单薄，如果读者想要继续深入理解李群和李代数知识，可以自行查阅相关文献或者书籍。在这里我就不详细介绍，只简单介绍Park文献中出现的知识。

<>指数映射

李代数到李群的转换满足指数映射关系，假设[w]∈so(3)，而exp⁡[w]∈SO(3)，则其指数映射满足罗德里格斯公式：

<>对数映射

李群到李代数的转换满足对数映射关系，假设θ∈SO(3)，则其对数映射为：

其中，1+2cos⁡ϕ=tr(θ)。

<>利用李群知识求解AX=XB

我们知道，手眼标定问题其实就是求解方程AX=XB，将X移到方程的右边，可以得到，根据上文介绍的对数映射关系，在方程的两边取对数，则可以得到



令 logA=[α],logB=[β]，则上式可以化为[α]=X[β]XT=[Xβ]，从而

α=Xβ

AX=XB写成矩阵形式为



将上式展开可以得到



与Tsai手眼标定类似，同样采用“两步法”求解上述方程，先解算旋转矩阵，再求得平移向量。由上面推导 α=Xβ，因此
θAθX=θXθB可以写成 α=θXβ。当存在多组观测值时，求解该方程可以转化为下面最小二乘拟合问题：



很显然，上述问题是典型的绝对定向问题，因而求解上式与绝对定向相同，其解为

其中，。

然而，上式的求解是有条件的，即MM不奇异，因而当AA、BB只有两组时，不能用上述方法求解。

当只有两组A、B时，即有A1,A2,B1,B2，

旋转矩阵的解为：

其中，，×表示叉乘。

在求得旋转矩阵后，求解平移向量的步骤与Tsai手眼标定相同，读者可以查看上篇文献，这里就不在赘述。

<>算法源代码

根据上述基本计算步骤，在利用OpenCV 2.0开源库的基础上，编写Tsai手眼标定方法的c++程序，其实现函数代码如下：
void Navy_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 alpha1(3, 1, CV_64FC1); Mat
beta1(3, 1, CV_64FC1); Mat alpha2(3, 1, CV_64FC1); Mat beta2(3, 1, CV_64FC1);
Mat A(3, 3, CV_64FC1); Mat B(3, 3, CV_64FC1); Mat alpha(3, 1, CV_64FC1); Mat
beta(3, 1, CV_64FC1); Mat M(3, 3, CV_64FC1, Scalar(0)); Mat MtM(3, 3,
CV_64FC1); Mat veMtM(3, 3, CV_64FC1); Mat vaMtM(3, 1, CV_64FC1); Mat pvaM(3, 3,
CV_64FC1, Scalar(0)); Mat Rx(3, 3, CV_64FC1); Mat Tgij(3, 1, CV_64FC1); Mat
Tcij(3, 1, CV_64FC1); Mat eyeM = Mat::eye(3, 3, CV_64FC1); Mat tempCC(3, 3,
CV_64FC1); Mat tempdd(3, 1, CV_64FC1); Mat C; Mat d; Mat Tx(3, 1, CV_64FC1);
//Compute rotation if (Hgij.size() == 2) // Two (Ai,Bi) pairs {
Rodrigues(Hgij[0](Rect(0, 0, 3, 3)), alpha1); Rodrigues(Hgij[1](Rect(0, 0, 3,
3)), alpha2); Rodrigues(Hcij[0](Rect(0, 0, 3, 3)), beta1);
Rodrigues(Hcij[1](Rect(0, 0, 3, 3)), beta2); alpha1.copyTo(A.col(0));
alpha2.copyTo(A.col(1)); (alpha1.cross(alpha2)).copyTo(A.col(2));
beta1.copyTo(B.col(0)); beta2.copyTo(B.col(1));
(beta1.cross(beta2)).copyTo(B.col(2)); Rx = A*B.inv(); } else // More than two
(Ai,Bi) pairs { 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,
alpha); Rodrigues(Rcij, beta); M = M + beta*alpha.t(); } MtM = M.t()*M;
eigen(MtM, vaMtM, veMtM); pvaM.at<double>(0, 0) = 1 / sqrt(vaMtM.at<double>(0,
0)); pvaM.at<double>(1, 1) = 1 / sqrt(vaMtM.at<double>(1, 0));
pvaM.at<double>(2, 2) = 1 / sqrt(vaMtM.at<double>(2, 0)); Rx =
veMtM*pvaM*veMtM.inv()*M.t(); } //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); tempCC = eyeM - Rgij; tempdd = Tgij - Rx * Tcij;
C.push_back(tempCC); d.push_back(tempdd); } Tx = (C.t()*C).inv()*(C.t()*d);
Rx.copyTo(Hcg(Rect(0, 0, 3, 3))); Tx.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; }

热门工具换一换