基于C#的3D Trilateration实现：如何用GPS算法由三点及距离计算目标点？

这是GPS定位的核心逻辑——和靠角度计算的三角测量不同，它通过已知点到目标点的距离来定位，下面分场景讲具体实现：

1. 平面坐标系下的基础计算 #

先从简单的平面场景入手，理解核心原理：

假设你有三个已知点：

• A: (x₁, y₁)，到目标点P的距离d₁
• B: (x₂, y₂)，到目标点P的距离d₂
• C: (x₃, y₃)，到目标点P的距离d₃

每个已知点与目标点的关系可以用圆的方程表示：

(x - x₁)² + (y - y₁)² = d₁²  ---(1)
(x - x₂)² + (y - y₂)² = d₂²  ---(2)
(x - x₃)² + (y - y₃)² = d₃²  ---(3)

消元得到线性方程组 #

把方程(2)-(1)、(3)-(1)，展开后消去x²和y²项，得到两个线性方程：

2*(x₁ - x₂)*x + 2*(y₁ - y₂)*y = d₂² - d₁² + x₁² - x₂² + y₁² - y₂²  ---(4)
2*(x₁ - x₃)*x + 2*(y₁ - y₃)*y = d₃² - d₁² + x₁² - x₃² + y₁² - y₃²  ---(5)

将其写成矩阵形式 M * [x; y] = K，其中：

M = [[2*(x₁-x₂), 2*(y₁-y₂)],
     [2*(x₁-x₃), 2*(y₁-y₃)]]
K = [d₂² - d₁² + x₁² - x₂² + y₁² - y₂²,
     d₃² - d₁² + x₁² - x₃² + y₁² - y₃²]

直接解这个线性方程组就能得到目标点坐标(x,y)。用Python实现的话，可以借助numpy的线性代数工具：

平面场景代码示例 #

import numpy as np

def trilateration_2d(points, distances):
    # points: 三个已知点坐标，格式[[x1,y1], [x2,y2], [x3,y3]]
    # distances: 对应到目标点的距离，[d1, d2, d3]
    x1, y1 = points[0]
    x2, y2 = points[1]
    x3, y3 = points[2]
    d1, d2, d3 = distances

    # 构建矩阵与向量
    M = np.array([
        [2*(x1 - x2), 2*(y1 - y2)],
        [2*(x1 - x3), 2*(y1 - y3)]
    ])
    K = np.array([
        d2**2 - d1**2 + x1**2 - x2**2 + y1**2 - y2**2,
        d3**2 - d1**2 + x1**2 - x3**2 + y1**2 - y3**2
    ])

    # 求解方程组
    x, y = np.linalg.solve(M, K)
    return (round(x, 4), round(y, 4))

# 测试用例：目标点(5,5)
points = [[0,0], [0,10], [10,0]]
distances = [5*np.sqrt(2), 5, 5]
print(trilateration_2d(points, distances))  # 输出(5.0, 5.0)

2. 实际GPS场景：球面坐标处理 #

GPS用的是经纬度（球面坐标），不能直接套用平面公式，需要先转换到地心地固坐标系（ECEF）——这是原点在地球质心的笛卡尔坐标系，X轴指向本初子午线与赤道交点，Y轴指向东经90°赤道交点，Z轴指向北极。

核心步骤： #

1. 经纬度转ECEF：将三个已知GPS点的(lat, lon, alt)（纬度、经度、海拔）转换为ECEF坐标(X,Y,Z)，使用WGS84椭球参数：

import math

def latlon_to_ecef(lat, lon, alt):
    a = 6378137.0  # 地球半长轴
    e_sq = 0.00669437999014  # 第一偏心率平方
    lat_rad = math.radians(lat)
    lon_rad = math.radians(lon)

    N = a / math.sqrt(1 - e_sq * math.sin(lat_rad)**2)
    X = (N + alt) * math.cos(lat_rad) * math.cos(lon_rad)
    Y = (N + alt) * math.cos(lat_rad) * math.sin(lon_rad)
    Z = (N*(1 - e_sq) + alt) * math.sin(lat_rad)
    return (X, Y, Z)

2. ECEF下的三边测量：和平面逻辑类似，但要处理三维场景。列出三个球面方程，两两相减得到线性方程组，用最小二乘法求解（实际测量有误差，三个球面不会严格共点）。

3. ECEF转回经纬度：将求解得到的ECEF坐标(X,Y,Z)转回到(lat, lon, alt)：

def ecef_to_latlon(X, Y, Z):
    a = 6378137.0
    e_sq = 0.00669437999014
    p = math.sqrt(X**2 + Y**2)
    lon_rad = math.atan2(Y, X)
    lat_rad = math.atan2(Z, p*(1 - e_sq))
    
    # 迭代修正纬度（提高精度）
    for _ in range(5):
        N = a / math.sqrt(1 - e_sq * math.sin(lat_rad)**2)
        lat_rad = math.atan2(Z + N*e_sq*math.sin(lat_rad), p)
    
    lat = math.degrees(lat_rad)
    lon = math.degrees(lon_rad)
    N = a / math.sqrt(1 - e_sq * math.sin(lat_rad)**2)
    alt = p / math.cos(lat_rad) - N
    return (lat, lon, alt)

3. 误差处理：最小二乘法 #

实际场景中，距离测量会有噪声，三个球面不会严格交于一点，这时候用最小二乘法拟合最优解。对于三维ECEF场景，将多个线性方程组成超定方程组，用np.linalg.lstsq求解即可。

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