姿态解算 四元数、方向余弦、欧拉角、Mahony滤波、四轴
姿态解算 四元数、方向余弦、欧拉角、Mahony滤波
说明:本文只是做了一些总结,需要一些对这方面的基础概念的了解。
一般人千万不要试图去深入探讨四元数
1. 方向余弦矩阵
方向余弦矩阵是使用欧拉角(pitch,roll,yaw)对机体坐标系(b系)和地理坐标系(R系)的旋转的描述,也就是说,一个机体坐标系的向量,乘上这个方向余弦矩阵,就可以转化为一个地理坐标系的向量(对调也一样,就是这个意思)。也就是说方向余弦矩阵是对坐标系旋转的一种描述。
方向余弦矩阵如下:
其中,ψ是绕Z轴旋转的角,也是yaw角;θ是绕Y轴旋转的角,也是pitch角;φ是绕X轴旋转的角,也是roll角
显然,姿态解算的终极目的是得到这三个角度,也就是欧拉角。
2. 四元数旋转矩阵
四元数可以用来描述坐标系的旋转,因为各种各样的原因,比如运算速度、死锁等,用四元数去解算出欧拉角是更加方便的办法。
四元数旋转矩阵:
这个矩阵与方向余弦矩阵是等价的,知道了了四元数旋转矩阵,通过对应关系就可以求出欧拉角了,姿态解算绝大部分都是对四元数、角速度、加速度进行操作,只有在最后一步才将两者对应起来求欧拉角。
3. 迭代求解四元数(四元数微分方程求解)
需要深入了解四元数后才知道这个微分方程怎么来的,而且还运用了龙格库塔法解微分方程才能得到以下迭代求解形式,一般来说知道这个形式就可以了,会用即可。总之,利用四元数微分方程+龙格库塔法可以得到下式:
最为关键的是,这个迭代求解的形式居然会引入三轴角速度wx、wy、wz(也就是陀螺仪的数据)。即便没有深入知道四元数,但是知道四元数可以看做是一种用来描述坐标旋转的方法,旋转跟角速度扯上关系也是符合感觉的。四元数的初始值不会影响迭代,取0即可。注意这里存在一个近似的角速度积分的形式。
由于陀螺仪存在温漂,就是说角速度的数据总会比实际的数据大一点或者小一点,温漂导致这个大多少小多少是不确定的,有时会一直大一点,有时会一直小一点,有时大一会儿小一会儿交替地来,所以长期这样积分会导致误差越来大,必须消除这种误差才能保证四元数长期正确性。并且陀螺仪角速度的累计误差会反映到四元数中。
4. 叉积补偿滤波(或称互补滤波)
这里就是Mahony所做的贡献,巧妙的利用了加速度计的数据、地球重力G和四元数的旋转矩阵、叉积、PI控制器来将陀螺仪的误差给消除掉,计算量非常小,并且通过四元数的旋转特性将滤波完美地融入到四元数姿态解算的整个过程中。
重力加速度G是一直存在的,并且加速度计测量的数据就是重力加速度的分量(忽略芯片自身的加速度),G所处的坐标系显然是地理坐标系。如果求出了四元数,那么就可以通过四元数将地理坐标系下的重力向量(0,0,G)旋转到机体坐标系中,这个应该就是机体的加速度(注意,这时跟加速度计测量无关)。
此外,我们还有加速度计这个传感器可以比较准确的测量到机体的加速度。加速度计的特点是静态性能好,比如如果加速度计时静止的,那么直接利用加速度计测量的加速度就可以准确地测量到角度了,但是加速度计动态高频特性不理想。这里我想表达的是,从长期来看,加速度计的数据是非常准确地,并且加速度计的数据几乎不会有温漂的影响。所谓互补就是说陀螺仪的短期积分跟准确,加速度的长期数据跟准确,综合取两者之长来确定最终结果。
这样我们就有了两种加速度的数据了,一个加速度计的实测值,另一个是可以说是利用旋转的预测值(预测值这个词其实不是很合适),陀螺仪的角速度积分漂移产生的误差也体现在了这个预测值中,通过求出预测值与实测值的误差,将这个误差补偿到新的角速度数据中就可以消除角速度的温漂。求误差的方式是实测值和预测值两向量的叉积,补偿的方式是PI控制器
通过旋转求出重力加速度在机体坐标系的分量:
注意这里的旋转矩阵是地理坐标系到机体坐标系的旋转矩阵,其实是机体坐标系到地理坐标系旋转矩阵的转置。这里的是1还是用9.8不影响,因为后面还会用到PI控制器。
为什么可以用叉积求出误差?两个向量如果是完全重合的,其叉积结果的模是0,如果有夹角,则其模不会是0,并且夹角越大,其模也会越大。测量值与重力G旋转过来的值得误差为:
ex=ay∗vbz−az∗vbyey=az∗vbx−ax∗vbzez=ax∗vby−ay∗vbx{e_x} = {a_y}*{v_{bz}} - {a_z}*{v_{by}} \\ {e_y} = {a_z}*{v_{bx}} - {a_x}*{v_{bz}} \\ {e_z} = {a_x}*{v_{by}} - {a_y}*{v_{bx}} ex=ay∗vbz−az∗vbyey=az∗vbx−ax∗vbzez=ax∗vby−ay∗vbx
三维向量叉积使用行列式运算非常方便。其中,ax、ay、az是加速度计测量的数据,ex、ey、ez是两者在三轴上的误差,然后再将通过PI控制补偿到陀螺仪测得的角速度当中,为什么可以这么补偿就完成了滤波前面已经说清楚了,这是最难理解的一个点。
ExSum += KI*ex;
EySum += KI*ey;
EzSum += KI*ez;
//忽略了积分封顶,其实很多情况下只用P控制就可以了,自己去实践
gx += KP*ex + ExSum; //gx gy gz原值是陀螺仪测量的角速度数据
gy += KP*ey + EySum;
gz += KP*ez + EzSum; //KP的范围非常大,这个参数非常好调
这样就得到了补偿后的角速度数据,然后就迭代求四元数微分方程就可以了。整个过程看源码吧
可以结合B站上一个视频看,这个视频讲的很好:https://www.bilibili.com/video/BV1px411p71z
后记:
- 其实用加速度计得到的数据直接做个反三角就可以得到角度了,但是为什么不用?一般对实时性要求不高的应用用三轴加速度计就可以得到倾角了,但是加速度的响应速度不够快!不能直接将其用在无人机等对实时数据要求非常高的场合当中
- mahony滤波这个滤波不是平滑操作!而是借助加速度计的数据校正陀螺仪的低频误差(温漂),还应该使用低通滤波进行平滑(可以使用MPU6050内部的低通滤波器,看手册)。如果自己做低通的话务必考虑低通给数据带来的延迟!必须满足高速实时
姿态解算 四元数、方向余弦、欧拉角、Mahony滤波、四轴相关推荐
- 姿态解算基础:欧拉角、方向余弦、四元数
什么是姿态解算: 飞行器的姿态解算过程涉及到两个坐标系,一个是运载体的机体坐标系,该坐标系与运载体固连,当运载体转动的时候,这个坐标系也跟着转动,我们假设运载体的坐标系为b系.另外一个是地理坐标系,即 ...
- 一阶高通滤波+二阶Mahony滤波的四元数姿态解算
此次实验我使用icm20602进行 icm20602输出有以下特点: 3轴陀螺仪可选量程有±250dps,±500dps,±1000dps,±2000dps.(dps:degrees per seco ...
- stm32 MPU6050 姿态解算 Mahony互补滤波算法
文章目录 0.介绍 1,理论分析 1.1 MPU6050 1.2 Mahony算法原理 2,代码实现 1.1 MPU6050初始化及数据读取 1.2 Mahony算法c语言实现 1.3 将代码移植到你 ...
- 无人机姿态解算:四元数及其与欧拉角的转换
无人机姿态解算:四元数及其与欧拉角的转换 引言:获得无人机飞行时的飞行姿态对于无人机稳定控制来说至关重要.无人机主要通过传感器数据融合来进行状态估计,常用于无人机的传感器包括:MPU(包含了三轴加速度 ...
- 【51单片机快速入门指南】4.3.3: MPU6050使用Mahony AHRS算法实现六轴姿态融合获取四元数、欧拉角
目录 源码 Mahony_6.c Mahony_6.h 使用方法 测试程序 main.c 效果 STC89C516 32MHz Keil uVision V5.29.0.0 PK51 Prof.Dev ...
- 基于STM32F407四旋翼无人机 --- 姿态解算讲解(四元数)(叉积法融合陀螺仪数据和加速度数据)(五)
基于STM32F407四旋翼无人机 --- 姿态解算讲解(五) 姿态解算 姿态解算定义 欧拉角 四元数 四元数性质 方向余弦矩阵 四元数方向余弦矩阵 叉积法融合陀螺仪数据和加速度数据 叉积运算 一阶龙 ...
- 四旋翼姿态解算——互补滤波算法及理论推导
转载请注明出处:http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/ 文章链接:http://blog.csdn.net/hongbin_xu ...
- 四元数姿态解算算法基础
文章目录 姿态的表示方法 四元数表示姿态的物理意义 使用四元数进行载体姿态更新方程 四元数微分方程 四元数初始值确定 姿态的表示方法 载体姿态有多种表示方法,常见的三种:欧拉角,姿态矩阵,四元数. 欧 ...
- 一、Mahony姿态解算——坐标系变换
坐标系 在飞行器姿态解算中,通常以地球坐标系(E系)为绝对坐标系,以机体坐标系(B系)为相对坐标系,并通常设初始状态时E系和B系重合. 坐标系变换 我们将空间中某一向量从一个坐标系,通过坐标系变换,映 ...
最新文章
- Redis 桌面管理工具 RedisDesktopManager 2019.0 发布
- [转载]利用@media screen实现网页布局的自适应,@media screen and
- Ubuntu 16.04 下安装VMware Tools(三行命令搞定,亲测好使)
- php实现电话拨打,jquery mobile实现拨打电话功能的几种方法_jquery
- 2年质保期刚过就翘辫子,这是什么骚设计?
- 执​行​o​r​a​c​l​e​函​数​的​四​种​方​法
- Simply Fortran 保姆级教程
- 关于PCBLayout的一些具体细节的认识(能力有限,请大家多多指点)
- 微信公众号网页分享设置及问题
- 百度 orc识别图片转文字
- 搜狗云输入法,实现原理.
- Gitea v1.17.0 正式发布 | 集成软件包管理器、容器镜像仓库
- Mac版kettle
- Youtube 评论数
- ngx-datatable的使用
- 【arcgis 批量将属性表批量导出成excel】
- 模拟QQ软件的基于多线程的流媒体加密传输软件技术
- treecnt 51Nod - 1677
- mp4视频文件损坏的修复方法
- Altium Designer-打开丝印层、位号,不显示器件位号问题