PX4从放弃到精通（七）：四元数互补滤波姿态解算代码解析

已于 2023-01-17 22:27:16 修改
阅读量6.2k 收藏 37
点赞数 7
分类专栏： PX4从放弃到精通 文章标签： 自动驾驶 计算机视觉 算法
于 2022-01-17 15:30:31 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/qq_38768959/article/details/122535430
版权
本文详细介绍了无人机硬件中四元数互补滤波在姿态解算中的应用，通过分析陀螺仪、加速度计和磁力计的特性，结合代码解析，展示了如何融合传感器数据，提高姿态信息的准确性。讨论了视觉、磁力计和运动捕捉数据对航向角的修正，并解释了如何处理陀螺仪漂移，以实现精确的四元数更新和归一化。

摘要生成于 C知道 ，由 DeepSeek-R1 满血版支持， 前往体验 >

👉👉👉**无人机硬件，提供全程指导**👈👈👈

文章目录

前言

一个人可以走的更快，一群人才能走的更远，交流学习加qq:2096723956
更多保姆级PX4+ROS学习视频：https://b23.tv/ZeUDKqy
PX4固件版本　1.11.3

一、传感器特性

互补滤波的作用是将不同传感器的数据进行融合，得出一个更加准确的姿态信息．
陀螺仪
陀螺仪，测量角速度，具有高动态特性，它是一个间接测量角度的器件。它测量的是角度的导数，即角速度，要将角速度对时间积分才能得到角度。由于噪声等误差影响，在积分作用下不断积累，最终导致陀螺仪的低频干扰和漂移。<

了解本专栏 订阅专栏 解锁全文 超级会员免费看
PX4从放弃到精通（八）：本地位置解算（LPE）
超维空间科技的博客
07-09 2390
文章目录前言一、组合导航系统模型二、代码解析1.引入库2.读入数据总结 前言 一个人可以走的更快，一群人才能走的更远，交流学习加qq:2096723956 更多保姆级PX4+ROS学习视频：https://b23.tv/ZeUDKqy 本文主要介绍PX4中基于卡尔曼滤波的位置解算（local_position_estimator），该模块通常与四元数互补滤波配合使用． PX4固件版本：1.11.3 一、组合导航系统模型 INS/GPS组合导航系统一般有两种信息融合方式:速度、位置组合(松耦合)和伪距、伪距率
PX4从放弃到精通（二十五）：EKF2
超维空间科技的博客
05-25 3818
PX4 ekf的传感器数据放在FIFO的环形缓冲区中，每次融合时取最新的数据，融合出的bias直接给输出滤波器，补偿到IMU的数据中，同时EKF融合出的位置速度等数据也要给输出滤波器，根据延时经过补偿输出当前时间线下的状态。
attitude_estimator_q_PX4姿态互补融合滤波_
09-29
姿态互补滤波解算
px4无人机姿态控制原理理解（获取传感器数据--四元数计算--控制输出）
最新发布
ddddddddddddaa的博客
02-26 1037
其中error为当前角度值和目标角度值的差异，也可以为当前姿态四元数和目标姿态四元数之间的差异。只需要获取旋转角度和目标位置向量即可得到目标姿态的四元数，也可以直接使用旋转角度转换为四元数。通过传感器数据，并结合互补滤波或卡尔曼滤波，估算无人机的姿态信息并生成当前姿态的四元数。PID控制器输出值分别通过转化为PWM信号输入到不同的电机，实现无人机的姿态旋转。利用计算得到的四元数和初始姿态四元数相乘即可得到无人机新的姿态四元数。利用当前的姿态信息和目标姿态信息之间的误差生成PID控制器输出。
四元数+互补滤波
weixin_30319153的博客
08-21 924
四元数圆点博士小四轴之四元数算法。四元数算法在小四轴上一般直接采用Madgwick的算法就差不多。这是一个居于GPL的协议，大家可以自由使用。//================// IMU.c// S.O.H. Madgwick// 25th September 2010//=================转载如下：void IMUupdate(float gx, float gy, floa...
【算法基础学习 4】互补滤波算法——PX4姿态估计
wb790238030的博客
01-31 8063
目录 应用场景 名词解释 陀螺仪 加速度计 磁力计（又叫磁罗盘） 坐标系 姿态表示 滤波原理 滤波主要过程 预备知识 预测 校正 加速度计校正 磁力计校正 更新四元数 源码分析 主程序运行流程图 函数功能简述 源码分析 头文件 using @@@ namespace attitude_estimator_q 类定义: class AttitudeEst...
PX4 代码中 position_estimator_inav（互补滤波）理解
Chasing的博客
03-20 1996
Local_position_estimator 为卡尔曼滤波估计 position_estimator_inav 需要解决三个问题： 如何由加速度进行速度和位置估计； 加速度偏差的计算； 如何使用融合过程。 借用网上一个图诠释下 这里px4_poll()函数设定最小50Hz频率，用来保证姿态数据能够实时获取到，根据返回值判断是否符合要求，不符合要求则mavlink_log_info...
四轴之互补滤波与四元数算法简单分析
《运放秘籍》系列视频原创作者、畅销书《硬件设计指南 从器件认知到手机基带设计》原创作者
09-07 1万+
有人问我关于四元数姿态解算算法的分析，每次都解释好久，今日空闲，特发一帖，供大家参考。本分析将结合程序，分析姿态解算思路，由于能力有限，难免有错误之处请谅解，同时希望能够抛砖引玉，得到大神指点。感谢圆点博士提供资料参考。为尊重原创，转载请注明原出处，谢谢。http://blog.youkuaiyun.com/u013608300/article/details/52459515四轴之互补滤波与四元数算法最详...
PX4姿态解算源码原理理解
weixin_44210896的博客
11-05 2121
PX4源码原理理解一．主要参考资料链接：1.1 取PX4源码一小部分姿态解算来进行讲解姿态解算源码中文注释：https://blog.youkuaiyun.com/zouxu634866/article/details/79806948(但是并不全面，下面有我添加注释的较为完整的版本1.2 姿态解算文件层级 Cmakelists是编译脚本为之后使用命令刷入固件提供方便 attitude_est...
基于四元数的简单互补滤波姿态解算
wuRDmemory
11-11 1万+
序言鉴于现在飞控的热度不减当年，越来越多“年轻人”加入飞控制作学习的阶段，也有许多新手上来就气势汹汹的进行姿态解算，往往使用的就是较为常用的Mahony的互补滤波姿态解算，笔者在多年前也一样，最开始接触的就是这个算法，但是当时懂得马马虎虎，直接把数据丢进去之后就得到了解算好的四元数，之后按照网上的公式一转换就OK了，但是后来才发现，当你的飞控出现问题的时候，大多数的时候，不见得是控制问题，有时候确实
姿态解算知识点1——四元数互补滤波求解欧拉角
software_public的博客
04-28 4121
1.目标 求四元数q0、q1、q2、q3； 求解飞行器、机器人的欧拉角pitch、roll、yaw； 2.算法总框图 3.模块解析 3.1 FIR滤波 为了消除部分噪声，6轴原始数据先经过FIR滤波。 3.2 四元数互补滤波 这里不对四元数相关模型和算法的推导讲解，直接粘贴代码： /******************************...
互补滤波器四元数姿态解算算法
05-19
基于互补滤波器的姿态解算算法，用的四元数表示的姿态，最后输出欧拉角度，适合于做自平衡小车等
四元数平方根容积卡尔曼滤波的姿态估计
12-27
四元数平方根容积卡尔曼滤波的姿态估计
PX4源码解读——传感器校正（陀螺仪）.pdf
10-17
该文档从陀螺仪的数学模型，校正流程，px4源码解读三个方面讲解了px4传感器校正代码
EKF 扩展卡尔曼 姿态解算 数据融合
05-10
EKF 扩展卡尔曼 姿态解算 数据融合
px4飞控磁罗盘校准算法理论与代码结合解读
08-09
px4飞控磁罗盘校准算法理论与代码结合解读，有分析图解，有代码注释，有理论有实践
基于互补滤波器的四元数算法（个人见解）最终版 2020.1.30
Enoch-dj
01-30 1756
代码解析（超详细！！！） 这部分关于姿态解算的代码解析有一部分是参考别人的基础上注释的，不喜勿喷！！！ gyro.x = gyro.x * DEG2RAD; //角度换成弧度 gyro.y = gyro.y * DEG2RAD; gyro.z = gyro.z * DEG2RAD; //大部分的传感器输出的角速度是“度每秒”，但是实际上，角速度的国际单位是“弧度每秒”，这点是一定要注意的，当...
【滤波算法】基于四元数的扩展卡尔曼滤波器算法Matlab实现
qq_59747472的博客
12-25 1335
摘要: 姿态估计是机器人技术、航空航天、虚拟现实等诸多领域的关键技术。精确的姿态信息对于系统控制和决策至关重要。本文将深入探讨基于四元数的扩展卡尔曼滤波器(EKF)算法在姿态估计中的应用。相比于欧拉角等其他表示方法，四元数能够有效避免奇异性问题，并提供更简洁、高效的姿态表示和计算方法。本文将首先介绍四元数的基本概念及其在旋转表示中的优势，然后详细阐述EKF算法的基本原理，并结合四元数对EKF进行改进，最终分析该算法在姿态估计中的应用效果，并探讨其优缺点及未来发展方向。1. 四元数及其在旋转表示中的优势。
卡尔曼（Kalman）滤波（六）--卡尔曼滤波的应用： 四元数卡尔曼滤波（QKF）的C代码实现姿态解算
热门推荐
凌桑的自我修养--You are the Eternity
06-19 2万+
0 引言 在捷联惯导工程实践[6]中，我们希望陀螺仪能够非常精确的获取信息，或者说希望陀螺仪能非常准确的地反映观测量（加速度，磁场等）[6,7]的真实值，但是这个过程或多或少是受到噪声干扰的，导致测量的不准确；为了能够让陀螺仪在状态更新时做到准确，必须对状态变量和观测量进行数据融合和滤波，从而尽最大限度的降低噪声的干扰。 最常用也最有效的方法非卡尔曼滤波莫属，其在处理高斯模型的系统上效果颇
四元数姿态解算C语言代码
02-26
四元数是一种高效表示旋转的方式，在姿态解算中非常有用，因为它可以避免万向节死锁并且更节省计算资源。下面是关于如何在C语言中利用四元数进行简单的姿态解算的一个简单例子： ```c #include <stdio.h> #include <math.h> // 定义四元数结构体 typedef struct { double w; double x; double y; double z; } Quaternion; // 函数用于将欧拉角转换成对应的四元数形式 void eulerAnglesToQuaternion(double roll, double pitch, double yaw, Quaternion* q) { // 角度转弧度并除以二 double cy = cos(yaw * 0.5); double sy = sin(yaw * 0.5); double cp = cos(pitch * 0.5); double sp = sin(pitch * 0.5); double cr = cos(roll * 0.5); double sr = sin(roll * 0.5); q->w = cr * cp * cy + sr * sp * sy; q->x = sr * cp * cy - cr * sp * sy; q->y = cr * sp * cy + sr * cp * sy; q->z = cr * cp * sy - sr * sp * cy; } int main() { Quaternion quaternion; // 示例输入的角度值 (单位: 弧度) double roll = M_PI / 4; // 绕X轴角度 double pitch = M_PI / 6; // 绕Y轴角度 double yaw = M_PI / 3; // 绕Z轴角度 // 执行从欧拉角到四元数的转换 eulerAnglesToQuaternion(roll, pitch, yaw, &quaternion); printf("根据给定的三个欧拉角得到对应四元数值为:\n"); printf("W:%f X:%f Y:%f Z:%f\n", quaternion.w, quaternion.x, quaternion.y, quaternion.z); return 0; } ``` 此段程序实现了基本的功能——把三维空间内的转动分解成为绕着固定坐标系各轴的一系列旋转变换，并通过构造一个四元数表达这种变换。该示例仅作为入门级演示用途；实际应用时还需考虑更多细节如传感器融合、噪声处理等。
评论 2
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

超维空间科技

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值