Primero veamos una relación conveniente entre las coordenadas de un vértice y sus lados.
En el triángulo de la figura, [latex]L_0[/latex] es un lado conocido y [latex]x[/latex] e [latex]y[/latex] las coordenadas buscadas.
Del teorema del coseno tenemos que [latex]x=\dfrac{L_0^2+L_1^2-L_2^2}{2L_0}[/latex]
pero de la figura vemos que [latex]L_1\cos\theta=x[/latex]
Por lo tanto [latex]x=\dfrac{L_0^2+L_1^2-L_2^2}{2L_0}[/latex]
que puede escribirse de esta manera [latex]x=\dfrac{L_0}{2}\left(1+S\;D\right)[/latex]
con [latex]S=\dfrac{L_1+L_2}{L_0}[/latex] y [latex]S=\dfrac{L_1-L_2}{L_0}[/latex]
Para la coordenada [latex]y[/latex] aplicamos Pitágoras [latex]y^2=L_1^2-x^2[/latex]
y trabajándola un poco llegamos a [latex]y=\dfrac{L_0}{2}\sqrt{(S^2-1)(1-D^2)}[/latex]
Estas relaciones de [latex]x[/latex] e [latex]y[/latex] son el punto de partida ya que conocemos la diferencia de distancias, y si se tienen mas triágulos (por lo tanto mas ecuaciones) se podrá conocer la suma.
-----------------------------------------------
Aplicando esas fórmulas con los 4 sensores en cruz
Sensores d y b
[latex]x=\frac{L_{db}}{2}\left(1+S_{db}\;D_{db}\right)-\frac{L_{db}}{2}=\frac{L_{db}}{2}\left(S_{db}\;D_{db}\right)[/latex]
[latex]y=\frac{L_{db}}{2}\sqrt{(S_{db}^2-1)(1-D_{db}^2)}[/latex]
Sensores a y c
[latex]x=\frac{L_{ac}}{2}\sqrt{(S_{ac}^2-1)(1-D_{ac}^2)}[/latex]
[latex]y=\frac{L_{ac}}{2}\left(1+S_{ac}\;D_{ac}\right)-\frac{L_{ac}}{2}=\frac{L_{ac}}{2}\left(S_{ac}\;D_{ac}\right)[/latex]
Debe cumplirse entonces
[latex]\dfrac{L_{db}^2}{4}({S_{db}}^2\;D_{db}^2)=\dfrac{L_{ac}^2}{4}(S_{ac}^2-1)(1-D_{ac}^2)}[/latex]
[latex]\dfrac{L_{ac}^2}{4}(S_{ac}^2\;D_{ac}^2)=\dfrac{L_{db}^2}{4}(S_{db}^2-1)(1-D_{db}^2)}[/latex]
Resultando un sistema de 2x2 cuyas incógnitas son [latex]S_{ac}^2[/latex] y [latex]S_{db}^2[/latex]
No voy a detallar los pasos correspondientes a la solución del sistema y su posterior reemplazo+simplificación porque no aportan nada.
En la imagen siguiente en Excel está descriptos el procedimiento de cálculo resultante.
Además
ya me está pudriendo la tipografía pixelada de este Latex
. El sentido de escribir fórmulas en Latex es hacerlas más legibles, pero este motor es
tan horrible que se entienden mejor en Ascii.
No se donde estará el problema de la espantosa tipografía, porque en otros sitios (como el rinconmatematico) las fórmulas salen como deben: Claras y legibles.
-----------------------------------------------
La otra forma para 4 sensores en cruz es planteando un sistema de ecuaciones partiendo de:
[latex](x_i-x)^2+(y_i-y)^2=(t_i+T)^2[/latex]
y la solución es la que publicó Alejandro.
Hay unos detalles a tener en cuenta:
- Si los sensores están dispuestos de tal manera que exista un punto equidistante de ellos ==> El sistema será singular ==> determinante = 0
Por lo tanto, no es un método apropiado para sensores en cruz o ubicados en una circunferencia.
Algo que no es problema, pues fácilmente se pueden ubicar los sensores de tal manera que nunca los tiempos de todos sean parecidos.
- Son necesarios como mínimo 4 sensores si se quiere "linealizar" el problema.
Si tuviéramos 3 sensores y combináramos entre sí las ecuaciones para eliminar los términos cuadráticos quedaría una matriz singular ==> No way! Aunque el problema igual tiene solución olvidémonos de linealizarlo.
---------------------------------------------------------------------------------------------------
Y ya que estamos, doy un ejemplo en Matlab con 5 sensores aplicando mínimos cuadrados truchos.
¿Por que truchos? Porque en realidad no se está minimizando el error cuadrático, aunque parezca que sí
.
Consideremos esta figura. Igual que antes, se conoce la posicion de los sensores y los tiempos de recepción del pulso pero no el instante de emisión.
Para cada sensor escribimos planteamos: [latex](x_i-x)^2+(y_i-y)^2=(t_i+T)^2[/latex]
que desarrollado queda: [latex]x_ix+y_iy+t_iT=\frac{1}{2}\left((x_i^2+y_i^2-t_i^2)-(T^2-x^2-y^2)\right)[/latex]
El recurso para "linealizar" este sistema como hizo Alejandro es trasladar las coordenadas (posición y tiempos) al baricentro. Ya que respecto del baricentro se cumple que
[latex]\displaystyle\sum_{i=1}^n{x_i}=\displaystyle\sum_{i=1}^n{y_i}=\displaystyle\sum_{i=1}^n{t_i}=0[/latex]
Entonces, al sumar todas las ecuaciones quedará:
[latex]\left(\displaystyle\sum_{i=1}^n{x_i}\right)x+\left(\displaystyle\sum_{i=1}^n{y_i}\right)y + \left( \displaystyle \sum_{i=1}^n{t_i}\right)T = 0 =\frac{1}{2}\left(\displaystyle\sum_{i=1}^n{x_i^2+y_i^2-t_i^2}-N(T^2-x^2-y^2)\right)[/latex]
y de ahí sacamos que debe ser [latex]T^2-x^2-y^2=\dfrac{1}{N}\displaystyle\sum_{i=1}^n{\left(x_i^2+y_i^2-t_i^2\right)}[/latex]
y reemplazando en las ecuaciones nos queda un bonito sistema de ecuaciones lineales
(con mas ecuaciones que incógnitas) que resolvemos por mínimos cuadrados.
La ampliación del método a 3 dimensiones es evidente.
------------------------
La figura anterior la generé con un CAD, las coordenadas de los sensores son:
A = [ 325.41377769 , 0 ]
B = [ 224.33606904 , 211.33832358 ]
C = [ -31.3004399 , 132.49461234 ]
D = [ -45.0387352 , 55.55330358 ]
E = [ 108.13874084 , -39.60976031 ]
puse el emisor en
x = 58.57614754
y = 84.23639344
y las longitudes-tiempos correspondientes de cada segmento quedan
ta_abs = 279.81796019
tb_abs = 208.88095227
tc_abs = 102.01302202
td_abs = 107.51169039
te_abs = 133.39535394
Para simular, supongo que la emisión del pulso ocurrió a T=100
luego, los tiempos leídos por cada sensor serán:
ta = 179.81796019 ;
tb = 108.88095227 ;
tc = 2.01302202 ;
td = 7.51169039 ;
te = 33.39535394 ;
El programa en Matlab podemos implementarlo así:
Código:
% Coordenadas de los sensores
A = [ 325.41377769 , 0 ] ;
B = [ 224.33606904 , 211.33832358 ] ;
C = [ -31.3004399 , 132.49461234 ] ;
D = [ -45.0387352 , 55.55330358 ] ;
E = [ 108.13874084 , -39.60976031 ] ;
u = ones(5,1); v = [1 1 -1]; % Matrices auxiliares
% Tiempo leido en cada sensor
% Como es un test, arbitrariamente le reste 100
% a las distancias originales
ta = 179.81796019 ;
tb = 108.88095227 ;
tc = 2.01302202 ;
td = 7.51169039 ;
te = 33.39535394 ;
Ti = [ta;tb;tc;td;te] ; % Matriz de tiempos leidos
%-------------------------------------------------------
% Resolucion
P = [[A;B;C;D;E],Ti] ; % Armo la matriz principal
Pm = mean(P) ; % Matriz con el promedio de la posicion y los tiempos
Pc = P-u*Pm ; % Traslacion de coordenadas
B = Pc.^2*v' ; % Matriz con xi^2 + yi^2 - ti^2
Bm = mean(B) ; % Su promedio
Bc = B - u*Bm ; % Sus valores respecto del promedio.
Xc = Pc\Bc/2 ; % Resuelve el sistema por LMS
X = Xc' +v.*Pm % Traslado para obtener [x,y,T+tm]
Tabs = Ti + u*X(3) % Tiempos de transito
Ejecutando Matlab devuelve lo que se esperaba que devuelva
===================================================================
Alejandro: No saqué el problema de ningún lado. Dibujé en un CAD los puntos y anoté las longitudes. El resto es álgebra en el baño.
En cuanto me reponga de escribir, subo la solución para solo 3 sensores...
tales que
