TDOA算法MATLAB实现：到达时间差定位

news/2025/12/9 12:31:41/文章来源:https://www.cnblogs.com/she20250124/p/19326000

一、TDOA算法原理概述

到达时间差（Time Difference of Arrival, TDOA） 是一种基于多传感器协同的定位技术，通过测量信号到达不同传感器的时间差，结合传感器位置信息，反推信号源（如声源、无线发射源）的空间坐标。其核心原理是：信号源到两个传感器的距离差为常数时，信号源位于以这两个传感器为焦点的双曲线上，多个双曲线的交点即为信号源位置。

1. 数学模型

设二维平面内有 \(N\)个传感器（\(N≥3\)），坐标为 \(S_i(x_i,y_i)\)\(（i=1,2,...,N）\)，信号源坐标为 \(P(x,y)\)，信号传播速度为 \(c\)（如声速 340m/s）。信号到达传感器 S_i的时间为 t_i，则时差 \(τ_{ij}\)=\(t_i−t_j\)，对应距离差：

\(cτ_{ij}=d_i−d_j\)其中\(d_i=\sqrt{(x−x_i)^2+(y−y_i)^2}\)

以传感器 \(S_1\)为参考，对其他传感器 \(S_i（i=2,3,...,N）\)，可得：

\(d_i−d_1=cτi1⟹d_i=d_1+cτi1\)

平方后消去二次项，整理得线性方程组，通过最小二乘法或Chan算法求解 \(P(x,y)\)。

二、MATLAB实现步骤

1. 场景设置

传感器布局：4个传感器（S1,S2,S3,S4），坐标分别为 (0,0)、(100,0)、(0,100)、(100,100)（单位：米）。
信号源真实位置：Ptrue(50,30)（单位：米）。
传播速度：声速 c=340m/s。
噪声模拟：测量时差加入高斯噪声（标准差 σ=0.001s）。

2. 完整MATLAB代码

% TDOA算法MATLAB实现：二维定位
% 日期：2025年10月clear; clc; close all;%% ==================== 1. 参数设置 ====================
% 传感器坐标 (x,y)，单位：米
S = [0, 0;       % S1100, 0;     % S20, 100;     % S3100, 100];  % S4 (冗余传感器，提高精度)
N = size(S, 1);  % 传感器数量% 信号源真实位置 (x,y)，单位：米
P_true = [50; 30];  % 传播速度 (声速，m/s)
c = 340;  % 噪声参数 (高斯白噪声标准差，s)
sigma = 0.001;  %% ==================== 2. 计算理论时差 ====================
% 计算信号源到各传感器的真实距离
d_true = vecnorm(S' - P_true, 2, 1);  % d_true(i)：信号源到Si的距离% 以S1为参考，计算理论时差 τi1 = (di - d1)/c (i=2,3,4)
tau_true = (d_true(2:end) - d_true(1)) / c;  %% ==================== 3. 模拟带噪声的测量时差 ====================
tau_meas = tau_true + sigma * randn(size(tau_true));  % 加入噪声%% ==================== 4. TDOA定位算法 (最小二乘法) ====================
% 输入：传感器坐标S，测量时差tau_meas (相对于S1)，传播速度c
% 输出：估计的信号源位置P_est (x,y)
P_est = tdoa_localization(S, tau_meas, c);%% ==================== 5. 结果评估与可视化 ====================
% 计算定位误差
error = norm(P_est - P_true);
fprintf('真实位置: (%.2f, %.2f) m\n', P_true(1), P_true(2));
fprintf('估计位置: (%.2f, %.2f) m\n', P_est(1), P_est(2));
fprintf('定位误差: %.4f m\n', error);% 可视化
figure;
plot(S(:,1), S(:,2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r'); hold on;
plot(P_true(1), P_true(2), 'gp', 'MarkerSize', 12, 'MarkerFaceColor', 'g');
plot(P_est(1), P_est(2), 'bx', 'MarkerSize', 12, 'LineWidth', 2);
legend('传感器', '真实位置', '估计位置');
xlabel('X (m)'); ylabel('Y (m)');
title('TDOA定位结果');
grid on; axis equal;%% ==================== TDOA定位核心函数 (最小二乘法) ====================
function P_est = tdoa_localization(S, tau_meas, c)% S: 传感器坐标矩阵 (N×2)，N≥3% tau_meas: 测量时差向量 (N-1)×1，相对于第1个传感器% c: 信号传播速度 (m/s)% P_est: 估计的信号源位置 (2×1)N = size(S, 1);  % 传感器数量if N < 3error('至少需要3个传感器进行二维定位！');end% 提取参考传感器S1和其他传感器Si (i=2..N)S1 = S(1,:)';       % 参考传感器坐标 (2×1)Si = S(2:end,:)';   % 其他传感器坐标 (2×(N-1))tau = tau_meas;     % 时差向量 ((N-1)×1)% 构造线性方程组 Ax = b (伪线性化)A = [];b = [];for i = 1:size(Si, 2)xi = Si(1,i); yi = Si(2,i);Ai = [xi-2*S1(1), yi-2*S1(2), c^2*tau(i)^2 - (xi^2 + yi^2) + (S1(1)^2 + S1(2)^2)];bi = c^2 * tau(i)^2;A = [A; Ai];b = [b; bi];end% 最小二乘求解：x = (A^T A)^-1 A^T bx = pinv(A) * b;  % x = [x; y; d1^2]，其中d1是信号源到S1的距离% 提取估计位置 (x,y)P_est = x(1:2);
end

三、进阶：Chan算法实现（高精度闭式解）

上述最小二乘法为简化版，实际应用中常用Chan算法（适用于高斯噪声，闭式解，精度更高）。以下是Chan算法的MATLAB实现：

function P_est = tdoa_chan(S, tau_meas, c, sigma)% Chan算法：TDOA高精度定位 (二维)% 输入：传感器坐标S(N×2)，时差tau_meas((N-1)×1)，传播速度c，噪声标准差sigma% 输出：估计位置P_est(2×1)N = size(S, 1);if N < 3, error('至少需要3个传感器！'); end% 步骤1：计算距离差向量和传感器间距离d1 = vecnorm(S(2:end,:) - S(1,:), 2, 2);  % 参考传感器S1到其他传感器的距离r = c * tau_meas;  % 距离差向量 (d_i - d_1)% 步骤2：构造矩阵G和hG = [-d1, ones(N-1,1)];h = 0.5 * (r.^2 - d1.^2);% 步骤3：第一次最小二乘求解中间变量z = pinv(G) * h;  % z = [x; y; d1^2] (简化形式)d1_est = sqrt(z(3));  % 估计d1% 步骤4：构造协方差矩阵和加权矩阵Q = (sigma^2) * eye(N-1);  % 噪声协方差W = inv(G' * inv(Q) * G);   % 加权矩阵% 步骤5：第二次最小二乘求解最终位置A = [];b = [];for i = 1:N-1xi = S(i+1,1); yi = S(i+1,2);Ai = [(xi-S(1,1))/d1_est, (yi-S(1,2))/d1_est, 1];bi = 0.5 * ((xi^2 + yi^2 - S(1,1)^2 - S(1,2)^2)/d1_est - r(i)^2/d1_est + d1_est^2);A = [A; Ai];b = [b; bi];endP_est = pinv(A' * W * A) * A' * W * b;
end