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

最新推荐文章于 2025-11-25 11:31:36 发布

原创最新推荐文章于 2025-11-25 11:31:36 发布 · 238 阅读

6 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #人工智能 #机器学习

基于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));
    end
    weights = 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

三、性能评估与可视化

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;