【MATLAB例程】水下AUV自主导航定位例程，定位使用TDOA（到达时间差），适用于三维环境，附代码下载链接

代码实现基于到达时间差（TDOA）的水下AUV（自动水下航行器）导航仿真系统，旨在通过模拟AUV在三维水下环境中的运动，利用水声信标来估算AUV的位置。仿真系统包括四个声学信标，并通过TDOA定位技术迭代求解AUV的实时位置。

程序简介

代码实现了基于到达时间差（TDOA）的水下AUV（自动水下航行器）导航仿真系统，旨在通过模拟AUV在三维水下环境中的运动，利用水声信标来估算AUV的位置。仿真系统包括四个声学信标，并通过TDOA定位技术迭代求解AUV的实时位置。

核心功能

1. 水声TDOA定位：通过测量AUV与多个声学信标之间的到达时间差，使用简化的TDOA定位算法来估算AUV的位置。
2. 仿真控制：根据估算的位置计算AUV的运动方向，并控制其以合适的速度向目标点前进，直到到达目标。
3. 误差统计与结果展示：记录每一步的定位误差，绘制AUV的真实轨迹与估计轨迹的三维图，同时展示定位误差随时间的变化。

代码流程

1. 参数设置：

   • 设置水声传播速度、测量噪声标准差等仿真参数。
   • 配置四个声学信标的三维坐标。
   • 配置AUV的初始位置、目标位置、最大速度和仿真步长等。

2. 初始化：

   • 初始化AUV的当前位置、轨迹记录、误差数组等。

3. 仿真循环：

   • TDOA测量：计算AUV与四个声学信标的距离，并转换为到达时间，加入噪声。
   • 位置估计：使用简化的TDOA定位算法，根据TDOA值估算AUV的位置。
   • 导航控制：根据估算位置与目标位置的差异，计算并更新AUV的运动速度和方向，逐步推进AUV到达目标位置。
   • 误差记录：记录每一步的真实位置与估算位置之间的误差，并输出每一步的仿真信息。

4. 结果展示：

   • 绘制AUV的真实轨迹与估算轨迹。
   • 绘制AUV定位误差的变化图。

5. 统计结果：

   • 输出仿真结束后的统计信息，包括总步数、平均定位误差、最大定位误差等。详见定位结果输出的示例

运行结果

导航轨迹（含真值与估计值轨迹的对比）：

定位误差变化曲线：

命令行输出的定位结果：

MATLAB源代码

以下代码，粘贴到MATLAB空脚本中即可直接运行：

%% AUV水声TDOA导航系统
% 基于到达时间差(TDOA)的AUV导航仿真，三维空间、 四个锚点的水下背景，两部迭代求TDOA
% author:
% 2025-08-03/Ver1
clear;clc;close all;
rng(0);

%% 1. 参数设置
sound_speed = 1500;               % 声速 (m/s)
noise_std = 1e-9;                % 测量噪声标准差 (s) - 修正：改为合理值

% 4个声学信标位置 [x, y, z] - 修正：调整为更合理的几何配置
beacons = [
    0,   0,   -5;       % 信标1
    100, 0,   0;       % 信标2  
    50,  100, 0;       % 信标3
    0,   50,  -10       % 信标4
];

% AUV参数
start_pos = [30, 30, -20];        % 起始位置
target_pos = [70, 70, -20];       % 目标位置
max_speed = 1;                    % 最大速度 (m/s)
dt = 1;                          % 时间步长 (s)
max_steps = 100;                  % 最大步数

%% 2. 初始化
auv_pos = start_pos;              % 当前位置
trajectory = zeros(max_steps, 3); % 轨迹记录
traj_est = zeros(max_steps, 3); % 轨迹记录
errors = zeros(max_steps, 1);     % 定位误差

%% 3. 主仿真循环
fprintf('开始AUV导航仿真...\n');

for step = 1:max_steps
    %% 3.1 TDOA测量
    % 计算各信标到AUV的距离和时间
    distances = zeros(4, 1);
    times = zeros(4, 1);
    
    for i = 1:4
        distances(i) = norm(auv_pos - beacons(i, :));
        times(i) = distances(i) / sound_speed;
        % 添加测量噪声
        times(i) = times(i) + randn() * noise_std;
    end
    
    % 计算时间差 (相对于第一个信标)
    tdoa = times - times(1);
    
    %% 3.2 位置估计
    estimated_pos = simple_tdoa_solve(beacons, tdoa(2:end)*sound_speed);
    
    % 记录轨迹和误差
    trajectory(step, :) = auv_pos;
    traj_est(step, :) = estimated_pos;
    errors(step) = norm(estimated_pos - auv_pos);
    
    fprintf('步骤 %d: 真实位置 [%.1f,%.1f,%.1f], 估计位置 [%.1f,%.1f,%.1f], 误差: %.4fm\n', ...
            step, auv_pos, estimated_pos, errors(step));

完整代码的下载链接：https://download.csdn.net/download/callmeup/91579488

或参考专栏文章的链接：https://blog.csdn.net/callmeup/article/details/149856084?spm=1011.2415.3001.5331

如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者