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，则所有观察结果都将完全满足。

最小化此类方程的一个方法是Newton's method。你必须为迭代提供一个初始起点 - 可能是观察点的平均值（如果它们是环(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))表示在X(k)评估的Hessian矩阵，在这种情况下是对称的3x3矩阵：

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。

希望这会有所帮助。

Answer 2

如果您可以花时间，迭代解决方案应该很快就能找到正确的解决方案。因此，从站点A中选择正确距离的任何点，然后围绕该点设置距离点的距离，然后调整点，使其与站点的方向相同但距离正确。继续，直到达到所需的精度（或者直到该点在每次迭代中不再移动得足以满足您的精度，根据近似输入数据的可能影响）。

对于分析方法，我想不出比你已经建议的更好的东西。

Answer 3

不需要非线性解决方案程序。您可以轻松地线性化系统。如果你采取成对差异

$（X-X_I）^ 2-（X-x_j）^ 2 +（Y-Y_I）^ 2-（Y-y_j）^ 2 +（Z-z_i）^ 2-（Z-z_j）^ 2 = d_i ^ 2-D_J ^ 2 $

然后一些代数产生线性方程

$（x_i-x_j）x +（y_i-y_j）y +（zi-zj）z = -1 / 2（d_i ^ 2-d_j ^ 2 + ds_i ^ 2-ds_j ^ 2）$，

其中$ ds_i $是$ i ^ {th} $传感器到原点的距离。这些是通过交叉$ i ^ {th} $和$ j ^ {th} $ spheres定义的平面方程式。

对于四个传感器，您将获得一个过度确定的线性系统，其中$ 4选择2 = 6 $方程式。如果$ A $是结果矩阵而$ b $是RHS的相应向量，那么你可以求解正规方程

$ A ^ T A r = A ^ T b $

表示位置向量$ r $。只要您的传感器不是共面的，这将起作用。

Answer 4

看，牛顿或牛顿高斯不会工作。这是因为你在这个函数的导数的空间中的某些点处具有不连续性。相信我，我尝试过牛顿 - 它几乎从来没有收敛过。而不是 - 我尝试了一组不同的算法。尝试google-ing直接搜索算法进行优化（找到函数的最小值或最大值）。它适用于我 - 但有时它会收敛于错误的点 - 可能是因为＆＃34;平面性＆＃34; 4个选定点。在这种情况下 - 你可以选择一个不同的起点 - 你可能会得到正确的答案。我发现的算法示例非常容易实现并且工作得非常好。希望有所帮助。欢呼声。

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

4 个答案: