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

Ps.729

于 2024-04-27 09:25:30 发布

阅读量958

点赞数 8

文章标签：无人机算法 matlab

本文链接：https://blog.csdn.net/weixin_67304359/article/details/138162452

版权

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

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

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

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

目录

⛳️赠与读者

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。当哲学课上老师问你什么是科学，什么是电的时候，不要觉得这些问题搞笑。哲学是科学之母，哲学就是追究终极问题，寻找那些不言自明只有小孩子会问的但是你却回答不出来的问题。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能让人胸中升起一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它居然给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥1 概述

多旋翼无人机的组合导航系统需要融合来自多种信息源的数据，以提供准确可靠的定位和导航能力。多源信息融合算法是实现这一目标的关键。这些算法通常包括以下几个方面：

1. **惯性导航系统（Inertial Navigation System, INS）**：通过加速度计和陀螺仪等传感器来测量无人机的加速度和角速度，从而推导出位置、速度和航向信息。然而，INS会出现漂移，需要通过其他传感器进行校正。

2. **全球定位系统（Global Positioning System, GPS）**：GPS是一种通过接收来自卫星的信号来确定位置的技术。它提供了高精度的位置信息，但在某些环境下，如城市高楼区域或山谷地带，信号可能受到遮挡或干扰。

3. **视觉传感器**：无人机可以通过摄像头或其他视觉传感器获取地面特征的图像，并利用视觉里程计技术来推断位置和姿态。这种方法对于短期定位非常有效，但在长时间飞行中可能受到光照、天气和地面特征变化的影响。

4. **地面测量系统**：地面测量系统可以通过地面上的基站或其他传感器来提供参考位置信息，例如差分GPS或无线定位系统。

多源信息融合算法通常采用卡尔曼滤波器或扩展卡尔曼滤波器等技术，将来自不同传感器的信息进行优化和整合，以提高定位和导航的精度和可靠性。这些算法能够有效地处理传感器的误差、漂移和不确定性，并提供稳定的导航解决方案，适用于各种环境和任务需求。

📚2 运行结果

部分代码：

% 为动态变量分配存储空间
att_SD = zeros(fix(total_time/ts) + 10, 4);
imu_SD.fb_ref = zeros(fix(total_time/ts) + 10, 4);
imu_SD.wb_ref = zeros(fix(total_time/ts) + 10, 4);
%% simulation
% 当前时刻,注意crt从0开始计时,att_SD中首元素为t=0时刻的载体姿态
crt = 0;
% 循环次数
i = 0;
while crt < total_time

% 循环次数+1
i = i+1;

% 分别提取三个姿态角运动信息
% 计算pitch(crt), roll(crt), yaw(crt)
[pitch, wnb_p] = caculate_theta( ang_motion.p, crt );
[roll, wnb_r] = caculate_theta( ang_motion.r, crt );
[yaw, wnb_y] = caculate_theta( ang_motion.y, crt );

% 姿态
Cbn = a2mat([pitch, roll, yaw]');

% wnbnx, 注意该角速度并不完全投影于n系，这里只是为了方便才这样表示
wnbnx = [wnb_p, wnb_r, wnb_y]';

% 计算wibb, Cnxb中的nx表示(n1, n2, b)这三个坐标系
Cnxb = [ cos(roll), 0, -cos(pitch)*sin(roll)
0, 1, sin(pitch)
sin(roll), 0, cos(pitch)*cos(roll)];
wibb_ref = Cnxb*wnbnx + Cbn'*eth.winn;

% 载体无线运动，加计输出应为b系下重力加速度
fb_ref = Cbn'*eth.gn;

% 保存结果
att_SD(i, :) = [pitch, roll, yaw, crt];
imu_SD.fb_ref(i, :) = [fb_ref', crt];
imu_SD.wb_ref(i, :) = [wibb_ref', crt];

% 增加一个步长的时间，为下次计算做准备
crt = crt + ts;

end
% 删除存储变量中的空位
att_SD(i+1:end, :) = [];
imu_SD.fb_ref(i+1:end, :) = [];
imu_SD.wb_ref(i+1:end, :) = [];

% imu_ref添加误差获得imu_msr
[ imu_SD.fb_msr, imu_SD.wb_msr ] = imuadderr( imu_SD.fb_ref, imu_SD.wb_ref, ...
imu_err.eb, imu_err.web, imu_err.db, imu_err.wdb, ts );

% 数据保存
save('angle_motion_data', 'att_SD', 'imu_SD');
%% 绘图
% 时间轴
Time_axis = (1 : 1 : i)*ts;

% 姿态角
msplot(311, Time_axis, att_SD(:, 1)*deg, '时间 /s', 'pitch /\circ');
title('姿态角变化');
msplot(312, Time_axis, att_SD(:, 2)*deg, '时间 /s', 'roll /\circ');
msplot(313, Time_axis, att_SD(:, 3)*deg, '时间 /s', 'yaw /\circ');

% imu.fb_ref
msplot(321, Time_axis, imu_SD.fb_ref(:, 1), '时间 /s', 'fbx m/s^2');
title('加计标准输出');
msplot(323, Time_axis, imu_SD.fb_ref(:, 2), '时间 /s', 'fby m/s^2');
msplot(325, Time_axis, imu_SD.fb_ref(:, 3), '时间 /s', 'fbz m/s^2');
% imu.wb_ref
msplot(322, Time_axis, imu_SD.wb_ref(:, 1)*deg, '时间 /s', 'wibb x rad/s');
title('陀螺标准输出');
msplot(324, Time_axis, imu_SD.wb_ref(:, 2)*deg, '时间 /s', 'wibb y rad/s');
msplot(326, Time_axis, imu_SD.wb_ref(:, 3)*deg, '时间 /s', 'wibb z rad/s');

% imu fb_msr
msplot(321, Time_axis, imu_SD.fb_msr(:, 1), '时间 /s', 'fbx m/s^2');
title('加计测量值');
msplot(323, Time_axis, imu_SD.fb_msr(:, 2), '时间 /s', 'fby m/s^2');
msplot(325, Time_axis, imu_SD.fb_msr(:, 3), '时间 /s', 'fbz m/s^2');
% imu.wb_msr
msplot(322, Time_axis, imu_SD.wb_msr(:, 1)*deg, '时间 /s', 'wibb x rad/s');
title('陀螺测量值');
msplot(324, Time_axis, imu_SD.wb_msr(:, 2)*deg, '时间 /s', 'wibb y rad/s');
msplot(326, Time_axis, imu_SD.wb_msr(:, 3)*deg, '时间 /s', 'wibb z rad/s');