https://sites.google.com/site/proyectosroboticos/cinematica-inversa-i?tmp...

Brazo Robot SCARA.

La cinemtica inversa ms sencilla es la que se aplica al brazo robot tipo SCARA y brazo robot
cilndrico. En el brazo Scara tenemos que resolver los dos ngulos de las articulaciones. En este
tipo de brazos el eje Z no interviene en la cinemtica inversa porque es un resultado en s mismo.
Desde el punto de vista del clculo slo tenemos en cuenta los ejes X e Y, y la longitud de las
articulaciones.

1 de 6

7/29/2014 6:43 PM

https://sites.google.com/site/proyectosroboticos/cinematica-inversa-i?tmp...

(Si no ves la imagen, haz clic en el botn -Actualizar- de tu navegador)

El tringulo formado por LongBrazo, LongAntBr e Hipotenusa suele ser del tipo irregular, sobre
todo si los dos catetos (LongBrazo y LongAntBr) no son iguales. Para resolver los ngulos de este
tipo de tringulo hay que aplicar el Teorema del Coseno.

2 de 6

7/29/2014 6:43 PM

https://sites.google.com/site/proyectosroboticos/cinematica-inversa-i?tmp...

Observa que en las frmulas trigonomtricas se usa la funcin arcocoseno "Acos()" y un tipo de
arcotangente llamado "Atan2()". No es lo mismo Atan(Y/X) que Atan2(Y, X).

Se diferencia la una de la otra, adems de la sintaxis, en que Atan2(), dependiendo de los signos
contenidos en los valores X e Y tiene presente el cuadrante en el que se encuentra y puede
recorrer los cuatro cuadrantes. Por contrapartida la funcin Atan() slo tiene efecto en los
cuadrantes 1 y 4. Comprueba que tu lenguaje de programacin contiene la funcin Atan2() porque
si no los datos no sern correctos. Ms info haciendo clic en el grfico a pie de este prrafo te
llevar a la Wikipedia(En).
En todos los lenguajes de programacin las operaciones y los resultados de las funciones
trigonomtricas son siempre en radianes. Si queremos dar la informacin de los ngulos en el
monitor es ms cmodo hacerlo en grados "normales" llamados -grados sexagesimales- (se llama
as porque cada grado son 60 minutos y a su vez, cada minuto son 60 segundos). Pasar radianes a
grados sexagesimales se hace multiplicando el resultado en radianes por 180/Pi (57.29577951...).
Y viceversa, para pasar grados sexagesimales a radianes hay que multiplicar por Pi/180
(0.017453292...). En el cdigo fuente podrs ver ejemplos de estas conversiones segn las
necesidades del programa.

3 de 6

7/29/2014 6:43 PM

4 de 6

https://sites.google.com/site/proyectosroboticos/cinematica-inversa-i?tmp...

Los nombres de las variables que vers en el cdigo fuente tambin estn en la imagen que ves
aqu arriba para que sepas de dnde viene cada una de las frmula trigonomtrica en las que
adems viene con el ajuste para situarlo en una zona de la pantalla. La aparente complejidad del
programa es debido a que tenemos que situar esos puntos en la pantalla para formar mediante dos
lneas el brazo tipo Scara de 2GL (GL= Grado de Libertad). En el monitor el eje X se corresponde
con el plano que estamos acostumbrados en matemticas, pero el eje Y no comienza en la esquina
inferior izquierda, sino en la parte superior izquierda. Por esta razn en el cdigo fuente
interviene la variable BaseY al comienzo de la cinemtica inversa.
Te animo a que experimentes y puedas comprobar que tu Brazo Robot tipo Scara puede por
ejemplo moverse en lnea recta. Aqu se muestra para dos grados de libertad (2GL). Si necesitas
que el brazo Scara tenga un ngulo de mueca o quieres mover 3 grados de libertad pasa al
siguiente captulo.
Cliquea aqu para bajarte el cdigo fuente y ejecutable.
(Si usas antivirus Avast has de aadir una exclusin para poder ejecutarlo. Para analizar este o cualquier otro
archivo puedes hacer clic aqu)

El cdigo est escrito en FreeBasic IDE, pero es 99% compatible con QBasic.
The translation could modify the code. Use the code without translating or download the program by clicking the link
above.
WindowTitle "Cinematica Inversa para brazo robot SCARA."
Declare Sub InverseK
Declare Sub DibujaBrazo
Screen 19

' Ventana de 800x600.

Cls
Dim Shared As Double
Dim Shared As Integer
Dim Shared As String

pi, grad, rad, Xaux, Yaux, x, y, AngAntBr, AngBrazo

BaseY, BaseX, LongBrazo, LongAntBr
Tecla

pi
= Atn(1)*4
rad = pi/180
grad = 180/pi
BaseX
BaseY

= 400
= 300

LongBrazo = 150
antebrazo.
LongAntBr = 150
x=(150)
del brazo.

' Punto X Base (hombro)

' punto Y Base (hombro)

Situamos el brazo en pantalla.

' Longitud Brazo.

Puedes modificar las longitudes del brazo o

' Longitud AnteBrazo.

' Posicion Inicial X.

Damos las coordenadas iniciales de la punta

7/29/2014 6:43 PM

5 de 6

https://sites.google.com/site/proyectosroboticos/cinematica-inversa-i?tmp...

y=(150)+BaseY
' Posicion Inicial Y.
Se puede modificar los valores que estn
dentro del parntesis.
'
Procurar que est dentro del rea de
trabajo.
Tecla=" "
While Tecla<>Chr(27) And Tecla<>Chr(13) And Tecla<>Chr(255)+"k"
Locate 35,33: Print "Pulsa W S A D para mover el Brazo."
Locate 36,35: Print "Para salir pulsa Esc o Enter."
InverseK
If Tecla <> "" Then

' Slo cuando se pulsa una tecla dibuja el brazo.

DibujaBrazo
EndIf
Tecla=LCase(InKey)
If Tecla="a" Then x=x-1
If Tecla="d" Then x=x+1
If Tecla="s" Then y=y-1
If Tecla="w" Then y=y+1
Wend
End
Sub InverseK
'---------------------------------'*******Cinemtica Inversa*********
'---------------------------------Dim As Double

LadoA, Alfa, Beta, Gamma, Hipotenusa

LadoA=y-BaseY
Hipotenusa=Sqr((LadoA^2)+(x^2))
Alfa=Atan2(LadoA,x)
Beta= ACos( ((LongBrazo^2)-(LongAntBr^2)+(Hipotenusa^2))/(2*LongBrazo*Hipotenusa) )
AngBrazo= Alfa+Beta
' ANGULO BRAZO(en radianes).
Gamma=ACos( ((LongBrazo^2)+(LongAntBr^2)-(Hipotenusa^2))/(2*LongBrazo*LongAntBr) )
AngAntBr=Gamma-(180*rad)

' ANGULO ANTEBRAZO(en radianes).

If Str(Beta)="-1.#IND" Or Str(Gamma)="-1.#IND" Then

x=Xaux
y=Yaux

' En caso de error (fuera de rango) se vuelve a llamar a s misma

' y carga con los valores anteriores correctos.

InverseK
EndIf
Xaux=x
Yaux=y
End Sub
Sub DibujaBrazo
'--------------------' PUNTOS PARA DIBUJAR.
'--------------------Dim As Double

PYa, PYb, PXa, PXb, BrazoPY, BrazoPX, AntBrazoPY, AntBrazoPX

PYa=LongBrazo*-Sin(AngBrazo)
PYb=LongAntBr*-Sin(AngAntBr+AngBrazo)

7/29/2014 6:43 PM

https://sites.google.com/site/proyectosroboticos/cinematica-inversa-i?tmp...

PXa=LongBrazo*Cos(AngBrazo)
PXb=LongAntBr*Cos(AngAntBr+AngBrazo)
'BRAZO (x,y)
BrazoPY=PYa+BaseY
BrazoPX=PXa+BaseX

' Punto de coordenada Y del Brazo.

' Punto de coordenada X del Brazo.

'ANTEBRAZO (x,y)
AntBrazoPY=PYb+PYa+BaseY ' Punto de coordenada Y del AnteBrazo.
AntBrazoPX=PXb+PXa+BaseX ' Punto de coordenada X del AnteBrazo.
Cls

' Borra lneas anteriores.

PSet

(BaseX,

BaseY

), 0

' Punto inicial.

Line

-(BrazoPX,

BrazoPY

), 4

' Dibuja lnea del brazo.

Line

-(AntBrazoPX, AntBrazoPY), 9

' Dibuja lnea del antebrazo.

'----------------------------------------------------------------'Muestra en pantalla los resultados de la Cinemtica Inversa (IK).

'----------------------------------------------------------------Locate 1,30: Print "Angulos ----> Brazo:"; CInt(AngBrazo*grad); Chr(248);_
" "; "AnteBrazo:"; CInt((AngAntBr*grad)+180); Chr(248);" "
Locate 2,30: Print "Posicion ---->
";x;"X"; " "; y-BaseY;
"Y
"
End Sub

6 de 6

7/29/2014 6:43 PM