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

一、TDOA算法原理概述

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

1. 数学模型

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

cτij=di−djcτ_{ij}=d_i−d_jcτij=di−dj其中di=(x−xi)2+(y−yi)2d_i=\sqrt{(x−x_i)^2+(y−y_i)^2}di=(x−xi)2+(y−yi)2

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

di−d1=cτi1⟹di=d1+cτi1d_i−d_1=cτi1⟹d_i=d_1+cτi1di−d1=cτi1⟹di=d1+cτi1

平方后消去二次项，整理得线性方程组，通过最小二乘法或Chan算法求解 P(x,y)P(x,y)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;       % S1
     100, 0;     % S2
     0, 100;     % S3
     100, 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 < 3
        error('至少需要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 b
    x = 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和h
    G = [-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-1
        xi = 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];
    end
    P_est = pinv(A' * W * A) * A' * W * b;
end

四、关键问题与优化

1. 传感器布局优化

• 避免共线：传感器尽量不共线，否则双曲线退化，定位精度下降。
• 对称布局：如正方形或正三角形布局，可提高定位稳定性。

2. 噪声处理

• 滤波预处理：对测量时差进行卡尔曼滤波或中值滤波，降低噪声影响。
• 冗余传感器：增加传感器数量（如4个以上），通过多余观测提高精度（代码中已加入S4作为冗余）。

3. 三维扩展

若需三维定位，传感器坐标为 (xi,yi,zi)(x_i,y_i,z_i)(xi,yi,zi)，信号源坐标 (x,y,z)(x,y,z)(x,y,z)，时差方程扩展为：

(x−xi)2+(y−yi)2+(z−zi)2−(x−x1)2+(y−y1)2+(z−z1)2=cτi1\sqrt{(x−x_i)^2+(y−y_i)^2+(z−z_i)^2}−\sqrt{(x−x_1)^2+(y−y_1)^2+(z−z_1)^2}=cτi1(x−xi)2+(y−yi)2+(z−zi)2 −(x−x1)2+(y−y1)2+(z−z1)2 =cτi1

定位算法类似，需至少4个传感器。

参考代码 TDOA算法的matlab实现 www.youwenfan.com/contentcsm/82961.html

五、应用场景

1. 声源定位：麦克风阵列定位说话人、机器异响源。
2. 无线定位：UWB（超宽带）室内定位、基站定位手机。
3. 雷达与声呐：目标跟踪、水下探测。
4. 地震监测：震源定位。

六、总结

TDOA算法通过多传感器时差测量实现定位，MATLAB实现需重点关注：

• 模型建立：距离差与时差的转换关系；
• 噪声模拟：通过高斯噪声贴近实际场景；
• 算法选择：最小二乘法（简单）或Chan算法（高精度）。

通过调整传感器布局、优化算法参数（如噪声标准差），可进一步提升定位精度和鲁棒性。代码中提供的核心函数可直接复用，只需修改传感器坐标、信号源位置和噪声参数即可适配不同场景。