CINEMATICA DIRECTA

CALIBRACION DE SERVOS

Para realizar la calibración se tomaron datos en ascenso y descenso para determinar el

comportamiento del servo, a continuación utilizando la librería de Matlab cftool se obtuvo un
polinomio de compensación.

 SERVO 1-BASE (JX PDI 5515 MG 180o )

Servo 1
Teórico Real ascenso Real descenso
0 0 0
10 8 8
20 22 17
30 26 26
40 34 34
50 42 42
60 50 49
70 58 57
80 65 65
90 73 73
100 80 80
110 89 89
120 97 97
130 105 105
140 114 114
150 123 123
160 132 133
170 141 141
180 151 151

P1=[2.056*10^-7 -7.939*10^-5 0.00923 0.8998 0.4643];

 SERVO 2-HOMBRO

Servo 2
Teórico Real ascenso Real descenso
0 0 1
10 9 10
20 18 19
30 27 28
40 36 37
50 46 47
60 56 58
70 66 68
80 76 77
90 85 86
100 96 95
110 205 105
120 115 115
130 125 124
140 135 134
150 145 145
160 155 154
170 165 164
180 171 171

Ascenso
p2=[1.656*10^-9 -6.785*10^-7 0.0001024 -0.007179 1.26 -0.3995];
 Descenso
p2=[2.661*10^-9 -1.144*10^-6 0.0001761 -0.01166 1.35 -1.859];

 SERVO 3-CODO (JX PDI 5515 MG 180o )

Servo 3
Teórico Real ascenso Real descenso
0 0 0
10 9 10
20 17 20
30 28 30
40 37 38
50 46 46
60 55 55
70 65 64
80 74 73
90 82 82
100 92 91
110 100 100
120 110 110
130 119 119
140 128 128
150 138 137
160 145 145
170 155 155
180 165 165
 Ascenso
p3=[1.094 -0.1183];

Descenso
p3=[1.105 -1.162];

 SERVO 4-MUÑECA (JX PDI 5515 MG 180o )

Servo 4
Teórico Real ascenso Real descenso
0 0 0
10 2 10
20 11 20
30 23 29
40 30 38
50 40 45
60 49 55
70 58 63
80 67 70
90 75 82
100 84 90
110 91 98
 120 100 105
130 110 112
140 120 125
150 127 134
160 137 144
170 147 150
180 157 157

Ascenso
p4=[4.526e-09 -1.878e-06 0.0002684 -0.01563 1.505 3.362];

Descenso
p4=[6.805e-09 -2.449e-06 0.0002974 -0.0141 1.403 -0.7575];

 SERVO 5-MUÑECA 2 (JX PDI 5515 MG 180o )

SERVO 5
TEORICO REAL
0 0
10 9
20 19
30 29
40 38
50 48
 60 56
70 64
80 74
90 82
100 91
110 100
120 110
130 119
140 126
150 136
160 145
170 154
180 164

Ascenso y descenso
p5=[-1.053*10^-8 -1.131*10^-5 0.003562 0.9178 0.1753];

CINEMATICA DIRECTA METODO ANALITICO

ARTICULACION θi di ai αi
1 90+Q1 L1 0 90
2 Q2+90 0 L2 0
3 Q3 0 L3 0
4 Q4-90 L4 0 -90
5 Q5 L5 0 0

𝐶(90 + 𝑄1) −𝐶(90)𝑆(90 + 𝑄1) 𝑆(90)𝑆(90 + 𝑄1) 0

𝑆(90 + 𝑄1) 𝐶(90)𝑆(90 + 𝑄1) −𝑆(90)𝐶(90 + 𝑄1) 0
𝑨𝟎𝟏 =
0 𝑆(90) 𝐶(90) 𝐿1
[ 0 0 0 1]
𝐶(90 + 𝑄2) −𝐶(0)𝑆(90 + 𝑄2) 𝑆(0)𝑆(90 + 𝑄2) 𝐿2 ∗ 𝐶(90 + 𝑄2)
𝑆(90 + 𝑄2) 𝐶(0)𝑆(90 + 𝑄2) −𝑆(0)𝐶(90 + 𝑄2) 𝐿2 ∗ 𝑆(90 + 𝑄2)
𝑨𝟏𝟐 =
0 𝑆(0) 𝐶(0) 0
[ 0 0 0 1 ]
 𝐶(𝑄3) −𝐶(0)𝑆(𝑄3) 𝑆(0)𝑆(𝑄3) 𝐿3 ∗ 𝐶(𝑄3)
𝑆𝑄3) 𝐶(0)𝑆(𝑄3) −𝑆(0)𝐶(𝑄3) 𝐿3 ∗ 𝑆(𝑄3)
𝑨𝟐𝟑 =
0 𝑆(0) 𝐶(0) 0
[ 0 0 0 1 ]
𝐶(−90 + 𝑄4) −𝐶(−90)𝑆(−90 + 𝑄4) 𝑆(−90)𝑆(−90 + 𝑄4) 0
𝑆(−90 + 𝑄4) 𝐶(−90)𝑆(−90 + 𝑄4) −𝑆(−90)𝐶(−90 + 𝑄4) 0
𝑨𝟑𝟒 =
0 𝑆(−90) 𝐶(−90) 𝐿4
[ 0 0 0 1]
𝐶(𝑄5) −𝐶(0)𝑆(𝑄5) 𝑆(0)𝑆(𝑄5) 0
𝑆(𝑄5) 𝐶(0)𝑆(𝑄5) −𝑆(0)𝐶(𝑄5) 0
𝑨𝟒𝟓 =
0 𝑆(0) 𝐶(0) 𝐿5
[ 0 0 0 1]

CINEMATICA DIRECTA POR METODO GEOMETRICO

Vista plano (Y, Z)

𝑅𝐿2 = 𝐿2 sin 𝑞2
𝑅𝐿3 = 𝐿3 sin(𝑞2 + 𝑞3 )
𝑅𝐿4 = 𝐿5 sin(𝑞2 + 𝑞3 + 𝑞4 )

𝑅 = 𝑅𝐿2 + 𝑅𝐿3 + 𝑅𝐿4

𝑃𝑧 = 𝐿1 + 𝐿2 cos 𝑞2 + 𝐿3 cos( 𝑞2 + 𝑞3 ) +𝐿5 cos( 𝑞2 + 𝑞3 + 𝑞4 )
Vista plano (X, Y)

𝑃𝑦 = 𝑅 cos 𝑞1 − 𝑑 cos 𝛼

𝛼 = 180 − (90 + 𝑞1 )
𝑃𝑦 = 𝑅 cos 𝑞1 − 𝑑 cos(180 − (90 + 𝑞1 ))
 𝑃𝑥 = 𝑅 sin 𝑞1 + 𝑑 sin(180 − (90 + 𝑞1 ))

Rot=Rotz(q1)* Roty(q2)* Roty(q3)* Roty(q4)* Rotz(q5)