本文章来自原创专栏《ESP32教学专栏 (基于ESP-IDF)》，讲解如何使用 ESP-IDF 构建 ESP32 程序，发布文章并会持续为已发布文章添加新内容！每篇文章都经过了精打细磨！
↓↓↓通过下方对话框进入栏目目录页面↓↓↓
CSDN 请求进入目录 _ O x
是否进入ESP32教学导航(基于ESP-IDF)?
当然
完整项目地址：（ESP32 + Android双端源代码）
https://gitee.com/augtons-zhengxie/esp32-wind-swing
1.姿态计算算法介绍
姿态解法，又称姿态分析、姿态估计、姿态融合。它是指通过传感器数据（如加速度、角速度）来估计物体当前姿态的过程。
本期项目使用MPU6050传感器测量加速度和角速度，进而解算出姿态。
1. 为什么要至少两种传感器
有人可能会有疑问，只用加速度传感器不就能测出姿态了吗。直接对重力加速度分解到3个坐标轴上不就读取出来了吗。
事实上，这是不正确的，因为物体不一定处于平衡状态。想象一下，当物体静止悬浮时，确实可以直接用加速度传感器读取当时的姿态，即俯仰角和横滚角。但是当物体在运动时，尤其是受到外力驱动时（比如风摆，会被风驱动），加速度传感器的值会变成重力加速度与加速度的矢量和运动的加速度。显然，这样做是不准确的。
2. 传感器的零点漂移问题
加速度计和陀螺仪（角速度）传感器都将具有零漂移。
对于加速度传感器，我们不能保证平放时会测量到垂直向下的加速度，但是会有一定的偏移量，这个偏移量也会浮动。
对于陀螺仪，即使当物体处于完全静止状态的时候，也不能保证陀螺仪的读数为0。即使物体无角速度，也会被传感器读出一个较小的角速度。
我知道这两个传感器会产生误差，那么两者的误差有什么区别。
我们的态度最终体现为一个角度。加速度传感器的读数可以通过分解直接与姿态相关，但无法区分重力加速度和运动加速度，因此会产生高频误差（误差主要来自高频运动噪音）。
陀螺仪与它不同。陀螺仪测量角速度，它与角度具有一阶微分关系。因此，陀螺仪需要经过积分环节才能转换成姿态，会造成积分误差。因此，误差主要体现在零漂引起的误差上，即会产生低频误差（该误差主要来自低频零漂噪声）。
加速度计的高频误差大，陀螺仪的低频误差大，所以需要对它们进行融合。这就是我们要研究的
2、表达态度的方式
描述物体姿态的方法有很多种，最常见的是四元数法和欧拉角法。
1. 欧拉角
（1）简介
欧拉角是指物体绕三个轴旋转相应的角度，表示当前的姿态。优点是非常直观。
一种欧拉角为绕固定坐标系的，另一种是绕自身（载体）坐标系的。一般通过绕自身坐标系来描述姿态。我们一般用小写字母xyz表示固定坐标系，用大写字母XYZ表示自身坐标系欧拉角的3个字母的顺序表示旋转顺序。
可以证明，围绕固定坐标系的
xyz欧拉角等价于围绕自身坐标系的ZYX欧拉角（角度不变，旋转顺序颠倒）
（2）缺陷：万向节死锁
2. 四元数
（1）四元数则描述了三维空间的旋转旋转
四元数是一种用一个四维向量来表示三维空间姿态的旋转的方法。
考虑矢量旋转：
围绕法线轴旋转向量：
向量在某一平面内旋转
角得到向量
，则
（等号右边的运算是向量的“直接乘法”；
是垂直于平面的单位向量（旋转轴），方向和向量旋转方向满足右手定则关系）
矢量旋转的四元数表示：
向量以单位向量
为轴旋转
角得到向量
，则
其中
是描述旋转的四元数，有
为
的逆，为u的共轭除以u的“模的平方”，因为此时u为单位四元数，模为1，故u的逆即为u的共轭。
（2）四元数与欧拉角的关系
对于ZYX欧拉角（绕自身坐标系ZYX分别旋转，或绕固定座标
xyz分别旋转），四元数
，有
3、姿态融合：根据加速度和角速度计算四元数
1. 使用一阶龙格——库塔法求四元数
使用一阶龙格-库塔法求四元数
其中分别为自身坐标系xyz下的分角速度，
为两次结果的时间
因此，我们需要通过角速度来更新四元数，但是不能直接使用角速度传感器的值，因为陀螺仪是零漂移的。所以我们使用加速度计的值来校正它。
2. 修正角速度
通过加速度校正角速度
因此，我们设计了一个系统，输入为当前加速度，输出为角速度的校正值。
首先将读取到的加速度归一化，即3个方向同时除以原模长使得其模变为为单位长度1，这样我们会得到一个总加速度单位向量
这是
通过最后时刻的四元数，估计出最后时刻姿态下的重力加速度的值，也进行归一化，得到如下公式
上式可以通过使用四元数表示的旋转矩阵，将重力的单位方向向量转换为自身坐标系得到，本文不再赘述。
两个向量的叉积被认为是错误的（想想叉积的几何意义：以两个向量为相邻边的平行四边形的面积）
错误
整合错误
修正角速度
其中是积分系数
如果想进一步纠正错误，还可以引入差分链接
其中
是微分系数
3. C语言代码完成更新四元数
转换成C语言代码即可
类型定义
// Vector3
typedef struct {
float x;
float y;
float z;
}Vector3;
// Quaternion
typedef struct{
float q0;
float q1;
float q2;
float q3;
}Quaternion;
// Imu
typedef struct {
uint64_t lastTime; //储存上一次更新的时间戳
Quaternion quaternion; // 当前的四元数对象
Vector3 error_Int; // 积分后的误差
float kp; // 积分系数
float ki; // 微分系数（本例未使用微分环节）
}Imu;
/**
* @brief 更新四元数，结果一定返回“规范四元数”
* @param[in] imu IMU对象
* @param[in] accel 加速度, 单位为g即可，因为结果一定返回“规范四元数”
* @param[in] gyro 角速度，单位为弧度每秒和度每秒均可，因为结果一定返回“规范四元数”
*/
void IMU_Update(Imu *imu, Vector3 *accel, Vector3 *gyro){
Quaternion preQuaternion = imu->quaternion;
if(imu->lastTime == 0){
imu->quaternion.q0 = 1;
imu->quaternion.q1 = 0;
imu->quaternion.q1 = 0;
imu->quaternion.q1 = 0;
imu->lastTime = esp_timer_get_time();
return;
}
float halfT = (float)(esp_timer_get_time() - imu->lastTime) / 2000000;
#define q(n) (preQuaternion.q##n)
#define a(d) (accel->d)
const float accelNorm = invSqrt(accel->x*accel->x + accel->y*accel->y + accel->z*accel->z);
accel->x *= accelNorm;
accel->y *= accelNorm;
accel->z *= accelNorm;
#define v(d) (gravity_accel_q.d)
Vector3 gravity_accel_q; // 根据四元数换算的重力加速度
v(x) = 2*(q(1)*q(3) - q(0)*q(2));
v(y) = 2*(q(0)*q(1) + q(2)*q(3));
v(z) = 1 - 2*(q(1)*q(1)) - 2*(q(2)*q(2));
#define e(d) (error.d)
Vector3 error; //误差
e(x) = a(y)*v(z) - a(z)*v(y);
e(y) = a(z)*v(x) - a(x)*v(z);
e(z) = a(x)*v(y) - a(y)*v(x);
#define eInt(d) (imu->error_Int.d)
#define Ki (imu->ki)
#define Kp (imu->kp)
eInt(x) += e(x) * Ki*halfT;
eInt(y) += e(y) * Ki*halfT;
eInt(z) += e(z) * Ki*halfT;
#define g(d) (gyro->d)
g(x) = g(x) + Kp*e(x) + eInt(x);
g(y) = g(y) + Kp*e(y) + eInt(y);
g(z) = g(z) + Kp*e(z) + eInt(z);
#define qn(n) (imu->quaternion.q##n)
qn(0) = q(0) + (-q(1)*g(x) - q(2)*g(y) - q(3)*g(z))*halfT;
qn(1) = q(1) + (q(0)*g(x) + q(2)*g(z) - q(3)*g(y))*halfT;
qn(2) = q(2) + (q(0)*g(y) - q(1)*g(z) + q(3)*g(x))*halfT;
qn(3) = q(3) + (q(0)*g(z) + q(1)*g(y) - q(2)*g(x))*halfT;
const float qNorm = invSqrt(qn(0)*qn(0) + qn(1)*qn(1) + qn(2)*qn(2) + qn(3)*qn(3));
qn(0) *= qNorm;
qn(1) *= qNorm;
qn(2) *= qNorm;
qn(3) *= qNorm;
#undef q
#undef qn
#undef a
#undef v
#undef e
#undef Ki
#undef Kp
#undef eInt
#undef g
imu->lastTime = esp_timer_get_time();
}
文章出处登录后可见！
