互补滤波算法姿态解算 共40页
基于四元数互补滤波的无人机姿态解算
基于四元数互补滤波的无人机姿态解算吕印新;肖前贵;胡寿松【期刊名称】《燕山大学学报》【年(卷),期】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）_⼀种互补滤波⽅法姿态解算⽅法（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微处理器，不适用于计算量大的姿态解算算法。
基于互补滤波器的四旋翼飞行器姿态解算
。 该过程要
用到载体坐标系到导航坐标系的旋转矩阵 R ， 本文采用方
r xx r xy r xz cos θ cos ψ r r r = cos θsin ψ Rn yx yy yz b = r zx r zy r zz － sin θ
－ ωx d t ． 1
ωy d t
（ 2）
在 d t 趋近于 0 时， 由式（ 2 ） 可得旋转矩阵的导数， 即 n b Rn = R ， （ 3） b b Ω nb
0 Ω = ωz ωx ． 0
yx xx zy zz
互补滤波器融合这三种传感器的数据， 提高测量精度和系 统的动态性能。 ^ 表示互 R 以 R 表示飞行器真实姿态的方向余弦矩阵， R0 代表由加速度 补滤波器计算输出的姿态方向余弦矩阵， u H 表示 传感器和数字罗盘观测到的姿态方向余弦矩阵， R O 的高频观测噪声， R c 代表由陀螺仪数据 则 Ro = R + uH ， u L 表示 R c 中的低频累积误差， 计算得到的姿态， 则 Rc = R + uL 。 取 GL （ s ） = C（ s） ， GH （ s ） = 1 － GL （ s ） = s + C（ s）
与目前常用的卡尔曼滤波算法相比采用互补滤波器算法的求解器能显著降低对处理器速度和精度的要求有效融合了捷联惯性测量单元的传感器数据实现了小型四旋翼飞行器的高精度姿态解算
56
传感器与微系统（ Transducer and Microsystem Technologies）
2011 年 第 30 卷 第 11 期
基于互补滤波器的四旋翼飞行器姿态解算
1 梁延德 ，程 1 1 敏 ，何福本 ，李
基于改进互补滤波算法的姿态解算
基于改进互补滤波算法的姿态解算摘要：本文主要是将可穿戴设备分别佩戴于运动员不同的部位，对由三个传感器采集的数据进行融合处理，求解高山滑雪运动员的动作姿态角变化。
由于每种仪器都有自己的优缺点，通过调整加速度计、磁力计采样得到的姿态角和陀螺仪采样得到的姿态角之间的权重，通过改进的互补滤波算法进行数据融合，求解出最终的姿态角，姿态角包含横滚角、俯仰角、偏航角三个角度，可以为我国高山滑雪运动员训练提供参考。
关键字:互补滤波算法；姿态解算；数据融合;姿态角。
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]。
互补滤波原理简单，能够滤去噪声和抑制漂移，确定合适的低通截止频率对加速度计采集到的数据进行预处理，操作简便，处理后的数据也能有效地完成四元数算法。
首先利用加速度计测得重力向量与估计姿态得到的重力向量的误差来矫正积分误差，然后利用矫正后的积分得到所需要的姿态角。
互补滤波算法姿态解算
地磁场向量 可不可以采用和加速度计一样的修正方法来修正？ 加速度计在静止时测量的是重力加速度(0,0,1)T ，是有大小和方向的； 同理，地磁计同样测量的是地球磁场的大小和方向，只不过这个方向不再是竖直向下 而是与x轴（或者y轴）呈一个角度，与z轴呈一个角度。记作(bx,by,bz)T ，这里我 们让x轴对准北边，所以by=0，即 (bx,0,bz)T 倘若我们知道bx和bz的精确值，那么我们就可以采用和加速度计一样的修正方法来)
23
01
q q q q 2
2
2
2
0
1
2
3
这样，四元数可以用方向余弦、欧拉角表示，同样，欧拉角也可以用 方向余弦和四元数表示。
用方向余弦表示四元数
对于小角度位移，四元数参数可以用表示为
q 0
1 2
1
(1c11c22c33) 2
q 1
1
4q
c c (
)
32
23
0
三、姿态表示方法
三、姿态表示方法
C 3.1 方向余弦矩阵 方向余弦矩阵用符号
n
b 表示，是一个3*3阶的矩阵，矩阵的列表示
载体坐标系中的单位矢量在参考坐标系中的投影。
三、姿态表示方法
C 3.1 方向余弦矩阵 方向余弦矩阵用符号
n
b 表示，是一个3*3阶的矩阵，矩阵的列表示
载体坐标系中的单位矢量在参考坐标系中的投影。
q 2
1
4q
c c (
)
13
31
0
q 3
1
4q
c c (
)
21
12
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
三、姿态表示方法
三、姿态表示方法
C 3.1 方向余弦矩阵 方向余弦矩阵用符号
n
b 表示，是一个3*3阶的矩阵，矩阵的列表示
载体坐标系中的单位矢量在参考坐标系中的投影。
三、姿态表示方法
C 3.1 方向余弦矩阵 方向余弦矩阵用符号
n
b 表示，是一个3*3阶的矩阵，矩阵的列表示
载体坐标系中的单位矢量在参考坐标系中的投影。
连续转动来实现。从参考系到一个新的坐标系的变换可以表示：
绕参考坐标系的z轴转动 角
绕新坐标系的y轴转动 角
绕新坐标系的x轴转动 角
称为欧拉转动角
3.3 四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标
系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单 次转动来实现。
为
q的虚部。
四元数的共轭为
1.2 四元数的表示方式
一、四元数
1.1 四元数定义
qq,qq qq q q 设
q
0
i1j2来自3k,, , R
01 2 3
其中，i，j，k满足
i^2=j^2=k^2=-1
ij=-ji=k, jk=-kj=i, ki=-ik=j
则称数q为四元数，而q0称为四元数q的实部，称
四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量 方向和转动大小的函数。
定义 的大小和方向是使参考系绕 标系重合。
转动一个角度 ，就能与载体坐
3.3 四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标
系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单
为
q的虚部。
四元数的共轭为
1.2 四元数的表示方式
一、四元数
1.1 四元数定义
qq,qq qq q q 设
q
0
i
1
j
2
3k,
, , R
01 2 3
其中，i，j，k满足
i^2=j^2=k^2=-1
ij=-ji=k, jk=-kj=i, ki=-ik=j
则称数q为四元数，
而q0称为四元数q的实部，
c c c n C c c c b
11 21
12 22
13 23
c c c 31
32
33
三、姿态表示方法
C 3.1 方向余弦矩阵 方向余弦矩阵用符号
n
b 表示，是一个3*3阶的矩阵，矩阵的列表示
载体坐标系中的单位矢量在参考坐标系中的投影。
c c c n C c c c b
则称数q为四元数，
而q0称为四元数q的实部，
称 qiqjqk 为q的虚部。
12
3
四元数的共轭为
qqqiqjqk
0
1
2
3
1.2 四元数的示方式
1.3 四元数运算
二、东北天坐标系
东北天坐标系（表示为n系）是一种当地地理坐标系，原点位于导航系统 所处的位置P点，坐标轴指向北、东和当地垂线方向（向下），也有称为 北东地坐标系
次转动来实现。 四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量
方向和转动大小的函数。
定义 的大小和方向是使参考系绕 标系重合。
转动一个角度 ，就能与载体坐
3.3 四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标
系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单
次转动来实现。 四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量
方向和转动大小的函数。
定义 的大小和方向是使参考系绕 标系重合。
转动一个角度 ，就能与载体坐
3.3 四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标
系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单
基于互补滤波AHRS 姿态解算算法介绍
Mini INS/GPS姿态仪
介绍内容：
1、四元数 2、姿态表示的方法 3、姿态解算原理
一、四元数
1.1 四元数定义
其中，i，j，k满足
i^2=j^2=k^2=-1
ij=-ji=k, jk=-kj=i, ki=-ik=j
则称数q为四元数，而q0称为四元数q的实部，称
称 qiqjqk 为q的虚部。
12
3
四元数的共轭为
1.2 四元数的表示方式
一、四元数
1.1 四元数定义
q q,q q qq q q 设 q i j k, , , R
01
2
3
01 2 3
其中，i，j，k满足
i^2=j^2=k^2=-1
ij=-ji=k, jk=-kj=i, ki=-ik=j
次转动来实现。 四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量
方向和转动大小的函数。
定义 的大小和方向是使参考系绕 转动一个角度 ，就能与载体坐
标系重合。
利用四元数进行矢量变换
首先定义一个四元数rb’ rb=ix+jy+kz rb’=0+ix+jy+kz
有：
rn’=q*rb’*q’ rn’=(q0+iq1+jq2+kq3)(0+ix+jy+kz)(q0-iq1-jq2-kq3)
称
为q的虚部。
四元数的共轭为
1.2 四元数的表示方式
一、四元数
1.1 四元数定义
q q,q q qq q q 设 q i j k, , , R
01
2
3
01 2 3
其中，i，j，k满足
i^2=j^2=k^2=-1
ij=-ji=k, jk=-kj=i, ki=-ik=j
则称数q为四元数，
而q0称为四元数q的实部，
=0+{(q0^2+q1^2-q2^2-q3^2)x+2(q1q2-q0q3)y+2(q1q3+q0q2)z}i +{2(q1q2+q0q3)x+(q0^2-q1^2+q2^2-q3^2)y+2(q2q3-q0q1)z}j +{2(q1q3-q0q2)x+2(q2q3+q0q1)y+(q0^2-q1^2-q2^2+q3^2)z}k
为
q的虚部。
四元数的共轭为
1.2 四元数的表示方式
一、四元数
1.1 四元数定义
qq,qq qq q q 设
q
0
i
1
j
2
3k,
, , R
01 2 3
其中，i，j，k满足
i^2=j^2=k^2=-1
ij=-ji=k, jk=-kj=i, ki=-ik=j
则称数q为四元数，而q0称为四元数q的实部，称
11 21
12 22
13 23
c c c 31
32
33
第i行、j列的元素表示参考坐标系i轴和载体坐标系j轴夹角的余弦。
在载体坐标系中定义的矢量 r b ，可以通过该矢量左乘方向余弦矩阵
r C r C n ，即 b
n
nb b
3.2 欧拉角 一个坐标系到另一个坐标系的变换，可以通过绕不同坐标轴的3次