多旋翼无人机组合导航系统-多源信息融合算法（Matlab代码实现）

砌墙_2301

于 2024-07-19 15:42:24 发布

阅读量584

点赞数 22

文章标签：无人机算法 matlab

本文链接：https://blog.csdn.net/2301_79155639/article/details/140551564

版权

👨‍🎓个人主页：研学社的博客

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

💥1 概述

多旋翼无人机已被广泛应用于军事与民用领域。导航系统是多旋翼无人机的重要组成部分,是其实现安全与稳定飞行的基础。采用INS/GPS组合导航系统可实现高精度导航,该组合导航系统具有优势互补、导航机构冗余的特点,其实质是一个多传感器导航信息优化处理系统。无人机的主要导航参数就是依靠多传感器信息融合获得的,因此信息融合技术是组合导航系统的关键技术,目前已成为国内外学者研究的热点问题。随着多旋翼无人机向自主化和智能化发展,多旋翼无人机对其自身导航系统的性能提出了更高的要求。但是受多旋翼无人机自身成本的制约,导航系统中选用的传感器精度较低。针对这一矛盾,本文提出将无人机自带的微型惯导系统与GPS通过信息融合技术相结合,构成INS/GPS组合导航系统,由此能够提升导航系统的整体性能。本文的研究工作围绕组合导航系统的设计展开。除此之外，本文以课题组自行研制的全新结构多旋翼小型无人机为研究平台,展开对机载多传感器组合导航系统信息融合这一关键技术的研究。

多旋翼无人机组合导航系统-多源信息融合算法研究是一个高度技术性和前瞻性的课题，它旨在提高无人机在复杂环境下的自主导航精度与鲁棒性。该领域的研究主要围绕如何有效整合来自不同传感器（如全球定位系统GPS、惯性导航系统INS、磁力计、气压高度计、视觉传感器等）的数据，通过先进的数据融合算法处理这些多源信息，以实现更加精确、可靠的导航解决方案。以下是该研究方向的几个关键点：

传感器选择与数据预处理：首先需要根据任务需求和环境特性选择合适的传感器组合。每种传感器都有其优点和局限性，例如GPS提供高精度的绝对位置信息但易受干扰，INS能提供连续的运动信息但存在累积误差。数据预处理包括噪声过滤、异常值检测等，为后续的融合处理打下基础。
多源信息融合算法：核心在于设计高效的算法来融合来自不同传感器的异构数据。常见的融合方法有：
- 卡尔曼滤波：经典的状态估计方法，适用于线性系统或可线性化的系统，通过最小均方误差原则估计状态。
- 粒子滤波：适用于非线性或非高斯分布问题，通过大量样本（粒子）的权重更新来逼近真实状态分布。
- 无损信息滤波（如无损卡尔曼滤波ULKF）：保持信息矩阵结构不变，适合于高维状态空间和分布式环境。
- 自适应融合算法：能够根据传感器的性能动态调整融合策略，增强系统的适应性和鲁棒性。
- 深度学习方法：利用神经网络进行端到端的学习，直接从原始数据中学习特征并进行融合，特别适合复杂非线性关系的处理。
故障检测与容错处理：在多传感器系统中，单个传感器的失效可能会严重影响整个导航系统的性能。因此，研究如何通过多源信息融合算法实现对传感器故障的实时检测和隔离，并采用冗余或替代信息源进行补偿，是提升系统可靠性的关键。
环境适应性：针对不同飞行环境（如城市峡谷、森林、室内等）的特点，研究如何优化融合算法，使其能够更好地适应特定环境中的信号遮挡、多路径效应等问题。
实时性与计算资源限制：无人机的资源有限，因此算法的设计需考虑实时性要求和计算资源的高效利用。轻量级算法和硬件加速技术（如GPU、FPGA应用）是重要研究方向。
实验验证与评估：通过实际飞行测试或仿真平台验证融合算法的性能，评估其在各种条件下的导航精度、稳定性、资源消耗等指标。

综上所述，多旋翼无人机组合导航系统中的多源信息融合算法研究是一个涉及信号处理、统计估计、机器学习等多个领域的跨学科课题，对于推动无人机技术的发展具有重要意义。

📚2 运行结果

部分代码：

% load Uav true trajectory data.
addpath UavTrajectorySim;

disp(' ')
disp('Available UAV Truth Trajectory Data Files:')
dir_mat_files = dir('UavTrajectorySim\*.mat');
for nFile=1:length(dir_mat_files)
fprintf(' %d: %s\n',nFile,dir_mat_files(nFile).name);
end
% nFileChoice = input('Choose a UAV Truth data file (e.g. 1<Enter>): ');
try
% load(dir_mat_files(nFileChoice).name)
load(dir_mat_files(1).name)
catch
error('Selected UAV Truth Trajectory data file (%d) is invalid.\n',nFileChoice);
end

gvar_earth;

% 单次更新中使用的子样数
nn = 2;
% 采样时间
ts = 0.01;
nts = nn*ts;

% 初始姿态、速度、位置
att0 = [0, 0, 90]'*arcdeg;
vn0 = [0, 0, 0]';
pos0 = [34*arcdeg, 108*arcdeg, 100]'; % lattitude, longtitude, height
qbn0 = a2qua(att0);

% 姿态四元数、速度、位置
qbn = qbn0;
vn = vn0;
pos = pos0;

eth = earth(pos, vn);

% *** 添加误差 ***
% 失准角
phi = [0.1, 0.2, 1]'*arcmin;
qbn = qaddphi(qbn, phi);

% 陀螺零偏，角度随机游走
eb_ref = [0.1, 0.15, 0.2]'*dph;
eb = [0.01, 0.015, 0.02]'*dph;
web = [0.001, 0.001, 0.001]'*dpsh;

% 加计零偏，速度随机游走
db_ref = [800, 900, 1000]'*ug;
db = [80, 90, 100]'*ug;
wdb = [1, 1, 1]'*ugpsHz;

Qk = diag([web', wdb', zeros(1, 9)]')^2*nts;

rk = [[0.1, 0.1, 0.1], [5/Re, 5/Re, 5]]';
Rk = diag(rk)^2;

% 协方差矩阵，x = [phi, delta_vn, delta_p, eb, db]
P0 = diag([[0.1, 0.1, 10]*arcdeg, [1, 1, 1], [10/Re, 10/Re, 10]...
[0.1, 0.1, 0.1]*dph, [80, 90, 100]*ug]')^2;

% 量测矩阵
Hk = [zeros(6,3), eye(6), zeros(6, 6)];

% Kalman filter initialization
kf = kfinit(Qk, Rk, P0, zeros(15), Hk);

% 与模拟轨迹时长一致
kTime = fix(t_SD/ts);
err = zeros(kTime, 10);
xkpk = zeros(kTime, 2*kf.n + 1);

pos_ref = zeros(kTime,3);
pos_est = zeros(kTime,3);
pos_gps = zeros(kTime,3);

kk = 1;
t = 0;
for k = 2 : nn : kTime
t = t + nts;

% 获取模拟轨迹对应的imu输出: 角增量和速度增量（参考值）
wm(1:nn,:) = imu_SD.wm(k-nn+1:k,:);
vm(1:nn,:) = imu_SD.vm(k-nn+1:k,:);

% 为IMU参考输出添加噪声
[wm1, vm1] = imuadderr(wm, vm, eb, web, db, wdb, ts);

% 惯导更新：姿态四元数、速度、位置
[qbn, vn, pos, eth] = insupdate(qbn, vn, pos, wm1, vm1, ts);

% 基于模型预测：导航误差系统模型卡尔曼滤波
kf.Phikk_1 = eye(15) + kfft15(eth, q2mat(qbn), sum(vm1, 1)'/nts)*nts;
kf = kfupdate(kf);

% 模拟GPS量测数据
gps = [avp_SD.vn(k,:)'; avp_SD.pos(k,:)'] + rk.*randn(6, 1);
pos_gps(kk,:) = gps(4:6)';
% 量测更新 5Hz
if mod(t, 0.2) < nts
Zk = [vn', pos']' - gps;
kf = kfupdate(kf, Zk, 'M');
end

% Indirect Kalman filter：feedback to IMU (反馈校正法)
qbn = qdelphi(qbn, kf.Xk(1:3));
vn = vn - kf.Xk(4:6);
pos = pos - kf.Xk(7:9);
pos_est(kk,:) = pos';
% 反馈校正：由于反馈项的存在导致卡尔曼滤波的先验估计值始终为零. Ref: 王辰熙
kf.Xk(1:3) = 0;
kf.Xk(4:6) = 0;
kf.Xk(7:9) = 0;
% kf.Xk(10:12) = 0;
% kf.Xk(13:15) = 0;


% compute the error between estimation & truth data
% Note that this 'error' is not the 'state vector' in the Kalman equ.
% In indirect kalman filter, the 'state vector' means the error of
% the IMU update (respect to True data.)
qbn_ref = a2qua(avp_SD.att(k,:));
vn_ref = avp_SD.vn(k,:)';
pos_ref(kk,:) = avp_SD.pos(k,:);
err(kk, :) = [qq2phi(qbn, qbn_ref)', (vn - vn_ref)', (pos - pos_ref(kk,:)')', t];
xkpk(kk, :) = [kf.Xk', diag(kf.Pk)', t]';