基于局部POE的机器人运动学标定
导语：定位精度作为影响机器人性能的重要因素，需要对机器人进行标定来提高机器人的定位精度从而满足工业上的需要。
Question： 为什么机器人需要标定？
Answer：为了使机器人本体达到与理论设计模型一致，提高机器人的点精度和轨迹精度或者能够完全以编程设定的动作运动，机器人在装配后需要进行标定工作。标定的方法主要有激光跟踪仪、拉线法、20点法等方法。
一. 标定过程
基于误差模型的标定方法可以提高机器人在整个空间里的精度。误差模型法通常分为四步：
1.建模：根据机器人的结构建立机器人运动学模型。
2.测量：利用外部设备测量末端执行器在某一坐标系下的实际位姿。
3.参数辨识：将机器人末端执行器的实际位姿代入误差模型得到机器人的几何参数误差。
4.参数补偿：将机器人的参数修改成计算出来的实际运动学参数。
1.1 建模
建模方法使用指数积建立机器人的运动学模型。基于指数积的机械臂正运动学 。这种建模方法是全局POE运动学建模，之所以使用指数积建模是因为使用DH法建模时不满足完整性和连续性，而POE建模可以满足其标定模型的要求。
1.2 测量
视觉捕捉Nokov红外测量或者使用激光传感器测量末端执行器在测量原坐标系下的位姿信息。
1.3 参数辨识
机器人运动学参数辨识，采用的方法使用递归最小二乘法。
1.4 参数补偿
几何参数补偿：这里采用局部POE方法标定，局部POE标定并不是还原实际的机器人几何参数，而是为了得到一台绝对精度更加准确的机器人，我们可以在标定完成之后更改机器人输入关节角使机器人走到理想的位置。
二. 基于局部POE的机器人正向运动学建模
旋量理论数学基础
全局POE运动学公式：(考虑末端执行器)
T
(
θ
)
=
e
[
S
1
]
θ
1
⋯
e
[
S
n
−
1
]
θ
n
−
1
e
[
S
n
]
θ
n
M
T
n
,
n
+
1
T\left( \theta \right) =e^{\left[ S_1 \right] \theta _1}\cdots e^{\left[ S_{n-1} \right] \theta _{n-1}}e^{\left[ S_n \right] \theta _n}M\,\,T_{n,n+1}
T(θ)=e[S1]θ1⋯e[Sn−1]θn−1e[Sn]θnMTn,n+1其中
T
n
,
n
+
1
T_{n,n+1}
Tn,n+1 为末端执行器到第六关节法兰盘中心的变换矩阵。
局部POE运动学公式：将初始矩阵
M
M
M改写成
M
=
T
0
,
1
(
0
)
T
1
,
2
(
0
)
T
2
,
3
(
0
)
T
3
,
4
(
0
)
T
4
,
5
(
0
)
T
5
,
6
(
0
)
M=T_{0,1}\left( 0 \right) T_{1,2}\left( 0 \right) T_{2,3}\left( 0 \right) T_{3,4}\left( 0 \right) T_{4,5}\left( 0 \right) T_{5,6}\left( 0 \right)
M=T0,1(0)T1,2(0)T2,3(0)T3,4(0)T4,5(0)T5,6(0)
T
0
,
1
(
0
)
T_{0,1}\left( 0 \right)
T0,1(0) 代表第一关节到第二关节的初始位姿变换矩阵。以此类推，有局部POE公式
T
(
θ
)
=
T
0
,
1
(
0
)
e
[
S
1
]
θ
1
⋯
⋯
T
n
−
1
,
n
(
0
)
e
[
S
n
]
θ
n
T
n
,
n
+
1
(
0
)
T\left( \theta \right) =T_{0,1}\left( 0 \right) e^{\left[ S_1 \right] \theta _1}\cdots \cdots T_{n-1,n}\left( 0 \right) e^{\left[ S_n \right] \theta _n}\,T_{n,n+1}\left( 0 \right)
T(θ)=T0,1(0)e[S1]θ1⋯⋯Tn−1,n(0)e[Sn]θnTn,n+1(0) 由此可知，局部POE需要再每个关节上建立坐标系，并在此坐标系下设置关节旋量。坐标系的建立只需得到关节坐标系变换关系即可。
三. 基于局部POE的误差建立方法
3.1 采用位姿测量的误差模型
定理：对于一个给定的转换矩阵 T ∈ S E ( 3 ) T\in SE\left( 3 \right) T∈SE(3) ，至少存在一个旋量 p ∧ ∈ s e ( 3 ) \overset{\land}{p}\in se\left( 3 \right) p∧∈se(3) 使得 e p ∧ i = T e^{\overset{\land}{p}_i}=T ep∧i=T，因此可以令 e p ∧ i = T i − 1 , i ( 0 ) e^{\overset{\land}{p}_i}=T_{i-1,i}\left( 0 \right) ep∧i=Ti−1,i(0) ，那么局部POE公式可以改写为 T ( θ ) = T 0 , n + 1 = e p 1 ∧ e [ S 1 ] θ 1 ⋯ ⋯ e p n ∧ e [ S n ] θ n e p n + 1 ∧ T\left( \theta \right) =T_{0,n+1}=e^{\overset{\land}{p_1}}e^{\left[ S_1 \right] \theta _1}\cdots \cdots e^{\overset{\land}{p_n}}e^{\left[ S_n \right] \theta _n}e^{\overset{\land}{p_{n+1}}} T(θ)=T0,n+1=ep1∧e[S1]θ1⋯⋯epn∧e[Sn]θnepn+1∧
对上式求导并经过处理，将 旋量
S
S
S 和 角度
θ
\theta
θ 看作是初始矩阵
T
(
0
)
T\left( 0 \right)
T(0) 的一个误差源。有局部POE的误差模型：
y
=
J
x
y=Jx
y=Jx 其中
y
=
log
(
T
0
,
n
+
1
a
,
T
0
,
n
+
1
−
1
)
∨
∈
R
6
×
1
x
=
[
δ
p
1
,
δ
p
2
,
δ
p
3
,
δ
p
4
,
δ
p
5
,
δ
p
6
]
T
∈
R
6
(
n
+
1
)
×
6
J
=
[
A
d
(
T
0
,
1
(
0
)
,
)
,
A
d
(
T
0
,
1
)
A
d
(
T
1
,
2
(
0
)
,
)
,
.
.
.
A
d
(
T
0
,
n
(
0
)
,
)
A
d
(
T
n
,
n
+
1
(
0
)
,
)
]
∈
R
6
(
n
+
1
)
×
6
y=\log \left( T_{0,n+1}^{a},T_{0,n+1}^{-1} \right) ^{\lor}\in R^{6\times 1} \\ x=\left[ \delta p_1,\delta p_2,\delta p_3,\delta p_4,\delta p_5,\delta p_6 \right] ^T\in R^{6\left( n+1 \right) \times 6} \\ J=\left[ Ad\left( T_{0,1}\left( 0 \right) , \right) , Ad\left( T_{0,1} \right) Ad\left( T_{1,2}\left( 0 \right) , \right) , ... Ad\left( T_{0,n}\left( 0 \right) , \right) Ad\left( T_{n,n+1}\left( 0 \right) , \right) \right] \in R^{6\left( n+1 \right) \times 6}
y=log(T0,n+1a,T0,n+1−1)∨∈R6×1x=[δp1,δp2,δp3,δp4,δp5,δp6]T∈R6(n+1)×6J=[Ad(T0,1(0),),Ad(T0,1)Ad(T1,2(0),),...Ad(T0,n(0),)Ad(Tn,n+1(0),)]∈R6(n+1)×6
T
0
,
n
+
1
−
1
T_{0,n+1}^{-1}
T0,n+1−1 和
J
J
J 可以用机器人名义运动学模型得到，而
T
0
,
n
+
1
a
T_{0,n+1}^{a}
T0,n+1a 可以通过实际测量数据得到，因此位姿误差
y
y
y 可以通过计算可得，
x
x
x 就是需要辨识的运动学参数误差。
辨识方程：
x
=
J
−
1
y
x=J^{-1}y
x=J−1y
基于局部POE法参数更新方法为
T
i
−
1
,
i
n
e
w
(
0
)
=
T
i
−
1
,
i
o
l
d
(
0
)
e
δ
p
∧
i
T_{i-1,i}^{new}\left( 0 \right) =T_{i-1,i}^{old}\left( 0 \right) e^{\delta \overset{\land}{p}_i}
Ti−1,inew(0)=Ti−1,iold(0)eδp∧i按照以上方法对参数误差进行辨识处理，当误差满足要求时迭代结束。
注：标定点数至少测量14组点数，20点最好。
四. 参数补偿
通过基于局部 POE 辨识出来的参数，虽然也不是真实参数，但是却是一组与真实参数效果相同的参数，即对于同一组关节角度，无论是采用真实参数还是采用基于局部 POE 辨识出来的参数，都将到达同一位姿。
将标定后得到的初始矩阵
T
d
T_d
Td 和名义关节角度
q
0
q_0
q0 作为初始值进行迭代，求逆解得到实际关节角
q
=
I
n
v
e
r
s
e
k
i
n
e
m
a
t
i
c
s
P
O
E
(
q
0
,
T
d
)
q=InversekinematicsPOE\left( q0,Td \right)
q=InversekinematicsPOE(q0,Td)
将这一组关节角输入到机器人控制器，位置值可以用这组关节角和机器人控制器中的名义参数计算出来，再输入到机器人。这样机器人就可以到达我们想要的位置，虽然机器人中的名义位置与我们想达到的位置不同，但是机器人的绝对精度提高了。
算例：6R型开链机器人
设理论位置为关节角为[0.1;0.1;0.1;0.1;0.1;0.1]的位姿
参数设置：设置位姿误差
δ
y
\delta y
δy 小于1e-8时停止迭代，循环100次，迭代不出来视为求解失败。
运算结果：
在迭代26次后，停止了迭代，此时位姿成功迭代到实际位姿，再继续求逆解得到实际的关节角为[0.1;0.1;0.1;0.1;0.1;0.1]，再将这组关节角输入给控制器，控制器根据这组关节角算出来的名义位置更加逼近于真实位姿，此为标定完成，显著地提高了机器人的绝对精度。
注：标定成功的最直接的验证方法是在机器人基坐标系下得到的测量位置与名义位置是否相同。在标定完成后，可以给机器人输入关节角观察机器人的名义位置和实际位置是否足够接近来观察标定是否成功。