当前位置：首页 > news >正文

惯性导航+DVL的组合导航算法

news 2025/9/23 14:35:35

基于MATLAB的惯性导航（INS）与多普勒测速仪（DVL）组合导航算法

一、仿真数据生成模块

1. 运动轨迹生成

%% 生成真实轨迹（匀速+匀加速段）
T = 600; % 总时间(s)
dt = 0.1; % 采样间隔
t = 0:dt:T;% 匀速段（前300s）
v0 = [1.5, 0, 0]; % 初始速度(m/s)
pos = cumsum(v0 * dt);
vel = repmat(v0, 1, length(t)/2);% 匀加速段（后300s）
a = [0.1, 0, 0]; % 加速度(m/s²)
vel(1501:end) = vel(1500) + a * (t(1501:end) - t(1500));
pos(1501:end) = pos(1500) + vel(1500)*(t(1501:end)-t(1500)) + 0.5*a*(t(1501:end)-t(1500)).^2;

2. 传感器数据仿真

%% INS原始数据生成
gyro_noise = 0.02 * (pi/180); % 角速度噪声(°/s)
accel_noise = 50e-6; % 加速度噪声(m/s²)% 生成带噪声的IMU数据
omega = gyro + gyro_noise * randn(size(t));
acc = accel + accel_noise * randn(size(t));%% DVL数据生成（四波束配置）
dvl_noise = 0.1; % 速度噪声(m/s)
dvl_data = vel + dvl_noise * randn(3,length(t));%% 添加异常数据（模拟DVL失效）
dvl_data(200:250,:) = dvl_data(199,:) + 5*randn(1,3); % 突变噪声

二、融合算法实现

1. 扩展卡尔曼滤波（EKF）

function [x_est, P] = EKF_INS_DVL(acc, gyro, dvl, dt)% 状态向量：[x,y,z,vx,vy,vz,roll,pitch,yaw](@ref)x = [0;0;0;0;0;0;0;0;0];P = eye(9)*1e-4;% 状态转移矩阵F = eye(9);F(4:6,:) = eye(3)*dt;% 量测矩阵H = zeros(3,9);H(1,4) = 1; H(2,5) = 1; H(3,6) = 1;for k = 1:length(t)% 预测x_pred = F*x;P_pred = F*P*F' + Q;% 量测更新z = dvl(:,k);K = P_pred*H'/(H*P_pred*H' + R);x = x_pred + K*(z - H*x_pred);P = (eye(9)-K*H)*P_pred;end
end

2. 无迹卡尔曼滤波（UKF）

function [x_est] = UKF_INS_DVL(acc, gyro, dvl, dt)% 定义Sigma点参数alpha = 1e-3; beta = 2; kappa = 0;lambda = alpha^2*(size(x,1)+kappa) - size(x,1);% 生成Sigma点[X, W] = genSigmaPoints(x, P, lambda);% 状态传播X_pred = zeros(size(X));for i = 1:size(X,2)X_pred(:,i) = propagateState(X(:,i), acc, gyro, dt);end% 量测更新Z_pred = DVL_measurement(X_pred);[x_est, P] = update(X_pred, W, Z_pred, dvl);
end

3. 交互多模型（IMM）

function [x_est] = IMM_INS_DVL(acc, gyro, dvl, dt)% 定义模型集合models = {@constant_velocity, @constant_acceleration};weights = [0.7, 0.3];% 模型预测x_pred = zeros(size(x,1), length(models));P_pred = zeros(size(P));for i = 1:length(models)[x_pred(:,i), P_pred(:,:,i)] = models{i}(x, dt);end% 模型概率更新gamma = zeros(length(models),1);for i = 1:length(models)gamma(i) = likelihood(x_pred(:,i), dvl(:,k));endweights = weights .* gamma / sum(weights.*gamma);% 混合估计x_est = sum(bsxfun(@times, weights, x_pred), 2);
end

4. 自适应卡尔曼滤波

function [x_est, P] = Adaptive_KF(acc, gyro, dvl, dt)% 初始参数x = [0;0;0;0;0;0;0;0;0](@ref)P = eye(9)*1e-4;% 新息计算for k = 1:length(t)% 预测步骤[x_pred, P_pred] = predictINS(x, P, acc, gyro, dt);% 新息统计y = dvl(:,k) - H*x_pred;S = H*P_pred*H' + R;K = P_pred*H'/S;% 自适应噪声估计[Q_adapt, R_adapt] = noiseEstimation(y, S, K);% 更新步骤x = x_pred + K*y;P = (eye(9)-K*H)*P_pred + Q_adapt;end
end

参考代码惯性导航+DVL的组合导航算法 youwenfan.com/contentcnb/78138.html

三、性能评估与可视化

1. 误差计算

%% 真实值与估计值对比
true_pos = pos;
est_pos_ekf = x_est_ekf(1:3,:);
est_pos_ukf = x_est_ukf(1:3,:);
est_pos_imm = x_est_imm(1:3,:);
est_pos_adaptive = x_est_adaptive(1:3,:);%% 误差统计
errors = struct();
errors.EKF = sqrt(mean((true_pos - est_pos_ekf).^2));
errors.UKF = sqrt(mean((true_pos - est_pos_ukf).^2));
errors.IMM = sqrt(mean((true_pos - est_pos_imm).^2));
errors.Adaptive = sqrt(mean((true_pos - est_pos_adaptive).^2));%% 可视化
figure;
subplot(2,1,1);
plot(t, true_pos(:,1), 'k', t, est_pos_ekf(:,1),'r--', ...t, est_pos_ukf(:,1),'b-.', t, est_pos_imm(:,1),'g:');
legend('真实位置', 'EKF', 'UKF', 'IMM');
title('东向位置估计');subplot(2,1,2);
bar([errors.EKF, errors.UKF, errors.IMM, errors.Adaptive]);
set(gca,'XTickLabel',{'EKF','UKF','IMM','自适应'});
ylabel('均方根误差(m)');

四、关键参数设置

%% 系统噪声参数
Q = diag([0.1, 0.1, 0.1, 1e-4, 1e-4, 1e-4, 1e-5, 1e-5, 1e-5]); % 过程噪声
R = diag([0.5, 0.5, 0.5](@ref)); % 初始量测噪声%% 滤波器参数
kalman_params.R = R;
kalman_params.Q = Q;
ukf_params.alpha = 1e-3;
ukf_params.beta = 2;