基于Matlab构建适用于无人机或四轴飞行器的IMU+GPS融合算法（附源码）

此示例演示如何构建适用于无人机 （UAV） 或四轴飞行器的 IMU + GPS 融合算法。此示例使用加速度计、陀螺仪、磁力计和 GPS 来确定无人机的方向和位置。

一、模拟设置

设置采样率。在典型系统中，加速度计和陀螺仪以相对较高的采样率运行。在融合算法中处理来自这些传感器的数据的复杂性相对较低。相反，GPS以及在某些情况下的磁力计以相对较低的采样率运行，并且与处理它们相关的复杂性很高。在该融合算法中，磁力计和GPS样本以相同的低速率一起处理，加速度计和陀螺仪样本以相同的高速率一起处理。

为了模拟此配置，IMU（加速度计、陀螺仪和磁力计）以 160 Hz 采样，GPS 以 1 Hz 采样。磁力计的每 160 个样本中只有一个被提供给融合算法，因此在实际系统中，磁力计的采样率要低得多。

二、融合过滤器

创建过滤器以融合 IMU + GPS 测量值。融合滤波器使用扩展的卡尔曼滤波器来跟踪方向（作为四元数）、速度、位置、传感器偏差和地磁矢量。

这有几种处理传感器数据的方法，包括，和。该方法将来自IMU的加速度计和陀螺仪样本作为输入。每次对加速度计和陀螺仪进行采样时调用该方法。此方法根据加速度计和陀螺仪提前一个时间步预测状态。扩展卡尔曼滤波器的误差协方差在此处更新。

该方法以GPS样本作为输入。该方法通过计算卡尔曼增益来更新基于 GPS 样本的滤波器状态，该增益根据各种传感器输入的不确定性对其进行加权。此处还更新了误差协方差，这次也使用卡尔曼增益。该方法类似，但根据磁力计样本更新状态、卡尔曼增益和误差协方差。虽然将加速度计和陀螺仪样本作为输入，但它们被集成以分别计算增量速度和三角角。滤波器跟踪磁力计和这些积分信号的偏置。

三、无人机轨迹

此示例使用从无人机记录的保存轨迹作为地面实况。该轨迹被馈送到多个传感器模拟器，以计算模拟加速度计、陀螺仪、磁力计和 GPS 数据流。

四、全球定位系统传感器

以指定的采样率和参考位置设置 GPS。其他参数控制输出信号中噪声的性质。

五、IMU 传感器

通常，无人机使用集成的MARG传感器（磁性，角速率，重力）进行姿态估计。要对 MARG 传感器进行建模，请定义包含加速度计、陀螺仪和磁力计的 IMU 传感器模型。在实际应用中，三个传感器可能来自单个集成电路或单独的集成电路。此处设置的属性值是低成本MEMS传感器的典型值。

六、初始化

6.1 初始化的状态向量insfilterMARG

跟踪 22 元素向量中的姿势状态。

 6.2 初始化insfilterMARG

测量噪声描述了有多少噪声破坏了传感器读数。这些值基于与参数。过程噪声描述了滤波器方程对状态演变的描述程度。过程噪声是使用参数扫描根据经验确定的，以共同优化滤波器的位置和方向估计值。

6.3 初始化作用域

示波器可以绘制变量随时间的变化。它在这里用于跟踪姿势错误。示波器允许滤波器估计和地面实况姿势的 3D 可视化。示波器可能会减慢模拟速度。

七、模拟循环

主模拟循环是一个带有嵌套 for 循环的 while 循环。while 循环以 GPS 采样率执行。嵌套的 for 循环以 IMU 采样率执行。示波器以 IMU 采样率更新。

 

 八、程序

使用Matlab R2022b版本，点击打开。

 打开下面的“IMUandGPSFusionExample.mlx”文件，点击运行，就可以看到上述效果。

关注下面公众号，后台回复关键词：无人机或四轴飞行器融合算法，发送源码链接。