手眼标定

博主： L.CH
发布时间：2024 年 03 月 08 日
1045 次浏览
暂无评论
10165字数
分类：机器人

### 手眼标定原理

手眼标定标定的是不变量，所以标定既可以是机器人基底到相机的变换矩阵$^{\ \ \ \ \ \ C}_{base}T$，也可以是tcp到标志架的变换矩阵$^{M_{F}}_{tcp}T$

针对骨科手术机器人，起初手眼标定机械臂不进行移动，可以得到下面的式子

$$
\begin{align}    \label{eq}
^{\ \ \ \ \ \ C}_{base}T &= ^{\ \ \ \ C}_{M_{F}}T \ \cdot \ ^{M_{F}}_{tcp}T \ \cdot \ ^{\ \ \ \ \ \ \ tcp}_{flange}T \ \cdot \ ^{flange}_{\ \ \ \ base}T \\
&= ^{\ \ \ \ C}_{M_{F}}T \ \cdot \ ^{M_{F}}_{tcp}T \ \cdot \ ^{\ \ \ tcp}_{base}T
\end{align}
$$

因为基底到相机的位置不变，所以拍摄多组照片后可以得到

$$
^{\ \ \ \ C}_{M_{F}}T_{i} \ \cdot \ ^{M_{F}}_{tcp}T \ \cdot \ ^{\ \ \ tcp}_{base}T_{i} = ^{\ \ \ \ C}_{M_{F}}T_{j} \ \cdot \ ^{M_{F}}_{tcp}T \ \cdot \ ^{\ \ \ tcp}_{base}T_{j}
$$

变换后可以得到

$$
^{\ \ \ \ C}_{M_{F}}T_{j}^{-1} \ \cdot \ ^{\ \ \ \ C}_{M_{F}}T_{i} \ \cdot \ ^{M_{F}}_{tcp}T = \ ^{M_{F}}_{tcp}T \ \cdot \ ^{\ \ \ tcp}_{base}T_{j} \cdot \ ^{\ \ \ tcp}_{base}T_{i}^{-1}
$$

令$^{\ \ \ \ C}_{M_{F}}T_{j}^{-1} \ \cdot \ ^{\ \ \ \ C}_{M_{F}}T_{i} = A$，$^{\ \ \ tcp}_{base}T_{j} \cdot \ ^{\ \ \ tcp}_{base}T_{i}^{-1} = B$，就得到著名的$A\ \cdot\ ^{M_{F}}_{tcp}T = ^{M_{F}}_{tcp}T \ \cdot \ B$等式，其中$A$、$B$皆可以通过机器人当前状态已知

即$AX=XB$方程，tcp到法兰标志架$M_{F}$的变换矩阵$^{M_{F}}_{tcp}T$，也就是$X$是我们需要得到的标定结果，后续可以用来进行基于双目视觉的机器人位姿计算

### AX=XB计算

令

$$
A = 
\begin{bmatrix}
R_{A} & t_{A} \\
0 & 1
\end{bmatrix}
,
X = 
\begin{bmatrix}
R_{X} & t_{X} \\
0 & 1
\end{bmatrix}
,
B = 
\begin{bmatrix}
R_{B} & t_{B} \\
0 & 1
\end{bmatrix}
$$

$AX=XB$可以解耦为

$$
\begin{cases}
R_{A}R_{X} = R_{X}R_{B} \\
(R_{A}-I)t_{X} = R_{X}t_{B}-t_{A} 
\end{cases}
$$

Tsai-lenz经典手眼标定算法：

```python
#!/usr/bin/env python
# coding: utf-8
import transforms3d as tfs
import numpy as np
import math

def get_matrix_eular_radu(x,y,z,rx,ry,rz):
    rmat = tfs.euler.euler2mat(math.radians(rx),math.radians(ry),math.radians(rz))
    rmat = tfs.affines.compose(np.squeeze(np.asarray((x,y,z))), rmat, [1, 1, 1])
    return rmat

def skew(v):
    return np.array([[0,-v[2],v[1]],
                     [v[2],0,-v[0]],
                     [-v[1],v[0],0]])

def rot2quat_minimal(m):
    quat =  tfs.quaternions.mat2quat(m[0:3,0:3])
    return quat[1:]

def quatMinimal2rot(q):
    p = np.dot(q.T,q)
    w = np.sqrt(np.subtract(1,p[0][0]))
    return tfs.quaternions.quat2mat([w,q[0],q[1],q[2]])

hand = [-644.44, -630.64, 519.19, -63.04, 57.67, 171.84,
        -690.96, -559.63, 484.44, -73.83, -27.64, 143.24,
        -576.38, -530.28, 384.78, -58.99, 72.47, -173.87,
        -686.37, -385.35, 678.78, -92.38, -6.87, 154.19,
        -723.91, -575.94, 504.64, -77.13, 26.91, 154.8,
        -601.19, -538.19, 647.04, 75.03, -37.07, -34.82,
        -618.34, -578.58, 651.82, 86.53, -19.73, -31.39,
        -655.8, -491.46, 399.56, 22.71, -24.56, 52.13,
        -700.41, -459.42, 680.10, -13.71, -30.68, 72.17,
        -556.48, -727.49, 338.97, 144.44, -59.83, -118.12,
        -559.71, -531.36, 463.89, -143.54, -28.41, 149.76]

camera = [-114.88, -58.42, -1360.42, 102.1, -86.7, -116.9,
          -137.38, -161.05, -1408.09, 55.9, -87.6, -159.4,
          -5.48, -30.51, -1509.98, -53.3, -83.6, 54.1,
          -336.24, -242.7, -1520.08, -90.7, -75.4, 9.0,
          -112.55, -173.66, -1375.43, 24.3, -86.9, -72.3,
          -331.33, -36.42, -1428.53, -126.9, -74.8, -93.3,
          -314.35, -18.73, -1385.09, 146.7, -80.9, -22.4,
          -105.23, -118.61, -1539.98, 20.5, -37.1, 157.0,
          -377.92, -200.33, -1483.99, 16.0, -44.0, -168.1,
          11.05, 55.78, -1344.8, -132.9, -37.6, -46.5,
          -120.81, -66.96, -1518.42, -80.7, -24.6, -25.6]

Hgs,Hcs = [],[]
for i in range(0,len(hand),6):
    Hgs.append(get_matrix_eular_radu(camera[i],camera[i+1],camera[i+2],camera[i+3],camera[i+4],camera[i+5]))  
    Hcs.append(np.linalg.inv(get_matrix_eular_radu(hand[i],hand[i+1],hand[i+2],hand[i+3],hand[i+4],hand[i+5])))

Hgijs = []
Hcijs = []
A = []
B = []
size = 0
for i in range(len(Hgs)):
    for j in range(i+1,len(Hgs)):
        size += 1
        Hgij = np.dot(np.linalg.inv(Hgs[j]),Hgs[i])
        Hgijs.append(Hgij)
        Pgij = np.dot(2,rot2quat_minimal(Hgij))
  
        Hcij = np.dot(Hcs[j],np.linalg.inv(Hcs[i]))
        Hcijs.append(Hcij)
        Pcij = np.dot(2,rot2quat_minimal(Hcij))
  
        A.append(skew(np.add(Pgij,Pcij)))
        B.append(np.subtract(Pcij,Pgij))
MA = np.asarray(A).reshape(size*3,3)
MB = np.asarray(B).reshape(size*3,1)
Pcg_  =  np.dot(np.linalg.pinv(MA),MB)
pcg_norm = np.dot(np.conjugate(Pcg_).T,Pcg_)
Pcg = np.sqrt(np.add(1,np.dot(Pcg_.T,Pcg_)))
Pcg = np.dot(np.dot(2,Pcg_),np.linalg.inv(Pcg))
Rcg = quatMinimal2rot(np.divide(Pcg,2)).reshape(3,3)

A = []
B = []
id = 0
for i in range(len(Hgs)):
    for j in range(i+1,len(Hgs)):
        Hgij = Hgijs[id]
        Hcij = Hcijs[id]
        A.append(np.subtract(Hgij[0:3,0:3],np.eye(3,3)))
        B.append(np.subtract(np.dot(Rcg,Hcij[0:3,3:4]),Hgij[0:3,3:4]))
        id += 1

MA = np.asarray(A).reshape(size*3,3)
MB = np.asarray(B).reshape(size*3,1)
Tcg = np.dot(np.linalg.pinv(MA),MB).reshape(3,)
print(np.linalg.inv(tfs.affines.compose(Tcg,np.squeeze(Rcg),[1,1,1])))

```

修改后的手眼标定算法（眼在手外）：

```python
#!/usr/bin/env python
# coding: utf-8
import transforms3d as tfs
import numpy as np
import math

def skew(v):
    return np.array([[0,-v[2],v[1]],
                     [v[2],0,-v[0]],
                     [-v[1],v[0],0]])

def rot2quat_minimal(m):
    quat =  tfs.quaternions.mat2quat(m[0:3,0:3])
    return quat[1:]

def quatMinimal2rot(q):
    p = np.dot(q.T,q)
    w = np.sqrt(np.subtract(1,p[0][0]))
    return tfs.quaternions.quat2mat([w,q[0],q[1],q[2]])

def matrix_to_eular(m):
    rx,ry,rz = tfs.euler.mat2euler(m[0:3,0:3])
    pos = np.squeeze(m[0:3,3:4])
    return (pos,math.degrees(rx),math.degrees(ry),math.degrees(rz))

flange2base = [-435.81, -308.25, 672.56, 63.34, -84.89, -6.34,
        -538.02, -357.60, 640.72, -96.61, -37.82, 140.52,
        -567.62, -310.76, 672.91, 109.63, -61.63, -37.69,
        -567.58, -331.70, 700.21, -96.22, -10.00, 157.01,
        -541.54, -314.71, 660.64, -148.55, -17.60, 175.61,
        -516.32, -308.14, 528.02, 1.39, 79.79, -131.98,
        -550.98, -272.29, 619.42, -85.66, -32.28, 132.68,
        -615.48, -12.08, 670.03, -104.04, -29.78, 165.95,
        -314.83, -114.29, 695.07, 179.00, -58.04, 174.77,
        -506.88, -119.69, 717.21, 163.76, -35.87, -112.42,
        -511.35, -97.14, 576.60, 32.43, -55.76, -5.19,
        -498.59, -160.02, 742.90, 135.37, -34.12, -103.55,
        -482.44, -221.95, 664.92, 171.78, -30.45, -134.56,
        -467.21, -232.71, 548.58, -107.84, 42.14, 126.59,
        -555.88, -314.63, 666.58, -29.91, 87.61, -162.18]

tcp2flange = [21.30, -252.55, 51.60, 90.00, 0.00, 0.00]

MF2camera = [-361.14, -144.50, -1609.35, -83.4, -79.4, 15.2,
          -274.64, -200.23, -1537.22, 22.4, -73.5, -35.0,
          -335.15, -233.16, -1538.53, -123.0, -61.9, 33.2,
          -332.73, -185.64, -1530.17, -11.8, -74.4, 28.8,
          -249.29, -197.61, -1649.42, -6.9, -27.5, 3.0,
          -253.77, -167.58, -1589.36, -163.5, -73.4, -81.7,
          -283.00, -256.76, -1597.68, 60.8, -78.1, -69.3,
          -342.51, -400.52, -1788.26, -34.6, -66.7, 31.1,
          -391.11, -198.77, -1945.99, 0.5, -18.3, -50.3,
          -361.34, -322.19, -1796.96, -68.2, -25.0, -6.2,
          -361.60, -359.30, -1792.45, 36.0, -65.5, -129.6,
          -413.84, -331.81, -1759.76, -64.4, -27.6, -32.0,
          -290.17, -248.32, -1762.29, -49.3, -17.0, -10.1,
          -221.08, -158.35, -1690.91, 99.2, -64.1, -30.3,
          -366.04, -198.63, -1534.22, 170.0, -71.1, -56.1]

T_base2tcp_s,T_MF2camera_s = [],[]
for i in range(0,len(flange2base),6):
    T_MF2camera_s.append(get_matrix_eular_radu(MF2camera[i],MF2camera[i+1],MF2camera[i+2],MF2camera[i+3],MF2camera[i+4],MF2camera[i+5]))  
    T_base2tcp_s.append(np.linalg.inv(np.dot(get_matrix_eular_radu(flange2base[i],flange2base[i+1],flange2base[i+2],flange2base[i+3],flange2base[i+4],flange2base[i+5]),get_matrix_eular_radu(tcp2flange[0],tcp2flange[1],tcp2flange[2],tcp2flange[3],tcp2flange[4],tcp2flange[5]))))

T_MF2camera_ij_s = []
T_base2tcp_ij_s = []
A = []
B = []
size = 0
for i in range(len(T_MF2camera_s)):
    for j in range(i+1,len(T_MF2camera_s)):
        size += 1
        T_MF2camera_ij = np.dot(np.linalg.inv(T_MF2camera_s[j]),T_MF2camera_s[i])
        T_MF2camera_ij_s.append(T_MF2camera_ij)
        P_MF2camera_ij = np.dot(2,rot2quat_minimal(T_MF2camera_ij))
  
        T_base2tcp_ij = np.dot(T_base2tcp_s[j],np.linalg.inv(T_base2tcp_s[i]))
        T_base2tcp_ij_s.append(T_base2tcp_ij)
        P_base2tcp_ij = np.dot(2,rot2quat_minimal(T_base2tcp_ij))
  
        A.append(skew(np.add(P_MF2camera_ij,P_base2tcp_ij)))
        B.append(np.subtract(P_base2tcp_ij,P_MF2camera_ij))
MA = np.asarray(A).reshape(size*3,3)
MB = np.asarray(B).reshape(size*3,1)
Pct_  =  np.dot(np.linalg.pinv(MA),MB)
pct_norm = np.dot(np.conjugate(Pct_).T,Pct_)
Pct = np.sqrt(np.add(1,np.dot(Pct_.T,Pct_)))
Pct = np.dot(np.dot(2,Pct_),np.linalg.inv(Pct))
R_tcp2MF = quatMinimal2rot(np.divide(Pct,2)).reshape(3,3)

A = []
B = []
id = 0
for i in range(len(T_MF2camera_s)):
    for j in range(i+1,len(T_MF2camera_s)):
        T_MF2camera_ij = T_MF2camera_ij_s[id]
        T_base2tcp_ij = T_base2tcp_ij_s[id]
        A.append(np.subtract(T_MF2camera_ij[0:3,0:3],np.eye(3,3)))
        B.append(np.subtract(np.dot(R_tcp2MF,T_base2tcp_ij[0:3,3:4]),T_MF2camera_ij[0:3,3:4]))
        id += 1

MA = np.asarray(A).reshape(size*3,3)
MB = np.asarray(B).reshape(size*3,1)
t_tcp2MF = np.dot(np.linalg.pinv(MA),MB).reshape(3,)
print("标志架MF到tcp的齐次变换矩阵为:\n",np.linalg.inv(tfs.affines.compose(t_tcp2MF,np.squeeze(R_tcp2MF),[1,1,1])))

print("换算欧拉角的参数为:\n", matrix_to_eular(np.linalg.inv(tfs.affines.compose(t_tcp2MF,np.squeeze(R_tcp2MF),[1,1,1]))))

```

最后修改：2024 年 09 月 18 日

如果觉得我的文章对你有用，请随意赞赏

发表评论取消回复
使用cookie技术保留您的个人信息以便您下次快速评论，继续评论表示您已同意该条款

评论 *

私密评论

名称 *

🎲

邮箱 *

地址

手眼标定

L.CH • 2024 年 03 月 08 日

### 手眼标定原理

手眼标定标定的是不变量，所以标定既可以是机器人基底到相机的变换矩阵$^{\ \ \ \ \ \ C}_{base}T$，也可以是tcp到标志架的变换矩阵$^{M_{F}}_{tcp}T$

针对骨科手术机器人，起初手眼标定机械臂不进行移动，可以得到下面的式子

因为基底到相机的位置不变，所以拍摄多组照片后可以得到

$$
^{\ \ \ \ C}_{M_{F}}T_{i} \ \cdot \ ^{M_{F}}_{tcp}T \ \cdot \ ^{\ \ \ tcp}_{base}T_{i} = ^{\ \ \ \ C}_{M_{F}}T_{j} \ \cdot \ ^{M_{F}}_{tcp}T \ \cdot \ ^{\ \ \ tcp}_{base}T_{j}
$$

变换后可以得到

$$
^{\ \ \ \ C}_{M_{F}}T_{j}^{-1} \ \cdot \ ^{\ \ \ \ C}_{M_{F}}T_{i} \ \cdot \ ^{M_{F}}_{tcp}T = \ ^{M_{F}}_{tcp}T \ \cdot \ ^{\ \ \ tcp}_{base}T_{j} \cdot \ ^{\ \ \ tcp}_{base}T_{i}^{-1}
$$

即$AX=XB$方程，tcp到法兰标志架$M_{F}$的变换矩阵$^{M_{F}}_{tcp}T$，也就是$X$是我们需要得到的标定结果，后续可以用来进行基于双目视觉的机器人位姿计算

### AX=XB计算

令

$$
A = 
\begin{bmatrix}
R_{A} & t_{A} \\
0 & 1
\end{bmatrix}
,
X = 
\begin{bmatrix}
R_{X} & t_{X} \\
0 & 1
\end{bmatrix}
,
B = 
\begin{bmatrix}
R_{B} & t_{B} \\
0 & 1
\end{bmatrix}
$$

$AX=XB$可以解耦为

$$
\begin{cases}
R_{A}R_{X} = R_{X}R_{B} \\
(R_{A}-I)t_{X} = R_{X}t_{B}-t_{A} 
\end{cases}
$$

Tsai-lenz经典手眼标定算法：

```python
#!/usr/bin/env python
# coding: utf-8
import transforms3d as tfs
import numpy as np
import math

def skew(v):
    return np.array([[0,-v[2],v[1]],
                     [v[2],0,-v[0]],
                     [-v[1],v[0],0]])

def rot2quat_minimal(m):
    quat =  tfs.quaternions.mat2quat(m[0:3,0:3])
    return quat[1:]

def quatMinimal2rot(q):
    p = np.dot(q.T,q)
    w = np.sqrt(np.subtract(1,p[0][0]))
    return tfs.quaternions.quat2mat([w,q[0],q[1],q[2]])

MA = np.asarray(A).reshape(size*3,3)
MB = np.asarray(B).reshape(size*3,1)
Tcg = np.dot(np.linalg.pinv(MA),MB).reshape(3,)
print(np.linalg.inv(tfs.affines.compose(Tcg,np.squeeze(Rcg),[1,1,1])))

```

修改后的手眼标定算法（眼在手外）：

```python
#!/usr/bin/env python
# coding: utf-8
import transforms3d as tfs
import numpy as np
import math

def skew(v):
    return np.array([[0,-v[2],v[1]],
                     [v[2],0,-v[0]],
                     [-v[1],v[0],0]])

def rot2quat_minimal(m):
    quat =  tfs.quaternions.mat2quat(m[0:3,0:3])
    return quat[1:]

def quatMinimal2rot(q):
    p = np.dot(q.T,q)
    w = np.sqrt(np.subtract(1,p[0][0]))
    return tfs.quaternions.quat2mat([w,q[0],q[1],q[2]])

def matrix_to_eular(m):
    rx,ry,rz = tfs.euler.mat2euler(m[0:3,0:3])
    pos = np.squeeze(m[0:3,3:4])
    return (pos,math.degrees(rx),math.degrees(ry),math.degrees(rz))

tcp2flange = [21.30, -252.55, 51.60, 90.00, 0.00, 0.00]

print("换算欧拉角的参数为:\n", matrix_to_eular(np.linalg.inv(tfs.affines.compose(t_tcp2MF,np.squeeze(R_tcp2MF),[1,1,1]))))

```

发表评论 取消回复 使用cookie技术保留您的个人信息以便您下次快速评论，继续评论表示您已同意该条款

手眼标定

发表评论取消回复
使用cookie技术保留您的个人信息以便您下次快速评论，继续评论表示您已同意该条款