Algoritmo 2-6-1: Representación D-H 0. Numere las articulaciones de la 1 a la n comenzando con la base () y terminando con la herramienta, en el orden yaw, pitch y roll. 1. Asigne un sistema coordenado L0 a la base del robot, asegurándose que el eje Z0 se alinee con el eje de la articulación 1. Haga K=1. 2. Alinee Zk con el eje de la articulación K+1. 3. Ubique el origen del sistema coordenado Lk en la intersección de los ejes Zk y Zk-1. Si no se intersectan, use la intersección de Zk con una normal común entre Zk y Zk-1. 4. Seleccione Xk tal que se ortogonal a ambos, Zk y Zk-1. Si Zk y Zk-1 son paralelos, ubique a Xk sobre la normal común a Zk y Zk-1, y que apunte alejándose de Zk-1. 5. Seleccione Yk para completar el sistema coordenado Lk. 6. Haga k=k+1. Si k n, ir al paso 2; si no, continuar. 7. Ubique el origen de Ln en la punta de la herramienta. Alinee Zn con el vector de aproximación, Yn con el vector de deslizamiento, y a Xn con el vector normal de la herramienta. Haga K=1. 8. Calcule θk como el ángulo de rotación de Xk-1 a Xk medido sobre Zk-1, para que queden paralelos. 9. Calcule dk como la distancia medida a lo largo de Zk-1, que habría que desplazar Lk-1 para que Xk- 1 y Xk quedaran alineados. 10. Calcule ak como la distancia medida a lo largo de Xk (que ahora coincidiría con Xk-1) que habría que desplazar el nuevo Lk-1 para que su origen coincidiese con Lk. 11. Calcule αk como el ángulo de rotación de Zk-1 a Zk medido sobre Xk, para que queden paralelos. 12. Haga k=k+1, si k n, ir al paso 8; si no, pare.
20cm Configuración Esférica 5GDL con pitch y roll
0. Numere las articulaciones de la 1 a la n comenzando con la base () y terminando con la herramienta, en el orden yaw, pitch y roll.
1. Asigne un sistema coordenado L0 a la base del robot, asegurándose que el eje Z0 se alinee con el eje de la articulación 1. Haga K=1. 2. Alinee Zk con el eje de la articulación K+1. 3. Ubique el origen del sistema coordenado Lk en la intersección de los ejes Zk y Zk-1. Si no se intersectan, use la intersección de Zk con una normal común entre Zk y Zk-1. Z 0
4. Seleccione Xk tal que se ortogonal a ambos, Zk y Zk-1. Si Zk y Zk-1 son paralelos, ubique a Xk sobre la normal común a Zk y Zk-1, y que apunte alejándose de Zk-1. X 1 X 2 X 3 X 4 Z 0 X 0
5. Seleccione Yk para completar el sistema coordenado Lk. 6. Haga k=k+1. Si k n, ir al paso 2; si no, continuar. Y 2 X 1 X 2 X 3 Y 3 Y 1 Z 0 Y 0 X 0 Y 4 X 4
7. Ubique el origen de Ln en la punta de la herramienta. Alinee Zn con el vector de aproximación, Yn con el vector de deslizamiento, y a Xn con el vector normal de la herramienta. Haga K=1. Y 2 X 1 X 2 X 3 Y 3 X 5 Y 1 Z 0 Y 0 X 0 Y 4 Y 5 X 4 Z 5
Y 2 X 1 X 2 X 3 Y 3 X 5 Y 1 8. Calcule θk como el ángulo de rotación de Xk-1 a Xk medido sobre Zk-1, para que queden paralelos. Z 0 Y 0 X 0 Y 4 Y 5 X 4 Z 5 Tabla de parámetros cinemáticos Ejes θ d a α Home 1 θ 1 0 2 θ 2 π/2 3 0 4 θ 4 0 5 θ 5 0
Y 2 X 1 X 2 X 3 Y 3 X 5 Y 1 Z 0 9. Calcule dk como la distancia medida a lo largo de Zk-1, que habría que desplazar Lk-1 para que Xk-1 y Xk quedaran alineados. Y 0 X 0 Y 4 Y 5 X 4 Z 5 Tabla de parámetros cinemáticos Ejes θ d a α Home 1 θ 1 50 0 2 θ 2 20 π/2 3 0 50 4 θ 4 0 0 5 θ 5 25 0
Y 2 X 1 X 2 X 3 Y 3 X 5 Y 1 Z 0 10. Calcule ak como la distancia medida a lo largo de Xk (que ahora coincidiría con Xk-1) que habría que desplazar el nuevo Lk-1 para que su origen coincidiese con Lk. Y 0 X 0 Y 4 Y 5 X 4 Z 5 Ejes θ d a α Home 1 θ 1 50 0 0 2 θ 2 20 0 π/2 3 0 0 50 4 θ 4 0 0 0 5 θ 5 25 0 0 Tabla de parámetros cinemáticos
Y 2 X 1 X 2 X 3 Y 3 X 5 Y 1 Z 0 11. Calcule αk como el ángulo de rotación de Zk-1 a Zk medido sobre Xk, para que queden paralelos. Y 0 X 0 Y 4 Y 5 X 4 Z 5 Tabla de parámetros cinemáticos Ejes θ d a α Home 1 θ 1 50 0 π/2 0 2 θ 2 20 0 π/2 π/2 3 0 0 -π/2 50 4 θ 4 0 0 π/2 0 5 θ 5 25 0 0 0
T T T T T T T T Herramienta Muñeca Herramienta Base Base Muñeca 1 2 3 4 5 0 1 2 3 4 C k C ksk SkSk akc k k (,,, ) Sk C kc k SkC k aks k Tk 1 k dk k ak 0 Sk Ck dk 0 0 0 1 C1 0 S1 0 1 S1 0 C1 0 T0( 1) 0 1 0 50 0 0 0 1 T C2 0 S2 0 S2 0 C2 0 ( ) 0 1 0 20 0 0 0 1 2 1 2 Tabla de parámetros cinemáticos Ejes θ d a α Home 1 θ 1 50 0 π/2 0 2 θ 2 20 0 π/2 π/2 3 0 0 -π/2 50 4 θ 4 0 0 π/2 0 5 θ 5 25 0 0 0 T 3 2 1 0 0 0 0 0 1 0 ( ) 0 1 0 0 0 0 1 T 4 3 C4 0 S4 0 S4 0 C4 0 ( 4) 0 1 0 0 0 0 0 1 T C5 S5 0 0 S5 C5 0 0 ( ) 0 0 1 25 0 0 0 1 5 4 5
Matrices de transformación T T T T T T T T Herramienta Muñeca Herramienta Base Base Muñeca 1 2 3 4 5 0 1 2 3 4 T Herramienta Base C1 0 S1 0 C2 0 S2 0 1 0 0 0 C4 0 S4 0 C5 S5 0 0 S 0 C 0 S 0 C 0 0 0 1 0 S 0 C 0 S C 0 0 1 1 2 2 4 4 5 5 0 1 0 50 0 1 0 20 0 1 0 d 3 0 1 0 0 0 0 1 25 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 De MatLab T50 = [ (c1c2c4-c1s2s4)c5+s1s5, -(c1c2c4-c1s2s4)s5+s1c5, c1c2s4+c1s2c4, 25c1c2s4+25c1s2c4+c1s2+20s1] [ (s1c2c4-s1s2s4)c5-c1s5, -(s1c2c4-s1s2s4)s5-c1c5, s1c2s4+s1s2c4, 25s1c2s4+25s1s2c4+s1s2-20c1] [ (s2c4+c2s4)c5, -(s2c4+c2s4)s5, s2s4-c2c4, 50+25s2s4-25c2c4-c2] [ 0, 0, 0, 1] Simplificar la ecuacion utilizando identidades trigonometricas
Posicion de la herramienta Encuentre la posicion de la herramienta para w=[0, pi/2, 50, 0, 0] De MatLab T50 = [ (c1c2c4-c1s2s4)c5+s1s5, -(c1c2c4-c1s2s4)s5+s1c5, c1c2s4+c1s2c4, 25c1c2s4+25c1s2c4+c1s2+20s1] [ (s1c2c4-s1s2s4)c5-c1s5, -(s1c2c4-s1s2s4)s5-c1c5, s1c2s4+s1s2c4, 25s1c2s4+25s1s2c4+s1s2-20c1] [ (s2c4+c2s4)c5, -(s2c4+c2s4)s5, s2s4-c2c4, 50+25s2s4-25c2c4-c2] [ 0, 0, 0, 1] T50 = 0.0000 0 1.0000 75.0000 0-1.0000 0-20.0000 1.0000 0-0.0000 50.0000 0 0 0 1.0000 X=75 Y=-20 Z=50