Robots Control: The Inverse momentum control strategy Curse 9-10.
26
Robots Control: The Inverse momentum control strategy Curse 9-10
-
Upload
cameron-allison -
Category
Documents
-
view
220 -
download
2
Transcript of Robots Control: The Inverse momentum control strategy Curse 9-10.
Robots Control: The Inverse momentum control strategy
Curse 9-10
Robot Control
pqmglqbqJ )cos(
Robot Modeling
21 uu
pqmglqbqJ )cos(
)(1 ekekqJu PVd
)cos(2 qmglqbu
pPV
PVdp
ekeke
qmglqbekekqJqmglqbqJ
)cos()()cos(
Robot Control
0 2 4 6 8
e
-0.08
-0.06
-0.04
-0.02
0
Time (second)
)(1 ekekqJu PVd
)cos(2 qmglqbu
Robot Control
dqqNqqM ),()( 21
uqqM d )(1
),(2 qqN ),()( qqNuqqM d
),()(),()( qqNuqqMqqNqqM dd
dqMue )(1
Robot Control
Robot Control
nPn
P
P
nVn
V
V
n e
e
e
k
k
k
e
e
e
k
k
k
u
u
u
.
.00
....
0.0
0.0
.
.00
....
0.0
0.0
.2
1
2
1
2
1
2
1
2
1
nIn
I
I
nPn
P
P
nVn
V
V
n k
k
k
e
e
e
k
k
k
e
e
e
k
k
k
u
u
u
.
.00
....
0.0
0.0
.
.00
....
0.0
0.0
.
.00
....
0.0
0.0
.2
1
2
1
2
1
2
1
2
1
2
1
2
1
),()( qqNuqqM d
Robot Control ),()( qqNuqqM d
dqqGN
IM
)(ekdiagekdiagu ViPi )()(
dqN
IM
ˆ
ˆ ekdiagekdiagu ViPi )()(
)()()( IiViPi kdiagekdiagekdiagu
Robot Control Example 1
Robot Control Example 1 (control strategy)
pPiVi
p
PiVid
qMekdiagekdiage
qqNqqM
qqNekdiagekdiagqqM
)()()(
),()(
),()()()(
1
PqM p )(1
Robot Control Example 1 (control strategy)
11111
122222
111111
2
1
2
1
2
1
2
1
2
1
2
1
..
.00
....
0.0
0.0
.
.00
....
0.0
0.0
...
Pekeke
Pekeke
Pekeke
P
P
P
e
e
e
k
k
k
e
e
e
k
k
k
e
e
e
nPV
PV
PV
nnPn
P
P
nVn
V
V
n
PiVii kskssG
2
1)(
2
2 2
1
2
niPi
PiVi
i
niPi
niiVi
k
kkk
k
i
iri J
k
2ri
ni
Robot Control Example 1 (desired position velocity and accelerations)
Robot Control Example 1 (robot model)
gm
qsqllmcm
qqsqqsqllmclm
q
q
q
m
aa
aa
3
212113222
2122221232212
3
2
1
3
2221
1211
3
2
1
0
0
0
)(
)2)((
00
0
0
))2()2(( 321221
22
2132221
222
212
211111 zzzzxx JJJcqllllmcqclclmcma
))()(( 32
221
2
232221
2
2222112 zzzz JJcqlllmcqclcmaa
)( 322
23
2
22222 zzzz JJlmcma
Robot Control Example 1 (robot model)
Robot Control Example 1 (controller design)
),()()()( qqNekdiagekdiagqqM PiVid
5.000
014.0075.014.0
0075.014.015.06538.0
)( 2
22
cq
cqcq
qM
0
075.0
15.0075.0
),( 221
212222
sqq
qqsqqsq
qqN
3
2
1
00
00
00
)(
V
V
V
Vi
k
k
k
kdiag
3
2
1
00
00
00
)(
P
P
P
Pi
k
k
k
kdiag
Robot Control Example 1 (simulation)
Robot Control Example 1 (simulation)
Robot Control Example 1 (results)
Robot Control Example 2 (task)
q3=1m
q1=1m
q2=1.5m
Ay0
z0
X0
z0
q3=3mm
q1=5m
q2
=2.5m
B
x0
y0
20s
5s
varianta.1
varianta.2
Robot Control Example 2 (desired p,v,a)
0 5 10 15 20 -0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8 ddq1 [m/s2]
Timp [s]
0 5 10 15 20 0
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6 dq1[m/s]
Timp [s]
0 5 10 15 20 1
1.5
2
2.5
3
3.5
4
4.5
5 q1[m]
Timp[s]
0 5 10 15 20 -0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
0.2 ddq2 [m/s2]
Timp[s]
0 5 10 15 20 0
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4 dq2[m/s]
Timp[s]
0 5 10 15 20 1.5
1.6
1.7
1.8
1.9
2
2.1
2.2
2.3
2.4
2.5 q2[m]
Timp[s]
0 5 10 15 20 -0.4
-0.3
-0.2
-0.1
0
0.1
0.2
0.3
0.4 ddq3 [m/s2]
Timp [s]
0 5 10 15 20 0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8 dq2 [m/s]
Timp[s]
0 5 10 15 20 1
1.2
1.4
1.6
1.8
2
2.2
2.4
2.6
2.8
3 q3[m]
Timp[s]
Robot Control Example 2 (dynamic model)
0
0
00
00
00
3
3
2
1
321
3
32
3
2
1
gm
q
q
q
mmm
m
mm
q3
q1
q2
c11
c22
c33
m1,J1
m2,J2
m3,J3
x0
y0
z0
x1
y2 y3
0
250
0
17500
0250
0075
3
2
1
3
2
1
q
q
q
3
2
1
1
3
2
1
250
17500
0250
0075
q
q
q
175
25
25075
3
2
1
Robot Control Example 2 (dynamic model)
-K-
1/175
-K-
1/75
-K-
1/25
+ -
Sum
1/s dq3
1/s q3
1/s q1
1/s q2
1/s dq1
1/s dq2
1
in_1
1
out_1
2
out_2
3
in_3
3
out_3
2
in_2 250
Constant
175
25
25075
3
2
1
Robot Control Example 2 (control strategy)
dqqGN
IM
)(
)()()(
)()(
)()(qGekdiagekdiag
ekdiagekdiagu
qqGuqIPiVi
PiVi
dd
pdPiVi
p
ddPiVi
qqVqIqMqekdiagekdiage
qqqGqqVqqM
qqqGekdiagekdiag
),())(()()(
)(),()(
)()()(
Robot Control Example 2 (controller design)
0
0
00
00
00
3
3
2
1
321
3
32
3
2
1
gm
q
q
q
mmm
m
mm
0
250
0
00
00
00
00
00
00
3
2
1
3
2
1
3
2
1
3
2
1
3
2
1
e
e
e
k
k
k
e
e
e
k
k
k
P
P
P
V
V
V
Robot Control Example 2 (simulation)
Robot_TTTControlerPD+Gravitational
Mux
Muxde
ep3d.mat
q3
v1d.mat
dq1
v2d.mat
dq2
v3d.mat
dq3
p2d.mat
q2
p1d.mat
q1
-+
Sum2
-+
Sum
-+
Sum1
1
out_1
2
out_2
3
out_3
1
in_1
6
in_6
5
in_5
4
in_4
2
in_23
in_3
e3
e2
e1
3
out_3
3
in_3
2
out_2
2
in_2
1
out_1
1
in_1
f(u)
cupla1
f(u)
cupla2
f(u)
cupla3
-+
Sum1
2
out_2
1
out_1
-+
Sum2
in_23
in_34
in_4
1
in_1
-+
Sum2
3
out_3
5
in_5
6
in_6
Robot Control Example 2 (results)
0 5 10 15 20
-6
-4
-2
0
2
4 x 10
-4
Time (second)
e3[m]
0 5 10 15 20
0
2
4
6
8
10
12 x 10 -3
Time (second)
e2[m]
0 5 10 15 20 -10
-5
0
5
x 10 -4
Time (second)
e1[m]
0 5 10 15 20
220
240
260
280
300
Time (second)
2[N]
0 5 10 15 20 -6
-4
-2
0
2
4
Time (second)
1[N]
0 5 10 15 20
-6
-4
-2
0
2
4
Time (second)
3[N]
