三种手眼标定方法的实现与比较

王海霞

目录

背景

基本知识

三种手眼标定方法

传统的两步法

Ma的Self-calibrationLei的Self-calibration

实验与比较

提出问题

背景

背景

①操作员处在高空、高压危险环境中，仍存在人从高空跌落和触电的危险；②主从控制的作业方式，作业速度慢、耗费时间长。

第一代 第二代

实验平台

• Euler表示旋转矩阵

• Rodrigues表示旋转矩阵R(k,θ)• Cayley表示旋转矩阵

• FOE点• 点线对偶(duality)• 手眼关系数学模型

基本知识

Euler表示旋转矩阵

旋转顺序 xyx,xyz,zyz,zyx

旋转坐标系 局部坐标参考：T=[first rotation]*[second rotation] 全局坐标参考: T=[second rotation]*[first rotation]

旋转矩阵 物理意义 表示坐标系A经过齐次变换T转换成坐标系B；

表示坐标系B相对于坐标系A的位置和姿态；

表示三维空间点的齐次坐标由坐标系B向坐标系A的映射。

TBA

z’ y’

x’

y

x

z

A

B

M

• M点在A-xyz下的坐标为（x,y,z），在B-x'y'z'下的坐标为（x',y',z'） ，坐标原点 之间的关系 为P=

AB

两个坐标的关系为：

=

1z'y'x'

T

1zyx

BA

=

1 0 0 0 P )'zz(cos )'zy(cos )'zx(cos

P )'yz(cos )'yy(cos )'yx(cos

P )'xz(cos )'xy(cos )'xx(cos

Tx

x

x

BA

R=Rz(z,90)*Ry(y,0)*Rx(x,180)

y

x

zx’

z’y’

R,t

E

W

R：{E}相对{W}的姿态

Cayley表示旋转矩阵

• Cayley表示旋转矩阵

1

1

)w][I)(w][I(

)w][I()w][I(R−

××

×−

×

+−=

−+=

• Cayley与Rodrigues关系

k)2/(tanw ⋅= θ

1

1

)RI)(RI(

)RI()RI(]w[−

−×

+−=

−+=

FOE点

• 当做纯平移运动时，极点e=e',此时的极点称为FOE点，对应点连线交于FOE点，且C1e与摄像机运动方向相同

点线关系

• 由点得线

xl'l =×

• 由线得点

• 点在线上

lx'x =×

0T =lx

手眼关系数学模型

xd

yd

W

C

R

1c λ −=X K u

• 摄像机坐标系{C}下的坐标与图像点之间的关系：

• 机器人末端{R}下的坐标与{C}下坐标之间的关系：r r

r c c c= +X R X t

• 世界坐标系{W}下的坐标与{R}下坐标之间的关系：w w

w r r r= +X R X t

• 图像点与世界坐标系{W}下的坐标之间的关系：w w wr r

c cw r r rλ= + +-1u R tX R KR t

传统的两步法

• 机器人运动一次得到关系式AX=XB

=

⇒=

1

1

1

1 ccrr

0tR

0tR

0tR

0tR

XBAX

• K已知，先求R，后求t。

• 经典的AX=XB求解方法

(2) -)-( (1)

rcr

cr

ttRtIRRRRR⋅=

⋅=⋅

W1

C1 C2C3

D3

D2D1

Rc12，Tc12 Rc23，Tc23

Rr23，Tr23Rr12，Tr12

W2

(Rc1,Tc1)

(Rc2，Tc2)

(Rc3,Tc3)

(Rr1，Tr1)

(Rr2，Tr2)

(Rr3,Tr3)

R,t R,tR,t

运动坐标关系图

求R

• 利用Rodrigues表示旋转矩阵，即R=Rot（k,θ）

),(kRRR),(kR 2cc1rr θθ ⋅=⋅

• kr和kc分别是Rr和Rc的旋转轴，也是它们1特征值对应的特征向量，θ1和θ2分别是旋转角

• 由上式得到两个推论：

cr-1

cr21 RRRRRR:proof, .1 ≈⇒⋅⋅=== θθθ

r c-1 -1

c r2. ,proof:R U U , R (RU)k (RU)R k = Λ = Λ= ⋅

根据kr=R*kc得到如下关系图：

α

θ

BA

C

D

O

2/kk/2)2/k(tan

k'

kk'k)kk(skewk)kk(skk

)kk(k

crcr

crcr

cr

=

−=⋅+⇒⋅+=−⇒

−⊥

根据左图得到几个关系：

R求解步骤:(在kri×kci≠0,θi≠0或π)

−=

++

×

×

c2r2

c1r1

r2r2

c1r1

k-kkk

'k]kk[]kk[

Rk)

2(tank'

'k2tan 1

⇒

=

= −

θ

θ

12c1c2c1c2r1rr2r1 ]kk k k[]kk k k[R −×⋅×=

13c2c1c3rr2r1 ]k k k[]k k k[R −⋅=

求t

−⋅

−⋅⋅

−

−=

⇒

−⋅=⋅−

−⋅=⋅−

−

23r23c

12r12c1

23r

12r

23r23c23r

12r12c12r

ttRttR

IRIR

t

ttRtI)(RttRtI)(R

误差分析

• ∠（kc1,kc2）(=∠（kr1,kr2）)小；

• θ1和θ2小；

• 摄像机坐标系原点到标定块的距离|Tci|大；

• 机器人末端在不同位置的距离|Tri-Trj|大；

解决方案

• 设计运动方案使∠（kc1,kc2）尽量大；

• 控制机器人使旋转角θ1和θ2尽量大；

• 选择小的标定板，调整相机位置使每次外参数距离|Tci|尽量小；

• 在运动过程中减小机器人移动距离|Tri-Trj|；• 增加运动次数，误差与sqrt(N)成反比，N是位置数；

• 尽量提高每次运动中相机外参数精度；

• 预标定机器人的平移和旋转参数。

• 同步法

1( ) 'n

i ii=1

f = −∑R k Rk'kRk =

2 ( , ) ( 'n

i ii=1

f = − −∑R t Rp R'- I)t prcr t-tRI)t-R( ⋅=

Ma的Self-calibration

• 其中

0ee jTi =

• 若运动方向相互垂直，则FOE点之间有：

3,...,1 ,) (( T =−−= if)dy,v(v)dx,uue 0i0ii

• 则得到如下关系：

Ma的Self-calibration

求K

• 前提条件：做纯平移运动

−−

=

−−−−−−−−

))

) )

312

321

1

10

0

312031031

321032032

u(uuu(uu

ttv

u

v(vv)vv(v)uu(uv(vv)vv(v)uu(u

2

1

=

dxdyt

2

2

=

dxft

0))(())(( 2030210302 =+−−+−− tvvvvtuuuu

求R

⋅=

⋅=⋅⇒=

rcr

cr

t-tRI)t-R(RRRR

XBAX

rc ttR =⋅

[ ] crcr ttwtt −=+ ×

)]w[I()[w](IR -1×× −+=

iuKt 1c

−=

求t• 重构方法：

• 控制机器人延摄像机X轴方向运动

TT1

3

2

1

rc ]0,0,d[rrrr

Rtt =

==

1 1 cm KXλ =

2 2 c cm KX Ktλ = + 1 2,λ λ⇒ cX⇒

运动：A-P，P-A，A-B，B-P，P-B

Rc,tc

（d1，

0，0）

‘

（d2，

0，0）

‘

Camera at position A

Camera at position B

O1

O2

P

r c r(R I)t Rt t− = −

c 2 c 1R (O P) t O P+ =

1c r rR R RR−=

r c r(R I)t Rt t− = −

ct⇒

t

Lei的Self-calibration

1C2C

1E

2E

P

rt

ct

CX

'CX

rX

'rX

cc'c

rr'r

'r

'c

rc

tXXtXX

tRXXtRXX

=−

=−

+=

+=

rλ =e KRt

Lei的Self-calibration

rc Rtt =

c' Ktee λ==

1

2

3

= =

RA aa

Ka

r11 r12 r13 11 r11 11 r12

r11 r12 r13 12 r11 12 r12

rN1 rN2 rN3 N1 rN

t t t 0 0 0 -e t -e t0 0 0 t t t -e t -e t... ... ... ... ... ... ... ... t t t 0 0 0 -e t

11

1211 r13

1312 r13

21

221 N1 rN2 N1 rN3

23rN1 rN2 rN3 N2 rN1 N2 rN2 N2 rN3

31

32

a'a'

e ta'

e ta'

...a'

-e t e ta'

0 0 0 t t t -e t -e t e ta'a'

=

1

33 2 33 3

3

r A a A ' K r a 1/ '

r

= = ⇒ =

aij ij 33 a ' a / a=其中，

• 重构法：

• 重构点方法与前面相同；

• 第二次：回到原处，做旋转R1，得到t的一个约束方程：

• 第三次：回到原处，做旋转R2，得到t的另一个约束方程：

求t

r c

c c c c1

(R -I)t Rtt X -R X

==

Rc,tc

（d1，

0，0）

‘

（d2，

0，0）

‘

Camera at position A

Camera at position B

O1

O2

P

r c(R -I)t Rt=

• 极点法：

• 运动两次得到关于 的方程组。

唯一解条件：

与 旋转轴不平行；

不在与 旋转轴正交的平面上。

1r rR(R I) e'Kt tλ −− = −

ce' 'Ktλ=

1 2, , tλ λ

r1R r2R

ritriR

r c r(R I)t Rt t− = −

1r1 1 r1

11r2r2 2

2

t(I R ) RK e ' 0 t

t(I R ) 0 RK e ' λλ

−

−

− = −

• 新重构方法：

w r1 1 r1 r1 1X λ= + +-1u R tK tRR

w r2 2 r2 r2 2X λ= + +-1u R tK tRR

Tr1 1 r2 2 r1 r2 r2 r1

Tr3 r2r2 2 r3 3 r2

1

2

3r3

-

λλλ

− − − = −−

-1 -1

-1 -1

R RK u R RK u 0 R R t tt t0 R RK u R R

tK u R R

r1 1 r2 2 r1 r2 1 21 2 r r( )λ λ− + − + − =-1 -1R RK u R RK u R t ttR 0

实验

第一种方法实验结果：

1022.466 0 435.572K= 0 1020.689 266.827

0 0 1

[ ]0.0115 0.0067 0.99989.942

T=[114.6563 5.0081 -2.1965]

kθ

= − − −

=[ ]0.0045 0.0088 1.0089.6176

T=[113.704 0.521 -13.7395]

kθ

= − − −

=

[ ]0.002 0.0004 1.0090.4396

T=[113.4890 1.821 4.609]

kθ

= − − −

=

第二种方法实验结果：

879.145 0 244.25K= 0 917.44 274.67

0 0 1

[ ]0.3089 0.331 0.89256.2252

T=[448.915 -55.11 710.55]

kθ

= −

=

923.049 0 300.96K= 0 1066.326 171.52

0 0 1

[ ]0.0167 0.011 -0.99990.9209

T=[68.89 -148.95 98.35]

kθ

=

=

1082.26 0 370.44K= 0 1071.55 205.123

0 0 1

[ ]0.232 0.1545 -0.9660.6559

T=[-131.3 -1245 -961.4]

kθ

= −

=

第三种方法实验结果：

1044.02 45.1 365.34K= 0 1029.05 264.37

0 0 1

[ ]

1

2

-0.025 0.01 -0.99987.4

T =[99.45 -33.34 419.08]T =[86.57 35.95 -123.44]

kθ

=

=

966.03 -15.224 385.61K= 0 967.33 390.16

0 0 1

[ ]

1

2

0.012 0.021 -0.99990.365

T =[101.14 -39.88 417.28]T =[88.35 17.13 37.105]

kθ

= −

=

994.65 -9.27 404.2K= 0 1003.69 349.4

0 0 1

[ ]

1

2

0.007 0.051 -0.99990.7148

T =[100.34 -43.5 415.89]T =[86.41 22.91 39.46]

kθ

= −

=

方法比较

• Tsai方法：参考物：棋盘；运动次数：2次；（保存3次数据）利用同一组数据，分别求出R和t；事先求出摄像机内参数K。

• Ma的Self-calibration:不需要参考物，任取2个点；运动次数：6次+8次；利用6次纯平移运动得到K，再利用其中的3组数据求R，最后做2次带旋转的运动，6次辅助运动求t；

不需要事先求摄像机内参数。

• Lei的Self-calibration:不需要参考物：任取8个点，或2个点；运动次数：4次+2次；（保存8次数据）利用4次纯平移运动得到RK，再做2次带旋转的运动求t；不需要事先求摄像机内参数K 。

提出问题• 在自标定方法中是否可以利用同一组数据求

RK和T?

1r( ) 'λ −= −r RKtR - I e t

w r r rX λ= + +-1u RK tRR t

c r=Rk k

c c d d⋅ = ⋅k t k t

c λ= -1t K e'

问题的提出

c c=k t d d=k t

想法：

Screw Motion螺丝运动

请各位老师和同学给与指导！