Cálculo geométrico de la ubicación de un robot

Question

Mi equipo busca un código que localice un robot en una cuadrícula X/Y con una precisión de ~cm. El robot está equipado con una combinación de cámara y servo que se utiliza para identificar de forma única columnas con posiciones conocidas que se sitúan fuera del tablero sobre el que se puede mover el robot.

El robot (R) puede determinar los ángulos entre él mismo y cualquier poste (A, B, C, etc) mediante visión por ordenador y medición del valor del servo que hace girar la cámara. No puede determinar la distancia de sí mismo a ningún poste, pero conoce la ubicación (y por tanto las distancias, los ángulos) entre cualquier poste. Se trata de un conocimiento a priori del tablero que está programado en el robot.

Para un triángulo simple formado por ABR, no parece intuitivamente posible localizar un robot, porque sólo se conoce un ángulo (R), y una distancia (AB). Pensando que podríamos resolver el problema incluyendo más puntos, probamos a incluir BCR. Esto parece proporcionar intuitivamente suficientes puntos para localizar definitivamente un robot, pero las matemáticas que hemos hecho no apuntan a una única respuesta. Esto podría deberse a suposiciones del mundo real que no hemos sido capaces de poner en forma de ecuación. Por ejemplo, el robot nunca abandona el tablero, por lo que no son válidas las soluciones fuera de ese recuadro de la cuadrícula.

• ¿Cómo puedo calcular la posición del robot R, dadas las ubicaciones A, B, C, etc.?
• ¿Cuál es el número mínimo de columnas necesario para localizar el robot?
• ¿Es necesario tener en cuenta los supuestos del "mundo real" mencionados anteriormente para localizar con éxito el robot?

Answer 1

Sólo se necesitan dos postes con posiciones conocidas para determinar la posición del robot, siempre que los dos postes y el robot no sean colineales. Pero siempre puedes evitar que esto ocurra colocando los postes de forma que la línea que los une no se cruce con la cuadrícula del robot.

Una vez que tengas un par de postes que no sean colineales con la posición del robot, elige un origen y un $x$ -eje y $y$ -eje. Supongamos que tiene puesto $A$ en $(x_A,y_A)$ y correo $B$ en $(x_B,y_B)$ y que $\theta_A$ sea el ángulo que el robot $R$ hace con poste $A$ y $\theta_B$ sea el ángulo que el robot $R$ hace con poste $B$ cada uno medido desde el positivo $x$ dirección.

Entonces la pendiente de la recta que pasa por el robot y el poste $A$ es $m_{AR}=\tan(\theta_A)$ y la pendiente de la recta que pasa por el robot y el poste $B$ es $m_{BR}=\tan(\theta_B)$ .

Así que podemos encontrar la ecuación de la recta que pasa por el robot y el poste $A$ utilizando la fórmula punto-pendiente como $$ y=m_{AR}(x-x_A)+y_A $$ y la ecuación de la recta que pasa por el robot y el poste $B$ como $$ y=m_{BR}(x-x_B)+y_B. $$

Queremos encontrar la solución simultánea a este sistema de ecuaciones, por lo que tenemos $$ m_{AR}(x-x_A)+y_A=m_{BR}(x-x_B)+y_B $$ para que el $x$ -La coordenada de la posición del robot es $$ x=\frac{y_B-y_A+m_{AR}x_A-m_{BR}x_B}{m_{AR}-m_{BR}}. $$ Entonces, sustituyendo nuestro $x$ -de nuevo en la primera ecuación, encontramos la $y$ -La coordenada de la posición del robot es $$ y=m_{AR}\left(\frac{y_B-y_A+m_{AR}x_A-m_{BR}x_B}{m_{AR}-m_{BR}}-x_A\right)+y_A. $$

Answer 2

Como dice caveat, si tienes una posición general, entonces necesitas 4 columnas. Sin embargo, en el caso que se muestra en la imagen, en el que las rectas que pasan por A y B y por B y C no cruzan la zona admisible para el robot, basta con tres columnas.

La razón es que, en este caso, B y C siempre se verán "desde el mismo lado". Esto significa que el ángulo que midas entre el robot y las líneas que van a B y C siempre será tal que sabrás su orientación. En la imagen, el ángulo CRB siempre será positivo, porque la semirrecta RC está "a la derecha" de la semirrecta RB.

Si esto se conoce, entonces el lugar geométrico que describe todas las posiciones posibles del robot bajo las cuales los puntos B y C son visibles bajo el ángulo orientado específico que usted midió ( $\alpha$ ) forma parte de un círculo (mostrado en verde más abajo). Ese círculo pasa por B y C, ver la imagen.

Es sólo una parte del círculo, porque en la parte restante se verían B y C bajo el ángulo de $180°-\alpha$ en su lugar. Si no conociéramos la orientación del ángulo, tendríamos que añadir la imagen de la parte del círculo verde cuando se refleja en la línea BC a nuestra lista de posibles posiciones del robot.

Por supuesto, se puede hacer lo mismo con el ángulo que mediste de R a A y B o de R a A y C. Si tienes 2 de esas partes del círculo y conoces sus ecuaciones, puedes calcular sus puntos de intersección. Digamos que eliges B,C y A,B. Como ambos círculos pasan por B (en el que es imposible que esté el robot), ¡el único otro punto posible en el que puede estar el robot es el otro punto de intersección!

Ahora tenemos que determinar la ecuación de cada círculo. Haré los cálculos para B,C aquí. La ecuación general de un círculo es

$$(x-x_M)^2 + (y-y_M^2) = r^2,$$

donde $(x_M,y_M)$ son las coordenadas del punto medio y $r$ es el radio del círculo.

Empecemos por el punto medio M. Como la circunferencia siempre pasará por B y C, M estará situado en la mediatriz m de BC. Puedes calcular de antemano el punto medio X de B y C y también un vector unitario ( $u_m$ ) de la dirección de m. Elígela de forma que el vector unitario apunte desde X hacia el lado en el que se encuentra el robot. Así que ahora tenemos

$$x_M = x_X + t\cdot {u_m}_x, y_M = y_X + t\cdot {u_m}_y$$ ,

donde $t$ es la longitud (por ahora) desconocida de XM.

Si recuerdas algunas lecciones sobre ángulos en un círculo, verás que el ángulo CMB es igual a $2\alpha$ y que el ángulo CMX es de nuevo igual a $\alpha$ . Por último, el ángulo MCX es igual a $90°-\alpha$ .

Como la longitud de CX es conocida (la mitad de la longitud de BC) obtenemos ahora

$$t={\rm length}(XM) =\tan(90°-\alpha)\cdot {\rm length}(CX) = \cot(\alpha)\cdot {\rm length}(CX)$$

Si introducimos esto en la fórmula anterior, obtendremos finalmente los valores de las coordenadas de nuestro punto medio M.

Me saltaré ahora explicar por qué esta fórmula sigue siendo correcta incluso cuando tenemos un $\alpha > 90°$ donde mi imagen anterior ya no es correcta porque entonces el punto medio M se desvía al otro lado de la línea BC. Pero en este caso nuestro $\cot(\alpha)$ se vuelve negativo y nuestro M calculado también se desplaza al otro lado de la línea BC.

Así que ya tenemos las coordenadas de nuestro Punto Medio M, ahora sólo necesitamos el radio para obtener la ecuación completa. Pero el radio es simplemente la distancia desde cualquier punto del círculo al punto medio, así que calcula la distancia de M a B,C o R y ya está.

¿Dónde estamos ahora? He mostrado cómo obtener la ecuación del círculo que contiene el lugar geométrico de todos los puntos bajo los cuales se ven 2 puntos dados bajo un ángulo orientado dado.

Obtén 2 de esas ecuaciones y determina los puntos comunes:

$$ (x-x_{M1})^2 + (y-y_{M1})^2 = r_1^2$$ $$ (x-x_{M2})^2 + (y-y_{M2})^2 = r_2^2$$

Toma la diferencia de esas 2 ecuaciones. Todos los términos cuadráticos de la incógnita $x$ y $y$ desaparecerá, te quedas con una ecuación lineal. Transforma esa ecuación lineal en la forma $y=ax+b$ e introdúcelo en una de las ecuaciones anteriores para los círculos. Obtendrás una ecuación cuadrática. Una de sus soluciones será el punto común que elijas para los ángulos (por ejemplo, si decides utilizar los ángulos entre B,C y A,B, B será el punto común). La otra es la posición del robot

---

Ahora bien, lo anterior (por complicado que sea) es sólo el lado puramente geométrico de las cosas. En realidad, se encontrará con

a) el mal funcionamiento de los sensores que, de vez en cuando, sólo producen datos incorrectos, y b) una precisión limitada de las mediciones.

Por lo tanto, debido a b) es mucho mejor no utilizar sólo las 3 columnas que he dicho que son necesarias, sino utilizar tantas como sea posible/factible. Además de estos cálculos, es posible que tengas que hacer algún cálculo para estimar la precisión de tu posición, teniendo en cuenta la inexactitud de las mediciones.

Para a), puede que tenga que llevar un registro de su posición calculada y descartarla si mostrara que se mueve 10cm en 1s cuando sólo puede moverse a tener esa velocidad, o algo similar.

Answer 3

Si la única medida que puedes hacer es el ángulo que forman 2 rayos desde los robots hasta 2 puntos, entonces 3 puntos son insuficientes, pero 4 sí. Esto se debe a una transformación de Mobius de la reflexión del círculo $z\rightarrow 1/z$ . Tratar el plano como $\mathbb{C}$ con el círculo unitario en el origen para ser el único círculo a través de 3 puntos (que existen a menos que 3 puntos son colineales, en cuyo caso la línea de reflexión muestran que hay 2 posibles soluciones). Entonces la reflexión del círculo fijará todos los ángulos y los 3 puntos, mientras que envía una solución a otra. Así que con 3 puntos sólo se puede fijar la posible posición a 2 puntos. Esto debería ser suficiente si hay restricciones adicionales (como la navegación a estima), pero teóricamente no es suficiente.

Supongamos ahora que hay 3 puntos no colineales, y que $O$ es el centro del círculo único que contiene 3 puntos, entonces sea $O$ sea el origen y reescalar su unidad para que el círculo sea un círculo unitario, y suponiendo que el robot siempre permanezca dentro del círculo. Sea $a,b,c$ denotan la posición de $A,B,C$ tratado como $\mathbb{R}^{2}$ . Sea $\alpha,\beta$ sea el $\cos$ de ángulo $ARB,BRC$ .

Ahora calcula $d=((a+b)/2)/\overline{(a+b)/2}$ . Resuelva ahora $t$ en $\alpha((a-td)\cdot d)^{2}=(a-td)\cdot(a-td)$ que es simplemente cuadrática, y asegúrate de que $t\leq 1$ . Esto da $td$ el punto $D$ tal que $ADB$ es dos veces $ARB$ . Haga lo mismo para $BC$ y $\beta$ para obtener $E$ . Ahora interseca el círculo centrado en $D$ que contienen $B$ con el círculo centrado en $E$ que contiene $B$ . Deben intersecarse en $B$ y otro punto, que es lo que necesitas (encontrar intersección de 2 circunferencias puedes buscar la fórmula).

Por supuesto, hay un montón de método probablemente más rápido, pero esta es la forma directa geometría.