本文作者作为大一学生在制作平衡单车时首次接触陀螺仪，分享了学习过程中的经验。文章详细介绍了如何获取陀螺仪的角度值，包括加速度计和陀螺仪数据的方法，并提到了一阶互补滤波在提高数据准确性上的作用。作者还分享了自己从误认为只有加速度计能获取角度到理解陀螺仪、角速度计和地磁计都能提供角度信息的过程。
如果觉得写的还可以，请关注加收藏，你的关注就是我继续创作优秀文章的动力，谢谢！！！！！！！！！！！！！！！！！！！！！！！
不好意思，终于迎来了2.0版本的更新，这次包括了地磁计和四元数解算的代码，希望大家多多提出意见！！！！！！！！！！！！！！
陀螺仪主要分为三轴,六轴和九轴陀螺仪,主要功能分别是三轴加速度计数据,三轴陀螺仪数据(角速度数据),和三轴地磁计数据(本篇中我还为学会地磁计的作用，现在我已经学会怎么使用地磁计了),经过几天对陀螺仪的使用和学习经验,做一个分享,并记录我自己的学习(以防后面忘记如何使用).
我是在制作平衡单车的时候,第一次接触和使用陀螺仪,刚开始对于陀螺仪的了解只有它能保持平衡,对于它其他方面,基本毫无了解,为了学习使用陀螺仪,我连续查了好几天资料,并对它进行了实际操作,发现他真的好难.
本人现在以及大三了，我再又一年的接触到平衡车，为了简单寻迹，所以寻迹也是使用了陀螺仪，但是由于只有陀螺仪和加速度计得出的角度，在短期内还算正常，但是时间一长就容易飘，因为我们是使用模拟地图得出的寻迹目标，所以光靠这两个融合得出的角度显然已经不够使用了，所以我们把陀螺仪改为了九轴陀螺仪，加了三轴的地磁计，地磁计的特性利用地面磁场确定航向角，类似于指南针，由于这个特性，大家也明白了一个道理，地磁计受金属，磁场影响巨大（本人亲测，影响巨大，会把指南针打死）
后面不说废话了,之接开始我的分享.
在这段时间,我纠结最多的就是如何获取陀螺仪的实际角度值,我使用了好多方法,使用一阶互补滤波(卡尔曼滤波属实看不懂)
其实我在学习陀螺仪的时候一直有一个误区，就是以为只有加速度计可以得出角度，但是后来，我通过不断的翻阅资料，慢慢发现，对于陀螺仪来说它的目的就是为了得出角度，所以他的角速度计，陀螺仪数据和地磁计都是可以得出角度的
在分享角度之前，我想给大家讲讲零漂
什么是零漂呢？其实很简单，可以见名之意一下就是在零的左右不断漂移，在静止的时候，陀螺仪、加速度计以及地磁计在零点的时候数值不断的在0附近飘动。这个漂移在短期内问题还是不大，但是随着时间的累加，漂移就会特别严重，甚至偏移的角度可能大于你的想象，不断累加，对于调直立、惯性导航以及一些对陀螺仪要求高的东西，这个零漂是致命的。我通过所学，提供我仍为比较好用的三种去零漂的代码。
（1）除10再乘10
这个就比较顾名思义了，对于零漂来说，仔细观察数据，其实他在0左右漂移的数值不会大于10，如果大于了那就不是误差了，而是你可能不小心动了一下桌子。除10再乘10就是把个位上的数据直接抹除作为不信任区。具体代码如下：
imu963ra_gyro_x = imu963ra_gyro_x /10 *10;
（2）减去误差
这个方法和上一个差不多，也是去除个位上的数据，具体思路就是计算一千次的数据累加起来然后求平均值，之后把每次获取的值减去这个平均值，也能把零漂去的还可以，注意：在计算误差的时候一定不要乱动。直接上代码：
int index=0;
void error_compute()
{
static error = 0;
if(index < 1000) error += imu963ra_gyro_y;
else if(index == 1000) error = error / 1000;
else return;
index++;
}（3）一阶低通滤波
这个方法就比较简单了，就是计算权重，这一次获取的数值乘以权重加上上一次获取的数值乘以（1-权重），这样的数据比较准确，但是不适合陀螺仪，只适合加速度计，直接上代码：
float a = 0.25;
float frist_pass_filter(float in_data)
{
static last_data;
float out_data = a * last_data + (1 - a) * in_data;
last_data = in_data;
return out_data;
}以下是加速度计获取角度的方法
选择哪两个轴的方法是把车放在原地不动,左右摇晃,示波器中波动最大的两个轴就是所需要的轴
关于tan2函数的原因可以参考这篇文章atan2反正切-CSDN博客
//angle_1是加速度计通过三角函数得出的角度
//imu963ra_acc_x和imu963ra_acc_z是两个轴的加速度计
angle_1=atan2(imu963ra_acc_x,-imu963ra_acc_z)*(180/PI)+90;
//PI 指的是圆周率3.1415926以下是陀螺仪数据获取角度的方法
选择的轴的方法就是将所有陀螺仪数据放入示波器中,左右摇摆,晃动最大的那个就是所需要的陀螺仪数据轴，记住，传过来的陀螺仪数据一定要先转化为实际的物理值，一定要先去零漂
//gyroRate是你得出来的那个轴的数据
//dt是积分的时间间隔,一般是ms,具体多少看实际情况
double integrateGyro(double gyroRate, double dt)
{
static double gyroAngle = 0.0;
// 对角速度进行积分得到角度
gyroAngle += gyroRate * dt;
// 返回角度
return gyroAngle;
}最后为了让数据更加准确,一般使用一阶互补滤波对加速度计得出的角度和陀螺仪数据得出的角度进行互补滤波,以下是我在学习中看到的一个互补滤波
float acc_ratio = 4.90; //加速度计比例
float gyro_ratio = 0.45; //陀螺仪比例,结合示波器调参
float dt = 0.005; //采样周期,赶紧实际情况填写
float angle_calc(float angle_m, float gyro_m)
{
float temp_angle;
float gyro_now;
float error_angle;
static float last_angle;
static uint8 first_angle;
if(!first_angle)
{
first_angle = 1;
last_angle = angle_m;
}
gyro_now = gyro_m * gyro_ratio;
error_angle = (angle_m - last_angle)*acc_ratio;
temp_angle = last_angle + (error_angle + gyro_now)*dt;
last_angle = temp_angle;
return temp_angle;
}以下是地磁计的学习经验
后面为大家分享的就是地磁计的学习经验了，但是我不知道是我的问题还是没有找到，我并没有找到利用地磁计得出角度的方法，我利用地磁计融合的方式是直接利用四元数解算方法得出欧拉角，相对更加准确，我觉得大家可以试试，至于九轴的四元数解算代码，容我找一找，代码太多了，太乱了，容我整理一下，过两天更新。
最后一部分终于来啦
个人亲测，使用九轴陀螺仪的时候一定不要靠近金属和磁场
以下是我四元数解算陀螺仪的代码
void MadgwickQuaternionUpdate(Axis3f acc, Axis3f gyro, Axis3f mag, Axis3f *Angle , float dt)
{
float norm;
float hx, hy, _2bx, _2bz;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
//提前计算好下面要用到的数值防止重复计算，减少运算量
float _2q1mx;
float _2q1my;
float _2q1mz;
float _2q2mx;
float _4bx;
float _4bz;
float _2q1 = 2.0f * q1;
float _2q2 = 2.0f * q2;
float _2q3 = 2.0f * q3;
float _2q4 = 2.0f * q4;
float _2q1q3 = 2.0f * q1 * q3;
float _2q3q4 = 2.0f * q3 * q4;
float 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;
//度转弧度
gyro.x = gyro.x * DEG2RAD; /* 度转弧度 */
gyro.y = gyro.y * DEG2RAD;
gyro.z = gyro.z * DEG2RAD;
//单位化加速度向量
norm = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
acc.x *= norm;
acc.y *= norm;
acc.z *= norm;
//单位化磁力计向量
norm = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
mag.x *= norm;
mag.y *= norm;
mag.z *= norm;
//提前计算好下面要用到的值
_2q1mx = 2.0f * q1 * mag.x;
_2q1my = 2.0f * q1 * mag.y;
_2q1mz = 2.0f * q1 * mag.z;
_2q2mx = 2.0f * q2 * mag.x;
//磁力计从机体到地球
hx = mag.x * q1q1 - _2q1my * q4 + _2q1mz * q3 + mag.x * q2q2 + _2q2 * mag.y * q3 + _2q2 * mag.z * q4 - mag.x * q3q3 - mag.x * q4q4;
hy = _2q1mx * q4 + mag.y * q1q1 - _2q1mz * q2 + _2q2mx * q3 - mag.y * q2q2 + mag.y * q3q3 + _2q3 * mag.z * q4 - mag.y * q4q4;
/*让导航坐标系中X轴指向正北方*/
_2bx = sqrtf(hx * hx + hy * hy);
_2bz = -_2q1mx * q3 + _2q1my * q2 + mag.z * q1q1 + _2q2mx * q4 - mag.z * q2q2 + _2q3 * mag.y * q4 - mag.z * q3q3 + mag.z * q4q4;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
//梯度下降法计算纠正误差
s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q2 * (2.0f * q1q2 + _2q3q4 - acc.y) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q1 * (2.0f * q1q2 + _2q3q4 - acc.y) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - acc.z) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q4 * (2.0f * q1q2 + _2q3q4 - acc.y) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - acc.z) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q3 * (2.0f * q1q2 + _2q3q4 - acc.y) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
//单位化计算好的误差向量
norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);
norm = 1.0f/norm;
s1 *= norm;
s2 *= norm;
s3 *= norm;
s4 *= norm;
// 将计算好的误差向量补偿到四元数
qDot1 = 0.5f * (-q2 * gyro.x - q3 * gyro.y - q4 * gyro.z) - beta * s1;
qDot2 = 0.5f * (q1 * gyro.x + q3 * gyro.z - q4 * gyro.y) - beta * s2;
qDot3 = 0.5f * (q1 * gyro.y - q2 * gyro.z + q4 * gyro.x) - beta * s3;
qDot4 = 0.5f * (q1 * gyro.z + q2 * gyro.y - q3 * gyro.x) - beta * s4;
//更新四元数的值
q1 += qDot1 * dt;
q2 += qDot2 * dt;
q3 += qDot3 * dt;
q4 += qDot4 * dt;
norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
norm = 1.0f/norm;
q1 = q1 * norm;
q2 = q2 * norm;
q3 = q3 * norm;
q4 = q4 * norm;
//四元数转换为欧拉角
Angle->z = atan2(2.0f * (q2 * q3 + q1 * q4), q1 * q1 + q2 * q2 - q3 * q3 - q4 * q4) * RAD2DEG;
Angle->x = -asin(2.0f * (q2 * q4 - q1 * q3)) * RAD2DEG;
Angle->y = atan2(2.0f * (q1 * q2 + q3 * q4), q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4) * RAD2DEG;
}以上是我对陀螺仪的了解,日后学习到更多的知识会进行实时更新,谢谢关注!
扫一扫
4万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
