基于三边测量法求解未知点XYZ坐标的代码误差排查

问题背景 #

已知3个（可按需增加）世界坐标系下的参考点XYZ坐标，以及一个未知点到这三个参考点的距离，尝试通过三边测量求解未知点坐标，但实现后80%的情况存在误差，最大误差可达立方体最大距离的30%。

示例参数 #

P = [10, 10, 10] # 用于验证的未知点真实坐标
P1 = [0, 0, 0]   # 第1个参考点
P2 = [20, 20, 0] # 第2个参考点
P3 = [0, 20, 20] # 第3个参考点

距离计算函数 #

def count_dist(x, y, z, x1, y1, z1):
    D = sqrt((x - x1)**2 + (y - y1)**2 + (z - z1)**2)
    return D

计算未知点到参考点的距离 #

P1 = [ 0,  0,  0]
P2 = [cube_side, cube_side,  0]
P3 = [ 0, cube_side, cube_side]

D1 = count_dist(P[0], P[1], P[2], P1[0], P1[1], P1[2])
D2 = count_dist(P[0], P[1], P[2], P2[0], P2[1], P2[2])
D3 = count_dist(P[0], P[1], P[2], P3[0], P3[1], P3[2])

基于Scipy的三边测量实现 #

def residuals(coords, P1, P2, P3, D1, D2, D3):
    x, y, z = coords
    r1 = np.sqrt((x - P1[0])**2 + (y - P1[1])**2 + (z - P1[2])**2) - D1
    r2 = np.sqrt((x - P2[0])**2 + (y - P2[1])**2 + (z - P2[2])**2) - D2
    r3 = np.sqrt((x - P3[0])**2 + (y - P3[1])**2 + (z - P3[2])**2) - D3
    return np.array([r1, r2, r3])

def trilateration(P1, P2, P3, D1, D2, D3):
    initial_guess = np.array(((P1[0] + P2[0] + P3[0]) / 3,
                              (P1[1] + P2[1] + P3[1]) / 3,
                              (P1[2] + P2[2] + P3[2]) / 3))
    result = least_squares(residuals, 
                           initial_guess, 
                           args=(P1, P2, P3, D1, D2, D3), 
                           method='lm')
    return result.x.tolist()

问题 #

上述实现80%的情况下会出现误差，误差可达预定义立方体最大距离的30%，请问问题出在代码实现上吗？

解答 #

你的代码本身没有语法错误，但有几个关键问题会导致大误差：

1. 参考点几何构型存在缺陷 #

你用的示例参考点P1(0,0,0)、P2(20,20,0)、P3(0,20,20)几乎共面（满足x + z = y的平面），这种近共面的布局会让三边测量的解极度不稳定——哪怕距离值只有微小误差，都会被放大成巨大的坐标偏差，这是最核心的问题。

2. 初始猜测的局限性 #

你用三个参考点的均值作为初始猜测，这个点可能离真实解较远。Levenberg-Marquardt（LM）方法对初始值非常敏感，如果初始点不在解的收敛范围内，很容易陷入局部最优，导致结果偏离真实值。

3. 非线性残差的精度问题 #

直接用带根号的残差函数，其数值导数计算容易引入精度误差。LM方法依赖残差的雅可比矩阵，非线性函数的数值微分在距离值较大时，误差会更明显。

改进方案 #

• 优化参考点布局：选空间分布均匀、不共面的参考点，比如P1(0,0,0)、P2(20,0,0)、P3(0,20,0)、P4(0,0,20)（增加到4个点），这种四面体构型能大幅提升解的稳定性。
• 改用线性化求解：将距离平方展开，消去二次项得到线性方程组，用最小二乘直接求解，避免非线性迭代的收敛问题，示例代码：

  def linear_trilateration(P1, P2, P3, D1, D2, D3):
      # 构造线性方程组 Ax = b
      A = np.array([
          [2*(P2[0]-P1[0]), 2*(P2[1]-P1[1]), 2*(P2[2]-P1[2])],
          [2*(P3[0]-P1[0]), 2*(P3[1]-P1[1]), 2*(P3[2]-P1[2])]
      ])
      b = np.array([
          D1**2 - D2**2 - P1[0]**2 - P1[1]**2 - P1[2]**2 + P2[0]**2 + P2[1]**2 + P2[2]**2,
          D1**2 - D3**2 - P1[0]**2 - P1[1]**2 - P1[2]**2 + P3[0]**2 + P3[1]**2 + P3[2]**2
      ])
      # 3个点求最小二乘解，4个及以上点直接用超定方程组求解
      if A.shape[0] >= 3:
          x, _, _, _ = np.linalg.lstsq(A, b, rcond=None)
      else:
          x = np.linalg.inv(A.T @ A) @ A.T @ b
      return x.tolist()
  

• 优化初始猜测：先用线性方法得到近似解，再作为LM方法的初始值，能大幅提升收敛精度。
• 增加参考点数量：用4个或更多参考点，通过超定方程组的最小二乘解，进一步降低距离误差带来的影响。

内容的提问来源于stack exchange，提问作者smbrine