TOA（到达时间）定位的核心是通过测量信号从目标到多个基站的传播时间，将其转换为距离信息，并利用几何关系解算目标位置。
本文给出具体的定位模型建模、位置解算（多种方法）、CRLB计算步骤及公式推导
文章目录
通用模型建立
假设条件：
- 已知 N N N个基站坐标 ( x i , y i ) (x_i, y_i) (xi,yi)（ i = 1 , 2 , . . . , N i=1,2,...,N i=1,2,...,N）。
- 目标位置为未知参数 ( x , y ) (x, y) (x,y)。
- 信号传播速度为光速 c c c（空中场景通常取 c = 3 × 10 8 m/s c=3 \times 10^8 \, \text{m/s} c=3×108m/s）。
距离模型 ：
信号到达时间 t i t_i ti对应的距离为：
r i = c ⋅ t i = ( x − x i ) 2 + ( y − y i ) 2 + n i r_i = c \cdot t_i = \sqrt{(x - x_i)^2 + (y - y_i)^2} + n_i ri=c⋅ti=(x−xi)2+(y−yi)2 +ni
其中 n i n_i ni为测量噪声（通常假设为高斯白噪声），实际距离为 d i = ( x − x i ) 2 + ( y − y i ) 2 d_i = \sqrt{(x - x_i)^2 + (y - y_i)^2} di=(x−xi)2+(y−yi)2 。
非线性方程组构建
每个基站对应一个圆方程：
( x − x i ) 2 + ( y − y i ) 2 = r i 2 ( i = 1 , 2 , . . . , N ) (x - x_i)^2 + (y - y_i)^2 = r_i^2 \quad (i=1,2,...,N) (x−xi)2+(y−yi)2=ri2(i=1,2,...,N)
由于噪声存在，方程需通过优化方法求解目标位置 ( x , y ) (x, y) (x,y)。
线性化处理（最小二乘法）
将非线性方程转化为线性方程组以减少计算复杂度：
-
展开方程 ：
对第 i i i个基站：
x 2 − 2 x i x + x i 2 + y 2 − 2 y i y + y i 2 = r i 2 x^2 - 2x_i x + x_i^2 + y^2 - 2y_i y + y_i^2 = r_i^2 x2−2xix+xi2+y2−2yiy+yi2=ri2 -
消去二次项 ：
选取第1个基站为参考，其余方程减去参考方程：
− 2 ( x i − x 1 ) x − 2 ( y i − y 1 ) y = r i 2 − r 1 2 − ( x i 2 + y i 2 − x 1 2 − y 1 2 ) -2(x_i - x_1)x - 2(y_i - y_1)y = r_i^2 - r_1^2 - (x_i^2 + y_i^2 - x_1^2 - y_1^2) −2(xi−x1)x−2(yi−y1)y=ri2−r12−(xi2+yi2−x12−y12)令 K i = x i 2 + y i 2 K_i = x_i^2 + y_i^2 Ki=xi2+yi2，则方程简化为：
− 2 ( x i − x 1 ) x − 2 ( y i − y 1 ) y = r i 2 − r 1 2 − ( K i − K 1 ) -2(x_i - x_1)x - 2(y_i - y_1)y = r_i^2 - r_1^2 - (K_i - K_1) −2(xi−x1)x−2(yi−y1)y=ri2−r12−(Ki−K1) -
矩阵形式 ：
对(N-1)个方程，构造线性方程组：
A X = b \mathbf{A} \mathbf{X} = \mathbf{b} AX=b其中：
A = [ − 2 ( x 2 − x 1 ) − 2 ( y 2 − y 1 ) ⋮ ⋮ − 2 ( x N − x 1 ) − 2 ( y N − y 1 ) ] , X = [ x y ] , b = [ r 2 2 − r 1 2 − ( K 2 − K 1 ) ⋮ r N 2 − r 1 2 − ( K N − K 1 ) ] \mathbf{A} = \begin{bmatrix} -2(x_2 - x_1) & -2(y_2 - y_1) \\ \vdots & \vdots \\ -2(x_N - x_1) & -2(y_N - y_1) \end{bmatrix}, \quad \mathbf{X} = \begin{bmatrix} x \\ y \end{bmatrix}, \quad \mathbf{b} = \begin{bmatrix} r_2^2 - r_1^2 - (K_2 - K_1) \\ \vdots \\ r_N^2 - r_1^2 - (K_N - K_1) \end{bmatrix} A= −2(x2−x1)⋮−2(xN−x1)−2(y2−y1)⋮−2(yN−y1) ,X=[xy],b= r22−r12−(K2−K1)⋮rN2−r12−(KN−K1) -
最小二乘解 ：
X ^ = ( A T A ) − 1 A T b \mathbf{\hat{X}} = (\mathbf{A}^T \mathbf{A})^{-1} \mathbf{A}^T \mathbf{b} X^=(ATA)−1ATb
最大似然估计（ML）
假设噪声 n i n_i ni服从均值为0、方差为 σ i 2 \sigma_i^2 σi2的高斯分布，构建似然函数：
P ( r ∣ x ) = ∏ i = 1 N 1 2 π σ i e − ( r i − d i ) 2 2 σ i 2 P(\mathbf{r} | \mathbf{x}) = \prod_{i=1}^N \frac{1}{\sqrt{2\pi}\sigma_i} e^{-\frac{(r_i - d_i)^2}{2\sigma_i^2}} P(r∣x)=i=1∏N2π σi1e−2σi2(ri−di)2
取对数似然函数并忽略常数项：
L ( x ) = − ∑ i = 1 N ( r i − d i ) 2 2 σ i 2 L(\mathbf{x}) = -\sum_{i=1}^N \frac{(r_i - d_i)^2}{2\sigma_i^2} L(x)=−i=1∑N2σi2(ri−di)2
优化目标为最小化残差平方和：
x ^ = arg min x ∑ i = 1 N ( r i − d i ) 2 σ i 2 \hat{\mathbf{x}} = \arg \min_{\mathbf{x}} \sum_{i=1}^N \frac{(r_i - d_i)^2}{\sigma_i^2} x^=argxmini=1∑Nσi2(ri−di)2
高斯-牛顿迭代法
当测量噪声较大时，需通过迭代优化求解非线性最小二乘问题：
- 初始化 ：假设初始估计值为 x ( 0 ) = [ x 0 , y 0 ] T \mathbf{x}^{(0)} = [x_0, y_0]^T x(0)=[x0,y0]T。
- 雅可比矩阵 ：定义误差函数 f i ( x ) = r i − ( x − x i ) 2 + ( y − y i ) 2 f_i(\mathbf{x}) = r_i - \sqrt{(x - x_i)^2 + (y - y_i)^2} fi(x)=ri−(x−xi)2+(y−yi)2 ，雅可比矩阵为：
J = [ ∂ f 1 ∂ x ∂ f 1 ∂ y ⋮ ⋮ ∂ f N ∂ x ∂ f N ∂ y ] , ∂ f i ∂ x = x − x i d i , ∂ f i ∂ y = y − y i d i \mathbf{J} = \begin{bmatrix} \frac{\partial f_1}{\partial x} & \frac{\partial f_1}{\partial y} \\ \vdots & \vdots \\ \frac{\partial f_N}{\partial x} & \frac{\partial f_N}{\partial y} \end{bmatrix}, \quad \frac{\partial f_i}{\partial x} = \frac{x - x_i}{d_i}, \quad \frac{\partial f_i}{\partial y} = \frac{y - y_i}{d_i} J= ∂x∂f1⋮∂x∂fN∂y∂f1⋮∂y∂fN ,∂x∂fi=dix−xi,∂y∂fi=diy−yi - 迭代更新 ：
x ( k + 1 ) = x ( k ) − ( J T W J ) − 1 J T W f ( x ( k ) ) \mathbf{x}^{(k+1)} = \mathbf{x}^{(k)} - (\mathbf{J}^T \mathbf{W} \mathbf{J})^{-1} \mathbf{J}^T \mathbf{W} \mathbf{f}(\mathbf{x}^{(k)}) x(k+1)=x(k)−(JTWJ)−1JTWf(x(k))
其中 W \mathbf{W} W为权重矩阵（通常取噪声方差的倒数）。
误差分析（克拉美罗界，CRLB）
定位误差的理论下限可通过Fisher信息矩阵（FIM）计算：
F I M = ∑ i = 1 N 1 σ i 2 [ ( x − x i ) 2 d i 2 ( x − x i ) ( y − y i ) d i 2 ( x − x i ) ( y − y i ) d i 2 ( y − y i ) 2 d i 2 ] \mathbf{FIM} = \sum_{i=1}^N \frac{1}{\sigma_i^2} \begin{bmatrix} \frac{(x - x_i)^2}{d_i^2} & \frac{(x - x_i)(y - y_i)}{d_i^2} \\ \frac{(x - x_i)(y - y_i)}{d_i^2} & \frac{(y - y_i)^2}{d_i^2} \end{bmatrix} FIM=i=1∑Nσi21 di2(x−xi)2di2(x−xi)(y−yi)di2(x−xi)(y−yi)di2(y−yi)2
CRLB为：
Var ( x ^ ) ≥ [ F I M − 1 ] 11 , Var ( y ^ ) ≥ [ F I M − 1 ] 22 \text{Var}(\hat{x}) \geq [\mathbf{FIM}^{-1}]{11}, \quad \text{Var}(\hat{y}) \geq [\mathbf{FIM}^{-1}]{22} Var(x^)≥[FIM−1]11,Var(y^)≥[FIM−1]22
关键公式总结
- 距离模型 ： r i = ( x − x i ) 2 + ( y − y i ) 2 + n i r_i = \sqrt{(x - x_i)^2 + (y - y_i)^2} + n_i ri=(x−xi)2+(y−yi)2 +ni 。
- 最小二乘解 ： X ^ = ( A T A ) − 1 A T b \mathbf{\hat{X}} = (\mathbf{A}^T \mathbf{A})^{-1} \mathbf{A}^T \mathbf{b} X^=(ATA)−1ATb 。
- 高斯-牛顿迭代：通过雅可比矩阵更新估计值。
- CRLB：定位误差的理论下限推导。
应用扩展
- 双向测距（TW-TOA） ：通过往返时间消除时钟同步误差，公式为：
T prop = 1 2 ( T round − T reply ) T_{\text{prop}} = \frac{1}{2}(T_{\text{round}} - T_{\text{reply}}) Tprop=21(Tround−Treply)
适用于无严格时钟同步的场景。参考：TWR定位(Two Way Ranging,双向定位)，原理解析、公式推导、参考文献、解算方法等 - 混合定位 ：结合AOA或RSS提升定位精度。参考：【MATLAB例程】AOA定位、AOA与TOA混合定位，二维环境下的对比，基站（锚点数量）自适应调整，附完整的MATLAB代码
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者