La cinemática inversa más sencilla es la que se aplica a brazos tipo Scara y brazos cilíndricos. Básicamente el problema es resolver los ejes X e Y, siendo el eje Z independiente del cálculo cinemático inverso porque ya es un resultado.
 Brazo tipo Scara.
La imagen de abajo explica gráficamente la cinemática inversa de un brazo tipo Scara, es decir, para dos grados de libertad contrastado con un pequeño programa. Los nombres de las variables que están en el programa también están en el gráfico. Las fórmulas trigonométricas las puedes extraer de mi programa si quisieras adaptarlo a otro lenguaje de programación.
O bien, puedes por ejemplo, usar los resultados de los ángulos de este mismo programa y enviarlo a tus servos, añadiendo por tu parte un poquito de programación. Te animo a que experimentes y puedas comprobar que tu Brazo Robot puede moverse en línea recta por ejemplo. Este es un ejemplo básico para dos grados de libertad. Si quieres mover 3 grados de libertad pasa al siguiente capítulo, pero sería bueno comenzar por lo más sencillo al principio e ir poco a poco subiendo de nivel.
 El código está escrito en FreeBasic IDE, pero es 100% compatible con QBasic.
' **************************** Dos Grados de Libertad *****************************" Screen 12 Cls Dim As String tecla pi = atn(1) * 4 BaseX = 300 ' Punto X Base (hombro) Aqui situamos el brazo en pantalla. LongBrazo = 120 ' Longitud Brazo. Puedes modificar las longitudes del brazo o antebrazo. x=(100) ' Posicion Inicial X. Aqui damos las coordenadas iniciales de la punta del brazo.
y=(120)+BaseY ' Posicion Inicial Y Se puede modificar los valores que están dentro del parentesis. ' Procurar que esté dentro del area de trabajo. tecla="" while tecla<>Chr(27) And tecla<>Chr(13) Locate 22,24: Print "Pulsa W S A D para mover el Brazo." Locate 23,27: Print "Para salir pulsa Esc o Enter." 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 LadoA=y-BaseY Hipotenusa=sqr((LadoA*LadoA)+(x*x)) Alfa=ATan2(LadoA,x) Beta=ACOS(((LongBrazo*LongBrazo)-(LongAntBr*LongAntBr)+(Hipotenusa*Hipotenusa))/((2*LongBrazo)*Hipotenusa)) AngBrazo= Alfa+Beta ' ANGULO BRAZO (en radianes). Gamma=ACOS(((LongBrazo*LongBrazo)+(LongAntBr*LongAntBr)-(Hipotenusa*Hipotenusa))/((2*LongBrazo)* LongAntBr)) AngAntBr=Gamma-(180*rad) ' ANGULO ANTEBRAZO (en radianes). PYa=LongBrazo*-sin(AngBrazo) PYb=LongAntBr*-sin(AngAntBr+AngBrazo) PXa=LongBrazo*COS(AngBrazo) PXb=LongAntBr*COS(AngAntBr+AngBrazo) '--------------------- ' PUNTOS PARA DIBUJAR. '--------------------- '------BRAZO (x,y) BrazoPY=PYa+BaseY ' Punto de coordenada Y del Brazo. BrazoPX=PXa+BaseX ' 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. '---------------- ' Dibuja Lineas. '---------------- If tecla<>"" Then Cls ' Una manera simple de borrar las lineas anteriores. PSet (BaseX, BaseY ), 0 ' Punto inicial. Line -(BrazoPX, BrazoPY ), 4 ' Dibuja linea del Brazo. Line -(AntBrazoPX, AntBrazoPY), 9 ' Dibuja linea del AnteBrazo. '----------------------------------------------------------------- 'Muestra en pantalla los resultados de la Cinematica Inversa (IK). '----------------------------------------------------------------- Locate 1,20: Print "Angulos ----> Brazo:"; Int(AngBrazo*grad); Chr(248);" "; "AnteBrazo:";_ Int((AngAntBr*grad)+180); Chr(248);" "
Locate 2,20: Print "Posicion ----> ";x;"X"; " "; y-BaseY; "Y " '----------------------------------------------------------------- Wend End
|