旋量理论在机器人标定中的应用

旋量理论在机器人标定
中的应用
王海霞

目录
1.
1.
1.
1.
1.
1.
1.
1. 机器人
机器人标定概述
3.
.
.
.
.
.
.
. 旋量理
旋量理论介绍
4.
.
.
.
.
.
.
. 旋量理
旋量理论描述机器人
描述机器人运动
运动
5.
.
.
.
.
.
.
. 标定方案
定方案
2.
.
.
.
.
.
.
.
机器人
机器人标定概述
1.
1.
1.
1.
1.
1.
1.
1.
背景
背景

目录
1.
1.
1.
1.
1.
1.
1.
1. 机器人
机器人标定概述
3.
.
.
.
.
.
.
. 旋量理
旋量理论介绍
4.
.
.
.
.
.
.
. 旋量理
旋量理论描述机器人
描述机器人运动
运动
5.
.
.
.
.
.
.
. 标定方案
定方案
2.
.
.
.
.
.
.
.
机器人
机器人标定概述
1.
1.
1.
1.
1.
1.
1.
1.
背景
背景

w
b
e
P =
T
任务一:定位
T
T
c
P
e
c
w
b
w
T
b
b
— 手眼标定
— 相机标定
T
逆 变 换 正 变 换
e
e
— 机器人本体标定 ( 机器人正变换 )
T
c
P
— 机器人外部姿态标定
任务二:抓取、装配
P θ P + ∆P
增 加 误 差
背景
c
W
B
E
C
P
P + ∆P
P

目录
1.
1.
1.
1.
1.
1.
1.
1. 机器人
机器人标定概述
3.
.
.
.
.
.
.
. 旋量理
旋量理论介绍
4.
.
.
.
.
.
.
. 旋量理
旋量理论描述机器人
描述机器人运动
运动
5.
.
.
.
.
.
.
. 标定方案
定方案
2.
.
.
.
.
.
.
.
机器人
机器人标定概述
1.
1.
1.
1.
1.
1.
1.
1.
背景
背景

机器人标定概述
标定定义
应用先进的测量手段和基于模型的参数识别方法辨识
出机器人模型的准确参数,从而提高机器人绝对精度
的过程。
误差来源
• 系统误差:制造和安装过程造成连杆长度偏差、关
节距离偏差、关节角度偏差、零位校准偏差等几何
误差
• 随机误差:环境(如温度)、对运动参数的不确切
认知、齿轮传动误差以及由于负重、应力和磨损等
引起的机械变形误差
重复精度只与随机误差有关
绝对定位精度与系统误差有关

标
定
步
骤
参数识别
建模
测量
修正
建立误差
模型

X
l1l 2
3
l4 ωθ θω ωl ω4
5 l1ω6
ωθ θ1 θ4 θ 2 23
53
56
ZXs t
s
Z t
机器人运动学模型
D-H模型 模型 MD MD-H模型 MD
S模型 CPC CPC模型 CPC
1955 1955 1983 1983 1986
1986
Denavit J
Harterberg R S
g =
be
g
b1
g
12
g
23
g
1992
1992
Hayati S A Stone H W Zhuang H
34
g
45
g
56
g
6e
B
E

测量方法:
双经纬仪位子测量系统
三坐标机
关节型多杆随动测量
静
态
测
量
激光跟踪系统
CCD CCD交互测量系统
超声波测量系统
位置测量系统
有接近传感器的测量系统
动
态
测
量

回
路
法
参数识别
轴
线
法
P θ P
逆 变 换 正 变 换
无 误 差
建立误差
模型
位
姿
误
差
模
型
逆 变 换
距
离
误
差
模
型
P − ∆P
θ − ∆θ
正 变 换
增 加 误 差
P

目录
1.
1.
1.
1.
1.
1.
1.
1. 机器人
机器人标定概述
3.
.
.
.
.
.
.
. 旋量理
旋量理论介绍
4.
.
.
.
.
.
.
. 旋量理
旋量理论描述机器人
描述机器人运动
运动
5.
.
.
.
.
.
.
. 标定方案
定方案
2.
.
.
.
.
.
.
.
机器人
机器人标定概述
1.
1.
1.
1.
1.
1.
1.
1.
背景
背景

Chasles定理:
任意刚体运动都可以通螺
旋运动即通过绕某轴的转
动与沿该轴移动的复合运
动实现。刚体变换就是螺
旋运动,螺旋运动就是刚
体变换。
1 8 3 1
C h a sles
旋量理论介绍
B a l l R S
1 9 0 0 1 9 7 8 1 9 8 5 1 9 9 0
H u nt K H
L ip ik in H &D uffy J
P h illip s J R
1 9 9 7
1 9 9 9 2 0 0 1
黄 真
T sa i W L
D a i J
旋量理论(screw screw screw screw theory theory theory theory)的发展
...

⎥
⎦
⎤
⎢
⎣
⎡
+
×
=
ω
ω
ω
ξ
h
0
S
运动旋量
是节距
是轴上一点
是运动的轴方向,
其中 θ
/
d
,
S 0
=
h
ω
⎥
⎦
⎤
⎢
⎣
⎡
×
=
ω
ω
ξ
0
S
旋转:
⎥
⎦
⎤
⎢
⎣
⎡
= v
ξ
0
平移:
⎥
⎦
⎤
⎢
⎣
⎡
=
1
ˆ
0
v
ω
ξ
ω
ω
v h
+
×
= 0
S

运动旋量的指数表达式:
ξ
e ˆ θ
e
⎡I
υθ
⎤
= ⎢ ω =
0
⎥,
⎣ 1 ⎦
θωˆ
θωˆ
T
⎡ e (I - e )( ω×
υ)
+ ωω υ⎤
= ⎢
⎥,
ω ≠
⎣0
1 ⎦
0
θξˆ
θ
υ= S0
× ω+
hω
ˆ ∈ se(
3)
SE ( 3)
定理:给定任一 ξ 和 ( θ ∈ℜ)
, θ 的指数
映射都是 的元素,即 e ( 3)
;
ˆ θξ
∈ SE
θ ξ ˆ
0

刚体变换
θd )
0
(
p 0
S
0
ω
0
υ
ωω
ωω
ωω
ωω
υ
ω
ω
ω
≠
⎥
⎦
⎤
⎢
⎣
⎡
⎥
⎦
⎤
⎢
⎣
⎡ +
×
=
⎥
⎦
⎤
⎢
⎣
⎡
,
1
)
0
(
p
1
)
)(
e
-
(I
e
1
)
(
p
T
ˆ
ˆ
θ
θ
θ
θ

g( θ ) =
g(
0)
含义: g(
0)
和 g( θ ) 都表示动坐标系相对于固定坐
标系的位姿,前者是运动前,后者是运动后; ξˆ 表示一点从起始位置旋转后的位置的变换
e
ˆξ
θ
e θ

目录
1.
1.
1.
1.
1.
1.
1.
1. 机器人
机器人标定概述
3.
.
.
.
.
.
.
. 旋量理
旋量理论介绍
4.
.
.
.
.
.
.
. 旋量理
旋量理论描述机器人
描述机器人运动
运动
5.
.
.
.
.
.
.
. 标定方案
定方案
2.
.
.
.
.
.
.
.
机器人
机器人标定概述
1.
1.
1.
1.
1.
1.
1.
1.
背景
背景

X
l1l 2
3
l4 ωθ θω ωl ω4
5 l1ω6
ωθ θ1 θθ4 23
253
56
ZXs t
Z t
s
旋量理论描述机器人运动
传统的运动模型
g =
be
B
g
b1
g
12
g
23
g
34
g
E
45
g
56
g
6e

X
1l
3
l4 ωθ θω ωl ω4
5 l1ω6
ωθ θ1 θθ4 23
253
56
Z t
l Z Xs
t
2 s
利用旋量描述运动模型:
对于机器人每个关节有:
B
θiωˆ
i
θiωˆ
i
θ ξˆ θ ˆ ⎡e ( I − e ) p ⎤
i i
iξi
i
g( θ i ) = e g(
0)
e = ⎢
⎥
⎣0
1 ⎦
E

)
(
g
e
)
g(
6
6 ˆ
6
0
ξ
θ
θ =
)
(
g
e
e
)
(
g
e
)
,
g(
6
6
5
5
5
5
ˆ
ˆ
6
ˆ
6
5
0
ξ
ξ
ξ θ
θ
θ
θ
θ
θ =
=
)
(
g
e
e
e
e
e
e
)
,
,
,
,
,
g(
6
6
5
5
4
4
3
3
2
2
1
1
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
6
5
4
3
2
1
0
ξ
ξ
ξ
ξ
ξ
ξ θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ =
...
根据矩阵指数的性质机器人的运动与关节运动顺序无关
)
(
g
e
e
e
e
e
e
)
g(
6
6
5
5
4
4
3
3
2
2
1
1
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
0
θ
ξ
ξ
ξ
ξ
ξ
ξ θ
θ
θ
θ
θ
θ
=

目录
1.
1.
1.
1.
1.
1.
1.
1. 机器人
机器人标定概述
3.
.
.
.
.
.
.
. 旋量理
旋量理论介绍
4.
.
.
.
.
.
.
. 旋量理
旋量理论描述机器人
描述机器人运动
运动
5.
.
.
.
.
.
.
. 标定方案
定方案
2.
.
.
.
.
.
.
.
机器人
机器人标定概述
1.
1.
1.
1.
1.
1.
1.
1.
背景
背景

X
l1l 2
3
l4 ωθ θω ωl ω4
5 l1ω6
ωθ θ1 θ4 θ
23
253
56
ZXs t
Z t
s
wb
标定方案
方案一:基于旋量分解的回路法
g be
B
g g =
be
g wb
E
W W
g
E
we
g we

g
g
be
we
( θ ) =
( θ ) =
e
e
ξˆ
θ
μˆ
θ
g
g
ξˆ θ
g
-1
e
μˆ
θ
wb
e =
ξˆ =
g
−1
μˆ
g wb
be
we
g
wb
( 0)
( 0)
wb

机器人外部姿态标定
• 根据机器人基座坐标系的安装定义

实验方法:

方案二:传统的轴线法

D-H参数名义值
序号 aii-1 -1
-1 αii-1 -1 di θi
1 0 0 0 0
2 150 -90 0 -90
3 260 180 0 0
4 60 -90 260 0
5 0 90 0 180
6 0 90 0 0

轴线法求D-H参数
序号
aii-1 -1 αii-1 -1 di θi
1 0.1381 0.015 0.0013 0.0105
2 150.49 -89.9771 0.0134 -90.7678
3 259.89 179.9771 0.0091 0.0515
4 60.24 -90.0057 260.29 0.4927
5 0.0042 90.0609 0.0075 180.0276
6 0.0128 90.0733 0.1308 0.0227

原始旋量参数
序号 1 2 3 4 5 6
ω
P
0 0 0 -1 0 -1
0 1 -1 0 -1 0
1 0 0 0 0 0
0 150 150 410 410 410
0 0 0 0 0 0
0 0 260 320 320
320

轴线法求旋量参数
序号 1 2 3 4 5 6
ω
P
0 0 0 -1 0.001 -0.9994
0 1 -1 0.0001 -1 -0.0323
1 0.0004 0 0.0043 0.0085 -0.0076
0 150.4918 147 407.03 407.03 406.972
3
0 0 0.27 0.5219 0.287 0.9182
0 0 259.87 321.14 321.18
321.1463

方案三:改进的距离误差模型

距离误差与位置误差的关系
AB
AB
B'
A'
R
/
)
A
'
A
-
B
'
B
(
AB l
l
l
l ⋅
≈
−
=
∆
)
(
g
e
e
e
e
e
e
)
g(
6
6
5
5
4
4
3
3
2
2
1
1
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
0
θ
ξ
ξ
ξ
ξ
ξ
ξ θ
θ
θ
θ
θ
θ
=
⎥
⎦
⎤
⎢
⎣
⎡ +
=
⎥
⎦
⎤
⎢
⎣
⎡
⎥
⎦
⎤
⎢
⎣
⎡
=
1
'
'
'
1
1
'
' 0
0
0
0
0
t
t
R
R
R
0
t
R
0
t
R
'
' 0
t
t
R
t +
=
t
d
B
B' =

X
G
...
t
t
t
t
t
t
t
...
t
t
t
t
t
t
t
t
t
t
t
t
t
t
...
t
t
t
t
t
t
t
t
t
t
t
t
t
t
...
t
t
t
t
t
t
t
dt
B
'
B 42
3
6z
6y
6x
6z
6y
6x
6
1z
1y
1x
1z
1y
1x
1
6z
z
6y
z
6x
z
6z
z
6y
z
6x
z
6
z
1z
z
1y
z
1x
z
1z
z
1y
z
1x
z
1
z
6z
y
6y
y
6x
y
6z
y
6y
y
6x
y
6
y
1z
y
1y
y
1x
y
1z
y
1y
y
1x
y
1
y
6z
x
6y
x
6x
x
6z
x
6y
x
6x
x
6
x
1z
x
1y
x
1x
x
1z
x
1y
x
1x
x
1
x
∆
=
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
⎡
∆
∆
∆
∆
∆
∆
∆
∆
∆
∆
∆
∆
∆
∆
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
⎡
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
=
= ×
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
ω
ω
ω
θ
ω
ω
ω
θ
ω
ω
ω
θ
ω
ω
ω
θ
ω
ω
ω
θ
ω
ω
ω
θ
ω
ω
ω
θ
ω
ω
ω
θ
X
)
G
G
(
dt
dt
A
'
A
B
'
B A
B
A
B
∆
−
=
−
=
−
X
J∆
≈
∆l

i
i
t
t
R
t +
= 0
⎥
⎦
⎤
⎢
⎣
⎡ +
=
⎥
⎦
⎤
⎢
⎣
⎡
⎥
⎦
⎤
⎢
⎣
⎡
=
=
1
1
1
)
(
g
e
)
g(
0
0
0
0
'
ˆ
0
t
t
R
R
R
0
t
R
0
t
R
0
ξ i
i
i
i
i
i
θ
θ
i
6
3
iz
iy
ix
iz
iy
ix
iz
z
iy
z
ix
z
iz
z
iy
z
ix
z
iz
y
iy
y
ix
y
iz
y
iy
y
ix
y
iz
x
iy
x
ix
x
iz
x
iy
x
ix
x
X
G
t
t
t
t
t
t
t
t
t
t
t
t
t
t
t
t
t
t
dt
B
'
B ∆
=
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
⎡
∆
∆
∆
∆
∆
∆
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
⎡
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
=
= ×
q
q
q
q
q
q
q
q
q
q
q
q
ω
ω
ω
ω
ω
ω
ω
ω
ω
ω
ω
ω
i
X
J∆
≈
∆l

步骤:
• 初始值
t ( ξ = R t + t
• 绕第一轴旋转得到 ,利用 修正
• 绕第二轴旋转得到 ,修正
6
• 绕第六轴旋转得到 ,修正
ξ
'
1
2
ξ
1 )
'
2
1
ξ g ξ ,..., ξ )
t ( ξ ) = R t ( ξ ' ) + t
6
0
...
1
2
'
1
2
0
0
3
1
4
1
0 ( 1 6
t ( ξ ) = R t ( ξ'
ξ'
ξ'
ξ'
ξ'
) + t
ξ
'
6
ˆ
θξ
'i
g(
θ ) = e g(
0,
ξ1'
,..., ξ 6 ')
g
g
0
0
5
2
( ξ'1
, ξ 2...,
ξ 6
( ξ'1
, ξ'2
..., ξ'5
, ξ 6
6
)
)

g
1
θξ
i ( θ ) = e g1
( 0)
θξ
i g 2 ( θ ) = e g 2 ( 0)
...
g
6
θξ
i ( θ ) = e g 6 ( 0)
ˆ
ˆ
ˆ