【MATLAB例程】水下机器人AUV的长基线定位，适用于三维环境，EKF融合长基线和IMU数据，锚点数量可自适应，附下载链接-优快云博客

在这里插入图片描述

本本文所述程序实现了水下机器人（AUV）长基线（LBL）+ IMU在三维空间中的定位融合，采用扩展卡尔曼滤波（EKF）方法来融合较高频率的IMU数据和较低频率的声学基站测距数据，从而获得更准确、稳定的轨迹估计结果。

文章目录

程序简介
- 仿真场景
- 程序介绍
运行结果
MATLAB源代码

程序简介

仿真场景

轨迹生成：采用 螺旋下潜 + 正弦调制 的复杂三维轨迹，符合水下探测场景。
基站配置：采用4~无穷个声学基站的三维布局，提升几何可观性。设置好基站坐标后，后续程序可自动调整架构
传感器建模：IMU 数据引入 噪声 + 偏差，声学测距包含 高斯噪声 + 多径效应。

程序介绍

本程序实现了 水下机器人（AUV）长基线（LBL）+ IMU 三维定位融合，采用 扩展卡尔曼滤波（EKF） 方法来融合 高频IMU数据 和 低频声学基站测距数据，从而获得更准确、稳定的轨迹估计结果。

功能特点：

轨迹生成
- 机器人按照 螺旋下潜 + 正弦调制 的复杂三维轨迹运动，模拟真实的水下探测任务。
传感器建模
- IMU：模拟加速度计与陀螺仪，包含噪声和偏差。
- LBL：声学基站测距，包含高斯噪声和多径效应。
滤波框架
- 采用 扩展卡尔曼滤波，实现对位置、速度及传感器偏差的估计。
- 声学观测低频、IMU观测高频，二者融合能弥补单一传感器的不足。

适用场景：

水下机器人自主导航与定位仿真
多传感器融合算法教学与演示
声学定位系统的性能分析
EKF滤波框架的工程应用研究

运行结果

轨迹图与锚点（基站）位置示意图，三维：
在这里插入图片描述

误差曲线：

在这里插入图片描述

二维轨迹图：

在这里插入图片描述

命令行窗口输出的结果：

开始EKF滤波处理...
处理进度: 1000/10001 (10.0%)
处理进度: 2000/10001 (20.0%)
处理进度: 3000/10001 (30.0%)
处理进度: 4000/10001 (40.0%)
处理进度: 5000/10001 (50.0%)
处理进度: 6000/10001 (60.0%)
处理进度: 7000/10001 (70.0%)
处理进度: 8000/10001 (80.0%)
处理进度: 9000/10001 (90.0%)
处理进度: 10000/10001 (100.0%)
滤波处理完成！

=== EKF融合定位性能评估 ===
3D RMSE:        5.995 m
平均误差:       4.718 m
最大误差:       32.136 m
误差标准差:     3.700 m
North RMSE:     3.303 m
East RMSE:      3.286 m
Down RMSE:      3.773 m

=== LBL纯声学定位性能 ===
3D RMSE:        21.907 m
平均误差:       16.954 m
最大误差:       62.525 m

=== 性能改进 ===
融合相对改进:   72.6%
EKF融合定位优于纯LBL定位

=== 系统配置信息 ===
仿真时长:       100.0 秒
IMU频率:        100 Hz
声学定位频率:   1.0 Hz
总计算点数:     10001
声学更新次数:   101

=== 滤波器一致性分析 ===
位置估计偏差 [N,E,D]: [-0.072, 0.073, -0.411] m
速度估计偏差 [N,E,D]: [-0.145, 0.250, -0.000] m/s
估计加速度偏差:       [-7.0385, -3.3170, 19.4977] m/s²
估计陀螺偏差:         [0.0000, 0.0000, 0.0000] rad/s

=== 收敛性分析 ===
初始位置误差:        1.827 m
最终位置误差:        0.112 m
误差收敛比:          0.06
滤波器成功收敛
>>

MATLAB源代码

部分代码如下：

%% 水下机器人长基线(LBL)，三维轨迹定位，使用EKF滤波，融合LBL和IMU，锚点数量自适应
% 适用于AUV自主导航与定位
% 作者: matlabfilter
% 2025-08-19/Ver1

clear; clc; close all;
rng(0);
% 水下长基线定位与IMU融合系统
% 使用扩展卡尔曼滤波进行多传感器融合
% 坐标系：NED (North-East-Down)

%% 系统参数设置
% 基站配置（四面体配置，单位：米）
beacon_positions = [
    -500, -500, -50;    % 基站1 (西北)
    500, -500, -50;    % 基站2 (东北)
    500,  500, -50;    % 基站3 (东南)
    -500,  500, -60    % 基站4 (西南，深度不同)
    100,500,-50
    ];

% 仿真参数
dt_imu = 0.01;          % IMU采样间隔 (100Hz)
dt_acoustic = 1;      % 声学定位间隔
total_time = 100;       % 总仿真时间 (秒)
sound_speed = 1500;     % 声速 m/s

% 噪声参数（符合实际水下设备精度）
range_noise_std = 0.2;     % 距离测量噪声标准差(20cm)
imu_acc_noise_std = 0.002;   % 加速度计噪声 (0.02 m/s²)
imu_gyro_noise_std = 0.001;  % 陀螺仪噪声 (0.01 rad/s)
imu_bias_acc = 0.01*[0.01, -0.02, 0.015]; % 加速度计偏差

%% 生成真实轨迹（复杂的水下探测轨迹）
t = 0:dt_imu:total_time;
n_steps = length(t);

% 轨迹参数
R = 150;                    % 基础螺旋半径
omega = 2*pi/40;            % 螺旋角频率
descent_rate = -1.5;        % 基础下沉速度 m/s

% 生成真实状态（位置、速度、加速度）
true_pos = zeros(n_steps, 3);
true_vel = zeros(n_steps, 3);
true_acc = zeros(n_steps, 3);
true_euler = zeros(n_steps, 3); % 欧拉角 (roll, pitch, yaw)

for i = 1:n_steps
    % 复杂轨迹：螺旋下降 + 正弦调制
    phase = omega * t(i);
    r_mod = R * (1 + 0.3 * sin(3 * phase)); % 半径调制
    
    % 位置 (NED坐标系)
    true_pos(i,1) = r_mod * cos(phase);              % North
    true_pos(i,2) = r_mod * sin(phase);              % East
    true_pos(i,3) = -50 + descent_rate * t(i) + 20 * sin(phase/2); % Down
    
    % 速度
    true_vel(i,1) = -r_mod * omega * sin(phase) + 0.3 * R * omega * sin(3*phase) * cos(phase);
    true_vel(i,2) = r_mod * omega * cos(phase) + 0.3 * R * omega * sin(3*phase) * sin(phase);
    true_vel(i,3) = descent_rate + 10 * omega * cos(phase/2);
    
    % 加速度
    if i > 1
        true_acc(i,:) = (true_vel(i,:) - true_vel(i-1,:)) / dt_imu;
    end