如何提升Python 3D TDoA多点定位算法嵌套循环的执行效率

1. 替换SymPy符号运算为数值运算 #

循环中每次调用subs做符号替换是最大的性能浪费，SymPy的符号运算本身为公式推导设计，不适合高频循环调用。你可以用SymPy自带的lambdify方法，把得到的x关于y、z的符号表达式直接编译成数值函数：

# 在sp.solve得到soln之后加这一行
x_func = sp.lambdify((y, z), soln[x], 'numpy')

后续计算x值时直接调用px = x_func(i, j)即可，比subs快至少100倍。

2. 向量化计算完全替换嵌套循环 #

直接用NumPy生成y和z的网格，一次性计算所有点的x值和误差，完全去掉Python层面的循环，速度还能再提升一个量级：

import numpy as np

# 生成y、z网格
y_grid, z_grid = np.meshgrid(np.arange(-100, 100), np.arange(-100, 100))
x_grid = x_func(y_grid, z_grid)

# 改写err函数为向量化版本
def err_vec(x, y, z, data, roc, c):
    arrival0 = np.sqrt((x - roc[0][0])**2 + (y - roc[0][1])**2 + (z - roc[0][2])**2) / c
    arrival1 = np.sqrt((x - roc[1][0])**2 + (y - roc[1][1])**2 + (z - roc[1][2])**2) / c
    return (arrival0 - data[0])**2 + (arrival1 - data[1])**2 + (arrival0 - data[2])**2

# 一次性计算所有点的误差
err_grid = err_vec(x_grid, y_grid, z_grid, data, self.roc, c)
# 找到误差最小的点的位置
min_idx = np.unravel_index(np.argmin(err_grid), err_grid.shape)
fx = x_grid[min_idx]
fy = y_grid[min_idx]
fz = z_grid[min_idx]
err1 = err_grid[min_idx]

这种向量化实现会把计算放到NumPy底层的C语言层面执行，原来需要跑几万次的Python循环直接变成一次批量计算，速度提升非常明显。

3. 替换暴力枚举为数值优化（可选） #

如果你的误差函数是平滑的，完全不需要遍历所有点，直接用scipy.optimize.minimize在平面上找最小值即可，通常只需要几十次迭代就能得到比网格遍历精度更高的结果，速度还能再提升10倍以上：

from scipy.optimize import minimize

# 定义优化目标：输入是[y,z]，返回对应的误差
def objective(yz):
    y_val, z_val = yz
    x_val = x_func(y_val, z_val)
    return self.err(x_val, y_val, z_val, data)

# 初始点可以选网格中心
res = minimize(objective, x0=[0, 0], bounds=[(-100,100), (-100,100)])
fy, fz = res.x
fx = x_func(fy, fz)
err1 = res.fun

其他小优化 #

• 你现有err函数里arrival_at_i和arrival_at_k是完全相同的计算，直接删掉arrival_at_k的计算复用arrival_at_i即可，减少一次开平方运算。
• 你贴的代码里有两处语法错误：一行是px = self.err(soln[x].subs([(y, i), (z, j)])少了右括号，还有最后print的error变量没有定义，改完再运行。

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