AHRS互补滤波(Mahony)算法及开源代码
文章目录
- 前言
- 算法流程
- 算法步骤
- 算法难点
- 开源源码
- 参考文献
前言
AHRS(Attitude and heading reference system,也就是航姿参考系统。在互补滤波算法中传感器主要采用了IMU(陀螺仪、加速度计)和磁力计。
AHRS的基本思想是,在载体没有平移运动的情况下,通过加速度感知重力分量,可以计算出载体的俯仰和横滚;磁力计可以感知磁北方向,因此可以计算载体的磁北航向;而陀螺仪测量输出载体的旋转角速度,通过积分可以计算得到横滚、俯仰、航向增量,但由于陀螺输出值含有误差,采用积分计算,误差会随着时间累积。陀螺仪动态响应特性良好,但计算姿态时会产生累积误差, 磁力计和加速度计测量姿态没有累积误差,但动态响应较差,那么采用加速度计和磁力计的即时输出值对陀螺进行修正,则可以达到优势互补的效果,提高测量精度和系统的动态性能。
算法流程
整体流程如下图,只不过图里面的公式用的四元数做旋转,而不是旋转矩阵。
算法步骤
所用的导航系N(地理系)为北东地,机体系B为前右下。假设加速度计测量值为ax,ay,az,陀螺测量值为gx,gy,gz,磁力计测量值为mx,my,mz。
1. 初始化四元数
由于姿态角和四元数是可以相互计算的,因此可以首先根据已知的姿态,或者根据加速度计和磁力计计算出来的姿态初始化四元数,具体公式如下。
2. 计算加速度计修正量
由四元数表示的从导航系到机体系的旋转矩阵为:
由于我用的导航系N是北东地,机体系为前右下,如果加速度计坐标系与机体系一致,载体静止且水平时加速度计测量值应为[0,0,-1],Z方向为负(由于加速度计测的实际是1g的支持力,而非重力)。利用Cnb矩阵将地球矢量转到机体系前右下,得到vx, vy, vz。
将vx,vy,vz与实际加速度计输出(位于机体系中)求向量积,即为误差修正量:
3. 计算磁力计修正量
如果不考虑误差,磁力计的测量值经过正确Cbn转到地理系,理论上水平方向上只有北向有值,因此地球磁场的切线在南北方向上。但实际上由于航向不准,造成Cbn有误差,所以转换后的磁力计测量值在北向和东向都有值。
互补滤波算法中,先将磁力计的值用Cbn转到地理系:
由于在水平方向,无论Cbn在航向上有没有误差,转换后水平方向矢量和应该相等。因此可以得到如果航向正确的情况下,通过Cbn转换后的磁力计为[bx, 0, bz] = [sqrt(hx * hx + hy * hy), 0, hz],再将这个值转到机体系下:
求向量积:
4. 修正陀螺仪输出值
根据pi调节,设置对陀螺测量值的修正量:
对陀螺仪测量值进行修正:
后面就按照正常四元数更新算法进行计算。
算法难点
我感觉难点在于怎么设置kp,ki这两个值,可能需要比较多的PID调节的经验和测试数据。
在参考【1】中提到:“关于这一块,现在研究的比较多就是如何实现自适应调参。固定的参数不能获得所有情况下的最优运动姿态角,可以设计参数可调的自适应算法在不同运动状态下进行调节参数的大小。其参数调节规则为:正常运动状态情况下,Kp和Ki值取为系统初始化值;当运动体具有较大运动加速度或姿态变化剧烈时,应选择较小的Kp值(可取其初始化值的0.1倍),而Ki值应在同一数量级内适当取大一点。具体取值需根据实际应用系统选取。”
严恭敏老师的博客最简单的航姿仪算法C程序(AHRS)中设置的kp,ki值为:
void MahonyInit(float tau)
{float beta = 2.146f/tau;Kp = 2.0f*beta, Ki = beta*beta;q0 = 1.0f, q1 = q2 = q3 = 0.0f;qua2cnb();exInt = eyInt = ezInt = 0.0f;tk = 0.0f;
}
Aceinna开源代码中设置的kp,ki值为:
def __init__(self):'''vars'''# algorithm descriptionself.input = ['fs', 'gyro', 'accel']#, 'mag']self.output = ['att_quat', 'wb', 'ab']self.batch = Trueself.results = Noneself.quat = Noneself.wb = Noneself.ab = None# algorithm vars# configself.innovationLimit = 0.1self.kp_acc_high = 1self.kp_acc_low = 0.01self.ki_acc_high = 0.5self.ki_acc_low = 0.001# stateself.ini = 0 # indicate if attitude is initializedself.dt = 1.0 # sample period, secself.q = np.array([1.0, 0.0, 0.0, 0.0]) # quaternionself.err_int = np.array([0.0, 0.0, 0.0]) # integral of errorself.kp_acc = 1self.ki_acc = 0.001self.gyro_bias = np.array([0.0, 0.0, 0.0])self.tmp = np.array([0.0, 0.0, 0.0])
开源源码
下面是x-IMU关于互补滤波的开源代码(具体参见Open-Source-AHRS-With-x-IMU)因为使用的坐标系不同,所以重力加速度分量的正负不太一样:
public MahonyAHRS(float samplePeriod, float kp, float ki){SamplePeriod = samplePeriod;Kp = kp;Ki = ki;Quaternion = new float[] { 1f, 0f, 0f, 0f };eInt = new float[] { 0f, 0f, 0f };}public void Update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz){float q1 = Quaternion[0], q2 = Quaternion[1], q3 = Quaternion[2], q4 = Quaternion[3]; // short name local variable for readabilityfloat norm;float hx, hy, bx, bz;float vx, vy, vz, wx, wy, wz;float ex, ey, ez;float pa, pb, pc;// Auxiliary variables to avoid repeated arithmeticfloat q1q1 = q1 * q1;float q1q2 = q1 * q2;float q1q3 = q1 * q3;float q1q4 = q1 * q4;float q2q2 = q2 * q2;float q2q3 = q2 * q3;float q2q4 = q2 * q4;float q3q3 = q3 * q3;float q3q4 = q3 * q4;float q4q4 = q4 * q4; // Normalise accelerometer measurementnorm = (float)Math.Sqrt(ax * ax + ay * ay + az * az);if (norm == 0f) return; // handle NaNnorm = 1 / norm; // use reciprocal for divisionax *= norm;ay *= norm;az *= norm;// Normalise magnetometer measurementnorm = (float)Math.Sqrt(mx * mx + my * my + mz * mz);if (norm == 0f) return; // handle NaNnorm = 1 / norm; // use reciprocal for divisionmx *= norm;my *= norm;mz *= norm;// Reference direction of Earth's magnetic fieldhx = 2f * mx * (0.5f - q3q3 - q4q4) + 2f * my * (q2q3 - q1q4) + 2f * mz * (q2q4 + q1q3);hy = 2f * mx * (q2q3 + q1q4) + 2f * my * (0.5f - q2q2 - q4q4) + 2f * mz * (q3q4 - q1q2);bx = (float)Math.Sqrt((hx * hx) + (hy * hy));bz = 2f * mx * (q2q4 - q1q3) + 2f * my * (q3q4 + q1q2) + 2f * mz * (0.5f - q2q2 - q3q3);// Estimated direction of gravity and magnetic fieldvx = 2f * (q2q4 - q1q3);vy = 2f * (q1q2 + q3q4);vz = q1q1 - q2q2 - q3q3 + q4q4;wx = 2f * bx * (0.5f - q3q3 - q4q4) + 2f * bz * (q2q4 - q1q3);wy = 2f * bx * (q2q3 - q1q4) + 2f * bz * (q1q2 + q3q4);wz = 2f * bx * (q1q3 + q2q4) + 2f * bz * (0.5f - q2q2 - q3q3); // Error is cross product between estimated direction and measured direction of gravityex = (ay * vz - az * vy) + (my * wz - mz * wy);ey = (az * vx - ax * vz) + (mz * wx - mx * wz);ez = (ax * vy - ay * vx) + (mx * wy - my * wx);if (Ki > 0f){eInt[0] += ex; // accumulate integral erroreInt[1] += ey;eInt[2] += ez;}else{eInt[0] = 0.0f; // prevent integral wind upeInt[1] = 0.0f;eInt[2] = 0.0f;}// Apply feedback termsgx = gx + Kp * ex + Ki * eInt[0];gy = gy + Kp * ey + Ki * eInt[1];gz = gz + Kp * ez + Ki * eInt[2];// Integrate rate of change of quaternionpa = q2;pb = q3;pc = q4;q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * SamplePeriod);q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * SamplePeriod);q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * SamplePeriod);q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * SamplePeriod);// Normalise quaternionnorm = (float)Math.Sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);norm = 1.0f / norm;Quaternion[0] = q1 * norm;Quaternion[1] = q2 * norm;Quaternion[2] = q3 * norm;Quaternion[3] = q4 * norm;}
参考文献
【1】基于AHRS的三类姿态解算算法对比(含代码)-基于手机端惯性传感器的航迹推算算法(下)
【2】Pixhawk-姿态解算-互补滤波
【3】Pixhawk之姿态解算篇(4)_补充篇
【4】最简单的航姿仪算法C程序(AHRS)
【5】Open-Source-AHRS-With-x-IMU
【6】Aceinna gnss_ins-sim
【7】PID控制算法原理(抛弃公式,从本质上真正理解PID控制)
AHRS互补滤波(Mahony)算法及开源代码相关推荐
- GNSS算法相关开源代码(含多传感器融合相关项目)
开源代码总览 名称 传感器类型 组合类型 滤波方法 其余相关 RTKLIB GNSS - 卡尔曼滤波 GAMP/rtklibexplorer GPSTK GNSS - 卡尔曼滤波 BNC GNSS - ...
- php 区块链算法_PoW/BFT等5种主流区块链共识算法的开源代码实现
共识算法是实现自主产权区块链的必不可少的关键环节,本文列出社区中相对成熟的区块链共识算法开源实现,包括BFT共识.Raft共识.Paxos共识.PoW共识等,可供希望开发自主产权区块链的团队参考学习. ...
- STM32F103之实验7一阶互补滤波求解移动机器人的姿态代码
上一篇博客,采用的是DMP解算姿态,且给出了底层硬件配置及驱动方式,此处摒弃DMP直接由单片机自己实现姿态解算,接下来介绍由三轴陀螺仪和加速度计的值来使用四元数软件解算姿态的方法. 我们先来看看如何用 ...
- 学生心理学优化(SPBO)算法(含开源代码)
先做一个声明:文章是由我的个人公众号中的推送直接复制粘贴而来,因此对智能优化算法感兴趣的朋友,可关注我的个人公众号:启发式算法讨论.我会不定期在公众号里分享不同的智能优化算法,经典的,或者是近几年提出 ...
- Mahony算法 AHRS系统
Mahony算法 AHRS系统 简介 1.陀螺仪 1.1 零点漂移 1.2 白噪声 1.3 温度/加速度计影响 1.4 积分误差 2.加速度计 3.坐标系定义 4.算法流程 4.1 初始状态确定 4. ...
- stm32 MPU6050 姿态解算 Mahony互补滤波算法
文章目录 0.介绍 1,理论分析 1.1 MPU6050 1.2 Mahony算法原理 2,代码实现 1.1 MPU6050初始化及数据读取 1.2 Mahony算法c语言实现 1.3 将代码移植到你 ...
- 基于RflySim平台的mahony(含磁罗盘)互补滤波在pixhawk仿真及实物实验(带实验数据)
写在前面 本案例实验采用RflySim平台,该平台可以高效快速编写代码,使用simulink模型搭建,可以见代码直接生成对应的C代码,并一键将代码烧录Pixhawk中,是一种快速开发平台,RflySi ...
- 四旋翼姿态解算——互补滤波算法及理论推导
转载请注明出处:http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/ 文章链接:http://blog.csdn.net/hongbin_xu ...
- 四轴之互补滤波与四元数算法简单分析
有人问我关于四元数姿态解算算法的分析,每次都解释好久,今日空闲,特发一帖,供大家参考.本分析将结合程序,分析姿态解算思路,由于能力有限,难免有错误之处请谅解,同时希望能够抛砖引玉,得到大神指点.感谢圆 ...
- 一条命通关,这个AI算法玩超级马里奥操作秀翻天丨视频+开源代码
郭一璞 发自 北四环 量子位 报道 | 公众号 QbitAI 把超级马里奥玩成下面这样,算什么水平? 能流畅的行走在妖魔鬼怪之间 能掐准食人花出现的时机 能灵巧的躲过烧火棍 能克服各种变态的地形 从 ...
最新文章
- Go 读取 yaml 文件并解析
- ajax获取数据用弹窗显示_Vue之 点击返回弹出推荐商品弹窗
- 学习路上遇到的Error2
- C语言 指针与字符串
- Expression Blend实例中文教程(12) - 样式和模板快速入门Style,Template
- java session使用_Nginx+tomcat实现session共享
- 点云的无序性_三维点云分类与分割-PointNet
- 使用R语言中的spgwr包进行GWR模型的相关运算
- 高速局域网文件传输工具(速度可达20M) 的企业云盘
- php调用itchat,itchat接口使用示例
- 联想硬盘保护系统计算机名,联想硬盘保护系统模式之间的切换方法
- Android版微信跳一跳小游戏如何利用技术手段达到高分!
- 数据库原理必背简答题【计算机考研复试】
- 公民住宅权不可侵犯!为阻强拆致人重伤,属正当防卫
- Mini MP3 Player播放器简介与STC12例程
- Y430刷新BIOS
- vimdiff简单使用
- NOI2016铜色记
- HTTP学习笔记(适合初学)2
- 自由轴法 matlab,自由轴法与固定轴法 三亿文库