2010-12-06 19 views
7

Intentando encontrar funciones que nos ayuden a dibujar una línea 3D a través de una serie de puntos.Puntos de ajuste de la curva en el espacio 3D

Para cada punto que conocemos: Fecha & Hora, Latitud, Longitud, Altitud, Velocidad y Rúbrica. Es posible que se graben los datos cada 10 segundos y nos gustaría poder valorar los puntos intermedios y aumentar la granularidad a 1 segundo. Así creando una ruta de vuelo virtual en el espacio 3D.

He encontrado una serie de algoritmos de ajuste de curvas que aproximarán una línea a través de una serie de puntos, pero no garantizan que los puntos estén intersecados. Tampoco tienen en cuenta la velocidad y el rumbo para determinar la ruta más probable que toma el objeto para llegar al siguiente punto.

Respuesta

16

Desde un punto de vista de la física:

Tiene que asumir algo sobre la aceleración en su int puntos intermedios para obtener la interpolación.

Si su sistema físico se comporta relativamente bien (como un automóvil o un avión), en oposición a, por ejemplo, una pelota que rebota, puede seguir suponiendo una aceleración que varía linealmente con el tiempo entre sus puntos.

la ecuación vectorial para una constante variando movimiento acelerado es:

x''[t] = a t + b 

donde todas las magnitudes excepto t son vectores.

Para cada segmento que ya conoce v (t = t0) x (t = t0) TFINAL yx (TFINAL) v (TFINAL)

Al resolver la ecuación diferencial que se obtiene:

Eq 1: 
x[t_] := (3 b t^2 Tf + a t^3 Tf - 3 b t Tf^2 - a t Tf^3 - 6 t X0 + 6 Tf X0 + 6 t Xf)/(6 Tf) 

y la imposición de las contraints inicial y final para la posición y la velocidad que se obtiene:

las ecuaciones 2:

a -> (6 (Tf^2 V0 - 2 T0 Tf Vf + Tf^2 Vf - 2 T0 X0 + 2 Tf X0 + 
     2 T0 Xf - 2 Tf Xf))/(Tf^2 (3 T0^2 - 4 T0 Tf + Tf^2)) 

b -> (2 (-2 Tf^3 V0 + 3 T0^2 Tf Vf - Tf^3 Vf + 3 T0^2 X0 - 
     3 Tf^2 X0 - 3 T0^2 Xf + 3 Tf^2 Xf))/(Tf^2 (3 T0^2 - 4 T0 Tf + Tf^2))}} 

Entonces, al insertar los valores para eqs 2 en eq 1, obtiene la interpolación temporal para sus puntos, en función de la posición y las velocidades iniciales y finales.

HTH!

Editar

Unos pocos ejemplos con cambio de velocidad abrupto en dos dimensiones (en 3D es exactamente el mismo). Si las velocidades inicial y final son similares, obtendrá rutas "más rectas".

Supongamos:

X0 = {0, 0}; Xf = {1, 1}; 
T0 = 0;  Tf = 1; 

Si

V0 = {0, 1}; Vf = {-1, 3}; 

alt text

V0 = {0, 1}; Vf = {-1, 5}; 

alt text

V0 = {0, 1}; Vf = {1, 3}; 

alt text

Aquí es una animación donde se puede ver el cambio de velocidad de V0 = {0, 1} a Vf = {1, 5}: alt text

Aquí puede ver un cuerpo de aceleración en 3D con posiciones tomadas en intervalos iguales:

alt text

Editar

Un problema completo:

Para mayor comodidad, trabajaré en coordenadas cartesianas. Si desea convertir de lat/log/alt para cartesiana simplemente hacer:

x = rho sin(theta) cos(phi) 
y = rho sin(theta) sin(phi) 
z = rho cos(theta) 

Dónde phi es la longitud, la latitud es theta y rho es la altitud más el radio de la Tierra.

Así Supongamos que empezamos nuestro segmento en:

t=0 with coordinates (0,0,0) and velocity (1,0,0) 

y terminan a las

t=10 with coordinates (10,10,10) and velocity (0,0,1) 

hice claramente un cambio en el origen de coordenadas para establecer el origen de mi punto de inicio. Esto es sólo para conseguir buenos números redondos ...

Así que reemplazar esos números en las fórmulas para A y B y bajar:

a = {-(3/50), -(3/25), -(3/50)} b = {1/5, 3/5, 2/5} 

Con los vamos a la ecuación 1, y la posición de la objeto está dado por:

p[t] = {1/60 (60 t + 6 t^2 - (3 t^3)/5), 
     1/60 (18 t^2 - (6 t^3)/5), 
     1/60 (12 t^2 - (3 t^3)/5)} 

Y eso es todo. Obtienes la posición de 1 a 10 segundos reemplazando t por su valor en la ecuación anterior.
Las carreras de animación:

alt text

Editar 2

Si usted no quiere meterse con la aceleración vertical (tal vez debido a que su "velocímetro" no lo lee), que podría simplemente asignar una velocidad constante al eje z (hay un error muy pequeño para considerarlo paralelo al eje Rho), igual a (Zfinal - Zinit)/(Tf-T0), y luego resolver el problema en el plano de olvido la altitud

+0

¿Qué piensas sobre el uso de los métodos de Catmull-Rom de Microsoft XNA Framework? – trackit

+0

@trackit Nunca vi que C-R se usara para resolver un modelo cinemático, pero creo que se puede adaptar. El problema es que C-R no resuelve su problema con la velocidad (inicial/final) que coincide con AFAIK. –

+1

@trackit No sé si está lo suficientemente claro en el texto de mi respuesta, pero solo tiene que escribir eqs 1 y 2 y listo ... –

1

Lo que estás preguntando es un problema general de interpolation. Supongo que su problema real no se debe al algoritmo de ajuste de curva que se utiliza, sino a su aplicación a todos los valores discretos registrados por el sistema en lugar del conjunto de valores correspondiente.

Vamos a descomponer su problema. Actualmente, estás dibujando un punto en el espacio 3D con un mapa esférico, ajustándote para trayectos lineales y curvos. Si discretizamos las operaciones realizadas por un objeto con seis grados de libertad (roll, pitch, and yaw), las únicas operaciones que le interesan particularmente son las trayectorias lineales y las curvas que representan el cabeceo y el guiñada en cualquier dirección. También es posible contabilizar la aceleración y desaceleración teniendo en cuenta basic physics.

El tratamiento de la asignación esférica es fácil. Simplemente desenvuelva sus puntos relativos a su posición en un plano, ajustándose a la latitud, longitud y altitud. Esto debería permitir aplanar datos que de otro modo existirían a lo largo de una trayectoria curva, aunque esto puede no ser estrictamente necesario para las soluciones a su problema (ver a continuación).

La interpolación lineal es fácil. Dado un número arbitrario de puntos hacia atrás en el tiempo que se ajustan a una línea dentro de n error según lo determinado por su sistema, * construya la línea y calcule la distancia en el tiempo entre cada punto. A partir de aquí, intente ajustar los puntos de tiempo a uno de los dos casos: velocidad constante o aceleración constante.

La interpolación de curvas es un poco más difícil, pero aún plausible. Para los casos de cabeceo, guiñada o cabeceo combinado + guiñada, construya un plano que contenga un número arbitrario de puntos hacia atrás en el tiempo, dentro del error m para lecturas curvas de su sistema. * A partir de estos datos, construya planar curve y una vez más la cuenta para velocidad constante o acceleration along the curve.

Puede hacer algo mejor que esto al intentar predecir las operaciones esperadas de un avión en vuelo como parte de un árbol de decisión o red neuronal relativa a la ruta de vuelo. Lo dejo como un ejercicio para el lector.

Lo mejor de la suerte diseñando su sistema.

-

* Se espera que ambas lecturas de error para ser recibidas por el GPS, dada la descripción del problema. La contabilidad y el ajuste de errores en estos datos es un problema interesante aparte.

+0

Gracias por los comentarios: sus pensamientos han inspirado una nueva serie de búsquedas y me han llevado a las splines de Catmull-Rom ... – trackit

1

Lo que necesita (en lugar de modelar la física) es ajustar una spline a través de los datos. Utilicé un libro de recetas numéricas (http://www.nrbook.com/a tiene algoritmos gratuitos de C y FORTRAN. Consulte la sección 3.3 del F77 para obtener los cálculos necesarios).Si quieres ser simple, simplemente ajusta las líneas a través de los puntos, pero eso no dará como resultado una trayectoria de vuelo uniforme. El tiempo será su valor x, y cada parámetro que se encuentre tendrá sus propios parámetros de spline cubico.

ya que nos gusta envíos largos para esta pregunta aquí es el código completo:

// conductor del programa

static void Main(string[] args) 
{ 
    double[][] flight_data = new double[][] { 
     new double[] { 0, 10, 20, 30 }, // time in seconds 
     new double[] { 14500, 14750, 15000, 15125 }, //altitude in ft 
     new double[] { 440, 425, 415, 410 }, // speed in knots 
    }; 

    CubicSpline altitude = new CubicSpline(flight_data[0], flight_data[1]); 
    CubicSpline speed = new CubicSpline(flight_data[0], flight_data[2]); 

    double t = 22; //Find values at t 
    double h = altitude.GetY(t); 
    double v = speed.GetY(t); 

    double ascent = altitude.GetYp(t); // ascent rate in ft/s 

} 

// spline cúbica definición

using System.Linq; 

/// <summary> 
///  Cubic spline interpolation for tabular data 
/// </summary> 
/// <remarks> 
///  Adapted from numerical recipies for FORTRAN 77 
///  (ISBN 0-521-43064-X), page 110, section 3.3. 
///  Function spline(x,y,yp1,ypn,y2) converted to 
///  C# by jalexiou, 27 November 2007. 
///  Spline integration added also Nov 2007. 
/// </remarks> 
public class CubicSpline 
{ 
    double[] xi; 
    double[] yi; 
    double[] yp; 
    double[] ypp; 
    double[] yppp; 
    double[] iy; 

    #region Constructors 

    public CubicSpline(double x_min, double x_max, double[] y) 
     : this(Sequence(x_min, x_max, y.Length), y) 
    { } 

    public CubicSpline(double x_min, double x_max, double[] y, double yp1, double ypn) 
     : this(Sequence(x_min, x_max, y.Length), y, yp1, ypn) 
    { } 

    public CubicSpline(double[] x, double[] y) 
     : this(x, y, double.NaN, double.NaN) 
    { } 

    public CubicSpline(double[] x, double[] y, double yp1, double ypn) 
    { 
     if(x.Length == y.Length) 
     { 
      int N = x.Length; 
      xi = new double[N]; 
      yi = new double[N]; 
      x.CopyTo(xi, 0); 
      y.CopyTo(yi, 0); 

      if(N > 0) 
      { 
       double p, qn, sig, un; 
       ypp = new double[N]; 
       double[] u = new double[N]; 

       if(double.IsNaN(yp1)) 
       { 
        ypp[0] = 0; 
        u[0] = 0; 
       } 
       else 
       { 
        ypp[0] = -0.5; 
        u[0] = (3/(xi[1] - xi[0])) * 
          ((yi[1] - yi[0])/(x[1] - x[0]) - yp1); 
       } 
       for (int i = 1; i < N-1; i++) 
       { 
        double hp = x[i] - x[i - 1]; 
        double hn = x[i + 1] - x[i]; 
        sig = hp/hn; 
        p = sig * ypp[i - 1] + 2.0; 
        ypp[i] = (sig - 1.0)/p; 
        u[i] = (6 * ((y[i + 1] - y[i])/hn) - (y[i] - y[i - 1])/hp) 
         /(hp + hn) - sig * u[i - 1]/p; 
       } 
       if(double.IsNaN(ypn)) 
       { 
        qn = 0; 
        un = 0; 
       } 
       else 
       { 
        qn = 0.5; 
        un = (3/(x[N - 1] - x[N - 2])) * 
          (ypn - (y[N - 1] - y[N - 2])/(x[N - 1] - x[N - 2])); 
       } 
       ypp[N - 1] = (un - qn * u[N - 2])/(qn * ypp[N - 2] + 1.0); 
       for (int k = N-2; k > 0; k--) 
       { 
        ypp[k] = ypp[k] * ypp[k + 1] + u[k]; 
       } 

       // Calculate 1st derivatives 
       yp = new double[N]; 
       double h; 
       for(int i = 0; i < N - 1; i++) 
       { 
        h = xi[i + 1] - xi[i]; 
        yp[i] = (yi[i + 1] - yi[i])/h 
          - h/6 * (ypp[i + 1] + 2 * ypp[i]); 
       } 
       h = xi[N - 1] - xi[N - 2]; 
       yp[N - 1] = (yi[N - 1] - yi[N - 2])/h 
          + h/6 * (2 * ypp[N - 1] + ypp[N - 2]); 
       // Calculate 3rd derivatives as average of dYpp/dx 
       yppp = new double[N]; 
       double[] jerk_ij = new double[N - 1]; 
       for(int i = 0; i < N - 1; i++) 
       { 
        h = xi[i + 1] - xi[i]; 
        jerk_ij[i] = (ypp[i + 1] - ypp[i])/h; 
       } 
       Yppp = new double[N]; 
       yppp[0] = jerk_ij[0]; 
       for(int i = 1; i < N - 1; i++) 
       { 
        yppp[i] = 0.5 * (jerk_ij[i - 1] + jerk_ij[i]); 
       } 
       yppp[N - 1] = jerk_ij[N - 2]; 
       // Calculate Integral over areas 
       iy = new double[N]; 
       yi[0] = 0; //Integration constant 
       for(int i = 0; i < N - 1; i++) 
       { 
        h = xi[i + 1] - xi[i]; 
        iy[i + 1] = h * (yi[i + 1] + yi[i])/2 
          - h * h * h/24 * (ypp[i + 1] + ypp[i]); 
       } 
      } 
      else 
      { 
       yp = new double[0]; 
       ypp = new double[0]; 
       yppp = new double[0]; 
       iy = new double[0]; 
      } 
     } 
     else 
      throw new IndexOutOfRangeException(); 
    } 

    #endregion 

    #region Actions/Functions 
    public int IndexOf(double x) 
    { 
     //Use bisection to find index 
     int i1 = -1; 
     int i2 = Xi.Length; 
     int im; 
     double x1 = Xi[0]; 
     double xn = Xi[Xi.Length - 1]; 
     bool ascending = (xn >= x1); 
     while(i2 - i1 > 1) 
     { 
      im = (i1 + i2)/2; 
      double xm = Xi[im]; 
      if(ascending & (x >= Xi[im])) { i1 = im; } else { i2 = im; } 
     } 
     if((ascending && (x <= x1)) || (!ascending & (x >= x1))) 
     { 
      return 0; 
     } 
     else if((ascending && (x >= xn)) || (!ascending && (x <= xn))) 
     { 
      return Xi.Length - 1; 
     } 
     else 
     { 
      return i1; 
     } 
    } 

    public double GetIntY(double x) 
    { 
     int i = IndexOf(x); 
     double x1 = xi[i]; 
     double x2 = xi[i + 1]; 
     double y1 = yi[i]; 
     double y2 = yi[i + 1]; 
     double y1pp = ypp[i]; 
     double y2pp = ypp[i + 1]; 
     double h = x2 - x1; 
     double h2 = h * h; 
     double a = (x-x1)/h; 
     double a2 = a*a; 
     return h/6 * (3 * a * (2 - a) * y1 
       + 3 * a2 * y2 - a2 * h2 * (a2 - 4 * a + 4)/4 * y1pp 
       + a2 * h2 * (a2 - 2)/4 * y2pp); 
    } 


    public double GetY(double x) 
    { 
     int i = IndexOf(x); 
     double x1 = xi[i]; 
     double x2 = xi[i + 1]; 
     double y1 = yi[i]; 
     double y2 = yi[i + 1]; 
     double y1pp = ypp[i]; 
     double y2pp = ypp[i + 1]; 
     double h = x2 - x1; 
     double h2 = h * h; 
     double A = 1 - (x - x1)/(x2 - x1); 
     double B = 1 - A; 

     return A * y1 + B * y2 + h2/6 * (A * (A * A - 1) * y1pp 
        + B * (B * B - 1) * y2pp); 
    } 

    public double GetYp(double x) 
    { 
     int i = IndexOf(x); 
     double x1 = xi[i]; 
     double x2 = xi[i + 1]; 
     double y1 = yi[i]; 
     double y2 = yi[i + 1]; 
     double y1pp = ypp[i]; 
     double y2pp = ypp[i + 1]; 
     double h = x2 - x1; 
     double A = 1 - (x - x1)/(x2 - x1); 
     double B = 1 - A; 

     return (y2 - y1)/h + h/6 * (y2pp * (3 * B * B - 1) 
        - y1pp * (3 * A * A - 1)); 
    } 

    public double GetYpp(double x) 
    { 
     int i = IndexOf(x); 
     double x1 = xi[i]; 
     double x2 = xi[i + 1]; 
     double y1pp = ypp[i]; 
     double y2pp = ypp[i + 1]; 
     double h = x2 - x1; 
     double A = 1 - (x - x1)/(x2 - x1); 
     double B = 1 - A; 

     return A * y1pp + B * y2pp; 
    } 

    public double GetYppp(double x) 
    { 
     int i = IndexOf(x); 
     double x1 = xi[i]; 
     double x2 = xi[i + 1]; 
     double y1pp = ypp[i]; 
     double y2pp = ypp[i + 1]; 
     double h = x2 - x1; 

     return (y2pp - y1pp)/h; 
    } 

    public double Integrate(double from_x, double to_x) 
    { 
     if(to_x < from_x) { return -Integrate(to_x, from_x); } 
     int i = IndexOf(from_x); 
     int j = IndexOf(to_x); 
     double x1 = xi[i]; 
     double xn = xi[j]; 
     double z = GetIntY(to_x) - GetIntY(from_x); // go to nearest nodes (k) (j) 
     for(int k = i + 1; k <= j; k++) 
     { 
      z += iy[k];        // fill-in areas in-between 
     }    
     return z; 
    } 


    #endregion 

    #region Properties 
    public bool IsEmpty { get { return xi.Length == 0; } } 
    public double[] Xi { get { return xi; } set { xi = value; } } 
    public double[] Yi { get { return yi; } set { yi = value; } } 
    public double[] Yp { get { return yp; } set { yp = value; } } 
    public double[] Ypp { get { return ypp; } set { ypp = value; } } 
    public double[] Yppp { get { return yppp; } set { yppp = value; } } 
    public double[] IntY { get { return yp; } set { iy = value; } } 
    public int Count { get { return xi.Length; } } 
    public double X_min { get { return xi.Min(); } } 
    public double X_max { get { return xi.Max(); } } 
    public double Y_min { get { return yi.Min(); } } 
    public double Y_max { get { return yi.Max(); } } 
    #endregion 

    #region Helpers 

    static double[] Sequence(double x_min, double x_max, int double_of_points) 
    { 
     double[] res = new double[double_of_points]; 
     for (int i = 0; i < double_of_points; i++) 
     { 
      res[i] = x_min + (double)i/(double)(double_of_points - 1) * (x_max - x_min); 
     } 
     return res; 
    } 

    #endregion 
} 
+0

¿Cómo se relacionan las velocidades inicial y final? –

+0

En la inicialización de la spline cúbica se definen las condiciones finales (si se conocen). – ja72

+0

¿Cómo usarías estas funciones para interpolar los puntos mencionados? Cada punto tiene dateTime, latitud, longitud, altitud, rumbo y velocidad. – trackit

0

Puede encontrar una aproximación de una línea que interseca puntos en el espacio 3d y 2d utilizando un algoritmo Hough Transformation. Sin embargo, estoy familiarizado con sus usos en 2d pero aún así funcionará para espacios en 3D dado que sabes qué tipo de línea estás buscando. Hay una descripción de implementación básica vinculada. Puede buscar pre-mades en Google y aquí hay un enlace a una implementación de 2d C CImg.

El proceso del algoritmo (aproximadamente) ... Primero encuentra la ecuación de una línea que cree que se aproximará mejor a la forma de la línea (en 2d parabólico, logarítmico, exponencial, etc.). Tomas esa fórmula y resuelves uno de los parámetros.

y = ax + b 

convierte

b = y - ax 

A continuación, para cada punto que está tratando de igualar, que plugin de los puntos a los valores de y y x. Con 3 puntos, tendrías 3 funciones separadas de b con respecto a a.

(2, 3) : b = 3 - 2a 
(4, 1) : b = 1 - 4a 
(10, -5): b = -5 - 10a 

A continuación, la teoría es que se encuentran todos los posibles líneas que pasan por cada uno de los puntos, que es un número infinito de cada punto individual Sin embargo, cuando se combina en un espacio acumulador de sólo unos pocos parámetros posibles mejor ajuste. En la práctica, esto se hace eligiendo un espacio de rango para los parámetros (elegí -2 < = a < = 1, 1 < = b < = 6) y empiezo a enchufar valores para los parámetros variantes y a resolver para el otro . Calcula el número de intersecciones de cada función en un acumulador. Los puntos con los valores más altos le dan sus parámetros.

Acumulador después del procesamiento b = 3 - 2a

a: -2 -1 0 1 
b: 1     
    2     
    3    1 
    4     
    5   1   
    6     

acumulador después de procesar b = 1 - 4a

a: -2 -1 0 1 
b: 1    1  
    2     
    3    1 
    4     
    4     
    5   2   
    6     

acumulador después de procesar b = -5 - 10a

a: -2 -1 0 1 
b: 1    1  
    2     
    3    1 
    4     
    5   3   
    6     

El conjunto de parámetros con el valor acumulado más alto es (b a) = (5 -1) y la función que mejor se ajusta a los puntos es y = 5 - x.

Lo mejor de la suerte.

+0

¿Cómo se relacionan las velocidades inicial y final? –

0

Supongo que una aplicación seria de esto usaría un http://en.wikipedia.org/wiki/Kalman_filter. Por cierto, eso probablemente no garantizaría que los puntos informados se crucen tampoco, a menos que haya jugueteado un poco con los parámetros. Esperaría cierto grado de error en cada punto de datos que se le asigna, de modo que cuando cree que el objeto está en el tiempo T no necesariamente estaría donde estaba en el tiempo T. Por supuesto, podría establecer la distribución de error para decir que usted era absolutamente seguro de que sabía dónde estaba en el tiempo T.

Si no utilizo un filtro de Kalman, trataría de convertirlo en un problema de optimización. El trabajo en la granularidad 1s y escribir ecuaciones como x_t'= x_t + (Vx_t + Vx_t ')/2 + e,

Vx_t_reported = Vx_t + f,

Vx_t'= Vx_t + g donde e , f y g representan el ruido.Luego crea una función de penalización como e^2 + f^2 + g^2 + ... o alguna versión ponderada como 1.5e^2 + 3f^2 + 2.6g^2 + ... de acuerdo con tu idea de lo que realmente son los errores y cuán suave quiere que sea la respuesta, y encuentre los valores que hacen que la penalización sea lo más pequeña posible - con mínimos cuadrados si las ecuaciones salen bien.

+0

Estamos utilizando un filtro de Kalman para suavizar los datos de punto recopilados. Esta fórmula es necesaria para completar los puntos faltantes y predecir puntos futuros. – trackit

Cuestiones relacionadas