RT-Thread 互补滤波器 (STM32 + 6 轴 IMU)

news2024/11/25 6:36:37

作者:wuhanstudio

原文链接:https://zhuanlan.zhihu.com/p/611568999

最近在看无人驾驶的 Prediction 部分,可以利用 EKF (Extended Kalman Filter) 融合不同传感器的数据,例如 IMU, Lidar 和 GNSS,从而给出更加准确的状态预测。

刚好手边开发板有一个 6 轴的 IMU,本来打算试一下 卡尔曼滤波器 (Kalman Filter),然而 Kalman Filter 更适合 9 轴的传感器,也就是在 6 轴的基础上(3-axis Accel + 3-axis Gyro)融合 3 轴的磁力计。

对于一个只有 6 轴 IMU 的 MCU,轻量级的 互补滤波器 (Complementary Filter) 更加合适,利用 3 轴陀螺仪和 3 轴加速度计来估计开发板的姿态 (Pitch, Roll, Yaw)。

13865c9bda9395c923ea10c3b757bb01.jpeg

大致流程:首先用 RT-Thread 的 icm20608 软件包读取 陀螺仪 (Gyroscope) 和 加速度计 (Accelerometer) 的数据,分别计算出估计的角度,再用互补滤波器 (Complementary Filter) 融合两个角度估计、进行校正,其实核心算法的代码就 7 行。最后串口把数据发到电脑上,用 Python + OpenGL 可视化。

Github - STM32 IMU 互补滤波器 (RT-Thread):https://github.com/wuhanstudio/stm32-imu-filter

IMU 传感器 (Inertial Measurement Unit)

我们先介绍下从 I2C 总线读取出传感器原始数值后,如何处理得到加速度和旋转角速度。

一个六轴的 IMU 可以测量 x, y, z 三个方向的重力加速度,和绕三个轴的旋转角速度。比如,开发板如果静止放置在桌面上,会测量到 z 方向的重力加速度。

d1c63dd56bf445e2fc45e33da9e7ed79.png

三个轴的加速度

当然,如果开发板静止不动,绕三个轴的旋转速度都是 0。

509b4bc7a53f2bc2c9be099396dc9e90.png

三个轴的旋转角速度

由于传感器的输出实际上是来自 ADC 的 16 位数字信号,我们需要把它的单位转换成重力加速度 g。例如,我们可以选择测量范围

63900f633cc99d9cfe485b5adf550720.png

,默认是

d3921fbaff95fe7b11798cabab59cb36.png

,也就是把传感器的 16 位输出

f58d1ce39660866965496ae34728f2e1.png

映射到 [-2g, 2g),于是

601d06f5f62eef79b049a8f7d0a2b75e.png

也就是下面 icm20608 芯片手册的 Sensitivity Scale Factor。

0e8a1f66b74f27273b5aa91f05b978ad.jpeg

于是在代码里面,将原始的 int16 加速度数据除以 16384。

double aSensitivity = 16384;

accel_x = accel_x / aSensitivity;
accel_y = accel_y / aSensitivity;
accel_z = accel_z / aSensitivity;

同样,我们可以换算出角速度

f8837e8fc284ee7f2ac44adb0a14a680.png

06c521d8d372d375ebf702b36c55fd60.png

于是在代码里面,将原始的 int16 角速度数据除以 131。

double gSensitivity = 131;

gyrX = gyro_x / gSensitivity;
gyrY = gyro_y / gSensitivity;
gyrZ = gyro_z / gSensitivity;

这样我们就把 ADC 输出的 int16 原始数据分布转换成了加速度单位 g,和旋转角速度单位 °/s.

互补滤波器 (Complementary Filter)

我们可以用 互补滤波器 结合 加速度 和 旋转速度 的测量值,得到更准确的姿态预测。

我们使用下面的图中的坐标系,绕 x 轴旋转的角度为 roll,绕 y 轴的旋转方向为 pitch,绕 z 轴旋转方向为 yaw。逆时针旋转为正,顺时针旋转为负。

f399cb35f8b19b268faae6afc5444a09.png

陀螺仪估计姿态

陀螺仪测量的是瞬间的旋转角速度,所以位置的估计其实就是时间的积分。例如,每过 100ms 测量一次旋转速度,旋转速度 x 时间 = 旋转角度。

// angles based on gyro (deg/s)
gx = gx + gyrX * TIME_STEP_MS / 1000;
gy = gy + gyrY * TIME_STEP_MS / 1000;
gz = gz + gyrZ * TIME_STEP_MS / 1000;

当然,由于环境存在大量噪声,陀螺仪测量数据会存在随机的波动,这些噪声经过积分累积,最后会造成位置的漂移。

比如下面这张图,过了很长时间后,虽然开发板是静止的,但是右边的陀螺仪估计的位置,就无法回到原点,这就是长时间的累计误差造成的。

9177499db2bf396b4aedb3c407c3e97f.jpeg

加速度计估计姿态

加速度计不需要积分,我们可以直接对当前加速度角度求 arctan 得到角度:

55626cba01108feb7bff5480682a2b20.jpeg
// angles based on accelerometer
ax = atan2(accelY, accelZ) * 180 / M_PI;                                     // roll
ay = atan2(-accelX, sqrt( pow(accelY, 2) + pow(accelZ, 2))) * 180 / M_PI;    // pitch

不管我们的开发板绕 z 轴旋转多少度,重力加速度始终朝向地面。因此开发板静止状态,我们无法利用重力加速度知道 z 轴的旋转角度 (yaw),所以上面只计算 roll 和 pitch,最终 z 轴的旋转角度 yaw 会出现累计积分误差。

互补滤波器

我们需要结合2个测量值是因为:旋转速度短时间内比较准确,但是由于环境的噪声会产生一些随机运动,时间长了就会漂移,而加速度短时间内不一定准确,但是最终会维持稳定。

于是我们就可以取长补短,线性叠加2个测量值的估计,给出更准确的估计。

// complementary filter
gx = gx * 0.96 + ax * 0.04;
gy = gy * 0.96 + ay * 0.04;

短时间内,我们相信陀螺仪测量的旋转角速度 (权值: 0.96);长时间内,环境噪声逐渐造成的漂移,由加速度计慢慢进行矫正 (权值: 0.04)。

总结

最后总结一下,其实核心代码一共就 7 行。我们先利用加速度求解姿态,再利用旋转角速度求解姿态,最后用互补滤波器进行一个线性叠加。

// angles based on gyro (deg/s)
gx = gx + gyrX * TIME_STEP_MS / 1000;
gy = gy + gyrY * TIME_STEP_MS / 1000;
gz = gz + gyrZ * TIME_STEP_MS / 1000;

// angles based on accelerometer
ax = atan2(accelY, accelZ) * 180 / M_PI;                                     // roll
ay = atan2(-accelX, sqrt( pow(accelY, 2) + pow(accelZ, 2))) * 180 / M_PI;    // pitch

// complementary filter
gx = gx * 0.96 + ax * 0.04;
gy = gy * 0.96 + ay * 0.04;

References

  • https://github.com/mattzzw/Arduino-mpu6050

  • https://github.com/RT-Thread-pa




2ae1918154df369ffbd7ea24075ed408.gif

👇👇👇 点击阅读原文查看近期赛事

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/746907.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

Go——基础语法

目录 Hello World! 变量和常量 变量交换 匿名变量 常量 iota——特殊常量 基本数据类型 数据类型转换 运算符 算数运算符 关系运算符 逻辑运算符 位运算符号 ​编辑 赋值运算符 输入输出方法 流程控制 函数 可变参数类型 值传递和引用传递 Hello Wor…

性能测试 jmeter 的 beanshell 脚本的 2 个常用例子

目录 前言: Bean Shell 内置变量大全 例子 1 例子 2 技巧 前言: JMeter是一个功能强大的性能测试工具,而Beanshell是JMeter中用于编写脚本的一种语言。 在利用 jmeter 进行接口测试或者性能测试的时候,我们需要处理一些复杂…

使用GithubAction自动构建部署项目

GitHub Actions 是一种持续集成和持续交付(CI/CD) 平台,可用于自动执行生成、测试和部署管道。 您可以创建工作流程来构建和测试存储库的每个拉取请求,或将合并的拉取请求部署到生产环境。 GitHub Actions 不仅仅是DevOps,还允许您在存储库中…

基于linux下的高并发服务器开发(第一章)-GCC(2)1.3

04 / gcc 和 g的区别 gcc 和 g都是GNU(组织)的一个编译器 【误区一】:gcc只能编译 C 代码,g 只能编译 c 代码。两者都可以,请注意: 后缀为 .c 的,gcc 把它当做是 C 程序,而 g 当做是…

Debezium系列之:prometheus采集debezium的jmx数据,grafana通过dashboard展示debezium的jmx数据

Debezium系列之:prometheus采集debezium的jmx数据,grafana通过dashboard展示debezium的jmx数据 一、需求背景二、实现的效果三、导出debezium jmx四、debezium jmx重要指标五、部署prometheus和grafana六、Debezium MySQL Connector的dashboard七、debezium-dashboard.json八…

二叉树(上)——“数据结构与算法”

各位CSDN的uu们好呀,好久没有更新我的数据结构与算法专栏啦,今天,小雅兰继续来更新二叉树的内容,下面,让我们进入链式二叉树的世界吧!!! 二叉树链式结构的实现 二叉树链式结构的实现…

性能测试工具 Jmeter 测试 Dubbo 接口脚本编写

目录 前言: 1、背景 2、工具准备 3、创建一个 maven 项目,此处可以创建一个 quickstart,参考截图 4、以上配置完毕后,开始撸代码 5、上面那个类是不需要从 jmeter 中获取参数,如果要从 jmeter 中获取相关的参数&…

低代码在边缘计算工业软件中的应用

近年来,边缘计算给工业现场带来了许多新的变化。由于计算、储存能力的大幅提升,边缘计算时代的新设备往往能够胜任多个复杂任务。另外,随着网络能力的提升,边缘设备与设备之间、边缘设备与工业互联网云平台之间的通讯延迟与带宽都…

Flowable边界事件-信号边界事件

信号边界事件 信号边界事件一、定义1. 图形标记2. 设置信号 选择信号3. XML标记 二、测试用例2.1 定时边界事件xml文件2.2 信号边界事件测试用例 总结 信号边界事件 一、定义 接收到信号触发事件 1. 图形标记 2. 设置信号 选择信号 3. XML标记 定时边界事件的XML <signal…

JMeter进行WebSocket压力测试

背景 之前两篇内容介绍了一下 WebSocket 和 SocketIO 的基础内容。之后用 Netty-SocketIO 开发了一个简单的服务端&#xff0c;支持服务端主动向客户端发送消息&#xff0c;同时也支持客户端请求&#xff0c;服务端响应方式。本文主要想了解一下服务端的性能怎么样&#xff0c;…

驱动开发-day9

驱动代码&#xff1a; #include <linux/cdev.h> #include <linux/device.h> #include <linux/fs.h> #include <linux/gpio.h> #include <linux/init.h> #include <linux/interrupt.h> #include <linux/module.h> #include <linu…

Hystrix熔断器

雪崩 当山坡积雪内部的内聚力抗拒不了它所受到的重力拉引时&#xff0c;积雪便向下滑动&#xff0c;引起⼤量雪体崩塌&#xff0c;人们把这种自然现象称作雪崩 微服务中&#xff0c;一个请求可能需要多个微服务接口才能实现&#xff0c;会形成复杂的调用链路 …

在Linux下通过MySQL二进制包安装MySQL5.7

在Linux下通过通用压缩包安装MySQL5.7 卸载MySQL 如果是第一次安装MySQL&#xff0c;在安装MySQL前&#xff0c;知道如何卸载MySQL是很有必要的。因为在安装过程中可能会 遇到各种各样的问题&#xff0c;自己玩的话 卸载重装即可。 1. find / -name mysql 查看MySQL相关包…

Layui之动态树 左侧树形菜单栏 详细全面

⭐ฅʕ•̫͡•ʔฅ本期看点&#xff1a;该篇是运用Layui框架来编写后台树形菜单栏&#xff0c;并且结合MySql来编写完成 目录 一.效果图 二.具体步骤 2.1 数据库 2.2 树形导航栏 第一个类&#xff1a;Treevo 第二个类&#xff1a;BuildTree&#xff1a; 2.3 Dao方法 2.3.…

【自我提升】Spring Data JPA之Specification动态查询详解

写在前面&#xff1a;刷完Spring Data JPA的课后&#xff0c;发现Specification动态查询还挺有意思的&#xff0c;还应用到了规约设计模式&#xff0c;在此记录下学习过程和见解。 目录 一、应用场景 二、源码解析 三、规约模式 四、实际应用 一、应用场景 1. 简介 有时我…

Linux中安装Tomcat

前提条件&#xff1a; 虚拟机中已经提前安装好jdk1.8。 安装步骤&#xff1a; 1.下载安装包 首先去Apache官网下载&#xff08;Apache Tomcat - Apache Tomcat 9 Software Downloads&#xff09; 2.上传到 linux 中&#xff0c;我这里上传的目录是&#xff1a; /opt 3. 解压…

element-plus坑总结

reactive和ref对比 // 定义变量 import { reactive } from vue; const person reactive({name: "John",age: 25, });// 赋值修改 person.name "Tom"; person.age 26;// 使用变量 <div>{{ person.name }}</div> <button click"perso…

layui介绍以及登录功能的实现

一. layui简介 1.1 layui介绍 Layui 是一套开源免费的 Web UI 组件库&#xff0c;采用自身轻量级模块化规范&#xff0c;遵循原生态的 HTML/CSS/JavaScript 开发模式&#xff0c;非常适合网页界面的快速构建。Layui 区别于一众主流的前端框架&#xff0c;它更多是面向于后端开…

3、Linux-进程管理类

进程管理类 进程是正在执行的一个程序或命令&#xff0c;每一个进程都是一个运行的实体&#xff0c;都有自己的地址空间&#xff0c;并占用一定的系统资源。 7.10.1 ps 查看当前系统进程状态 ps:process status 进程状态 1&#xff09;基本语法 ps aux | grep xxx &#xff08…

Python 导入引用其他文件的函数(持续更新)

文章目录 构造初始化文件结构&#xff0c;以此为例。【1】导入同目录且同级下其他文件的函数&#xff08;c.py文件导入d.py文件的函数&#xff09;&#xff08;1&#xff09;只引入d.py文件&#xff08;2&#xff09;直接引入函数&#xff08;3&#xff09;引入全部函数 【2】导…