导航资料

最近在学习运动传感器，就此做一些笔记，大概总结一下我在硬件以及软件方面上的学习。

常见的运动传感器有分六轴传感器和九轴传感器，代表芯片分别为MPU6050和MPU9250，

六轴传感器又称IMU：包含三轴线加速度计，测量物体坐标系下的三轴线加速度；三轴角速度计，测量物体坐标系下的三轴欧拉角变化率。

九轴传感器又称MARG或AHRS：除了前述两类，还包含三轴磁力计，测量地球磁力线在物体坐标系下投影。

而我们在淘宝上所看到的十轴模块又是什么呢？淘宝上卖的十轴模块是在九轴传感器的基础上添加了气压传感器，代表芯片为MPB280。通过测量气压从而计算出传感器的海拔高度。因此被称为十轴传感器。

在之前的博客中，我用Arduino驱动了MPU6050。其实，驱动MPU6050有两种方式。

第一种是最简单的读取传感器的数据，即使用里面的RAW例程，我们可以在Arduino上通过算法解析这六个方向上的数据。

第二种是使用DMP库，即使用里面的DMP6例程，在MPU6050内部有一个运动处理器，通过调用DMP库可直接获得欧拉角或四元数等数据格式。

对于单片机来说，使用DMP库，直接获得已经格式化的数据不会占用太多的系统资源，能直接输出给上位机进行运算（我是想做ROS导航，ROS能直接处理四元数格式），因此是最方便的。但是使用DMP库也有一些限制，输出频率和无法进行滤波调整等问题。因此当我们需要更快的传感器输出频率或者需要调整滤波等时（我想通过这个学习快速卡尔曼滤波算法），我们可以使用单片机直接读取传感器数据，然后在单片机中运行滤波算法和姿态解析。

而当我想要驱动MPU9250时，在网上无法找到合适的DMP库驱动它，我在淘宝上看其他人也是通过stm32对它进行滤波和姿态解析，因此我想可能的原因是MPU9250的DMP库的无法达到工作要求，因此直接读取里面数据再进行驱动是最好的方法。

要想对九轴传感器进行滤波和姿态解析，我在网上找到的姿态解析算法是Madgwick Quaternion和Mahony Quaternion。

参考链接：6轴IMU+磁力计，9轴传感器讲解

参考代码：

        void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)//输入九个传感器数据
        {
            float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // 定义四元数变量
            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;

            // 加速度计的数据进行归一化处理。
            norm = sqrtf(ax * ax + ay * ay + az * az);
            if (norm == 0.0f) return; // 如果加速计各轴的数均是0，那么忽略该加速度数据。否则在加速计数据归一化处理的时候，会导致除以0的错误。
            norm = 1.0f/norm;
            ax *= norm;
            ay *= norm;
            az *= norm;

            // 地磁的数据进行归一化处理。
            norm = sqrtf(mx * mx + my * my + mz * mz);
            if (norm == 0.0f) return; // 如果地磁传感器各轴的数均是0，那么忽略该地磁数据。否则在地磁数据归一化处理的时候，会导致除以0的错误。
            norm = 1.0f/norm;
            mx *= norm;
            my *= norm;
            mz *= norm;

            // 地球磁场参考方向计算
            _2q1mx = 2.0f * q1 * mx;
            _2q1my = 2.0f * q1 * my;
            _2q1mz = 2.0f * q1 * mz;
            _2q2mx = 2.0f * q2 * mx;
            hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
            hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
            _2bx = sqrtf(hx * hx + hy * hy);
            _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
            _4bx = 2.0f * _2bx;
            _4bz = 2.0f * _2bz;

            //梯度计算
            s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            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 * gx - q3 * gy - q4 * gz) - beta * s1;
            qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
            qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
            qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;

            // 积分产生四元数
            q1 += qDot1 * deltat;
            q2 += qDot2 * deltat;
            q3 += qDot3 * deltat;
            q4 += qDot4 * deltat;
            norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // 四元数归一化
            norm = 1.0f/norm;
            q[0] = q1 * norm;
            q[1] = q2 * norm;
            q[2] = q3 * norm;
            q[3] = q4 * norm;

        }