Fang‘s Method实战：5步搞定TDOA定位中的双曲线方程求解（附Python代码）

Fang's Method实战指南：5步构建高精度TDOA定位系统（附Python完整实现）

在室内定位、无人机导航和智能仓储等领域，到达时间差（TDOA）技术因其无需时钟同步的优势备受青睐。而Fang's Method作为经典的双曲线方程求解算法，能以较低计算复杂度实现米级定位精度。本文将彻底拆解该算法的实现细节，从坐标系变换到模糊解处理，最后给出可直接集成到项目的Python代码。

1. 坐标系简化：降低方程复杂度的关键第一步

任何TDOA算法的起点都是处理那双曲线方程组。Fang's Method的巧妙之处在于通过坐标系变换将问题简化——将第一个锚点（Anchor1）置于坐标原点，第二个锚点（Anchor2）放在x轴上。这种安排不是随意为之，而是经过深思熟虑的设计选择。

实际操作中，我们需要先将所有锚点的原始坐标转换为新坐标系。假设原始坐标系中有三个锚点A、B、C，其坐标分别为$(x_A,y_A)$、$(x_B,y_B)$、$(x_C,y_C)$。转换步骤如下：

1. 计算平移向量：$ \vec{t} = -[x_A, y_A]^T $
2. 计算旋转角度：$ \theta = \text{atan2}(y_B - y_A, x_B - x_A) $
3. 构建旋转矩阵：$ R = \begin{bmatrix} \cos\theta & \sin\theta \ -\sin\theta & \cos\theta \end{bmatrix} $
4. 转换所有锚点坐标：$ \begin{bmatrix} x' \ y' \end{bmatrix} = R \cdot \left( \begin{bmatrix} x \ y \end{bmatrix} + \vec{t} \right) $

经过这样的变换后，在新的坐标系中：

• Anchor1坐标变为(0,0)
• Anchor2坐标变为($d_{12}$,0)，其中$d_{12}$是两锚点间距离
• 其他锚点坐标相应更新