Hola a todos! El desarrollo del hexapod está avanzando y finalmente la parte matemática básica ha sido probada y lista para la documentación. Para que el proyecto sobreviva hasta el final y no acumule polvo en el estante, debe ver sus cambios positivos, incluso si son insignificantes. En este artículo, hablaré sobre un algoritmo para resolver el problema de cinemática inversa y lo demostraré en acción. Espero que sea interesante.
Etapas de desarrollo:Parte 1 - DiseñoParte 2 - montajeParte 3 - CinemáticaParte 4 - trayectorias y secuencias matemáticasParte 5 - ElectrónicaParte 6: transición a la impresión 3DParte 7 - nuevo alojamiento, software de aplicación y protocolos de comunicaciónSistemas de coordenadas
Para comenzar, es necesario determinar los sistemas de coordenadas del robot: hay hasta tres de ellos:
De hecho, no son diferentes entre sí, sino que se encuentran en diferentes lugares de la caja y se pueden rotar entre sí en algún ángulo. Cuando se ve desde arriba, la ubicación de los ejes será la siguiente (el parámetro coxa_zero_rotate se describirá un poco más adelante):
Como probablemente ya haya notado, los sistemas de coordenadas en los lados izquierdo y derecho están ubicados simétricamente en relación con el centro del cuerpo. Me pareció que sería más fácil.
Las coordenadas de cada punto se establecen en relación con la extremidad a la que está destinado este punto. Este enfoque le permite mover la extremidad sin dolor a cualquier parte sin interferir con la configuración.
Y así se ubican en relación con la extremidad:
Las coordenadas del punto objetivo son relativas al SISTEMA DE COORDINACIÓN PRINCIPAL. Inicialmente, quería establecer las coordenadas relativas al centro de la caja, pero resultó que no era muy conveniente y requería el almacenamiento de una serie de parámetros adicionales.
En adelante, la notación X *, X **, etc. será reemplazado por X ′, X ′ ′.
Resolviendo el problema de cinemática inversa
Comencemos con el momento más difícil e interesante para mí: la cinemática. Yo mismo inventé el algoritmo, a veces mirando las propiedades de un triángulo y las funciones trigonométricas. Con la mecánica, es difícil para mí, y con la trigonometría es aún peor, así que quizás en algún lugar no podría tener algo en cuenta, pero este algoritmo funciona (video al final).
1. Declaración del problema
Supongamos que necesitamos la extremidad delantera derecha para alcanzar el punto A con coordenadas (150; -20; 100). También se sabe que la extremidad gira 45 grados con respecto al cuerpo (parámetro coxa_zero_rotate):
La extremidad misma tiene los siguientes parámetros:
Creo que no necesita describir lo que significan estos parámetros, los nombres hablan por sí mismos. Solo puede agregar que todos estos parámetros están determinados por la configuración del caso, son permanentes y se almacenan en la memoria FLASH con la capacidad de editar desde el exterior (para mayor flexibilidad).
Por eso lo necesitas
La imagen muestra varias variaciones del casco y la ubicación de las patas del hexápodo. Ahora coincide con la configuración de ARACNID. Suponga que se me ocurrió cambiar el caso a REPTILES, y para esto será suficiente solo cambiar los valores de los parámetros sin medición en el programa mismo, incluso los puntos objetivo no tendrán que cambiarse (a menos que, por supuesto, se acerquen correctamente a esto).
Todos los parámetros anteriores son suficientes para resolver el problema.
2. La solución al problema.
2.1 Encontrar el ángulo de rotación de COXAEsta etapa es la más simple. Primero debe volver a calcular las coordenadas del punto A en relación con el SISTEMA DE COORDINACIÓN DE MIEMBROS. Obviamente, debe rotar el ángulo coxa_zero_rotate, puede hacerlo utilizando las siguientes fórmulas:
$$ display $$ x ′ = x ⋅ cos (α) + z ⋅ sin (α) = 150 ⋅ cos (45) + 100 ⋅ sin (45) = 176.78 $$ display $$
y′=y=−20
$$ display $$ z ′ = -x ⋅ sin (α) + z ⋅ cos (α) = -150 ⋅ sin (45) + 100 ⋅ cos (45) = -35.36 $$ display $$
Por lo tanto, obtuvimos las coordenadas del punto objetivo A (176.78; -20; -35.36) en relación con el SISTEMA DE COORDINACIÓN DE MIEMBROS.
Ahora puede encontrar el ángulo COXA utilizando la función atan2:
COXA=atan2(z′,x′)=atan2(−35.36,176.78)=−11.30°
Y así, obtuvimos el ángulo por el cual necesitas rotar el servo COXA para que el punto A esté en el plano X′Y ′. Veamos ahora nuestros cálculos en KOMPAS 3D:
De acuerdo
2.2 Encontrar el ángulo de rotación de FEMUR y TIBIAPara encontrar estos ángulos, es necesario ir al plano X′Y ′. Para ir al plano, debe rotar el punto por el ángulo COXA, que ya calculamos anteriormente.
$$ display $$ x ′ = x ′ ⋅ cos (α) + y ′ ⋅ sin (α) = 176.78 ⋅ cos (-11) + -20 ⋅ sin (-11) = 180.28 $$ display $$
y′=y=−20
La coordenada
y ' no cambia, ya que giramos a lo largo del eje Y'.
A continuación, debe eliminar la longitud COXA del cálculo, es decir pasamos al plano X′′Y ′ ′, para esto realizamos un desplazamiento de la coordenada
x ′ del punto por la longitud COXA:
x′′=x′−coxaLength=180.28−40=140.28
y′′=y′
Después de todas estas manipulaciones, la solución adicional del problema se reduce a encontrar los ángulos
ayb del triángulo:
Antes de encontrar los ángulos, debes encontrar el tercer lado C de este triángulo. Esta distancia no es más que la longitud del vector y se calcula mediante la fórmula:
$$ display $$ C = \ sqrt {x ′ ′ ^ 2 + y ′ ′ ^ 2} = \ sqrt {140.28 ^ 2 + (-20) ^ 2} = 141.70 $$ display $$
Ahora debe verificar si la extremidad puede llegar a este punto. Si C es mayor que la suma de las longitudes de FEMUR y TIBIA, entonces no se puede alcanzar el punto. En nuestro caso, 141.70 <141 + 85 - el punto es alcanzable.
Ahora conocemos todos los lados del triángulo y podemos encontrar los ángulos
ayb que necesitamos usando el teorema del coseno:
a=acos(A2+C2−B2 over2AC)=72.05°
b=acos(B2+A2−C2 over2BA)=72.95°
Los ángulos obtenidos no son aplicables para alimentarlos a los servos, ya que esto no tiene en cuenta la posición inicial y el ángulo de inclinación de la línea recta C al eje X. Si conocemos los ángulos iniciales FEMUR y TIBIA (135 ° y 45 °), entonces el ángulo de inclinación C al eje X no es conocido. Puede encontrarlo usando la función atan2 (y ′ ′, x ′ ′):
$$ display $$ φ = atan2 (y ′ ′, x ′ ′) = atan2 (-20, 140.28) = -8.11 ° $$ display $$
Finalmente, puede calcular el ángulo de rotación de los servos FEMUR y TIBIA:
FEMUR=femurZeroRotate−a−φ=135−72.05+8.11=71.06°
FEMUR=b−tibiaZeroRotate=45−72.95=27.95°
Veamos nuestros cálculos:
Parece la verdad.
Resumen
Los ángulos calculados COXA, FEMUR y TIBIA son adecuados para alimentar sus servos. Puede notar que el ángulo COXA es negativo y, en consecuencia, surge la pregunta: "¿Cómo girar la unidad en -11.3 grados?". El truco es que uso la posición neutral del servo COXA como un cero lógico, esto permite que la unidad gire los ángulos positivo y negativo. Por supuesto, esto es obvio, pero creo que no estaría fuera de lugar mencionar esto. Hablaré sobre esto con más detalle en los siguientes artículos cuando hable sobre la implementación de todo lo anterior.
Código fuente
Suficientes palabras, déjame ver el código#define RAD_TO_DEG(rad) ((rad) * 180.0 / M_PI) #define DEG_TO_RAD(deg) ((deg) * M_PI / 180.0) typedef enum { LINK_COXA, LINK_FEMUR, LINK_TIBIA } link_id_t; typedef struct {
Algoritmo en acción
Luego, por casualidad, apareció una secuencia que recordaba un poco a un baile
PS
Me alegraría si alguien puede simplificar este algoritmo. Lo logré para poder entenderlo después de, digamos, medio año.