三维空间中的曲线拟合点

试图找到有助于我们通过一系列点绘制3D线的函数。

对于我们知道的每个点:日期和时间,纬度,经度,海拔高度,速度和航向。 数据可能每10秒记录一次,我们希望能够猜测两者之间的点数,并将粒度增加到1秒。 从而在3D空间中创建虚拟飞行路径。

我发现了许多曲线拟合算法,这些算法将通过一系列点近似于一条直线,但它们不能保证这些点相交。 他们也没有考虑速度和航向来确定物体到达下一个点所采取的最可能路径。

从物理学的角度来看:

您必须假设中间点的加速度才能获得插值。

如果您的物理系统相对良好(如汽车或飞机),而不是例如弹跳球,您可以继续假设加速度随点之间的时间线性变化。

恒定变化加速运动的矢量方程是:

x''[t] = at + b 

除了t之外的所有幅度都是向量。

对于你已经知道的每个段v(t = t0)x(t = t0)tfinal和x(tfinal)v(tfinal)

通过求解微分方程,您得到:

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

并强加了您获得的位置和速度的初始和最终约束:

方程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))}} 

因此,将方程2的值插入到方程1中,您可以根据初始和最终位置和速度获得点的时间插值。

HTH!

编辑

一些具有两个维度的突然速度变化的例子(在3D中完全相同)。 如果初始和最终速度相似,您将获得“更直”的路径。

假设:

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

如果

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

替代文字

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

替代文字

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

替代文字

这是一个动画,您可以看到速度从V0 = {0,1}变为Vf = {1,5}: 替代文字

在这里,您可以看到3D中的加速体,其间位置相等:

替代文字

编辑

一个完整的问题:

为方便起见,我将使用笛卡尔坐标。 如果你想从lat / log / alt转换为Cartesian,你可以:

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

phi是经度,theta是纬度,rho是你的高度加上地球的半径。

所以假设我们开始我们的细分市场:

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

结束于

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

我清楚地对坐标原点进行了更改,以便在起点处设置原点。 这只是为了得到漂亮的圆形数字……

所以我们在a和b的公式中替换这些数字并得到:

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

有了那些我们去eq 1,对象的位置由下式给出:

 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)} 

就是这样。 你得到1到10秒的位置,用上面的等式中的valus代替t。
动画运行:

替代文字

编辑2

如果你不想弄乱垂直加速度(也许是因为你的“速度计”没有读取它),你可以给z轴分配一个恒定的速度(考虑到它平行于z轴有一个非常小的错误) Rho轴),等于(Zfinal-Zinit)/(Tf-T0),然后解决飞机忘记高度的问题。

你问的是一般的插值问题。 我的猜测是你的实际问题不是由于使用了曲线拟合算法,而是你将它应用于系统记录的所有离散值而不是相关的值集。

让我们分解你的问题。 您目前正在绘制球形贴图3D空间中的一个点,调整线性和弯曲路径。 如果我们对具有六个自由度( 滚动,俯仰和偏航 )的物体执行的操作进行离散化,那么您唯一感兴趣的操作是线性路径和弯曲路径,可以解决任何方向的俯仰和偏航。 考虑到基本物理学,也可以考虑加速和减速。

处理球形映射很容易。 只需将您的点相对于它们在平面上的位置展开,调整纬度,经度和海拔高度。 这应该允许您展平沿弯曲路径存在的数据,尽管这对于您的问题的解决方案可能不是必需的(见下文)。

线性插值很容易。 给定一个任意数量的点,这些点在时间上适合由系统确定的n个误差内的线,*构造线并计算每个点之间的时间距离。 从这里开始,尝试将时间点拟合为两种情况之一:恒定速度或恒定加速度。

曲线插值有点困难,但仍然合理。 对于俯仰,偏航或组合俯仰+偏航的情况,构建一个平面,其中包含任意数量的时间点,在m误差范围内,用于系统的曲线读数。*从这些数据中,构建平面曲线并再次考虑沿曲线的恒定速度或加速度 。

通过尝试预测飞行中飞机的预期操作作为决策树或神经网络相对于飞行路径的一部分,您可以做得更好。 我将把它作为读者的练习。

祝您好好设计系统。

*考虑到问题的描述,预计两个错误读数都来自GPS数据。 计算和调整这些数据中的错误是一个单独的有趣问题。

您需要的(而不是物理建模)是通过数据拟合样条曲线。 我使用了一本数字收件簿( http://www.nrbook.com/a有免费的C和FORTRAN算法。请查看F77第3.3节以获得所需的数学计算)。 如果你想要简单,那么只需通过点拟合线,但这根本不会产生平滑的飞行路径。 时间将是你的x值,每个参数loged将拥有它自己的公共样条参数。

由于我们喜欢这个问题的长贴子,因此这里是完整的代码:

//驱动程序

 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 } 

//三次样条定义

 using System.Linq; ///  /// Cubic spline interpolation for tabular data ///  ///  /// 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. ///  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 } 

您可以使用霍夫变换算法找到与3d和2d空间中的点相交的线的近似值。 我只熟悉它在2d中的用途但是它仍然适用于3d空间,因为你知道你正在寻找什么样的线。 有一个基本的实现描述链接。 你可以谷歌预制,这里是一个2D C实施CImg的链接。

算法过程(粗略地)……首先,您会发现一条线的方程,您认为该方程最接近线的形状(以2d抛物线,对数,指数等)。 您采用该公式并求解其中一个参数。

 y = ax + b 

 b = y - ax 

接下来,对于您尝试匹配的每个点,您将点插入y和x值。 有3分,你将有3个单独的b函数。

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

接下来,理论是您找到通过每个点的所有可能的线,对于每个单独的点无限多,但是当在累加器空间中组合时,只有几个可能的参数最合适。 在实践中,这是通过为参数选择范围空间(我选择-2 <= a <= 1,1 <= b <= 6)并开始插入变量参数的值并解决其他参数来完成的。 。 计算累加器中每个函数的交点数。 具有最高值的点为您提供参数。

处理后的累加器b = 3 - 2a

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

处理后的累加器b = 1 - 4a

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

处理后的累加器b = -5 - 10a

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

具有最高累积值的参数集是(ba) = (5 -1)并且最适合给定点的函数是y = 5 - x

祝你好运。

我的猜测是,严重的应用程序将使用http://en.wikipedia.org/wiki/Kalman_filter 。 顺便说一句,这可能不能保证报告的点也相交,除非你稍微摆弄参数。 它会在每个数据点给出一定程度的错误,因此它认为对象在时间T不一定是在时间T的位置。当然,你可以设置错误分布来说明你是绝对肯定你知道它在时间T的位置。

如果不使用卡尔曼滤波器,我会尝试将其转化为优化问题。 以1s粒度工作并写下类似x_t’= x_t +(Vx_t + Vx_t’)/ 2 + e的方程式,

Vx_t_reported = Vx_t + f,

Vx_t’= Vx_t + g其中e,f和g表示噪声。 然后创建一个惩罚函数,如e ^ 2 + f ^ 2 + g ^ 2 + …或某些加权版本,如1.5e ^ 2 + 3f ^ 2 + 2.6g ^ 2 + ……根据你的想法错误究竟是什么以及你得到答案的顺利程度,并找到使惩罚函数尽可能小的值 – 如果方程式结果很好,则使用最小二乘法。