Así, el sistema de coordenadas del robot es $X, Y, Z$ donde los vectores son de longitud unitaria y ortogonales entre sí. $R$ es el punto del robot al que está unido el sistema de coordenadas. Supongamos que su vector $X'$ viene dada por las coordenadas $(u,v)$ es decir $$X' = u \, X + v \, Z$$ y que la ubicación de la etiqueta está en el punto $T$ . Su objetivo es ejecutar el movimiento que toma el vector $X$ al vector $-X' = -u\, X - v\, Z$ y el vector $Z$ al vector $Z' = v\, X - u \, Z$ . Así pues, basta con tomar el producto punto $\big(X\cdot (- X')\big) = - (X \cdot X') = \cos(\alpha)$ suponiendo que $X $ es la unidad de longitud, es decir $|X'|^2 = (X' \cdot X') = 1$ . Si no, toma $$\frac{\big(X\cdot (-X')\big)}{\sqrt{(X'\cdot X')}} = - \frac{\big(X\cdot (u\, X + v\,Z)\big)}{\sqrt{u^2 + v^2}} = - \frac{u \big(X\cdot X\big) + v\big(X\cdot Z\big)}{\sqrt{u^2 + v^2}} = - \frac{u \big(X\cdot X\big)}{\sqrt{u^2 + v^2}}= \frac{- u}{\sqrt{u^2 + v^2}}$$ porque $\big(X\cdot Z\big) = 0$ debido a la ortogonalidad entre el vector $X$ y $Z$ y $\big(X\cdot X\big) = 1$ porque $X$ es el vector unitario del marco ortonormal del robot. Entonces $$\cos(\alpha) = \frac{- u}{\sqrt{u^2 + v^2}}$$ Para determinar el ángulo, sin embargo, usted quiere tomar $$\alpha = \text{arccos}\left(\frac{- u}{\sqrt{u^2 + v^2}}\right)$$ que sólo da ángulos entre $[0,\pi]$ . Sin embargo, es posible que desee ángulos entre $[-\pi,\pi]$ o equivalentemente entre $[0,2\pi]$ . Entonces el signo de un segundo producto punto $$\sin(\alpha) = \frac{\big(Z\cdot(- X')\big)}{\sqrt{(X'\cdot X')}} = - \frac{\big(Z\cdot (u\, X + v\,Z)\big)}{\sqrt{u^2 + v^2}} = - \frac{u \big(Z\cdot X\big) + v\big(Z\cdot Z\big)}{\sqrt{u^2 + v^2}} = - \frac{v \big(Z\cdot Z\big)}{\sqrt{u^2 + v^2}}= \frac{v}{\sqrt{u^2 + v^2}}$$ Por lo tanto, el signo del ángulo $\alpha$ debe ser $\text{sign}(\sin(\alpha)) = - \frac{v}{|v|}$ Así que finalmente, su ángulo debe ser $$\alpha = -\frac{v}{|v|} \text{arccos}\left(\frac{-u}{\sqrt{u^2 + v^2}}\right)$$
Tengo un poco de prisa, así que es posible que haya que volver a comprobar las señales.
Por lo que tengo entendido, básicamente quieres que el robot gire primero en una dirección ortogonal a la dirección $X'$ se mueven en línea recta a lo largo de la dirección determinada por el vector $Z'$ ortogonal al vector $X'$ hasta que llegue a la línea determinada por el punto de etiqueta $T$ y el vector $X'$ y luego girar en la dirección paralela al vector $X'$ hasta la etiqueta. ¿Estoy en lo cierto?
A continuación, primero hay que calcular la distancia que el robot tiene que recorrer perpendicularmente a $X'$ para llegar a la línea determinada por $T$ y $X'$ . Sean las coordenadas de $T$ en el marco del robot sea $(t_1,t_2)$ es decir $T = t_1 \, X + t_2 \, Z$ . La ecuación de la recta determinada por $T$ y $X'$ puede escribirse como $$v(x - t_1) - u(z - t_2) = 0$$ donde $x,z$ denotan coordenadas en el plano. El robot debe recorrer primero la línea determinada por un vector ortogonal a $X'$ que es $Z' = v \, X - u\, Z$ . Entonces la trayectoria a lo largo de $Z'$ debe ser $sZ' = sv \, X - su \, Z$ y $s$ debe elegirse de forma que $sZ'$ se encuentra realmente en la línea determinada por $T$ y $X'$ . En otras palabras, $s$ satisface la ecuación
$$v(sv - t_1) - u(-su - t_2) = 0$$ que se transforma en $$sv^2 - t_1v + su^2 + t_2u = 0$$ Resolución de $s$ conduce a $$s = \frac{t_1 v - t_2 u}{u^2 + v^2}$$ Por último, la distancia orientada entre el punto $R$ del robot y el punto $P$ en la línea determinada por $T$ y $X'$ es $$d = \text{sign}(s) |sZ'| = \text{sign}(s) |s|\, |Z'| = s \sqrt{u^2 + v^2} = \frac{t_1 v - t_2 u}{\sqrt{u^2 + v^2}}$$
Así que para guiar al robot a la etiqueta, de la manera que usted desea, usted tiene que decidir que la rotación orientada positivamente debe ser una rotación en sentido antihorario (cuando se mira el plano desde arriba), mientras que la orientación negativa debe ser una rotación en sentido horario. Entonces, siempre que el vector $X' = u\, X + v\, Z$ calcula la distancia orientada $$d = \frac{t_1 v - t_2 u}{\sqrt{u^2 + v^2}}$$ y el ángulo $$\alpha = -\frac{v}{|v|} \text{arccos}\left(\frac{-u}{\sqrt{u^2 + v^2}}\right)$$ Luego haz que el robot gire de ángulo $\frac{\pi}{2}-\alpha$ de modo que esté preparado para desplazarse en la dirección determinada por el vector $Z' \perp X'$ y si $d>0$ dejar que el robot avance una distancia $d$ (si $d<0$ el robot tiene que ir hacia atrás), luego deja que se detenga, haz que gire $90^{\circ}$ en el sentido de las agujas del reloj hacia la etiqueta y luego dejar que baje hacia la etiqueta a lo largo de la dirección $-X'$ . Si $d<0$ el robot tiene que retroceder después de girar inicialmente. El caso en que $v=0$ creo que deberían tratarse por separado.
