第三讲 AHRS姿态解算
AHRS是自动航向基准系统(Automatic Heading Reference System)的简称。目前,使用四元数来进行AHRS姿态解算的算法被广泛采用于四轴飞行器上。该算法源自英国Bristol大学的Ph.D Sebastian Madgwick,他在2009年开发并发布了该算法。下面我们来对该算法的代码进行详细分析。
我们首先来看IMU部分。IMU是惯性测量装置(Inertial Measurement Unit)的简称,通常包含陀螺仪和加速度计。陀螺仪测量的是角速度,即物体转动的速度,把速度和时间相乘,即可以得到某一时间段内物体转过的角度。加速度计测量的是物体的加速度,我们知道,重力加速度是一个物体受重力作用的情况下所具有的加速度。当物体处于静止状态时,加速度计测量出来的值就等于重力加速度1g, 约等于9.8米每平方秒。重力加速度g的方向总是竖直向下的,通过获得重力加速度在其X轴,Y轴上的分量,我们可以计算出物体相对于水平面的倾斜角度。典型的IMU惯性测量芯片为MPU6050,它被广泛采用在四轴飞行器上。
对于陀螺仪的角速度测量,大家比较好理解,简单来说,相当于一个人绕着一个圆圈行走,假如他的速度是1度没秒,那么通过速度乘以时间,我们就可以知道他距离起点走了多少度。
那么我们如何理解用加速度计来测量倾角呢?一个简单的例子如下: 一个单轴的加速计位于重力水平面上的时候,它在垂直方向上受到的加速度为1g,在水平方向上受到的加速度为0。当我们把它旋转一个角度的时候,就会在水平轴上产生一个加速度分量。通过它们的关系,就可以计算出该单轴加速计的倾角。
gravity
在一个三轴的加速度传感器中,可以通过下列的反正切函数运算公式来计算加速度计各个轴的倾角。
gravity_xyz
另外,我们还要注意各传感器的方向。下图是MPU6050和MPU9150的传感器方向定义,其中MPU6050只包含陀螺仪和加速计共六个轴,而MPU9150还包含磁力计,共九个轴。从MPU6050和MPU9150获得的数据可以直接在下面的算法中使用,而不需要做符号变换。
imu_dir
现在,让我们回到姿态解算代码的分析。下述代码的核心思想是:通过陀螺仪的积分来获得四轴的旋转角度,然后通过加速度计的比例和积分运算来修正陀螺仪的积分结果。下面代码中的gx,gy,gz分别代表陀螺仪在X轴,Y轴和Z轴三个轴上的分量,ax,ay,az分别代表加速度计在在X轴,Y轴和Z轴三个轴上的分量。
void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float halfvx, halfvy, halfvz;
float halfex, halfey, halfez;
float qa, qb, qc;
如果加速计各轴的数均是0,那么忽略该加速度数据。否则在加速计数据归一化处理的时候,会导致除以0的错误。
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
把加速度计的数据进行归一化处理。其中invSqrt是平方根的倒数,使用平方根的倒数而不是直接使用平方根的原因是使得下面的ax,ay,az的运算速度更快。通过归一化处理后,ax,ay,az的数值范围变成-1到+1之间。
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
根据当前四元数的姿态值来估算出各重力分量。用于和加速计实际测量出来的各重力分量进行对比,从而实现对四轴姿态的修正。
// Estimated direction of gravity and vector perpendicular to magnetic flux
halfvx = q1 * q3 – q0 * q2;
halfvy = q0 * q1 + q2 * q3;
halfvz = q0 * q0 – 0.5f + q3 * q3;
使用叉积来计算估算的重力和实际测量的重力这两个重力向量之间的误差。
// Error is sum of cross product between estimated and measured direction of gravity
halfex = (ay * halfvz – az * halfvy);
halfey = (az * halfvx – ax * halfvz);
halfez = (ax * halfvy – ay * halfvx);
把上述计算得到的重力差进行积分运算,积分的结果累加到陀螺仪的数据中,用于修正陀螺仪数据。积分系数是Ki,如果Ki参数设置为0,则忽略积分运算。
// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
把上述计算得到的重力差进行比例运算。比例的结果累加到陀螺仪的数据中,用于修正陀螺仪数据。比例系数为Kp。
// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}
通过上述的运算,我们得到了一个由加速计修正过后的陀螺仪数据。接下来要做的就是把修正过后的陀螺仪数据整合到四元数中。
// Integrate rate of change of quaternion
gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx – qc * gy – q3 * gz);
q1 += (qa * gx + qc * gz – q3 * gy);
q2 += (qa * gy – qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy – qc * gx);
把上述运算后的四元数进行归一化处理。得到了物体经过旋转后的新的四元数。
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
在一个三维的空间内,由于重力加速度的存在,加速度计为我们提供了一个水平位置的绝对参考,但是它无法给我们提供一个方向的参考。这时候,我们首先考虑到的是指南针,磁力计就是这样的一个传感器,它给人们提供了一个正北方向的绝对参考。在四轴中常使用的地磁传感器是HMC5883L。
有了上面IMU惯性测量姿态解算的基础,我们就能很容易理解AHRS姿态解算的代码。它是在IMU惯性测量的基础上加入了地磁姿态的解算。下面代码中的gx,gy,gz分别代表陀螺仪在X轴,Y轴和Z轴三个轴上的分量,ax,ay,az分别代表加速度计在在X轴,Y轴和Z轴三个轴上的分量。mx,my,mz分别代表地磁在在X轴,Y轴和Z轴三个轴上的分量。
void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float recipNorm;
float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
float hx, hy, bx, bz;
float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
float halfex, halfey, halfez;
float qa, qb, qc;
如果地磁传感器各轴的数均是0,那么忽略该地磁数据。否则在地磁数据归一化处理的时候,会导致除以0的错误。
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);
return;
}
如果加速计各轴的数均是0,那么忽略该加速度数据。否则在加速计数据归一化处理的时候,会导致除以0的错误。
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
把加速度计的数据进行归一化处理。
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
把地磁的数据进行归一化处理。
// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
预先进行四元数数据运算,以避免重复运算带来的效率问题。
// Auxiliary variables to avoid repeated arithmetic
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// Reference direction of Earth’s magnetic field
hx = 2.0f * (mx * (0.5f – q2q2 – q3q3) + my * (q1q2 – q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f – q1q1 – q3q3) + mz * (q2q3 – q0q1));
bx = sqrt(hx * hx + hy * hy);
bz = 2.0f * (mx * (q1q3 – q0q2) + my * (q2q3 + q0q1) + mz * (0.5f – q1q1 – q2q2));
根据当前四元数的姿态值来估算出各重力分量Vx,Vy,Vz和各地磁分量Wx,Wy,Wz。
// Estimated direction of gravity and magnetic field
halfvx = q1q3 – q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 – 0.5f + q3q3;
halfwx = bx * (0.5f – q2q2 – q3q3) + bz * (q1q3 – q0q2);
halfwy = bx * (q1q2 – q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f – q1q1 – q2q2);
使用叉积来计算重力和地磁误差。
// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex = (ay * halfvz – az * halfvy) + (my * halfwz – mz * halfwy);
halfey = (az * halfvx – ax * halfvz) + (mz * halfwx – mx * halfwz);
halfez = (ax * halfvy – ay * halfvx) + (mx * halfwy – my * halfwx);
把上述计算得到的重力和磁力差进行积分运算,
// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
把上述计算得到的重力差和磁力差进行比例运算。
// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}
把由加速计和磁力计修正过后的陀螺仪数据整合到四元数中。
// Integrate rate of change of quaternion
gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx – qc * gy – q3 * gz);
q1 += (qa * gx + qc * gz – q3 * gy);
q2 += (qa * gy – qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy – qc * gx);
把上述运算后的四元数进行归一化处理。得到了物体经过旋转后的新的四元数。
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
第三讲 AHRS姿态解算相关推荐
- 四元数AHRS姿态解算和IMU姿态解算分析
ref:https://blog.csdn.net/xiaoxie613520/article/details/78227170 AHRS是自动航向基准系统(Automatic Heading Ref ...
- 【惯性导航姿态仪】 04 -Mini AHRS 姿态解算说明
[惯性导航姿态仪] 04 -Mini AHRS 姿态解算说明 引言 加速度计 陀螺仪测量什么? 从ADC值到 dps 参考链接:( MiniIMU AHRS 姿态板销售网址:) http://chip ...
- AHRS姿态解算说明(加速度+陀螺仪+磁力计原理及原始数据分析)
转载链接:http://www.51hei.com/bbs/dpj-92911-1.html AHRS俗称航姿参考系统,AHRS由加速度计,磁场计,陀螺仪构成,AHRS的真正参考来自于地球的重力场和地 ...
- Pixhawk代码分析-姿态解算篇B
姿态解算篇B 前言 本篇博文主要是以mahony的算法为基础理解姿态解算的过程,主要参考的论文就是William Premerlani and Paul Bizard的关于DCM的一篇经典论文< ...
- Pixhawk之姿态解算篇(2)_mahony算法分析
一.开篇 还是没能进入到源码部分研究,对姿态解算过程太过于模糊,所以主要开始研究一下关于姿态解算的过程和实现,本篇博文主要是以mahony的算法为基础理解姿态解算的过程,主要参考的论文就是Willia ...
- LPMS-IMU姿态解算
参考文章:AHRS姿态解算说明(加速度+陀螺仪+磁力计原理及原始数据分析) AHRS俗称航姿参考系统,AHRS由加速度计,磁场计,陀螺仪构成,AHRS的真正参考来自于地球的重力场和地球的磁场--他的静 ...
- 卡尔曼滤波五个公式_基于ROS的卡尔曼滤波姿态解算
前段时间由于项目关系需要实现基于卡尔曼滤波的姿态解算,也就是说融合加速度计.陀螺仪及磁罗盘进行AHRS姿态解算,得到机器人的姿态角. 本文的学习需要有一定的卡尔曼滤波器基础,可以参考白巧克力亦唯心的卡 ...
- STM32入门笔记(02):MPU6050、MPU9250、ICM20948及姿态解算(SPL库函数版)
目录 MPU6050 什么是MPU6050? MPU6050的特点 MPU6050框图 MPU6050初始化 MPU6050寄存器 电源管理寄存器1(0X6B) 陀螺仪配置寄存器(0X1B) 加速度传 ...
- 树莓派采集MPU9250运行AHRS进行姿态解算
文章目录 1.几种概念的区分 2.消费级IMU的AHRS 3.树莓派玩转MPU9250 3.1树莓派配置 3.2在树莓派中移植MPU9250库 3.3使用MPU9250库 4.校准 4.1IMU误差模 ...
最新文章
- 【2021年度训练联盟热身训练赛第二场】g2g c u l8r(python)
- 深度学习利器:TensorFlow与NLP模型
- TatukGIS - GisDefs - CheckDir 函数
- .NET开发的一些小技巧
- Linux学习之01_基础命令介绍
- 带权中位数-算法导论第三版第九章思考题9-2
- 在C# 中 如何限制在文本框(textBox)中输入的类型为正整数
- RTTI decltype declval
- mac自带自动化java_java Mac自动化-java和ant环境搭建
- 大数据应用现状:从发现价值到创造价值
- 苹果新的编程语言 Swift 语言进阶(七)--枚举、结构、类
- 两个整形变量值交换的五种境界
- lua.c:82:10: fatal error: readline/readline.h: 没有那个文件或目录
- react实现异步插件_初识react(四) react中异步解决方案之 redux-saga
- 福州发布《关于运用大数据加强对市场主体服务和监管实施方案》
- 2020年高压电工考试APP及高压电工模拟考试软件
- sql 触发器(Insert,update,delete)
- Dynamics AX 2012 的工业物联网解决方案
- c 语言条件运算符,C 语言条件运算符详细讲解
- windows密码_Windows登录密码忘记怎么办?