用Python求解多解非线性方程组实现3D空间信号发射源定位
考虑如下场景：实验包含三台接收机i,j,k，其在3D空间中的已知坐标为(xi,yi,zi)、(xj,yj,zj)、(xk,yk,zk)。坐标未知的静止发射机(x,y,z)发射传播速度为v的信号，信号到达三台接收机的时间分别记录为ti,tj,tk，信号发射时间t未知，要求仅通过接收机坐标和信号到达时间求解发射机位置。
一般情况下，N维空间中求解该类发射机坐标需要N+1台接收机，因此本场景无法得到唯一解，但可通过数值方法得到有限数量的解。该问题可建模为如下方程组：
Sqrt[(x-xi)^2 + (y-yi)^2 + (z-zi)^2] + v*(tj-ti) = Sqrt[(x-xj)^2 + (y-yj)^2 + (z-zj)^2] Sqrt[(x-xj)^2 + (y-yj)^2 + (z-zj)^2] + v*(tk-tj) = Sqrt[(x-xk)^2 + (y-yk)^2 + (z-zk)^2] Sqrt[(x-xi)^2 + (y-yi)^2 + (z-zi)^2] + v*(tk-ti) = Sqrt[(x-xk)^2 + (y-yk)^2 + (z-zk)^2]
每个方程对应一个双曲面，理想条件下三个双曲面恰好交于两点：一个为真实发射点，另一个为该点关于三台接收机所在平面的镜像解，本次求解目标即为得到这两个点。
计划用Python实现求解，对NumPy、SciPy熟悉度较低，有较多SymPy使用经验因此优先尝试该工具：
SymPy的符号求解类函数如solve()，即便在模拟理想场景（随机生成发射点，计算到达时间后输入算法）下也无法得到解；- 尝试调用
SymPy的数值求解器nsolve()，代码如下：
f = sym.Eq(sym.sqrt((x - x_i)**2 + (y - y_i)**2 + (z - z_i)**2) - sym.sqrt((x - x_j)**2 + (y - y_j)**2 + (z - z_j)**2), D_ij) g = sym.Eq(sym.sqrt((x - x_i)**2 + (y - y_i)**2 + (z - z_i)**2) - sym.sqrt((x - x_k)**2 + (y - y_k)**2 + (z - z_k)**2), D_ik) h = sym.Eq(sym.sqrt((x - x_j)**2 + (y - y_j)**2 + (z - z_j)**2) - sym.sqrt((x - x_k)**2 + (y - y_k)**2 + (z - z_k)**2), D_jk) print("Soln. ", sym.nsolve((f,g,h),(x,y,z), (1,1,1)))
运行时报错ZeroDivisionError: Matrix is numerically singular。据了解nsolve()依赖矩阵运算，奇异矩阵无法求逆导致求解失败，而本问题恰好存在两个解，是触发该错误的原因。查阅NumPy和SciPy文档后发现其求解器与SymPy的实现逻辑基本一致。
模拟信号的简易测试代码如下（已修正原代码笔误）：
from random import randrange import math # SIMPLE CODE FOR SIMULATING A SIGNAL # Set range N=10 P=100 # Pick nodes to be at random locations x_1 = randrange(N); y_1 = randrange(N); z_1 = randrange(N) x_2 = randrange(N); y_2 = randrange(N); z_2 = randrange(N) x_3 = randrange(N); y_3 = randrange(N); z_3 = randrange(N) # Pick source to be at random location x = randrange(P); y = randrange(P); z = randrange(P) # Set velocity c = 299792 # km/ns # Generate simulated source t_1 = math.sqrt( (x - x_1)**2 + (y - y_1)**2 + (z - z_1)**2 ) / c t_2 = math.sqrt( (x - x_2)**2 + (y - y_2)**2 + (z - z_2)**2 ) / c t_3 = math.sqrt( (x - x_3)**2 + (y - y_3)**2 + (z - z_3)**2 ) / c # Normalize times to remove information about 'true' emission time earliest = min(t_1, t_2, t_3) t_1 = t_1 - earliest; t_2 = t_2 - earliest; t_3 = t_3 - earliest
你遇到的奇异矩阵错误本质是三个方程不独立，第三个方程是前两个的线性组合，雅可比矩阵天然不满秩，和多解特性没有直接关系，优先做问题改造即可解决：
适配SymPy的改造方案
- 第一步：删除冗余的第三个方程，仅保留两个独立的双曲面方程，此时方程组的解空间为空间曲线，正好对应两个交点的连线。
- 第二步：新增第三个约束，先计算三个接收机所在的平面方程
ax + by + cz + d = 0，将点到平面的距离分别取正负两种预期值作为约束，拆分为两组独立的三方程求解任务，正好对应两个解。 - 第三步：调整
nsolve的初始值，两组方程分别传入接收机平面两侧的初始点，比如(接收机平均坐标x, 接收机平均坐标y, 接收机平均坐标z + 10)和(接收机平均坐标x, 接收机平均坐标y, 接收机平均坐标z -10)，即可分别得到两个解，不会再触发奇异矩阵错误。
更简单的SciPy实现方案
如果可以接受少量SciPy代码，scipy.optimize.root的鲁棒性更高，不需要复杂的符号操作：
import numpy as np from scipy.optimize import root # 接收机坐标、传播速度、时间差提前赋值 r = np.array([[x_1,y_1,z_1], [x_2,y_2,z_2], [x_3,y_3,z_3]]) v = 299792 d12 = v*(t_2 - t_1) d13 = v*(t_3 - t_1) def residual(pos): x,y,z = pos r1 = np.linalg.norm([x-r[0,0], y-r[0,1], z-r[0,2]]) r2 = np.linalg.norm([x-r[1,0], y-r[1,1], z-r[1,2]]) r3 = np.linalg.norm([x-r[2,0], y-r[2,1], z-r[2,2]]) return [r2 - r1 - d12, r3 - r1 - d13, 0] # 传入两个不同初始值得到两个解 mean_pos = np.mean(r, axis=0) sol1 = root(residual, x0 = mean_pos + np.array([0,0,10])) sol2 = root(residual, x0 = mean_pos + np.array([0,0,-10]))
最优方案：直接用闭式解
该TDOA场景存在闭式解，不需要数值迭代：将两个双曲面方程平方后展开消去二次项，得到两个线性方程，结合接收机平面约束直接解线性方程组，即可快速得到两个解，完全避免收敛问题。
内容的提问来源于stack exchange，提问作者10GeV
