Tsai两步法求手眼标定矩阵
Tsai方法介绍
本篇介绍推导的手眼标定方法参考
ROGER Y. TSAI AND REIMAR K. LENZ
于1989年发表的论文
’A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration '
该论文提出了一种手眼标定的解析算法,并给出了这种方法下的手眼标定数据集的筛选原则
术语概念
齐次变换矩阵和坐标系的定义
G
i
G_{i}
Gi : 末端坐标系(工具坐标系)
C
i
C_{i}
Ci : 相机坐标系
C
W
CW
CW : 标定板世界坐标系
R
W
RW
RW : 机器人世界坐标系
H
g
i
\mathbf{H}_{gi}
Hgi: 从
G
i
G_{i}
Gi到
R
W
RW
RW的齐次转换阵,即在机器人坐标系下的末端位姿矩阵
H
c
i
\mathbf{H}_{ci}
Hci: 从
C
W
CW
CW到
C
i
C_{i}
Ci的齐次转换矩阵,即可以理解为相机坐标系下的标定板位姿矩阵
H
g
i
j
\mathbf{H}_{gij}
Hgij: 从
H
g
i
\mathbf{H}_{gi}
Hgi到
H
g
j
\mathbf{H}_{gj}
Hgj的齐次转换矩阵,涉及到两次标定过程的参数
H g i j = H g j − 1 H g i \quad\mathbf{H}_{gij}=\mathbf{H}_{gj}^{-1}\mathbf{H}_{gi} Hgij=Hgj−1Hgi
H c i j \mathbf{H}_{cij} Hcij: 从 H c i \mathbf{H}_{ci} Hci到 H c j \mathbf{H}_{cj} Hcj的齐次转换矩阵
H
c
i
j
=
H
c
j
H
c
i
−
1
\quad\mathbf{H}_{cij}=\mathbf{H}_{cj}\mathbf{H}_{ci}^{-1}
Hcij=HcjHci−1
为什么
H
g
i
j
\mathbf{H}_{gij}
Hgij 乘积中是取
j
j
j 的逆矩阵,而
H
c
i
j
\mathbf{H}_{cij}
Hcij 是取
i
i
i 的逆呢?
其实由于这种计算是专门用于eye-in-hand,也就是相机在机械臂的末端上,所以
C
W
CW
CW代表的是标定板的世界坐标,默认位置是不变的,而相机在两次标定中的位置是变化的,可以用以下方式重新说明会更清楚
H
g
i
=
[
w
o
r
l
d
M
r
o
b
o
t
]
−
1
⋅
w
o
r
l
d
M
g
r
i
p
p
e
r
−
i
=
r
o
b
o
t
M
g
r
i
p
p
e
r
−
i
H
g
i
j
=
g
r
i
p
p
e
r
−
j
M
g
r
i
p
p
e
r
−
i
=
[
r
o
b
o
t
M
g
r
i
p
p
e
r
−
j
]
−
1
⋅
r
o
b
o
t
M
g
r
i
p
p
e
r
−
i
=
H
g
j
−
1
H
g
i
H
c
i
=
[
w
o
r
l
d
M
c
a
m
−
i
]
−
1
⋅
w
o
r
l
d
M
c
a
l
=
c
a
m
−
i
M
c
a
l
H
c
i
j
=
c
a
m
−
j
M
c
a
m
−
i
=
c
a
m
−
j
M
c
a
l
⋅
[
c
a
m
−
i
M
c
a
l
]
−
1
=
H
c
j
H
c
i
−
1
\begin{aligned} &\mathbf{H}_{gi}=[_{world}\mathbf{M}^{robot}]^{-1}\cdot_{world}\mathbf{M}^{gripper-i}=_{robot}\mathbf{M}^{gripper-i}\\ &\mathbf{H}_{gij}=_{gripper-j}\mathbf{M}^{gripper-i} \quad \\ &\quad\quad=[_{robot}\mathbf{M}^{gripper-j}]^{-1}\cdot_{robot}\mathbf{M}^{gripper-i}\\ &\quad\quad=\mathbf{H}_{gj}^{-1}\mathbf{H}_{gi} \\ \\ &\mathbf{H}_{ci}=[_{world}\mathbf{M}^{cam-i}]^{-1}\cdot_{world}\mathbf{M}^{cal}=_{cam-i}\mathbf{M}^{cal}\\ &\mathbf{H}_{cij}=_{cam-j}\mathbf{M}^{cam-i} \quad \\ &\quad\quad=_{cam-j}\mathbf{M}^{cal}\cdot[_{cam-i}\mathbf{M}^{cal}]^{-1}\\ &\quad\quad=\mathbf{H}_{cj}\mathbf{H}_{ci}^{-1} \end{aligned}
Hgi=[worldMrobot]−1⋅worldMgripper−i=robotMgripper−iHgij=gripper−jMgripper−i=[robotMgripper−j]−1⋅robotMgripper−i=Hgj−1HgiHci=[worldMcam−i]−1⋅worldMcal=cam−iMcalHcij=cam−jMcam−i=cam−jMcal⋅[cam−iMcal]−1=HcjHci−1
然后是关键的矩阵
H
c
g
\mathbf{H}_{cg}
Hcg:
C
i
Ci
Ci到
G
i
Gi
Gi的齐次转换矩阵,在eye-in-hand中,这个矩阵是不变的,也就是需要求取的关键矩阵
齐次变换矩阵由旋转和平移矩阵构成,具体不赘述
旋转轴和旋转角度
根据 Rodrigues 公式可以将旋转矩阵转换为旋转轴的单位向量和旋转角,三维空间中的旋转都可以看作是绕着某一旋转轴转动一定角度
公式的具体推导在另外一篇文章中已经推导过一遍
这里要补充的是旋转轴的单位向量是旋转矩阵的特征向量,特征值为1
r
o
t
a
t
i
o
n
a
x
i
s
:
P
r
=
[
n
1
n
2
n
3
]
T
R
P
r
=
P
r
−
−
(
1
)
\begin{aligned} &rotation \ axis:\ \mathbf{P}_{r}=[n_{1} \quad n_{2} \quad n_{3} ]^{T}\\ &\mathbf{RP}_{r}=\mathbf{P}_{r}\quad \color{red}{--(1)} \end{aligned}
rotation axis: Pr=[n1n2n3]TRPr=Pr−−(1)
在这里需要使用修改版的Rodriguees公式,
根据参考论文 'Optimal Spacecraft Rotational Manervers’
(这篇论文没看过,但是大概是推导了一种变形,使得公式中的角度量
θ
\theta
θ在公式中隐去)
重新定义
P
r
=
2
sin
θ
2
[
n
1
n
2
n
3
]
T
−
−
(
2
)
\mathbf{P}_{r}=2\sin \frac{\theta}{2} [n_{1} \quad n_{2} \quad n_{3} ]^{T} \quad \color{red}{--(2)}
Pr=2sin2θ[n1n2n3]T−−(2)
同样的,变形后的Rodrigues公式中不存在三角函数,这是优势,注意其中的
[
n
1
n
2
n
3
]
[n_{1} \quad n_{2} \quad n_{3} ]
[n1n2n3]是单位向量
R
=
(
1
−
∣
P
r
∣
2
2
)
I
+
1
2
(
P
r
P
r
T
+
α
⋅
Skew
(
P
r
)
)
w
h
e
r
e
α
=
4
−
∣
P
r
∣
2
−
−
(
3
)
\mathbf{R}=\left(1-\frac{\left|\mathbf{P}_{r}\right|^{2}}{2}\right) \mathbf{I}+\frac{1}{2}\left(\mathbf{P}_{r} \mathbf{P}_{r}^{T}+\alpha \cdot \operatorname{Skew}\left(\mathbf{P}_{r}\right)\right)\\ where \quad \alpha=\sqrt{4-\left|\mathbf{P}_{r}\right|^{2}}\quad \color{red}{--(3)}
R=(1−2∣Pr∣2)I+21(PrPrT+α⋅Skew(Pr))whereα=4−∣Pr∣2−−(3)
然后给出关于旋转轴和旋转角度的符号定义
θ
R
\theta_{\mathbf{R}}
θR:
R
\mathbf{R}
R 旋转矩阵的旋转角度
P
g
i
j
\mathbf{P}_{gij}
Pgij:
R
g
i
j
\mathbf{R}_{gij}
Rgij的旋转轴
P
c
i
j
\mathbf{P}_{cij}
Pcij:
R
c
i
j
\mathbf{R}_{cij}
Rcij的旋转轴
P
c
g
\mathbf{P}_{cg}
Pcg:
R
c
g
\mathbf{R}_{cg}
Rcg的旋转轴
P
c
g
\mathbf{P}_{cg}
Pcg的变形:
P
c
g
′
\quad\mathbf{P}_{c g}^{\prime}
Pcg′
P
c
g
′
=
1
2
cos
(
θ
R
c
g
2
)
P
c
g
=
1
4
−
∣
P
c
g
∣
2
P
c
g
−
−
(
4
)
\mathbf{P}_{c g}^{\prime} \quad=\frac{1}{2 \cos \left(\frac{\theta_{\mathbf{R}_{c g}}}{2}\right)} \mathbf{P}_{c g}=\frac{1}{\sqrt{4-\left|\mathbf{P}_{c g}\right|^{2}}} \mathbf{P}_{c g}\quad \color{red}{--(4)}
Pcg′=2cos(2θRcg)1Pcg=4−∣Pcg∣21Pcg−−(4)
反对称矩阵:
Skew
(
V
)
=
[
0
−
ν
z
ν
y
ν
z
0
−
ν
x
−
ν
y
ν
x
0
]
\text { Skew }(\mathbf{V})=\left[\begin{array}{ccc} 0 & -\nu_{z} & \nu_{y} \\ \nu_{z} & 0 & -\nu_{x} \\ -\nu_{y} & \nu_{x} & 0 \end{array}\right]
Skew (V)=⎣⎡0νz−νy−νz0νxνy−νx0⎦⎤
引理的证明和解释
论文中首先给出了一个结论性式子
Skew
(
P
g
i
j
+
P
c
i
j
)
P
c
g
′
=
P
c
i
j
−
P
g
i
−
−
(
5
)
\text { Skew }\left(\mathbf{P}_{g i j}+\mathbf{P}_{c i j}\right) \mathbf{P}_{c g}^{\prime}=\mathbf{P}_{c i j}-\mathbf{P}_{g i}\quad \color{red}{--(5)}
Skew (Pgij+Pcij)Pcg′=Pcij−Pgi−−(5)
AX=XB构造
这里首先证明这个式子是怎么来的
首先先简单证明一个基本的AX=XB公式
H
g
i
j
H
c
g
=
H
c
g
H
c
i
j
l
e
f
t
=
g
j
M
g
i
⋅
g
i
M
c
i
=
g
j
M
c
i
r
i
g
h
t
=
g
j
M
c
j
⋅
c
j
M
c
i
=
g
j
M
c
i
l
e
f
t
=
r
i
g
h
t
\begin{aligned} &\mathbf{H}_{gij}\mathbf{H}_{cg}=\mathbf{H}_{cg}\mathbf{H}_{cij}\\ &left=_{gj}\mathbf{M}^{gi}\cdot_{gi}\mathbf{M}^{ci}= _{gj}\mathbf{M}^{ci}\\ &right=_{gj}\mathbf{M}^{cj}\cdot _{cj}\mathbf{M}^{ci}=_{gj}\mathbf{M}^{ci}\\ &left=right \end{aligned}
HgijHcg=HcgHcijleft=gjMgi⋅giMci=gjMciright=gjMcj⋅cjMci=gjMcileft=right
把这个齐次矩阵拆开,就有两个等式成立
分别是
R
g
i
j
R
c
g
=
R
c
g
R
c
i
j
−
−
(
6
)
R
g
i
j
t
c
g
+
t
g
i
j
=
R
c
g
t
c
i
j
+
t
c
g
−
−
(
7
)
\begin{aligned} &\mathbf{R}_{gij}\mathbf{R}_{cg}=\mathbf{R}_{cg}\mathbf{R}_{cij}\quad \color{red}{--(6)}\\ &\mathbf{R}_{gij}\mathbf{t}_{cg}+\mathbf{t}_{gij}=\mathbf{R}_{cg}\mathbf{t}_{cij}+\mathbf{t}_{cg}\quad \color{red}{--(7)}\\ \end{aligned}
RgijRcg=RcgRcij−−(6)Rgijtcg+tgij=Rcgtcij+tcg−−(7)
然后需要渐进地简单证明几个引理
引理1
引理1直接由公式5可以得到
L
e
m
m
a
I
:
R
g
i
j
=
=
R
c
g
R
c
i
j
R
c
g
T
−
−
(
8
)
\begin{aligned} Lemma\ I:\\ &\mathbf{R}_{gij}==\mathbf{R}_{cg}\mathbf{R}_{cij}\mathbf{R}_{cg}^{T}\quad \color{red}{--(8)}\\ \end{aligned}
Lemma I:Rgij==RcgRcijRcgT−−(8)
引理2
引理2则是关于旋转轴之间的转换,可以用引理1证明,同时容易知道旋转轴向量是旋转矩阵的唯一特征值为1的实特征向量
L
e
m
m
a
I
I
:
P
g
i
j
=
R
c
g
P
c
i
j
−
−
(
9
)
P
r
o
o
f
:
R
g
i
j
=
R
c
g
R
c
i
j
R
c
g
T
R
g
i
j
P
g
i
j
=
R
c
g
R
c
i
j
R
c
g
T
P
g
i
j
−
−
a
c
c
o
r
d
i
n
g
t
o
f
o
m
u
l
a
(
1
)
P
g
i
j
=
R
c
g
R
c
i
j
R
c
g
T
P
g
i
j
R
c
g
T
P
g
i
j
‾
=
R
c
i
j
R
c
g
T
P
g
i
j
‾
o
b
v
i
o
u
s
l
y
t
h
i
s
i
s
a
n
e
i
g
e
n
v
e
c
t
o
r
o
f
R
c
i
j
P
c
i
j
=
R
c
g
T
P
g
i
j
P
g
i
j
=
R
c
g
P
c
i
j
P
r
o
o
f
d
o
n
e
\begin{aligned} Lemma\ II:\\ &\mathbf{P}_{gij}=\mathbf{R}_{cg}\mathbf{P}_{cij}\quad \color{red}{--(9)}\\ Proof:\\ \mathbf{R}_{gij}&=\mathbf{R}_{cg}\mathbf{R}_{cij}\mathbf{R}_{cg}^{T}\\ \mathbf{R}_{gij}\mathbf{P}_{gij}&=\mathbf{R}_{cg}\mathbf{R}_{cij}\mathbf{R}_{cg}^{T}\mathbf{P}_{gij} \quad--according \ to \ fomula \ (1)\\ \mathbf{P}_{gij}&=\mathbf{R}_{cg}\mathbf{R}_{cij}\mathbf{R}_{cg}^{T}\mathbf{P}_{gij}\\ \underline{\mathbf{R}_{cg}^{T}\mathbf{P}_{gij}}&=\mathbf{R}_{cij}\underline{\mathbf{R}_{cg}^{T}\mathbf{P}_{gij}}\quad obviously\ this\ is\ an\ eigenvector\ of\ \mathbf{R}_{cij}\\ \mathbf{P}_{cij}&=\mathbf{R}_{cg}^{T}\mathbf{P}_{gij}\\ \mathbf{P}_{gij}&=\mathbf{R}_{cg}\mathbf{P}_{cij}\\ Proof\ done \end{aligned}
Lemma II:Proof:RgijRgijPgijPgijRcgTPgijPcijPgijProof donePgij=RcgPcij−−(9)=RcgRcijRcgT=RcgRcijRcgTPgij−−according to fomula (1)=RcgRcijRcgTPgij=RcijRcgTPgijobviously this is an eigenvector of Rcij=RcgTPgij=RcgPcij
引理3
引理3证明了三个旋转轴之间的垂直关系,这一部分的证明论文上好像有一点问题,不知道是不是符号写错,这里重新证明一下
L
e
m
m
a
I
I
I
:
P
c
g
⊥
(
P
g
i
j
−
P
c
i
j
)
−
−
(
10
)
P
r
o
o
f
:
(
P
g
−
P
c
)
T
P
c
g
=
(
R
c
g
P
c
−
P
c
)
T
R
c
g
P
c
g
−
−
a
c
c
o
r
d
i
n
g
t
o
l
e
m
m
a
I
I
=
P
c
T
R
c
g
T
R
c
g
P
c
g
−
P
c
T
R
c
g
P
c
g
=
P
c
T
P
c
g
−
P
c
T
P
c
g
=
0
P
r
o
o
f
d
o
n
e
\begin{aligned} Lemma\ III:\\ &\mathbf{P}_{cg}\perp (\mathbf{P}_{gij}-\mathbf{P}_{cij})\quad \color{red}{--(10)}\\ Proof:\\ \left(\mathbf{P}_{g}-\mathbf{P}_{c}\right)^{T} \mathbf{P}_{c g}&=(\mathbf{R}_{cg}\mathbf{P}_{c}-\mathbf{P}_{c})^{T}\mathbf{R}_{cg}\mathbf{P}_{c g}\quad -- according\ to\ lemma\ II\\ &=\mathbf{P}_{c}^{T}\mathbf{R}_{cg}^{T}\mathbf{R}_{cg}\mathbf{P}_{cg}-\mathbf{P}_{c}^{T}\mathbf{R}_{cg}\mathbf{P}_{cg}\\ &=\mathbf{P}_{c}^{T}\mathbf{P}_{cg}-\mathbf{P}_{c}^{T}\mathbf{P}_{cg}\\ &=0\\ Proof\ done \end{aligned}
Lemma III:Proof:(Pg−Pc)TPcgProof donePcg⊥(Pgij−Pcij)−−(10)=(RcgPc−Pc)TRcgPcg−−according to lemma II=PcTRcgTRcgPcg−PcTRcgPcg=PcTPcg−PcTPcg=0
事实上根据引理3就能够计算得到
R
c
g
\mathbf{R}_{cg}
Rcg 的旋转轴
P
c
g
\mathbf{P}_{cg}
Pcg 了,即根据两次转移得到的两对
P
g
1
P
c
1
\mathbf{P}_{g1}\quad\mathbf{P}_{c1}
Pg1Pc1和
P
g
2
P
c
2
\mathbf{P}_{g2}\quad\mathbf{P}_{c2}
Pg2Pc2其叉乘后乘以一个缩放系数即可得到
P
c
g
=
s
(
P
g
i
1
j
1
−
P
c
i
1
j
1
)
×
(
P
g
i
2
j
2
−
P
c
i
2
j
2
)
\mathbf{P}_{c g}=s\left(\mathbf{P}_{g i_{1} j_{1}}-\mathbf{P}_{c i_{1} j_{1}}\right) \times\left(\mathbf{P}_{g i_{2} j_{2}}-\mathbf{P}_{c i_{2} j_{2}}\right)
Pcg=s(Pgi1j1−Pci1j1)×(Pgi2j2−Pci2j2)
但是这一方法对误差的敏感性高,并且也没有考虑其他角度的影响,总之就是不太适用,但是论文没有给出具体的解释,这里不做讨论了
引理4
引理4证明了三个向量之间的共线关系-即平行关系
L
e
m
m
a
I
V
:
(
P
g
i
j
−
P
c
i
j
)
∥
(
P
g
i
j
+
P
c
i
j
)
×
P
c
g
P
r
o
o
f
:
1.
{
P
c
g
⊥
(
P
g
i
j
−
P
c
i
j
)
−
−
L
e
m
m
a
I
I
I
P
c
g
⊥
(
P
g
i
j
+
P
c
i
j
)
×
P
c
g
2.
(
P
g
i
j
+
P
c
i
j
)
⊥
(
P
g
i
j
+
P
c
i
j
)
×
P
c
g
−
−
o
b
v
i
o
u
s
l
y
(
P
g
i
j
+
P
c
i
j
)
(
P
g
i
j
−
P
c
i
j
)
=
P
g
i
j
2
−
P
c
i
j
2
=
1
−
1
=
0
S
o
,
(
P
g
i
j
+
P
c
i
j
)
⊥
(
P
g
i
j
−
P
c
i
j
)
(
P
g
i
j
−
P
c
i
j
)
∥
(
P
g
i
j
+
P
c
i
j
)
×
P
c
g
P
r
o
o
f
d
o
n
e
\begin{aligned} Lemma\ IV:\\ &(\mathbf{P}_{gij}-\mathbf{P}_{cij})\parallel(\mathbf{P}_{gij}+\mathbf{P}_{cij})\times\mathbf{P}_{cg}\\ Proof:\\ &1.\left\{\begin{matrix} \mathbf{P}_{cg}\perp (\mathbf{P}_{gij}-\mathbf{P}_{cij})\quad --Lemma\ III\\ \mathbf{P}_{cg}\perp(\mathbf{P}_{gij}+\mathbf{P}_{cij})\times\mathbf{P}_{cg} \end{matrix}\right.\\ &2.\\ &\quad(\mathbf{P}_{gij}+\mathbf{P}_{cij})\perp(\mathbf{P}_{gij}+\mathbf{P}_{cij})\times\mathbf{P}_{cg}\quad --obviously\\ &\quad (\mathbf{P}_{gij}+\mathbf{P}_{cij})(\mathbf{P}_{gij}-\mathbf{P}_{cij})\\ &\quad=\mathbf{P}_{gij}^{2}-\mathbf{P}_{cij}^{2}\\ &\quad=1-1=0\\ So,\\ &(\mathbf{P}_{gij}+\mathbf{P}_{cij})\perp(\mathbf{P}_{gij}-\mathbf{P}_{cij})\\ \\ &(\mathbf{P}_{gij}-\mathbf{P}_{cij})\parallel(\mathbf{P}_{gij}+\mathbf{P}_{cij})\times\mathbf{P}_{cg}\\ Proof\ done \end{aligned}\\
Lemma IV:Proof:So,Proof done(Pgij−Pcij)∥(Pgij+Pcij)×Pcg1.{Pcg⊥(Pgij−Pcij)−−Lemma IIIPcg⊥(Pgij+Pcij)×Pcg2.(Pgij+Pcij)⊥(Pgij+Pcij)×Pcg−−obviously(Pgij+Pcij)(Pgij−Pcij)=Pgij2−Pcij2=1−1=0(Pgij+Pcij)⊥(Pgij−Pcij)(Pgij−Pcij)∥(Pgij+Pcij)×Pcg
两个向量同时垂直于两条不平行的向量,证明这两条向量是平行的
引理5
引理5需要根据图来理解,使用解析几何的方式来证明
图中向量的意思不用说明,比较清楚了,其中的这个圆所在平面垂直于
P
c
g
\mathbf{P}_{cg}
Pcg
引理5证明了两个向量的长度相同,主要是为了引出接下来两个向量相等的引理
L
e
m
m
a
V
:
∣
P
g
i
j
−
P
c
i
j
∣
=
∣
(
P
g
i
j
+
P
c
i
j
)
×
P
c
g
′
∣
P
r
o
o
f
:
∣
(
P
g
+
P
c
)
×
P
c
g
′
∣
=
∣
(
P
g
+
P
c
)
∣
∣
P
c
g
′
∣
sin
α
−
−
O
m
i
t
t
h
e
i
j
=
∣
(
P
g
+
P
c
)
∣
⋅
2
sin
θ
2
(
4
−
4
sin
2
θ
2
)
−
1
2
sin
α
−
−
a
c
c
o
r
d
i
n
g
t
o
f
o
r
m
u
l
a
(
2
)
(
4
)
=
∣
(
P
g
+
P
c
)
∣
⋅
tan
θ
2
⋅
sin
α
−
−
t
r
i
g
o
n
o
m
e
t
r
i
c
t
r
a
n
s
f
o
m
a
t
i
o
n
=
2
∣
O
B
→
∣
⋅
sin
α
⋅
tan
θ
2
−
−
a
s
s
h
o
w
i
n
f
i
g
u
r
e
=
2
∣
A
B
→
∣
⋅
tan
θ
2
=
2
∣
C
B
→
∣
=
∣
C
G
→
∣
=
∣
P
g
−
P
c
∣
P
r
o
o
f
d
o
n
e
.
\begin{aligned} Lemma\ V:\\ &|\mathbf{P}_{gij}-\mathbf{P}_{cij}|=|(\mathbf{P}_{gij}+\mathbf{P}_{cij})\times\mathbf{P}_{cg}'|\\ Proof:\\ &|(\mathbf{P}_{g}+\mathbf{P}_{c})\times\mathbf{P}_{cg}'|=|(\mathbf{P}_{g}+\mathbf{P}_{c})||\mathbf{P}_{cg}'|\sin \alpha\quad--Omit\ the\ ij\\ &\qquad\qquad\qquad\quad\ \ \ =|(\mathbf{P}_{g}+\mathbf{P}_{c})|\cdot 2 \sin \frac{\theta}{2}\left(4-4 \sin ^{2} \frac{\theta}{2}\right)^{-\frac {1}{2}}\sin \alpha\quad --according\ to\ formula\ (2)(4)\\ &\qquad\qquad\qquad\quad\ \ \ =|(\mathbf{P}_{g}+\mathbf{P}_{c})|\cdot\tan{\frac{\theta}{2}}\cdot\sin\alpha\quad -- trigonometric\ transfomation\\ &\qquad\qquad\qquad\quad\ \ \ =2|\overrightarrow{O B}|\cdot\sin\alpha\cdot\tan{\frac{\theta}{2}}\quad--as\ show\ in\ figure\\ &\qquad\qquad\qquad\quad\ \ \ =2|\overrightarrow{A B}|\cdot\tan{\frac{\theta}{2}}\\ &\qquad\qquad\qquad\quad\ \ \ =2|\overrightarrow{C B}|\\ &\qquad\qquad\qquad\quad\ \ \ =|\overrightarrow{C G}|\\ &\qquad\qquad\qquad\quad\ \ \ =|\mathbf{P}_{g}-\mathbf{P}_{c}|\\ Proof\ done. \end{aligned}
Lemma V:Proof:Proof done.∣Pgij−Pcij∣=∣(Pgij+Pcij)×Pcg′∣∣(Pg+Pc)×Pcg′∣=∣(Pg+Pc)∣∣Pcg′∣sinα−−Omit the ij =∣(Pg+Pc)∣⋅2sin2θ(4−4sin22θ)−21sinα−−according to formula (2)(4) =∣(Pg+Pc)∣⋅tan2θ⋅sinα−−trigonometric transfomation =2∣OB∣⋅sinα⋅tan2θ−−as show in figure =2∣AB∣⋅tan2θ =2∣CB∣ =∣CG∣ =∣Pg−Pc∣
引理6 --得到公式(5)
根据引理4和引理5可以直接得到引理6
L
e
m
m
a
V
I
:
(
P
g
i
j
+
P
c
i
j
)
×
P
c
g
′
=
P
c
i
j
−
P
g
i
j
\begin{aligned} Lemma\ VI:\\ &(\mathbf{P}_{gij}+\mathbf{P}_{cij})\times\mathbf{P}_{cg}'=\mathbf{P}_{cij}-\mathbf{P}_{gij}\\ \end{aligned}
Lemma VI:(Pgij+Pcij)×Pcg′=Pcij−Pgij
根据上图可以确定向量的方向问题
这一引理直接可以得到公式(5)
Skew
(
P
g
i
j
+
P
c
i
j
)
P
c
g
′
=
P
c
i
j
−
P
g
i
−
−
(
5
)
\text { Skew }\left(\mathbf{P}_{g i j}+\mathbf{P}_{c i j}\right) \mathbf{P}_{c g}^{\prime}=\mathbf{P}_{c i j}-\mathbf{P}_{g i}\quad \color{red}{--(5)}
Skew (Pgij+Pcij)Pcg′=Pcij−Pgi−−(5)
接下来的引理则说明了如何求解公式(5)
引理7
引理7证明了
Skew
(
P
g
i
j
+
P
c
i
j
)
\text {Skew}(\mathbf{P}_{gij}+\mathbf{P}_{cij})
Skew(Pgij+Pcij) 的是奇异的,秩为2,通过初等行列式变换就能够容易证明这个反对称阵的秩为2
L
e
m
m
a
V
I
I
:
r
a
n
k
(
Skew
(
P
g
i
j
+
P
c
i
j
)
)
=
2
P
r
o
o
f
:
A
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
=
[
a
2
a
3
0
−
a
1
a
2
0
a
1
a
3
−
a
1
a
2
0
0
0
]
r
a
n
k
(
A
)
=
2
P
r
o
o
f
d
o
n
e
.
\begin{aligned} Lemma\ VII:\\ &rank(\text{Skew}(\mathbf{P}_{gij}+\mathbf{P}_{cij}))=2\\ Proof:\\ &A=\left[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right]= \left[\begin{array}{ccc} a_{2} a_{3} & 0 & -a_{1} a_{2} \\ 0 & a_{1} a_{3} & -a_{1} a_{2} \\ 0 & 0 & 0 \end{array}\right]\\ &rank(A)=2\\ Proof\ done. \end{aligned}
Lemma VII:Proof:Proof done.rank(Skew(Pgij+Pcij))=2A=⎣⎡0a3−a2−a30a1a2−a10⎦⎤=⎣⎡a2a3000a1a30−a1a2−a1a20⎦⎤rank(A)=2
引理7说明了公式(5)需要两个以上的方程组才能存在
R
c
g
\mathbf{R}_{cg}
Rcg 的唯一解,也就是需要三个以上的状态
引理8
引理8直接由公式(7)得到
L
e
m
m
a
V
I
I
I
:
(
R
g
i
j
−
I
)
t
c
g
=
R
c
g
t
c
i
j
+
t
g
i
j
\begin{aligned} Lemma\ VIII:\\ &(\mathbf{R}_{gij}-\mathbf{I})\mathbf{t}_{cg}=\mathbf{R}_{cg}\mathbf{t}_{cij}+\mathbf{t}_{gij}\\ \end{aligned}
Lemma VIII:(Rgij−I)tcg=Rcgtcij+tgij
这一引理用于计算
t
c
g
\mathbf{t}_{cg}
tcg
引理9
从引理8得到的
(
R
g
i
j
−
I
)
(\mathbf{R}_{gij}-\mathbf{I})
(Rgij−I) 是奇异的,秩小于2
论文中没有直接证明这个结论,但是我们可以根据Rodrigues公式和秩的性质来得到这一结论
L
e
m
m
a
I
X
:
r
a
n
k
(
R
g
i
j
−
I
)
≤
2
P
r
o
o
f
:
r
a
n
k
(
R
g
i
j
−
I
)
=
r
a
n
k
(
I
+
sin
θ
⋅
K
+
(
1
−
cos
θ
)
⋅
K
2
−
I
)
=
r
a
n
k
(
sin
θ
⋅
K
+
(
1
−
cos
θ
)
⋅
K
2
)
=
r
a
n
k
(
K
(
sin
θ
+
(
1
−
cos
θ
)
⋅
K
)
)
≤
m
i
n
{
r
a
n
k
(
K
)
,
r
a
n
k
(
(
1
−
cos
θ
)
⋅
K
)
}
=
2
P
r
o
o
f
d
o
n
e
.
\begin{aligned} Lemma\ IX:\\ &rank(\mathbf{R}_{gij}-\mathbf{I})\le 2\\ Proof:\\ &rank(\mathbf{R}_{gij}-\mathbf{I})=rank(\mathbf{I}+\sin \theta\cdot \mathbf{K}+(1-\cos \theta)\cdot\mathbf{K}^{2}-\mathbf{I})\\ &\quad\quad\quad\quad\quad\quad\ \ =rank(\sin \theta\cdot \mathbf{K}+(1-\cos \theta)\cdot\mathbf{K}^{2})\\ &\quad\quad\quad\quad\quad\quad\ \ =rank(\mathbf{K}(\sin \theta+(1-\cos \theta)\cdot\mathbf{K}))\\ &\quad\quad\quad\quad\quad\quad\ \ \le min\{rank(\mathbf{K}),rank((1-\cos \theta)\cdot\mathbf{K})\}=2\\ Proof\ done. \end{aligned}
Lemma IX:Proof:Proof done.rank(Rgij−I)≤2rank(Rgij−I)=rank(I+sinθ⋅K+(1−cosθ)⋅K2−I) =rank(sinθ⋅K+(1−cosθ)⋅K2) =rank(K(sinθ+(1−cosθ)⋅K)) ≤min{rank(K),rank((1−cosθ)⋅K)}=2
这一引理说明了要计算引理8中的
t
c
g
\mathbf{t}_{cg}
tcg 方程组要两个以上,即要测量三个以上的状态
引理10
观察公式(4)可以得出引理10
L
e
m
m
a
X
I
:
I
f
a
n
d
o
n
l
y
i
f
θ
R
c
g
≠
π
,
o
r
e
q
u
i
v
a
l
e
n
t
l
y
,
∣
P
c
g
∣
,
≠
±
2
,
P
g
i
1
j
1
∦
P
g
i
2
j
2
(
P
c
i
1
j
1
∦
P
c
i
2
j
2
)
[
Skew
(
P
g
i
1
j
1
+
P
c
i
1
j
1
)
Skew
(
P
g
i
2
j
2
−
P
c
i
2
j
2
)
]
h
a
s
f
u
l
l
c
o
l
u
m
n
r
a
n
k
3
\begin{aligned} Lemma\ XI:\\ &If\ and\ only\ if\ \theta_{\mathbf{R}_{c g}}\ne \pi,\quad or\ equivalently,\quad|\mathbf{P}_{cg}|,\ne \pm 2,\quad \mathbf{P}_{g i_{1} j_{1}}\nparallel\mathbf{P}_{g i_{2} j_{2}}(\mathbf{P}_{c i_{1} j_{1}}\nparallel\mathbf{P}_{c i_{2} j_{2}})\\ \\ &\left[\begin{array}{ll} \text { Skew } \left(\mathbf{P}_{g i_{1} j_{1}}+\mathbf{P}_{c i_{1} j_{1}}\right)\\ \text { Skew }\left(\mathbf{P}_{g i_{2} j_{2}}-\mathbf{P}_{c i_{2} j_{2}}\right) \end{array}\right]\quad has\ full\ column\ rank\ 3 \end{aligned}
Lemma XI:If and only if θRcg=π,or equivalently,∣Pcg∣,=±2,Pgi1j1∦Pgi2j2(Pci1j1∦Pci2j2)[ Skew (Pgi1j1+Pci1j1) Skew (Pgi2j2−Pci2j2)]has full column rank 3
引理10保证给定至少三个状态后,
R
c
g
\mathbf{R}_{cg}
Rcg的解是唯一的
引理11
与引理10类似的,也是为了证明唯一性
L
e
m
m
a
X
:
I
f
a
n
d
o
n
l
y
i
f
P
g
i
1
j
1
∦
P
g
i
2
j
2
(
P
c
i
1
j
1
∦
P
c
i
2
j
2
)
[
R
g
i
1
j
1
−
I
R
g
i
2
j
2
−
I
]
h
a
s
f
u
l
l
c
o
l
u
m
n
r
a
n
k
3
\begin{aligned} Lemma\ X:\\ &If \ and\ only\ if\ \quad \mathbf{P}_{g i_{1} j_{1}}\nparallel\mathbf{P}_{g i_{2} j_{2}}(\mathbf{P}_{c i_{1} j_{1}}\nparallel\mathbf{P}_{c i_{2} j_{2}})\\ \\ &\left[\begin{array}{ll} \mathbf{R}_{g i_{1}j_{1}}-\mathbf{I} \\ \mathbf{R}_{g i_{2}j_{2}}-\mathbf{I} \end{array}\right]\quad has\ full\ column\ rank\ 3 \end{aligned}
Lemma X:If and only if Pgi1j1∦Pgi2j2(Pci1j1∦Pci2j2)[Rgi1j1−IRgi2j2−I]has full column rank 3
引理11保证给定至少三个状态后,
t
c
g
\mathbf{t}_{cg}
tcg的解是唯一的
引理的总结
引理1到3是公式推导的准备阶段,理清了几个向量的基本关系
从引理4和5导出了引理6的公式可以用于计算旋转轴向量
引理7到引理11,证明了需要三个状态以上才能得到唯一解
H
c
g
\mathbf{H}_{cg}
Hcg
后面几个引理没有具体证明,但是应该是正确的
解法步骤
Step1: 计算 R c g \mathbf{R}_{cg} Rcg
对于任意一组的i j,
R
g
i
j
\mathbf{R}_{gij}
Rgij或者
R
c
i
j
\mathbf{R}_{cij}
Rcij旋转角要尽可能大一些
建立引理6中得到的线性方程
Skew
(
P
g
i
j
+
P
c
i
j
)
P
c
g
′
=
P
c
i
j
−
P
g
i
j
−
−
(
5
)
\text { Skew }\left(\mathbf{P}_{g i j}+\mathbf{P}_{c i j}\right) \mathbf{P}_{c g}^{\prime}=\mathbf{P}_{c i j}-\mathbf{P}_{g ij}\quad \color{red}{--(5)}
Skew (Pgij+Pcij)Pcg′=Pcij−Pgij−−(5)
由于引理7说明了这个反对称阵的奇异性,必须要两对以上的ij才能得到方程的唯一解,即需要三个以上的状态
然后得到线性方程组
[
Skew
(
P
g
i
1
j
1
+
P
c
i
1
j
1
)
Skew
(
P
g
i
2
j
2
+
P
c
i
2
j
2
)
⋮
Skew
(
P
g
i
n
j
n
+
P
c
i
n
j
n
)
]
⋅
P
c
g
′
=
[
P
c
i
1
j
1
−
P
g
i
1
j
1
P
c
i
2
j
2
−
P
g
i
2
j
2
⋮
P
c
i
n
j
n
−
P
g
i
n
j
n
]
−
−
(
11
)
\left[\begin{array}{ll}\text { Skew }\left(\mathbf{P}_{g i_{1} j_{1}}+\mathbf{P}_{c i_{1} j_{1}}\right)\\ \text { Skew }\left(\mathbf{P}_{g i_{2} j_{2}}+\mathbf{P}_{c i_{2} j_{2}}\right)\\ \quad\quad\quad\quad\ \ \ \vdots\\ \text { Skew }\left(\mathbf{P}_{g i_{n} j_{n}}+\mathbf{P}_{c i_{n} j_{n}}\right) \end{array}\right]\cdot \mathbf{P}_{c g}^{\prime}=\left[\begin{array}{ll}\mathbf{P}_{c i_{1} j_{1}}-\mathbf{P}_{g i_{1} j_{1}}\\ \mathbf{P}_{c i_{2} j_{2}}-\mathbf{P}_{g i_{2} j_{2}}\\ \quad\quad\ \ \ \vdots\\ \mathbf{P}_{c i_{n} j_{n}}-\mathbf{P}_{g i_{n} j_{n}} \end{array}\right]\quad \color{red}{--(11)}
⎣⎢⎢⎢⎡ Skew (Pgi1j1+Pci1j1) Skew (Pgi2j2+Pci2j2) ⋮ Skew (Pginjn+Pcinjn)⎦⎥⎥⎥⎤⋅Pcg′=⎣⎢⎢⎢⎡Pci1j1−Pgi1j1Pci2j2−Pgi2j2 ⋮Pcinjn−Pginjn⎦⎥⎥⎥⎤−−(11)
得到
A
x
=
b
\mathbf{Ax}=\mathbf{b}
Ax=b,其中
A
∈
R
3
n
×
3
\mathbf{A}\in \mathfrak{R} ^{3n\times3}
A∈R3n×3,
x
∈
R
3
×
1
\mathbf{x}\in \mathfrak{R} ^{3\times1}
x∈R3×1,
b
∈
R
3
n
×
1
\mathbf{b}\in \mathfrak{R} ^{3n\times1}
b∈R3n×1
可以通过线性最小二乘的方法进行求解
需要注意的是,在结算之前需要检查一下
R
c
g
\mathbf{R}_{cg}
Rcg的旋转角是否为180°
检查的方法是计算
(
P
g
i
1
j
1
+
P
c
i
1
j
1
)
(\mathbf{P}_{g i_{1} j_{1}}+\mathbf{P}_{c i_{1} j_{1}})
(Pgi1j1+Pci1j1)是否与
(
P
g
i
2
j
2
+
P
c
i
2
j
2
)
(\mathbf{P}_{g i_{2} j_{2}}+\mathbf{P}_{c i_{2} j_{2}})
(Pgi2j2+Pci2j2)共线的同时
P
g
i
1
j
1
∦
P
g
i
2
j
2
\mathbf{P}_{g i_{1} j_{1}}\nparallel\mathbf{P}_{g i_{2} j_{2}}
Pgi1j1∦Pgi2j2
这是由于计算式中的
P
c
g
′
\mathbf{P}_{cg}^{\prime}
Pcg′的定义问题导致的,具体可以看一下它的定义式(4)
P
c
g
′
=
1
2
cos
(
θ
R
c
g
2
)
P
c
g
=
1
4
−
∣
P
c
g
∣
2
P
c
g
−
−
(
4
)
\mathbf{P}_{c g}^{\prime} \quad=\frac{1}{2 \cos \left(\frac{\theta_{\mathbf{R}_{c g}}}{2}\right)} \mathbf{P}_{c g}=\frac{1}{\sqrt{4-\left|\mathbf{P}_{c g}\right|^{2}}} \mathbf{P}_{c g}\quad \color{red}{--(4)}
Pcg′=2cos(2θRcg)1Pcg=4−∣Pcg∣21Pcg−−(4)
Step2:计算 θ R c g \theta_{\mathbf{R}_{cg}} θRcg
由公式(2)(4)可以得到
θ
R
c
g
\theta_{\mathbf{R}_{cg}}
θRcg的计算式
P
c
g
′
=
1
4
−
∣
P
c
g
∣
2
P
c
g
=
1
4
−
∣
2
sin
θ
2
[
n
1
n
2
n
3
]
T
∣
2
P
c
g
=
1
4
−
4
sin
2
θ
2
P
c
g
=
1
2
cos
θ
2
P
c
g
=
1
2
cos
θ
2
2
sin
θ
2
[
n
1
n
2
n
3
]
T
=
tan
θ
2
[
n
1
n
2
n
3
]
T
∣
P
c
g
′
∣
=
tan
θ
2
θ
R
c
g
=
2
arctan
∣
P
c
g
′
∣
−
−
(
12
)
\begin{aligned} \mathbf{P}_{c g}^{\prime}&=\frac{1}{\sqrt{4-\left|\mathbf{P}_{c g}\right|^{2}}} \mathbf{P}_{c g}\\ &=\frac{1}{\sqrt{4-|2\sin \frac{\theta}{2} [n_{1} \quad n_{2} \quad n_{3} ]^{T}|^{2}}} \mathbf{P}_{c g}\\ &=\frac{1}{\sqrt{4-4\sin^{2} \frac{\theta}{2}}} \mathbf{P}_{c g}\\ &=\frac{1}{2\cos\frac{\theta}{2}} \mathbf{P}_{c g}\\ &=\frac{1}{2\cos\frac{\theta}{2}} 2\sin \frac{\theta}{2} [n_{1} \quad n_{2} \quad n_{3} ]^{T}\\ &=\tan\frac{\theta}{2} [n_{1} \quad n_{2} \quad n_{3} ]^{T}\\ |\mathbf{P}_{c g}^{\prime}|&=\tan\frac{\theta}{2}\\ \theta_{\mathbf{R}_{cg}}&=2\arctan|\mathbf{P}_{c g}^{\prime}|\quad \color{red}{--(12)}\\ \end{aligned}
Pcg′∣Pcg′∣θRcg=4−∣Pcg∣21Pcg=4−∣2sin2θ[n1n2n3]T∣21Pcg=4−4sin22θ1Pcg=2cos2θ1Pcg=2cos2θ12sin2θ[n1n2n3]T=tan2θ[n1n2n3]T=tan2θ=2arctan∣Pcg′∣−−(12)
需要解释的是这个步骤2不是必须的,步骤3可以完整地包括进去,只是这一步骤比较直观
Step3: 计算 P c g \mathbf{P}_{cg} Pcg
还是通过公式(4)可以反求
P
c
g
′
=
1
4
−
∣
P
c
g
∣
2
P
c
g
P
c
g
=
2
P
c
g
′
1
+
∣
P
c
g
′
∣
2
−
−
(
13
)
\begin{aligned} \mathbf{P}_{c g}^{\prime}&=\frac{1}{\sqrt{4-\left|\mathbf{P}_{c g}\right|^{2}}} \mathbf{P}_{c g}\\ \mathbf{P}_{c g}&=\frac{2 \mathbf{P}_{c g}^{\prime}}{\sqrt{1+\left|\mathbf{P}_{c g}^{\prime}\right|^{2}}}\quad \color{red}{--(13)}\\ \end{aligned}
Pcg′Pcg=4−∣Pcg∣21Pcg=1+∣∣Pcg′∣∣22Pcg′−−(13)
这里直接给出结果,容易推导
Step4: 计算 t c g \mathbf{t}_{cg} tcg
用引理8的公式来计算
t
c
g
\mathbf{t}_{cg}
tcg
(
R
g
i
j
−
I
)
t
c
g
=
R
c
g
t
c
i
j
+
t
g
i
j
(\mathbf{R}_{gij}-\mathbf{I})\mathbf{t}_{cg}=\mathbf{R}_{cg}\mathbf{t}_{cij}+\mathbf{t}_{gij}
(Rgij−I)tcg=Rcgtcij+tgij
同样的,至少两组ij,或者说三个状态以上这个线性方程组才有唯一解
得到矩阵表示
[
R
g
i
1
j
1
−
I
R
g
i
2
j
2
−
I
⋮
R
g
i
n
j
n
−
I
]
t
c
g
=
[
R
c
g
t
c
i
1
j
1
−
t
g
i
1
j
1
R
c
g
t
c
i
2
j
2
−
t
g
i
2
j
2
⋮
R
c
g
t
c
i
n
j
n
−
t
g
i
n
j
n
]
\left[\begin{array}{ll}\mathbf{R}_{g i_{1} j_{1}}-\mathbf{I}\\ \mathbf{R}_{g i_{2} j_{2}}-\mathbf{I}\\ \quad\ \ \ \ \vdots\\ \mathbf{R}_{g i_{n} j_{n}}-\mathbf{I} \end{array}\right]\mathbf{t}_{cg}=\left[\begin{array}{ll} \mathbf{R}_{cg}\mathbf{t}_{ci_{1}j_{1}}-\mathbf{t}_{gi_{1}j_{1}}\\ \mathbf{R}_{cg}\mathbf{t}_{ci_{2}j_{2}}-\mathbf{t}_{gi_{2}j_{2}}\\ \quad\quad\ \ \ \ \ \vdots\\ \mathbf{R}_{cg}\mathbf{t}_{ci_{n}j_{n}}-\mathbf{t}_{gi_{n}j_{n}} \end{array}\right]
⎣⎢⎢⎢⎡Rgi1j1−IRgi2j2−I ⋮Rginjn−I⎦⎥⎥⎥⎤tcg=⎣⎢⎢⎢⎡Rcgtci1j1−tgi1j1Rcgtci2j2−tgi2j2 ⋮Rcgtcinjn−tginjn⎦⎥⎥⎥⎤
同样的得到
A
x
=
b
\mathbf{Ax}=\mathbf{b}
Ax=b,其中
A
∈
R
3
n
×
3
\mathbf{A}\in \mathfrak{R} ^{3n\times3}
A∈R3n×3,
x
∈
R
3
×
1
\mathbf{x}\in \mathfrak{R} ^{3\times1}
x∈R3×1,
b
∈
R
3
n
×
1
\mathbf{b}\in \mathfrak{R} ^{3n\times1}
b∈R3n×1
同样可以使用线性最小二乘方法求解
结尾
论文还对速度和误差做了定义和分析,这里不在叙述,以后有时间再仔细看看