2

我正在尝试执行三边测量。Funtion 为每个坐标获取三个 3d 坐标和距基站的距离。它必须在 3d 空间三边测量中返回该点的位置。

def trilateration(P1, P2, P3, r1, r2, r3):

  p1 = np.array([0, 0, 0])
  p2 = np.array([P2[0] - P1[0], P2[1] - P1[1], P2[2] - P1[2]])
  p3 = np.array([P3[0] - P1[0], P3[1] - P1[1], P3[2] - P1[2]])
  v1 = p2 - p1
  v2 = p3 - p1

  Xn = (v1)/np.linalg.norm(v1)

  tmp = np.cross(v1, v2)

  Zn = (tmp)/np.linalg.norm(tmp)

  Yn = np.cross(Xn, Zn)

  i = np.dot(Xn, v2)
  d = np.dot(Xn, v1)
  j = np.dot(Yn, v2)

  X = ((r1**2)-(r2**2)+(d**2))/(2*d)
  Y = (((r1**2)-(r3**2)+(i**2)+(j**2))/(2*j))-((i/j)*(X))
  Z1 = np.sqrt(r1**2-X**2-Y**2)
  Z2 = np.sqrt(r1**2-X**2-Y**2)*(-1)

  K1 = P1 + X*Xn + Y * Yn + Z1 * Zn
  K2 = p1 + X * Xn + Y * Yn - Z2 * Zn
  return K1

我有一个测试示例。有了这些坐标和距离 P1=(2,2,0), P2=(3,3,0), P3=(1,4,0​​) r1=1, r2=1, r3=1.4142，它应该返回 P =（2,3,0）。

但它是回报 [3.33253331 1.66746669 1.33373281]

4

2 回答 2

0

我没有足够的声誉来发表评论，所以我必须写一个新的答案。

您的代码仍然有一些错误。经过多次测试，我发现K2的计算有一些错别字。

完美的功能应该是

def trilateration(P1, P2, P3, r1, r2, r3):

  p1 = np.array([0, 0, 0])
  p2 = np.array([P2[0] - P1[0], P2[1] - P1[1], P2[2] - P1[2]])
  p3 = np.array([P3[0] - P1[0], P3[1] - P1[1], P3[2] - P1[2]])
  v1 = p2 - p1
  v2 = p3 - p1

  Xn = (v1)/np.linalg.norm(v1)

  tmp = np.cross(v1, v2)

  Zn = (tmp)/np.linalg.norm(tmp)

  Yn = np.cross(Xn, Zn)

  i = np.dot(Xn, v2)
  d = np.dot(Xn, v1)
  j = np.dot(Yn, v2)

  X = ((r1**2)-(r2**2)+(d**2))/(2*d)
  Y = (((r1**2)-(r3**2)+(i**2)+(j**2))/(2*j))-((i/j)*(X))
  Z1 = np.sqrt(max(0, r1**2-X**2-Y**2))
  Z2 = -Z2

  K1 = P1 + X * Xn + Y * Yn + Z1 * Zn
  K2 = P1 + X * Xn + Y * Yn + Z2 * Zn
  return K1,K2

两个返回数组是两个可能的位置。

此代码非常有用，因为此答案中的代码是 Python 形式的https://en.wikipedia.org/wiki/True-range_multilateration#Three_Cartesian_dimensions,_three_measured_slant_ranges ，它是 3D 坐标中最简单的三边测量代码。

于 2021-10-15T07:37:58.893 回答
0

问题来自于sqrt由于数值不精确而给出的表达式略为负数。这将解决它：

Z1 = np.sqrt(max(0, r1**2-X**2-Y**2))
Z2 = -Z1

更改这些行会给我正确的结果：[1.99999361 3.00000639 0.]

注意：如果该点与其他 3 个点位于同一平面上，则 Z 值为 0，否则有两种解。此外，如@meowgoesthedog 所述，为r1、r2和提供精确值非常重要。r3但是，即使使用精确值，您也始终需要注意浮点不精确性并安全使用sqrt.

于 2019-05-09T12:36:26.037 回答