基于三球定位的3D空间点坐标求解：Z坐标偏低时计算异常

我用三个球体进行3D空间点定位，已知每个球体的球心坐标和半径，通过球方程方程组求解目标点坐标。当目标点Z坐标高于或等于球心Z坐标时，求解结果正常；但当目标点Z坐标低于球心时，Z坐标计算结果错误。我怀疑问题出在最小二乘法的实现上，相关代码如下：

import numpy as np
from scipy.optimize import least_squares

def GetTag3DCoordinates(anchors, radii):

    # 按ID排序半径和球体
    anchors = sorted(anchors, key=lambda x: x[0])
    radii = sorted(radii, key=lambda x: x[0])

    # 提取半径的第二个值用于计算
    radii = [r[1] for r in radii]

    def residuals(vars, anchors, radii):
        x, y, z = vars
        debug = [np.sqrt((x - ax)**2 + (y - ay)**2 + (z - az)**2) - r 
                 for (_, ax, ay, az), r in zip(anchors, radii)]
        return debug
    
    # 排序半径并选择最小的三个
    sorted_radii = np.argsort(radii)
    selected_indices = sorted_radii[:3]

    selected_anchors = [anchors[i] for i in selected_indices]
    selected_radii = [radii[i] for i in selected_indices]
    
    # 目标点坐标的初始猜测值（锚点中心）
    x0 = np.mean([a[1] for a in selected_anchors])
    y0 = np.mean([a[2] for a in selected_anchors])
    z0 = np.mean([a[3] for a in selected_anchors])

    initial_guess = [x0, y0, z0]

    try:
        # 使用所有锚点通过最小二乘法优化
        result1 = least_squares(residuals, initial_guess, args=(selected_anchors, selected_radii))

        final_result1 = np.array([result1.x[0], result1.x[1], result1.x[2]])

        # 返回结果
        return final_result1
    except Exception as e:
        print(f"发生错误: {e}")
        return np.array([-1, -1, -1])
    
# 函数使用示例
if __name__ == "__main__":
    anchors = [
        (1, 222.8, 125.4, 69.4),
        (2, 222.8, 191.2, 69.4),
        (3, 347.9, 125.6, 69.4),
        (4, 347.9, 191.2, 69.4)
    ]

    radii = [
        (1, 164.4),
        (2, 109.6),
        (3, 221.4),
        (4, 184.4)
    ]

    tag_coords1 = GetTag3DCoordinates(anchors, radii)
    print("目标点坐标1: x={:.2f}, y={:.2f}, z={:.2f}".format(*tag_coords1))

核心问题 #

你的代码使用了带平方根的非线性残差函数，加上初始猜测的Z值与球心Z值完全一致（所有锚点Z均为69.4，初始猜测z0=69.4），当目标点Z低于球心时，最小二乘法极易收敛到局部最优解（即Z≥球心的那个交点），而非正确的Z<球心的解。此外，3D空间中三个球相交通常存在两个有效交点，当前实现仅返回优化收敛到的一个解，无法自动匹配Z坐标的条件。

解决方案1：线性化球方程（最稳定） #

将球方程展开并消去二次项，转化为线性方程组求解，彻底避免非线性优化的局部最优问题，同时可直接计算出两个交点，再根据Z坐标条件选择正确解。

修改后的代码示例：

import numpy as np

def GetTag3DCoordinatesLinear(anchors, radii):
    # 按ID排序并提取数据
    anchors = sorted(anchors, key=lambda x: x[0])
    radii = sorted(radii, key=lambda x: x[0])
    radii = np.array([r[1] for r in radii])
    
    # 选择最小的三个半径对应的锚点
    sorted_radii_idx = np.argsort(radii)
    selected_anchors = np.array([anchors[i][1:] for i in sorted_radii_idx[:3]])
    selected_radii = radii[sorted_radii_idx[:3]]

    # 构建线性方程组：通过球方程相减消去二次项
    A = []
    b = []
    for i in range(1, 3):
        ax0, ay0, az0 = selected_anchors[0]
        ax, ay, az = selected_anchors[i]
        r0_sq = selected_radii[0]**2
        r_sq = selected_radii[i]**2
        
        A.append([2*(ax0 - ax), 2*(ay0 - ay), 2*(az0 - az)])
        b.append(r0_sq - r_sq + ax**2 - ax0**2 + ay**2 - ay0**2 + az**2 - az0**2)
    
    A = np.array(A)
    b = np.array(b)

    try:
        # 推导x、y关于z的表达式：x = px*z + qx，y = py*z + qy
        A_xy = A[:, :2]
        A_z = A[:, 2:3]
        A_xy_inv = np.linalg.inv(A_xy)
        
        p = -A_xy_inv @ A_z
        q = A_xy_inv @ b.reshape(-1,1)
        px, py = p.flatten()
        qx, qy = q.flatten()

        # 代入第一个球的方程，解二次方程求z
        ax0, ay0, az0 = selected_anchors[0]
        r0_sq = selected_radii[0]**2
        
        a = px**2 + py**2 + 1
        b_coeff = 2*(px*(qx - ax0) + py*(qy - ay0) - az0)
        c = (qx - ax0)**2 + (qy - ay0)**2 + az0**2 - r0_sq

        discriminant = b_coeff**2 - 4*a*c
        if discriminant < 0:
            return np.array([-1,-1,-1])
        
        sqrt_d = np.sqrt(discriminant)
        z1 = (-b_coeff + sqrt_d)/(2*a)
        z2 = (-b_coeff - sqrt_d)/(2*a)

        # 计算两个解对应的x、y
        solution1 = np.array([px*z1 + qx, py*z1 + qy, z1])
        solution2 = np.array([px*z2 + qx, py*z2 + qy, z2])

        # 根据Z坐标条件选择解：优先选Z低于球心平均Z的解
        avg_anchor_z = np.mean(selected_anchors[:,2])
        if solution1[2] < avg_anchor_z:
            return solution1
        elif solution2[2] < avg_anchor_z:
            return solution2
        else:
            # 若都不满足，选择残差更小的解
            def calc_residual(sol):
                x,y,z = sol
                return np.sum([(np.sqrt((x-ax)**2 + (y-ay)**2 + (z-az)**2) - r)**2 for (ax,ay,az),r in zip(selected_anchors, selected_radii)])
            return solution1 if calc_residual(solution1) < calc_residual(solution2) else solution2
    except Exception as e:
        print(f"线性求解错误: {e}")
        return np.array([-1,-1,-1])

# 测试
if __name__ == "__main__":
    anchors = [
        (1, 222.8, 125.4, 69.4),
        (2, 222.8, 191.2, 69.4),
        (3, 347.9, 125.6, 69.4),
        (4, 347.9, 191.2, 69.4)
    ]

    radii = [
        (1, 164.4),
        (2, 109.6),
        (3, 221.4),
        (4, 184.4)
    ]

    tag_coords_linear = GetTag3DCoordinatesLinear(anchors, radii)
    print("线性求解目标点坐标: x={:.2f}, y={:.2f}, z={:.2f}".format(*tag_coords_linear))

解决方案2：调整初始猜测（快速修复） #

若想保留原有非线性最小二乘逻辑，只需修改初始猜测的Z值，引导优化收敛到Z更低的解：

修改原代码中初始猜测部分：

# 目标点坐标的初始猜测值（锚点中心）
x0 = np.mean([a[1] for a in selected_anchors])
y0 = np.mean([a[2] for a in selected_anchors])
# 将初始Z设为球心Z减去一个较大值，引导收敛到Z更低的解
z0 = np.mean([a[3] for a in selected_anchors]) - 50  

initial_guess = [x0, y0, z0]

注意：该方法需要提前明确目标点Z的范围（高于/低于球心），否则可能收敛到错误解。

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