假设我有3个点（如果需要，我们可以增加数量）。每一个都有它们与我们所研究的世界中心的X，Y和Z距离。我们还有一个未知点，它的坐标未知。我们只知道3个参考点和我们要搜索的点之间的距离。为了方便起见，这里有一些部分：

P = [10, 10, 10] // Point with the unknown coordinates. I defined them to check whether the formula work or not.
P1 = [0, 0, 0] // 1st reference point
P2 = [20, 20, 0] // 2nd reference point
P3 = [0, 20, 20] // 3rd reference point

我有一个非常简单的勾股定理实现，它获取点的坐标并计算它们的距离。

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和numpy构建一个三边测量方程，以找到隐藏点的坐标：

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%。问题可能出在代码实现的某个地方吗？