数学 - 3d定位/多点定位

Question

我有一个涉及3D定位的问题 - 有点像GPS.给定一组已知的3d坐标(x,y,z)和它们距未知点的距离d,我想找到未知点.可以有任意数量的参考点,但至少有四个.

因此,例如,点的格式为(x,y,z,d).我可能有:

(1,0,0,1)
(0,2,0,2)
(0,0,3,3)
(0,3,4,5)

这里未知的点是(0,0,0,0).

最好的方法是什么？是否有支持3d多点定位的现有库？(我一直找不到).由于我的数据不太可能具有精确的解决方案(所有4个球体可能都不会有一个完美的交叉点),因此算法需要能够逼近它.

到目前为止,我正在考虑获取三个点的每个子集,基于这三个点对未知数进行三角测量,然后对所有结果求平均值.有一个更好的方法吗？

Answer 1

您可以采用非线性优化方法,通过定义"成本"函数,该函数包含每个观察点的距离误差.

设置未知点(x,y,z),并考虑一组N观察点(xi,yi,zi,di),可以使用以下函数来表征总距离误差:

C(x,y,z) = sum( ((x-xi)^2 + (y-yi)^2 + (z-zi)^2 - di^2)^2 )
           ^^^
           ^^^ for all observation points i = 1 to N

这是集合中所有点的平方距离误差之和.(它实际上是基于平方距离的误差,因此没有平方根!)

当此函数处于最小值时,目标点将(x,y,z)处于最佳位置.如果解决方案给出C(x,y,z) = 0所有观察结果将完全满足.

最小化这种方程式的一个方法是牛顿方法.您必须为迭代提供一个初始起点 - 可能是观察点的平均值(如果它们是圆形(x,y,z))或可能是来自任何三个观测值的初始三角测量值.

编辑:牛顿方法是一种可用于优化的迭代算法.一个简单的版本可以用这些方法:

H(X(k)) * dX = G(X(k));  // solve a system of linear equations for the 
                         // increment dX in the solution vector X
X(k+1) = X(k) - dX;      // update the solution vector by dX

在G(X(k))表示在评估的梯度向量X(k),在这种情况下:

G(X(k)) = [dC/dx
           dC/dy
           dC/dz]

在H(X(k))表示在评估的Hessian矩阵X(k),在这种情况下,对称的3×3矩阵:

H(X(k)) = [d^2C/dx^2 d^2C/dxdy d^2C/dxdz
           d^2C/dydx d^2C/dy^2 d^2C/dydz
           d^2C/dzdx d^2C/dzdy d^2C/dz^2]

您应该能够以分析方式区分成本函数,因此最终得到解析表达式G,H.

另一种方法 - 如果你不喜欢衍生物 - 是G,H使用有限差分在数值上近似.

希望这可以帮助.