使用Kronecker积计算AX=XB
最近使用matlab实现了手眼标定
参考论文 "Solving the Robot-World/Hand-Eye Calibration Problem Using the Kronecker Product"
使用 K积 得到AX=XB的可分离的闭合解
构造 A B 矩阵
eye-to-hand其 A B 矩阵的构造如下
A
=
[
b
a
s
e
i
M
t
o
o
l
]
∙
[
b
a
s
e
j
M
t
o
o
l
]
−
1
B
=
[
c
a
m
i
M
c
a
l
]
∙
[
c
a
m
j
M
c
a
l
]
−
1
i
≠
j
i
,
j
=
1
,
2
,
3
,
.
.
.
,
n
\begin{aligned} & \mathbf{A} =[_{base}^{\quad i}\mathbf{M} ^{tool}]\bullet [_{base}^{\quad j}\mathbf{M} ^{tool}]^{-1} \\ \\ & \mathbf{B}=[_{cam}^{\quad i}\mathbf{M}^{cal}]\bullet [_{cam}^{\quad j}\mathbf{M}^{cal}]^{-1} \\ \\ &i\ne j\quad i,j=1,2,3,...,n \end{aligned}
A=[baseiMtool]∙[basejMtool]−1B=[camiMcal]∙[camjMcal]−1i=ji,j=1,2,3,...,n
eye-in-hand其 A B 矩阵的构造如下
A
=
[
b
a
s
e
i
M
t
o
o
l
]
−
1
∙
[
b
a
s
e
j
M
t
o
o
l
]
B
=
[
c
a
m
i
M
c
a
l
]
∙
[
c
a
m
j
M
c
a
l
]
−
1
i
≠
j
i
,
j
=
1
,
2
,
3
,
.
.
.
,
n
\begin{aligned} & \mathbf{A} =[_{base}^{\quad i}\mathbf{M} ^{tool}]^{-1}\bullet [_{base}^{\quad j}\mathbf{M} ^{tool}] \\ \\ & \mathbf{B}=[_{cam}^{\quad i}\mathbf{M}^{cal}]\bullet [_{cam}^{\quad j}\mathbf{M}^{cal}]^{-1} \\ \\ &i\ne j\quad i,j=1,2,3,...,n \end{aligned}
A=[baseiMtool]−1∙[basejMtool]B=[camiMcal]∙[camjMcal]−1i=ji,j=1,2,3,...,n
n表示得到的标定样本数量。
baseMtool 表示机器人基坐标系下的末端工具位姿
camMcal 表示相机坐标系中标定板的位姿
通过两两组合可以得到 n(n-1) 个A B
要求两个位姿 i j 之间的旋转角度尽可能大一些，特别是旋转主轴不能平行
通过奇异性和稳定性等判别，需要剔除一些不好的AB矩阵，比如接近零值
而所需要求的 X 矩阵如下
X
=
b
a
s
e
M
c
a
m
(
e
y
e
−
t
o
−
h
a
n
d
)
X
=
t
o
o
l
M
c
a
m
(
e
y
e
−
i
n
−
h
a
n
d
)
\begin{aligned} &\mathbf{X} =_{base}\mathbf{M} ^{cam} \quad (eye-to-hand)\\ &\mathbf{X} =_{tool}\mathbf{M} ^{cam} \quad (eye-in-hand) \end{aligned}
X=baseMcam(eye−to−hand)X=toolMcam(eye−in−hand)
如果相机固定在机架上，与机械臂位置是固定的，所求即为机器人坐标系下的相机位姿
如果相机固定在末端工具上，则与末端位姿不变，所求为在工具坐标系下的相机位姿
具体的构造过程这里不进行赘述，可以比较简单地通过刚性坐标变换和消元方法得到
求解AX=XB
根据前一个步骤获得了m组 AB 矩阵，这里使用两步求解
即从方程中分离旋转与平移分量，先计算旋转分量 RX ，再计算平移分量 tx
A
X
=
X
B
[
R
A
t
A
0
1
]
[
R
X
t
X
0
1
]
=
[
R
X
t
X
0
1
]
[
R
B
t
B
0
1
]
[
R
A
R
X
R
A
t
X
+
t
A
0
1
]
=
[
R
X
R
B
R
X
t
B
+
t
X
0
1
]
{
R
A
R
X
=
R
X
R
B
−
−
(
1
)
R
A
t
X
+
t
A
=
R
X
t
B
+
t
X
−
−
(
2
)
\begin{aligned} &\mathbf{AX} =\mathbf{XB} \\\ \\ & \begin{bmatrix} \mathbf{RA} & \mathbf{tA} \\ \mathbf{0} & \mathbf{1} \end{bmatrix} \begin{bmatrix} \mathbf{RX} & \mathbf{tX} \\ \mathbf{0} & \mathbf{1} \end{bmatrix}= \begin{bmatrix} \mathbf{RX} & \mathbf{tX} \\ \mathbf{0} & \mathbf{1} \end{bmatrix} \begin{bmatrix} \mathbf{RB} & \mathbf{tB} \\ \mathbf{0} & \mathbf{1} \end{bmatrix}\\ \ \\ & \begin{bmatrix} \mathbf{RA}\mathbf{RX} & \mathbf{RA}\mathbf{tX}+\mathbf{tA} \\ \mathbf{0} & \mathbf{1} \end{bmatrix} = \begin{bmatrix} \mathbf{RX}\mathbf{RB} & \mathbf{RX}\mathbf{tB}+\mathbf{tX} \\ \mathbf{0} & \mathbf{1} \end{bmatrix} \\\ \\ & \begin{cases} \mathbf{RARX} =\mathbf{RXRB} \quad \quad \quad \quad \quad \ \color{Red}-- \color{Red}\left ( 1 \right ) \\ \mathbf{RAtX} +\mathbf{tA} =\mathbf{RXtB} +\mathbf{tX} \quad\color{Red}-- \color{Red}\left ( 2 \right ) \end{cases} \end{aligned}
AX=XB[RA0tA1][RX0tX1]=[RX0tX1][RB0tB1][RARX0RAtX+tA1]=[RXRB0RXtB+tX1]{RARX=RXRB −−(1)RAtX+tA=RXtB+tX−−(2)
求解 RX
构造线性方程组Ax=0
首先，显然，RA RB RX 都是旋转矩阵，属于特殊正交群SO(3)
在乘法上是封闭的
这里使用向量化算子vec(*)将矩阵按列向量化
即按列排布
这里涉及到Kronecker积的概念
简单来说就是构造了一个分块矩阵
Kronecker积有几个性质，用于解算会很方便
v
e
c
(
A
X
B
)
=
v
e
c
(
C
)
=
(
B
T
⊗
A
)
v
e
c
(
X
)
(
A
⊗
B
)
T
=
A
T
⊗
B
T
(
A
⊗
B
)
−
1
=
A
−
1
⊗
B
−
1
(
A
⊗
B
)
(
C
⊗
D
)
=
A
C
⊗
B
D
\begin{aligned} & vec(\mathbf{AXB} )=vec(\mathbf{C} )=({\mathbf{B} }^{T}\otimes \mathbf{A} )vec(\mathbf{X} ) \\ &({\mathbf{A} }\otimes \mathbf{B} )^{T}={\mathbf{A}^{T} }\otimes \mathbf{B}^{T} \\ &({\mathbf{A} }\otimes \mathbf{B} )^{-1}={\mathbf{A}^{-1} }\otimes \mathbf{B}^{-1} \\ &({\mathbf{A} }\otimes \mathbf{B} )({\mathbf{C} }\otimes \mathbf{D})={\mathbf{AC} }\otimes \mathbf{BD} \end{aligned}
vec(AXB)=vec(C)=(BT⊗A)vec(X)(A⊗B)T=AT⊗BT(A⊗B)−1=A−1⊗B−1(A⊗B)(C⊗D)=AC⊗BD
由第一个转换公式可以将 RARX=RXRB进行转换计算
R
A
R
X
=
R
X
R
B
R
A
R
X
I
=
I
R
X
R
B
v
e
c
(
R
A
R
X
I
)
=
v
e
c
(
I
R
X
R
B
)
−
−
通
过
上
述
公
式
转
换
(
I
⊗
R
A
)
v
e
c
(
R
X
)
=
(
R
B
T
⊗
I
)
v
e
c
(
R
X
)
(
I
⊗
R
A
−
R
B
T
⊗
I
)
v
e
c
(
R
X
)
=
0
令
(
I
⊗
R
A
−
R
B
T
⊗
I
)
=
K
K
v
e
c
(
R
X
)
=
0
\begin{aligned} {\mathbf{RARX} }&= \mathbf{RXRB} \\ {\mathbf{RARX{\color{Red} I} } }&= \mathbf{{\color{Red} I} RXRB}\\ vec({\mathbf{RARX{\color{Red} I} } })&= vec(\mathbf{{\color{Red} I} RXRB}) \quad--通过上述公式转换\\ ({\mathbf{\color{Red} I} \otimes \mathbf{RA}})vec(\mathbf{RX})&= ({\mathbf{RB}^{T} \otimes \mathbf{\color{Red} I}})vec(\mathbf{RX})\\ ({\mathbf{\color{Red} I} \otimes \mathbf{RA}}-{\mathbf{RB}^{T} \otimes \mathbf{\color{Red} I}})vec(\mathbf{RX})&= \mathbf{0} \\ 令({\mathbf{\color{Red} I} \otimes \mathbf{RA}}-{\mathbf{RB}^{T} \otimes \mathbf{\color{Red} I}})=\mathbf{K}\\ \mathbf{K}vec(\mathbf{RX})&=\mathbf{0} \end{aligned}
RARXRARXIvec(RARXI)(I⊗RA)vec(RX)(I⊗RA−RBT⊗I)vec(RX)令(I⊗RA−RBT⊗I)=KKvec(RX)=RXRB=IRXRB=vec(IRXRB)−−通过上述公式转换=(RBT⊗I)vec(RX)=0=0
这样就能得到一个 Ax=0 形式的线性方程组
需要注意的是，通过向量化算子，这个 K 矩阵的大小为9x9，而 vec(RX) 是一个大小为 9 的列向量
假设通过上一步得到了m组 AB 值，那就得到m个K矩阵，可以直接堆叠该矩阵，得到一个9m*9大小的矩阵
SVD值分解求解线性方程组Ax=0
通过上一步骤将问题转化为求解 Kvec(RX)=0 的问题
这一步可以使用奇异值分解进行解算
实际上就是一个最小二乘的问题
将K分解为USV，V中的最后一列vn对应S中的最小奇异值，能够最小化目标函数||Ax||
即
vec(RX)=vn
将这个9维的列向量重新排列为3*3的矩阵，即
[
v
n
1
v
n
4
v
n
7
v
n
2
v
n
5
v
n
8
v
n
3
v
n
6
v
n
9
]
=
R
X
\begin{bmatrix} \mathbf{vn}_{1}&\mathbf{vn}_{4} &\mathbf{vn}_{7} \\ \mathbf{vn}_{2}&\mathbf{vn}_{5} &\mathbf{vn}_{8} \\ \mathbf{vn}_{3}&\mathbf{vn}_{6} &\mathbf{vn}_{9} \end{bmatrix}=\mathbf{RX}
⎣⎡vn1vn2vn3vn4vn5vn6vn7vn8vn9⎦⎤=RX
需要注意的是，通过这一方式最后得到的RX可能会失去正交性，需要重新正交化
其方法如下
对RX进行奇异值分解，得到U V矩阵
R
X
=
U
V
T
\mathbf{RX}=\mathbf{UV}^{T}
RX=UVT
这样得到的新的 RX 就是所求的旋转阵
PS：这里还需要注意一点，旋转矩阵的行列式值应该为1，所以在重新正交化之后，需要计算一下这个旋转矩阵的行列式值，如果等于-1，则需要乘以-1，否则后续的平移向量的求解会有很大问题
求解tX平移矩阵
通过上述的公式（2）
{
R
A
R
X
=
R
X
R
B
−
−
(
1
)
R
A
t
X
+
t
A
=
R
X
t
B
+
t
X
−
−
(
2
)
\begin{cases} \mathbf{RARX} =\mathbf{RXRB} \quad \quad \quad \quad \quad \ {\color{red}-- ( 1 )} \\ \mathbf{RAtX} +\mathbf{tA} =\mathbf{RXtB} +\mathbf{tX} \quad {\color{red}-- ( 2 ) } \end{cases}
{RARX=RXRB −−(1)RAtX+tA=RXtB+tX−−(2)
得到 tX 的表达式
(
R
A
−
I
)
t
X
=
R
X
t
B
−
t
A
令
R
A
−
I
=
A
,
R
X
t
B
−
t
A
=
b
得
到
线
性
方
程
:
A
∙
t
X
=
b
(\mathbf{RA}-\mathbf{I})\mathbf{tX}=\mathbf{RXtB}-\mathbf{tA} \\ 令\ \mathbf{RA}-\mathbf{I}=\mathbf{A}, \mathbf{RXtB}-\mathbf{tA}=\mathbf{b}\\ 得到线性方程:\quad \mathbf{A}\bullet \mathbf{tX}=\mathbf{b}
(RA−I)tX=RXtB−tA令 RA−I=A,RXtB−tA=b得到线性方程:A∙tX=b
其中A为3x3矩阵，b和tX都是三维列向量
同样的，因为有多组 AB 值
其中A与b都可以堆叠，形成3mx3的矩阵和3m维的列向量
可以在matlab中直接使用A\b来进行求解
X
最终可以得到完整的 旋转矩阵 X
X
=
[
R
X
t
X
0
1
]
\mathbf{X}=\begin{bmatrix} \mathbf{RX} &\mathbf{tX} \\ \mathbf{0}& 1 \end{bmatrix}
X=[RX0tX1]
误差计算
关于误差计算，也需要分为计算RX的误差和tX的误差，但是关于tX的误差其实就包含了RX的误差
所以可以分两步计算
Y
1
=
R
A
R
X
Y
2
=
R
X
R
B
E
r
r
=
Y
1
−
Y
2
\begin{aligned} \mathbf{Y1}&=\mathbf{RARX}\\ \mathbf{Y2}&=\mathbf{RXRB}\\ \mathbf{Err}&=\mathbf{Y1}-\mathbf{Y2} \end{aligned}
Y1Y2Err=RARX=RXRB=Y1−Y2
再计算 ||Err|| 标量化误差
然后再计算X的整体误差
Y
1
=
A
X
Y
2
=
X
B
E
r
r
=
Y
1
−
Y
2
\begin{aligned} \mathbf{Y1}&=\mathbf{AX}\\ \mathbf{Y2}&=\mathbf{XB}\\ \mathbf{Err}&=\mathbf{Y1}-\mathbf{Y2} \end{aligned}
Y1Y2Err=AX=XB=Y1−Y2
同样，计算 Err 矩阵的L2范数来标量化误差
得到的多组 误差 可以取均值或者输出打印为折线图分析统计
MATLAB代码
function [ X ] = AXXB( A,B )
%AXXB 使用了K积方法，计算手眼标定问题中的AX=XB问题
n=length(A);
K=[];
I=eye(3,3);
for i=1:n
RA=A(1:3,1:3,i);
RB=B(1:3,1:3,i);
K=[K;kron(I,RA)-kron(RB',I)];
end
[U,S,V]=svd(K);
col=length(V);
RX=-[V(1:3,col),V(4:6,col),V(7:9,col)];
[Ux,Sx,Vx]=svd(RX);
RX=Ux*Vx';
RX=sign(det(RX))*RX;
[ Error,err,average_err ] = AXXB_Error( A(1:3,1:3,:),B(1:3,1:3,:),RX );
At=[];
bt=[];
for i=1:n
RA=A(1:3,1:3,i);
tB=B(1:3,4,i);
tA=A(1:3,4,i);
At=[At;I-RA];
bt=[bt;tA-RX*tB];
end
tx=At\bt;
X=[RX,tx;0,0,0,1];
[ Error,err,average_err ] = AXXB_Error( A,B,X );
end
function [ Error,err,average_err ] = AXXB_Error( A,B,X )
%AXXB_Error 用于简单得到标定的误差
n=length(A);
sz=size(A);
Error=zeros(sz(1),sz(1),n);
err=zeros(n,1);
average_err=0;
Y1=zeros(sz(1),sz(1),n);
Y2=zeros(sz(1),sz(1),n);
for i=1:n
Y1(:,:,i)=A(:,:,i)*X;
Y2(:,:,i)=X*B(:,:,i);
Error(:,:,i)=Y1(:,:,i)-Y2(:,:,i);
err(i)=norm(Error(:,:,i));
average_err=average_err+err(i);
end
average_err=average_err/length(err);
end
10万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
