代码功能概述

本文所述代码实现了一个基于水下超短基线（USBL）的三维目标定位仿真系统，通过方位角（θ）、俯仰角（φ）和斜距（r）的直接测量值进行坐标系转换，生成目标轨迹并评估定位误差。其核心功能包括：

1. 目标运动建模：生成螺旋下降轨迹
2. 传感器噪声模拟：添加方位角、俯仰角、测距的高斯噪声
3. 极坐标-笛卡尔坐标转换：实现USBL定位核心算法
4. 三维可视化与误差分析：轨迹对比及误差曲线绘制

关键技术解析

坐标系定义与转换

% 极坐标转局部坐标系公式
x_local = r * cos(phi) * cos(theta);
y_local = r * cos(phi) * sin(theta);
z_local = r * sin(phi);

该转换过程体现了三维球坐标系到笛卡尔坐标系的映射关系，符合几何声学定位原理。全局坐标系通过叠加传感器位置实现平移变换。

目标运动模型

% 螺旋下降轨迹参数
pos_true(1,:) = 50*sin(0.1*t);  % X轴正弦运动（水平方向振幅50m）
pos_true(2,:) = 50*cos(0.1*t);  % Y轴余弦运动（圆周半径50m）
pos_true(3,:) = -100 - 0.5*t;   % Z轴匀速下降（初始深度100m，速率0.5m/s）

该模型模拟了水下航行器典型的三维机动模式，水平面做圆周扫描，垂直方向缓速下潜。

噪声模型

• 方位角噪声：标准差1°（对应约17.45mrad）
• 俯仰角噪声：标准差1°
• 测距噪声：标准差0.5m
  该噪声水平模拟了实际USBL系统中由声速剖面误差、多径效应等因素引起的测量不确定性。

性能分析指标

err = sqrt(sum((pos_true - measured_xyz).^2,1));

通过计算欧氏距离误差，代码输出以下关键指标：

• 平均误差：反映系统整体定位精度
• 最大误差：评估极端情况下的误差边界
• 标准差：表征误差分布的离散程度

运行结果

定位示意图（目标运动轨迹为螺旋下降）：

误差曲线图：

MATLAB源代码

部分源代码如下：

%% 基于方位角与距离测量的目标定位仿真（无EKF实现）
% 观测模型：方位角θ：基于目标与传感器的XY平面投影计算,俯仰角φ：基于高度差和目标水平距离计算,斜距r：欧氏距离
% 2025-06-04/Ver2
clc; clear; close all;
rng(0)
%% 参数设置
dt = 1;           % 采样时间(s)
T = 60;           % 总时长(s)
c = 1500;         % 声速(m/s) 
sigma_theta = 1*pi/180; % 方位角噪声标准差(rad) 
sigma_phi = 1*pi/180;   % 俯仰角噪声标准差(rad)
sigma_r = 0.5;          % 测距噪声标准差(m)

%% 传感器位置（水下USBL阵列模拟）
sensor_pos = [0, 0, -10]; % 传感器三维坐标(x,y,z)

%% 生成目标轨迹（螺旋上升运动）
t = 0:dt:T;
N = length(t);
pos_true = zeros(3,N);
pos_true(1,:) = 50*sin(0.1*t);      % X轴正弦运动
pos_true(2,:) = 50*cos(0.1*t);      % Y轴余弦运动
pos_true(3,:) = -100 - 0.5*t;       % Z轴匀速下降

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

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