Tsai手眼标定算法

A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration(Tsai手眼标定算法)

符号定义

坐标系定义

$G_i$: 手爪（末端执行器）的坐标系。

$C_i$: 相机的坐标系。

$CW$: 对于相机的世界坐标系（一般由标定板确定）。

$RW$: 机器人基座坐标系。

坐标系变换定义

1. 手爪——>基座
   $$\left[\begin{array}{cc}{R}_{gi}& T_{gi}\\ 0& 1\end{array}\right]$$

2. camera world ——>camera
   $$\left[\begin{array}{cc}{R}_{ci}& T_{ci}\\ 0& 1\end{array}\right]$$

3. 手爪——>手爪
   $$\left[\begin{array}{cc}{R}_{gij}& T_{gij}\\ 0& 1\end{array}\right]$$

4. camera——>camera
   $$\left[\begin{array}{cc}{R}_{cij}& T_{cij}\\ 0& 1\end{array}\right]$$

5. camera——>手爪
   $$\left[\begin{array}{cc}{R}_{cg}& T_{cg}\\ 0& 1\end{array}\right]$$

${\theta }_R$: 旋转矩阵$R$的旋转角

$P_{gij}$: 旋转矩阵$R_{gij}$对应的旋转向量，它可以用于表示$G_i$到$G_j$的旋转变换

$P_{cij}$: 同上

$P_{cg}$: 旋转矩阵$R_{cg}$对应的旋转向量

$P_{cg}^{\prime }=\frac{1}{2\cos \left(\frac{{\theta }_{R_{c8}}}{2}\right)}{P}_{cg}=\frac{1}{\sqrt{4-{|P_{cg}|}^2}}{P}_{cg}$

$Skew(V)$: 反对称矩阵，相当于$V\times$，$V$是3维列向量
$$Skew(V)=\left[\begin{array}{ccc}0& -v_z& v_y\\ v_z& 0& -v_x\\ -v_y& v_x& 0\end{array}\right]$$
$N$: 数据组数，每组数据由两个位姿组成

计算步骤

1. 利用罗德里格斯公式将旋转矩阵转换为旋转向量
   $$\{\begin{array}{l}{r}_{gij}=\text{rodrigues}\left(R_{gij}\right)\\ r_{cij}=\text{rodrigues }\left(R_{cij}\right)\end{array}$$

   从线性代数的角度看，旋转向量实际上是旋转矩阵的特征值为1所对应的特征向量

   a. 旋转矩阵必有一个特征值为1

   b. 根据特征值和特征向量的定义有：
   $$R{P}_r=\lambda P_r\lambda =1$$
   因此对应的几何意义是该向量经过$R$旋转作用后依然是自身，那很显然它就是旋转轴

2. 旋转向量归一化，得到单位旋转轴和旋转角*(将旋转向量解耦)*
   $$\{\begin{array}{c}{\theta }_{gij}={\Vert r_{gij}\Vert }_2{N}_{r_{gij}}=\frac{r_{gij}}{{\theta }_{gij}}\\ {\theta }_{cij}=\Vert r_{cij}{\Vert }_2{N}_{r_{cij}}=\frac{r_{cij}}{{\theta }_{cij}}\end{array}$$

3. 修正的罗德里格斯参数表示姿态变换
   $$\begin{gathered} P_{gij}=2\sin ({\theta }_{gij}/2)N_{rgij} \\ {P}_{cij}=2\sin ({\theta }_{cij}/2)N_{rcij} \end{gathered}$$

4. 计算初始的${P_{cg}}^{\prime }$

   对于每对位姿，它们之间的旋转变化要尽可能大，然后计算：
   $$Skew(P_{gij}+P_{cij}){P_{cg}}^{\prime }=P_{cij}-P_{gij}\text{\hspace{1em}(8)}$$
   其中，$P_{gij}$容易由机械臂的正向运动学公式求得，$P_{cij}$容易由机器视觉方法求得，因此式(8)实际上就是$Ax=b$的求解问题，可以直接对$A$求逆，即$x=A^{-1}b$，如果有多组数据，那就构建出优化问题：
   $$\min f(x)=\Vert Ax-b{\Vert }^2\text{\hspace{1em}(12)}$$
   即优化出外参中的旋转部分

5. 计算旋转角${\theta }_{R_{gc}}$

6. 计算$P_{cg}$
   $$P_{cg}=\frac{2P_{cg}{}^{\prime }}{\sqrt{1+{\left|P_{cg}{}^{\prime }\right|}^2}}$$

7. 利用罗德里格斯计算旋转矩阵$R_{cg}$
   $$R=\left(1-\frac{{\left|P_r\right|}^2}{2}\right)I+\frac{1}{2}\left(P_r{P}_r^T+\alpha \cdot \mathrm{Skew}\left(P_r\right)\right)$$
   或者
   $$R=\left[\begin{array}{ccc}{n}_1^2+\left(1-n_1^2\right)\cos \theta & n_1{n}_2(1-\cos \theta )-n_3\sin \theta & n_1{n}_3(1-\cos \theta )+n_2\sin \theta \\ n_1{n}_2(1-\cos \theta )+n_3\sin \theta & n_2^2+\left(1-n_2^2\right)\cos \theta & n_2{n}_3(1-\cos \theta )-n_1\sin \theta \\ n_1{n}_3(1-\cos \theta )+n_2\sin \theta & n_2{n}_3(1-\cos \theta )+n_1\sin \theta & n_3^2+\left(1-n_3^2\right)\cos \theta )\end{array}\right]$$

8. 计算平移向量$T_{cg}$

$$\left(R_{gij}-I\right)T_{cg}=R_{cg}{T}_{cij}-T_{gij}\text{\hspace{1em}(15)}$$

​ 实际上(15)也是一个求解$Ax=b$的问题，可以对$A$求逆计算，如果有多组数据，则类似(12)构建最小二乘的优化问题，即优化得到外参中的平移部分

算法代码

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;
 
        // 论文公式(12)
        tempA = skew(Pgij + Pcij);
        tempb = Pcij - Pgij;
 
        A.push_back(tempA);
        b.push_back(tempb);
    }
 
    //Compute rotation
    // 论文公式(12)
    invert(A, pinA, DECOMP_SVD);
    // 求初始的旋转向量P_{cg}'
    Pcg_prime = pinA * b;
    
    // 论文公式(14)
    Pcg = 2 * Pcg_prime / sqrt(1 + norm(Pcg_prime) * norm(Pcg_prime));
    PcgTrs = Pcg.t();   // 矩阵转置
    // 论文公式(10) 旋转向量->旋转矩阵
    Rcg = (1 - norm(Pcg) * norm(Pcg) / 2) * eyeM + 0.5 * (Pcg * PcgTrs + sqrt(4 - norm(Pcg)*norm(Pcg))*skew(Pcg));
 
    //Computer Translation 
    // 计算平移向量, 论文公式(15)
    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.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;
 
}

参考：

手眼标定Tsai方法的Matlab仿真分析

经典手眼标定算法之Tsai-Lenz