下载文档原格式
基于四元数互补滤波的无人机姿态解算吕印新;肖前贵;胡寿松【期刊名称】《燕山大学学报》【年(卷),期】2014(000)002【摘要】针对无人机低成本姿态解算这一基本问题，考虑到传统姿态算法运算量大、不易调试，采用微惯性单元（MEMS）测量无人机原始姿态数据，采用基于四元数的互补滤波算法，有效降低姿态解算的运算量，实现MEMS各传感器的信息融合。
从理论上证明了基于四元数的互补滤波器的稳定性，分析了滤波器的性能。
采用无人机真实数据验证了算法的有效性，解算得到的俯仰角、滚转角精度小于1°，航向角精度小于2°。
与传统姿态算法比较，本算法简单有效、运算量小、易于调试。
%Aiming at the basic problem of attitude estimation of unmanned aerial vehicles, in consideration of the quality of the traditional algorithm which is complex and hard to debug, MEMS is used to measure the data of UAV primitive attitude. Quaternion complementary filtering algorithm is applied to reduce the operation quantity of attitude estimation and make the fuse of information collected by every sensor. The complementaryfilter stability based on quaternion is theoretically proved. And the qualityof filter was analysed here. The effectiveness of the algorithm used here is testified by real data from UAV. The precision of pitch and yaw are less than 1° and 2° respectively. Compared with traditional attitude algorithm, this algorithm is simple and effective, small computational complexity, and easy to debug.【总页数】6页(P175-180)【作者】吕印新;肖前贵;胡寿松【作者单位】南京航空航天大学自动化学院，江苏南京 210016;南京航空航天大学无人机研究院，江苏南京 210016;南京航空航天大学自动化学院，江苏南京210016【正文语种】中文【中图分类】V243.5【相关文献】1.基于ROS与四元数互补滤波的IMU姿态解算研究 [J], 曹旭东;胡越黎;王涛2.基于四元数互补滤波的小型四旋翼姿态解算 [J], 李二闯;张建杰;袁亮;吴金强3.基于四元数二阶互补滤波的四旋翼姿态解算 [J], 伏家杰;周翟和;尹辉;陈燕4.基于重力和磁场双重互补滤波的无人机姿态解算算法 [J], 王健;厉彦一5.基于四元数改进型互补滤波的MEMS姿态解算 [J], 陈孟元;谢义建;陈跃东因版权原因，仅展示原文概要，查看原文内容请购买。
姿态解算⽅法（1）_⼀种互补滤波⽅法姿态解算⽅法（1）_ ⼀种互补滤波⽅法本⽂内容主要是参考《An efficient orientation filter for inertial and inertial/magnetic sensor arrays 》此篇论⽂，由于其在普通飞控中的⼴泛运⽤，所以这⾥简单记录学习感受以期后续详细思索。
当然也是因为这篇⽐较易懂，正好可以锻炼⼀下我捉襟见肘的表述能⼒。
写到这⾥可能已经开始被骂了。
⼏个较好的博⽂链接如下：（1）（2）这是在博客园的第⼀篇博⽂，想来⾃⼰⼀直攒在电脑⾥的东西，不仅容易被弄没了，也没办法被⼤家纠错批评，故也就不在乎好坏把想到的都写下来。
⾔归正传，⾸先，需要了解下四元数的知识，这在维基（3）和（1）中都有很好的解释了，这⾥为了争取篇幅讲述算法核⼼部分就不再赘述。
（3）⾓加速度的事那么进⼊关于姿态确定的部分。
三轴陀螺仪模块可以测量传感器所在平⾯的⾓加速度，⼀般来说MPU6050模块返回的是三个⾓速度值。
如果初始姿态是已知的，时间的计量⼜是准确的，那么就可以积分出姿态了。
可惜的是⾓速度的测量准确度实在是不能积分出⼀个准确的位置值。
不过这⾥还是说⼀下这个积分过程。
sω=0ωxωyωzS E ˙q=12SEˆq⊗Sωsω为传感器获得的⾓速度值，再次提醒这⾥的值可能是不准确的。
SEˆq是表⽰传感器相对于地⾯坐标系的SO（3）变换四元数的估计，这⾥⽤于表⽰传感器姿态的估计。
⊗是四元数的⼀种运算⽅式，在上⽂的参考⽂献中可以找得到的。
差分的形式可以表⽰为：δS E qω,t=12SEˆqt−1⊗SωtSE qω,t=S E ˆqt−1+δSE qω,tΔtΔt是两次预测之间的时间差，准确的说是传感器上⼀次数据和这⼀次数据获取时间差。
其他的部分和连续的形式对应就不多说了。
基于场⽅向观测的姿态求解⾸先把姿态观测问题看做是⼀个求尽可能满⾜观测结果的姿态的优化问题。
蛇形机器人互补滤波和四元数的姿态解算王超杰;苏中;时佳斌;陈庚【摘要】针对蛇形机器人姿态解算问题,陀螺存在漂移特性,加速度计的测量值包含重力加速度和运动加速度,磁强计易受周围环境地磁干扰,并且蛇形机器人采用嵌入式微处理器,需要减少计算量.设计了用互补滤波器来实现惯性传感器的数据融合,用四元数进行姿态解算的方法.经过实验验证表明:采用互补滤波和四元数进行姿态解算能有效融合各个惯性传感器的数据,计算量小,能够满足蛇形机器人对精度和实时性的要求.【期刊名称】《传感器与微系统》【年(卷),期】2014(033)010【总页数】4页(P137-140)【关键词】蛇形机器人;姿态解算;互补滤波器;惯性传感器;四元数【作者】王超杰;苏中;时佳斌;陈庚【作者单位】北京信息科技大学高动态导航技术北京市重点实验室,北京100101;北京信息科技大学高动态导航技术北京市重点实验室,北京100101;北京信息科技大学高动态导航技术北京市重点实验室,北京100101;北京信息科技大学高动态导航技术北京市重点实验室,北京100101【正文语种】中文【中图分类】TP212蛇形机器人由于具有广阔的应用前景，使其成为了当前的研究热点。
其中，姿态检测是控制机器人在未知环境空间运动的关键问题之一，通过融合各种传感器的数据来获取机器人的运动姿态，进而实现对机器人的运动控制与路径规划。
姿态解算的精度和速度将直接影响对蛇形机器人的控制的稳定性和可靠性。
随着MEMS技术的发展与对成本因素的考虑，低成本的捷联惯性测量单元越来越多的被应用到移动机器人中,其主要包括陀螺仪、加速度计和磁强计[1]。
MEMS陀螺仪具有漂移特性，加速度计的测量值中同时包含了重力加速度和运动加速度，同时磁强计容易受到周围环境中外部磁场的干扰。
怎样有效地融合这些惯性传感器的数据，滤除外部干扰，得到高精度的姿态数据，是一项非常具有挑战性的工作。
同时，蛇形机器人采用STM32F103微处理器，不适用于计算量大的姿态解算算法。
基于改进互补滤波算法的姿态解算摘要：本文主要是将可穿戴设备分别佩戴于运动员不同的部位，对由三个传感器采集的数据进行融合处理，求解高山滑雪运动员的动作姿态角变化。
由于每种仪器都有自己的优缺点，通过调整加速度计、磁力计采样得到的姿态角和陀螺仪采样得到的姿态角之间的权重，通过改进的互补滤波算法进行数据融合，求解出最终的姿态角，姿态角包含横滚角、俯仰角、偏航角三个角度，可以为我国高山滑雪运动员训练提供参考。
关键字:互补滤波算法；姿态解算；数据融合;姿态角。
1引言可穿戴设备由加速度计、陀螺仪、磁力计组成，分别配戴于运动员的胸部、腰部、右大腿、左大腿、右足、左足6个部位，便可测得运动员这6个部位的三维九轴数据[1]。
各个传感器有着自己的优缺点，存在误差，利用比较合适的算法将多个传感器采集到的数据进行数据融合，是目前研究工作的重点[2]。
在目前的研究中，主要是采用陀螺仪为主体，对陀螺仪求解的姿态角“信任度”比较高[3]。
但是在求解过程中，陀螺仪在短时间内十分精准，但长期积分求解过程只出现积分累计误差，导致精度变差[4]。
在姿态解算的过程中，利用三个传感器之间的性质互补性，为了提高解算精度将数据进行融合，这也就成为姿态解算的研究的主要方向[5]。
2基于互补滤波算法的姿态解算通过加速度计求解横滚角、俯仰角，通过磁力计求解出偏航角，通过陀螺仪求解出横滚角、俯仰角、偏航角。
利用互补滤波算法将加速度计、磁力计、陀螺仪求解的姿态角进行数据融合，求解出最终的姿态角。
2.1利用加速度计得到横滚角与俯仰角当运动员处于静止时期，根据第二牛顿定律，运动员只受重力，加速度计测得的是重力加速度，设静止状态下加速度计测量到的加速度向量为，对做归一化处理，有。
是重力加速度向量，对做归一化处理，有，可得：(1)可得到横滚角与俯仰角：(2)可以求解出横滚角和俯仰角，为了方便区分和经陀螺仪得到的横滚角、俯仰角，将由加速度计计算得到的横滚角记作，将由加速度计计算得到的俯仰角记作。
单球驱动自平衡机器人的互补滤波姿态解算余义;章政【摘要】针对单球驱动自平衡机器人的低成本、高精度、实时姿态估计需求,设计一种基于比例-积分(PI)型互补滤波的姿态估计算法.在四元数姿态角更新方程的基础上,通过融合陀螺仪/加速度计的姿态信息实现对机器人水平姿态信息的估计,实现多传感器间频率点的切换.考虑到磁力计易受到干扰的情况,采用陀螺仪和磁力计单独对自平衡机器人的偏航角姿态解算,避免磁力计参与四元数姿态解算引起的水平姿态误差.实验结果表明,该互补滤波算法有效融合了陀螺仪的高频特性、加速度计和磁力计的低频特性,系统的姿态估计精度在±0.15°内.【期刊名称】《计算机工程与设计》【年(卷),期】2019(040)004【总页数】7页(P969-975)【关键词】单球驱动自平衡机器人;互补滤波;姿态解算;陀螺仪/加速度计/磁力计;四元数【作者】余义;章政【作者单位】武汉科技大学信息科学与工程学院,湖北武汉430081;武汉科技大学信息科学与工程学院,湖北武汉430081;武汉科技大学冶金自动化与检测技术教育部工程研究中心,湖北武汉430081【正文语种】中文【中图分类】TP270 引言近十年来，一类以单球体作为驱动轮的轮式自平衡机器人逐渐引起了国内外学者的关注[1-10]。
该类自平衡机器人利用万向轮与底部支撑球体的摩擦产生驱动力，实现机器人的自平衡、自旋以及全方位移动，即使在狭窄区域内机器人也能灵活移动。
根据单球驱动自平衡机器人(self-balancing robot driven by single ball, SRDSB)的动力学模型[8]分析可知，这一类机器人具有多变量、强耦合、欠驱动等非线性特征[11]。
在有效的轮球式驱动结构基础上，为了维持机器人的姿态角(俯仰、横滚和偏航)动态平衡稳定，且同时保证机器人快速、灵活的全向移动，需要精确有效的姿态解算方法实现机器人的姿态实时估计。
四元数互补滤波的机械手手势姿态解算吴聪;刘俊【摘要】针对远程操控机械手手势姿态解算这一问题,考虑到目前传统的姿态解算算法运算量大、调试过程复杂,通过对比分析欧拉角法、方向余弦法、四元数法的优缺点,采用基于四元数互补滤波解算手势姿态,融合传感器的数据信息有效地解算姿态,从而验证解算信息的有效性.实验中通过对航向角、横滚角、俯仰角的解算分析,角度误差均在士1°以内,满足控制误差,具有良好的实时性.【期刊名称】《上海电机学院学报》【年(卷),期】2018(021)003【总页数】5页(P13-17)【关键词】姿态解算;四元数法;互补滤波;实时性【作者】吴聪;刘俊【作者单位】上海电机学院电气学院,上海201306;上海电机学院机械学院,上海201306【正文语种】中文【中图分类】TP7在现代工业领域，远程控制的机械手得到了广泛应用。
基于多传感器融合的远程控制机械手已成为研究热点，在危险应急的场合有不可替代的作用。
基于多传感器融合控制机械手是利用传感器传输手部的参数信息，进而获取运动信息。
要完成对机械手的控制首先要获取传感器传输的控制信息,通过构建手势姿态运动系统来得到这一信息。
惯性导航系统是将惯性传感器直接固连在载体上[1]，手势姿态运动系统则是将传感器固定在手部的相应位置，利用计算机中的导航程序建立起姿态，通过惯性传感器得到载体信息的同时更新姿态矩阵和解算航姿参数，通过在机械手上的惯性器件(如加速度计,陀螺仪等)定时测定得到机械手的即时运动参数，通过姿态解算算法，解算出机械手的运动姿态。
被应用在控制器姿态角上的数据融合算法有互补滤波[2]、共轭梯度法滤波[3]、扩展卡尔曼滤波( EKF)[4]和无迹卡尔曼滤波(UKF)[5]。
互补滤波原理简单，能够滤去噪声和抑制漂移，确定合适的低通截止频率对加速度计采集到的数据进行预处理，操作简便，处理后的数据也能有效地完成四元数算法。
首先利用加速度计测得重力向量与估计姿态得到的重力向量的误差来矫正积分误差，然后利用矫正后的积分得到所需要的姿态角。
Python 互补滤波扩展卡尔曼滤波解算姿态一、介绍在航空航天领域以及其他相关领域，姿态解算是一个重要的问题。
姿态解算是指通过传感器（如陀螺仪、加速度计、磁力计等）采集到的数据，计算出飞行器或者其他对象的姿态（即俯仰、偏航、横滚角度）。
在实际的应用场景中，通常需要使用滤波算法对传感器数据进行处理，从而得到更加准确和稳定的姿态信息。
本文将介绍如何使用Python 编程语言实现互补滤波和扩展卡尔曼滤波算法，来解算姿态。
二、互补滤波算法1. 什么是互补滤波算法互补滤波算法是一种简单而有效的滤波算法，常用于姿态解算中。
它的原理很简单，即将两种不同的数据（通常是陀螺仪数据和加速度计数据）进行加权平均，从而得到更加稳定和准确的姿态信息。
2. 互补滤波算法的实现在 Python 中实现互补滤波算法非常简单。
我们需要获取陀螺仪和加速度计的原始数据。
我们可以使用如下的公式来计算互补滤波的输出：angle = alpha * (angle + gyroRate * dt) + (1 - alpha) * accAngle其中，angle 表示最终的姿态角度，gyroRate 表示陀螺仪的角速度，dt 表示采样时间间隔，accAngle 表示由加速度计计算得到的角度，alpha 表示权重系数。
3. 互补滤波算法的优缺点互补滤波算法具有简单、低成本、易实现的优点，适用于一些资源有限的场景。
但是它也有一些缺点，比如对参数的选择比较敏感，需要经过一定的调试和优化。
三、扩展卡尔曼滤波算法1. 什么是扩展卡尔曼滤波算法扩展卡尔曼滤波算法是卡尔曼滤波算法的一种扩展，常用于非线性系统的状态估计。
在姿态解算中，由于传感器的非线性特性，扩展卡尔曼滤波算法通常能得到更加准确的姿态信息。
2. 扩展卡尔曼滤波算法的实现扩展卡尔曼滤波算法涉及到一些复杂的数学推导和矩阵运算，在Python 中可以使用一些成熟的库来实现。
通常，我们需要将系统的动力学模型线性化，然后使用卡尔曼滤波算法进行状态估计。
姿态解算一、主线姿态暗示方法：矩阵暗示，轴角暗示，欧拉角暗示，四元数暗示。
惯性丈量单位IMU（Inertial Measurement Unit）：MPU6050芯片，包含陀螺仪和加速度计，辨别丈量三轴加速度和三轴角速度。
注意，传感器所测数据是原始数据，包含了噪声，无法直接用于遨游翱翔器的姿态解算，因此需要对数据进行滤波。
滤波算法：非线性互补滤波算法，卡尔曼滤波算法，Mahony 互补滤波算法。
二、知识点弥补加速度计和陀螺仪加速度计：加速度计,可以丈量加速度,包含外力加速度和重力加速度,因此，当被测物体在静止或匀速运动（匀速直线运动）的时候,加速度计仅仅丈量的是重力加速度,而重力加速度与R 坐标系（绝对坐标系）是固连的,通过这种关系,可以获得加速度计所在平面与空中的角度关系也就是横滚角和俯仰角。
把加速度传感器水平静止放在桌子上，它的Z轴输出的是1g的加速度。
因为它Z轴标的目的被重力向下拉出了一个形变。
可惜的是，加速度传感器不会区分重力加速度与外力加速度。
所以，当系统在三维空间做变速运动时，它的输出就不正确了，或者说它的输出不克不及标明物体的姿态和运动状态。
陀螺仪：陀螺仪丈量角速度。
陀螺仪模型如图1所示，陀螺仪的每个通道检测一个轴的旋转。
图1[引自网络]上图中，Rxz是R在XZ面上的投影，与Z轴的夹角为Axz。
Ryz是R在ZY面上的投影，与Z轴的夹角为Ayz。
陀螺仪就是丈量上面界说角度的变更率，换句话说，它会输出一个与上面这些角度变更率线性相关的值。
加速度计工作原理介绍（摘自网络）年夜大都加速度计可归为两类：数字和模拟。
数字加速度计可通过I2C，SPI或USART方法获取信息，而模拟加速度计的输出是一个在预定规模内的电压值，你需要用ADC（模拟量转数字量）模块将其转换为数字值。
不管使用什么类型的ADC模块，城市获得一个在一定规模内的数值。
例如一个10位ADC模块的输出值规模在01023间。
假设我们从10位ADC模块获得了以下的三个轴的数据：每个ADC模块都有一个参考电压，假设在我们的例子中，它是3.3V。
