多传感器融合（三）

2024-06-30 19:19| 来源: 网络整理| 查看: 265

一介绍

NaveGo：一个开源的MATLAB/GNU-Octave工具箱，用于处理集成导航系统并执行惯性传感器剖析分析。

NaveGo 是一个开源的 MATLAB/GNU Octave 工具箱，用于处理集成导航系统、模拟惯性传感器和 GNSS 接收器。它还使用艾伦方差对惯性传感器进行分析。它可以在线免费获得。它是在MATLAB / GNU-Octave下开发的，因为这种编程语言已成为物理系统工程和建模的事实标准。

NaveGo的座右铭是“为大众带来综合导航”。

二特征

NaveGo的主要特点是：

惯性导航系统（INS）的处理。

处理松散耦合的综合导航系统（INS/GNSS）。

处理带有磁力计（INS/GNSS/MAG）的松散耦合集成导航系统。

新增功能处理松散耦合的集成视觉导航系统（VISUAL/INS/GNSS）。

使用磁力计的指南针航向。

惯性传感器和全球导航卫星系统的仿真。

零速度更新（ZUPT）检测算法。

艾伦方差技术来表征惯性传感器的确定性和随机性误差。

更好地可视化 GNSS 中断。

三例子

该文件夹包含几种类型的示例，这些示例试图成为使用NaveGo的一种介绍。example

3.1 Allan variance example

此示例可以通过执行文件来分析。首先，将艾伦方差应用于Sensonor STIM2 IMU的300小时实际静态测量。然后，创建近 5 小时的合成惯性数据，并在这些模拟数据上运行 Allan 方差。navego_example_allan.m

3.2 INS/GNSS integration example using synthetic (simulated) data 合成数据

具有合成数据的 NaveGo 示例基于轨迹生成器的输出。该程序为先前定义的轨迹提供了真实加速度和角速度。NaveGo不提供轨迹生成器。该文件试图逐步公开如何使用NaveGo。根据ADIS16405 IMU和ADIS16488 IMU的误差曲线仿真两个IMU测量值。然后，使用模拟的GNSS传感器融合两个IMU。最后，比较了两个模拟INS/GNSS系统的性能。navego_example_synth.m

3.3 INS/GNSS integration example using real data 真实数据

提供了两个如何使用NaveGo对真实数据进行后处理的示例，一个用于战术级Ekinox-D IMU，另一个用于消费级MPU-6000 IMU。两个IMU都与Ekinox-D GNSS集成。这些数据集是通过驾驶车辆穿过都灵市（意大利）的街道生成的。navego_example_real_xxxx.m

3.4 VISUAL/INS/GNSS integration example using real data 真实数据

为VISUAL/INS/GNSS 集成提供了两个示例：examples/visual-data/

1.navego_example_canada_data.m

2.navego_example_katwijk_data.m

本文重点介绍此示例内容。

两个示例的区别是，一个使用的是katwijk数据集，一个是canada数据集，代码结构上没有区别。

4 源码解释

4.1 程序初始化，并引入脚本，以及添加路径，并打印信息

clc %用于清除MATLAB的命令行窗口 close all %关闭了MATLAB中所有已打开的图形窗口，以便在开始任务前清除之前的图形。 clear matlabrc %在启动MATLAB时运行MATLAB启动脚本，该脚本可以设置MATLAB的默认选项和环境变量。 addpath ../../ins/ %将指定的目录添加到MATLAB的搜索路径中，以便MATLAB可以在这些目录中找到所需的函数和脚本。 addpath ../../ins-gnss/ addpath ../../ins-visual/ addpath ../../conversions/ addpath ../../performance-analysis/ addpath ../../plot/ addpath ../../misc/ addpath ../../simulation/ addpath ./canada-planetary-data/data/ addpath ./canada-planetary-data/scripts/ navego_print_version; %用于打印NaveGo工具箱的版本信息。 fprintf('\nNaveGo: starting Canada planetary data integration... \n') %打印一条消息，表示NaveGo正在开始Katwijk行星数据的整合工作。

4.2 参数设置，用于判断是否生成数据，选择哪种融合方式，是否产生GPS失灵等干扰场景

%% PARAMETERS % Comment any of the following parameters in order to NOT execute a % particular portion of code 通过取消注释特定的参数，可以选择是否执行相应部分的代码。 GEN_DATA = 'ON'; %设置了一个名为GEN_DATA的变量，它的值是字符串'ON'。这个变量可能用于控制是否生成数据。 if (~exist('GEN_DATA','var')), GEN_DATA = 'OFF'; end %检查是否已经定义了名为GEN_DATA的变量，如果没有，则将GEN_DATA的值设置为字符串'OFF'。这样，如果代码的其他地方没有定义GEN_DATA变量，它默认为'OFF'。 % FusionCase = "inertial_gnss"; FusionCase = "inertial_visual"; %设置了名为FusionCase的变量，它的值是字符串"inertial_visual"。该变量可能用于选择数据融合的方案。 %FusionCase = "inertial_visual_gnss"; % Method to remove GNSS measurements for a certain time window. This is % done to showcase that visual measurements provide a failsafe option in % case satellite data is not available. 该变量用于控制是否在一定时间窗口内移除GNSS测量数据，以演示当卫星数据不可用时，视觉测量提供的故障容错选项。 % Sparse = "true"; %制作特殊场景 Sparse = "false"; fprintf('NaveGo: parameter FusionCase = %s \n', FusionCase) fprintf('NaveGo: parameter Sparse = %s \n', Sparse)

4.3 生成数据或者加载数据

%% Generating Data 生成数据 if strcmp(GEN_DATA, 'ON') %用于检查GEN_DATA变量的值是否等于字符串'ON'。如果是，则执行条件内的代码块，否则跳过该代码块。 if FusionCase == "inertial_gnss" %用于检查FusionCase变量的值是否等于字符串"inertial_gnss"。如果FusionCase是"inertial_gnss"，则生成GNSS数据，要么是正常的GNSS数据要么是稀疏的GNSS数据，具体是哪种数据由Sparse参数控制。 fprintf('NaveGo: generating IMU data... \n') %打印一条消息，指示正在生成惯性测量单元（IMU）数据。 imu_structure; %函数调用，它可能会生成IMU数据的结构。 fprintf('NaveGo: generating GNSS data... \n') if Sparse == "true" gnss_sparse_structure; else gnss_structure; end else %如果FusionCase不是"inertial_gnss"，则继续生成视觉（visual）数据，然后根据Sparse参数的设置生成相应的GNSS数据。 fprintf('NaveGo: generating IMU data... \n') imu_structure; fprintf('NaveGo: generating VISUAL data... \n') visual_structure; if Sparse == "true" fprintf('NaveGo: generating GNSS data... \n') gnss_sparse_structure; else gnss_structure; end end else %这段代码块负责加载数据而不是生成数据。 fprintf('NaveGo: loading data... \n') %里加载先前保存的数据而不是生成新的数据。 if FusionCase == "inertial_gnss" load imu_planetary if Sparse == "true" load gnss_planetary_r load gnss_planetary_sparse_r load gnss_planetary else load gnss_planetary_r load gnss_planetary end else load imu_planetary load visual_planetary if Sparse == "true" load gnss_planetary_r load gnss_planetary_sparse_r load gnss_planetary else load gnss_planetary_r load gnss_planetary end end end

4.4 开始融合并计算导航解，以及插值计算后的导航解

%% Estimation 估计 switch FusionCase %这是一个switch语句，它将根据FusionCase变量的值选择执行不同的代码块。 case "inertial_gnss" %匹配FusionCase等于"inertial_gnss"的情况。 fprintf('NaveGo: processing INS/GNSS integration... \n') %打印函数，用于在MATLAB命令行窗口打印不同数据融合方法的处理信息。 nav_e = ins_gnss(imu_planetary,gnss_planetary,'dcm'); %这行代码根据"惯性-导航系统/GNSS"（Inertial Navigation System/GNSS）融合方案，调用ins_gnss函数进行数据融合。函数接受IMU数据和GNSS数据作为输入，并输出融合后的导航解。 case "inertial_visual" fprintf('NaveGo: processing INS/VISUAL integration... \n') nav_e = ins_visual(imu_planetary,gnss_planetary_r,visual_planetary,'dcm'); % 这行代码根据"惯性-导航系统/视觉"（Inertial Navigation System/Visual）融合方案，调用ins_visual函数进行数据融合。函数接受IMU数据、GNSS数据和视觉数据作为输入，并输出融合后的导航解。 case "inertial_visual_gnss" fprintf('NaveGo: processing INS/GNSS/VISUAL integration... \n') nav_e = ins_visual_gnss(imu_planetary,gnss_planetary,visual_planetary,'dcm'); %这行代码根据"惯性-导航系统/视觉/GNSS"（Inertial Navigation System/Visual/GNSS）融合方案，调用ins_visual_gnss函数进行数据融合。函数接受IMU数据、GNSS数据和视觉数据作为输入，并输出融合后的导航解。 end [nav_i, nav_ref] = navego_interpolation (nav_e, gnss_planetary_r); %这行代码调用navego_interpolation函数对导航解进行插值处理。这个函数接受导航解和GNSS数据作为输入，并输出插值后的导航解和参考数据。 if Sparse == "true" [gnss_i,gnss_ref] = navego_interpolation(gnss_planetary,gnss_planetary_sparse_r); else [gnss_i,gnss_ref] = navego_interpolation(gnss_planetary,gnss_planetary_r); end

4.5 出图，分为三种情况，但结构相同仅换参数，只以第一中 IMU+GNSS 为例

%% Plotting 标图 fprintf('NaveGo: plotting... \n') switch FusionCase %% Plotting: IMU + GNSS case "inertial_gnss" % Position 位置数据 figure(); %新建窗口和画布 hold on; %图保持，让多个图效果出现在一张图上 plot(rad2deg(gnss_planetary.lon),rad2deg(gnss_planetary.lat),'.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5); %使用plot函数绘制gnss_planetary的经度和纬度数据，以点的形式表示，并使用灰色显示。 % 其中: rad2deg 是MATLAB中的一个函数，用于将弧度值转换为角度值。，ones(1,3) 是一个MATLAB函数，它返回一个包含三个元素的行向量，其中所有元素的值都为1。这样得到的行向量表示了一个RGB颜色，因为RGB颜色由三个分量（红、绿和蓝）组成，每个分量的取值范围是0到1。 scatter(rad2deg(gnss_planetary_r.lon),rad2deg(gnss_planetary_r.lat),'y.','MarkerEdgeAlpha',.6); %使用scatter函数绘制gnss_planetary_r的经度和纬度数据，以黄色点的形式表示，带有边缘透明度。 %rad2deg(gnss_planetary_r.lon) 和 rad2deg(gnss_planetary_r.lat) 是将GNSS数据中的经度和纬度值从弧度转换为度数。这样得到的经度和纬度值将以度为单位表示，方便在图形上显示。 %scatter 函数用于绘制散点图。它以经度和纬度值作为输入，以散点的形式在图形上显示这些点。 %'y.' 设置散点的样式。'y' 表示散点使用黄色（yellow）作为颜色，'.' 表示散点使用小圆点作为标记。 %'MarkerEdgeAlpha', .6 设置散点的边缘透明度为0.6。这样做可以使散点的边缘有一定的透明度，让散点更具可视化效果。 plot(rad2deg(nav_e.lon),rad2deg(nav_e.lat),'Color', [0, 0, 0], 'LineWidth', 1.5); %使用plot函数绘制nav_e的经度和纬度数据，以黑色线条表示。 grid on; %显示网格线 xlabel('Longitude [deg]'); %设置x轴和y轴的标签。 ylabel('Latitude [deg]'); legend('degraded GNSS','RTK','IMU + degraded GNSS','Location','Southeast'); %添加图例说明不同数据的含义。 axis equal; %将x轴和y轴的刻度比例设置为相等，保持图形的长宽比一致。 % Position Errors 位置误差 [RN,RE] = radius(nav_i.lat); %RN 和 RE 是计算赤道半径（RN）和极半径（RE）的结果。这两个值用于计算地球上不同纬度处的纬度和经度的距离单位。 %radius 函数计算地球上特定纬度位置的赤道半径和极半径。nav_i.lat 是导航解的纬度值。 LAT2M = RN + nav_i.h; %LAT2M = RN + nav_i.h; 计算了纬度误差的距离单位。它将导航解中的纬度误差 nav_i.lat 转换为米，加上导航解中的高度 nav_i.h，得到了纬度误差对应的地球上的纬度距离。 LON2M = (RE + nav_i.h).*cos(nav_i.lat); %LON2M = (RE + nav_i.h).*cos(nav_i.lat); 计算了经度误差的距离单位。它将导航解中的纬度误差 nav_i.lat 转换为米，乘以对应纬度处的纬度圆的周长，即 RE + nav_i.h 乘以 cos(nav_i.lat)，得到了经度误差对应的地球上的经度距离。 [RN,RE] = radius(gnss_i.lat); LAT2M_GR = RN + gnss_i.h; LON2M_GR = (RE + gnss_i.h).*cos(gnss_i.lat); % 【 .* 】是MATLAB中的一个运算符，用于执行逐元素相乘操作。 figure(); subplot(2,1,1); %创建两个子图中的第一个子图，表示纬度误差的变化。 hold on; plot(gnss_i.t,LAT2M_GR.*(gnss_i.lat - gnss_ref.lat), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5) %gnss_i.t 是GNSS数据的时间向量。 % LAT2M_GR.*(gnss_i.lat - gnss_ref.lat) 计算了GNSS纬度误差距离单位（LAT2M_GR）乘以 GNSS纬度误差 (gnss_i.lat - gnss_ref.lat)。这样得到的结果是一个向量，表示了纬度误差随时间的变化。 plot(nav_i.t, LAT2M.*(nav_i.lat - nav_ref.lat),'Color', [0, 0, 0], 'LineWidth', 1.5) %nav_i.t 是IMU+降级GNSS数据的时间向量；； % LAT2M.*(nav_i.lat - nav_ref.lat) 计算了IMU+降级GNSS纬度误差距离单位（LAT2M）乘以 IMU+降级GNSS纬度误差 (nav_i.lat - nav_ref.lat)。这样得到的结果是一个向量，表示了纬度误差随时间的变化。 grid on; xlabel('Time [s]') ylabel('[m]') legend('GNSS', 'IMU + degraded GNSS', 'Location', 'northoutside'); title('Latitude Error'); %%设置子图形的标题。 xlim([0,max(gnss_planetary.t)]); %设置x轴的显示范围。 subplot(2,1,2); hold on; plot(gnss_i.t, LON2M_GR.*(gnss_i.lon - gnss_ref.lon), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5) plot(nav_i.t, LON2M.*(nav_i.lon - nav_ref.lon),'Color', [0, 0, 0], 'LineWidth', 1.5) grid on; xlabel('Time [s]') ylabel('[m]') legend('GNSS', 'IMU + degraded GNSS', 'Location', 'northoutside'); title('Longitude Error'); xlim([0,max(gnss_planetary.t)]);

【本文地址】

公司简介

联系我们