本文章 来自原创专栏《ESP32教学专栏 (基于ESP-IDF)》，讲解如何使用 ESP-IDF 构建 ESP32 程序，发布文章并会持续为已发布文章添加新内容！ 每篇文章都经过了精打细磨！
↓↓↓通过下方对话框进入专栏目录页↓↓↓
CSDN 请求进入目录 _ O x
是否进入ESP32教学导航(基于ESP-IDF)?
确定
完整项目地址：（ESP32 + Android双端源代码）
https://gitee.com/augtons-zhengxie/esp32-wind-swing
文章目录
一、姿态解算算法简介
姿态解算，也叫做姿态分析，姿态估计，姿态融合。是指通过传感器的数据（如加速度，角速度）推算出物体当前姿态的过程。
本期项目使用MPU6050传感器测量加速度和角速度，进而解算出姿态。
1. 为什么要至少两种传感器
有人可能会有疑问，只用加速度传感器不就能测出姿态了吗。直接对重力加速度分解到3个坐标轴上不就读取出来了吗。
其实这是不正确的，因为物体并非就一定处于平衡状态。想象一下当物体被静止悬挂的时候，确实可以直接使用加速度传感器读取出当时的姿态，即俯仰角度和横滚角度。但是当物体在运动状态呢，尤其是有外部作用力驱动的时候呢（如风力摆，将采用风力作为驱动），加速度传感器的值就会变成重力加速度和运动加速度的矢量和了，显然这样做是不准确的。
2. 传感器的零点漂移问题
加速度传感器和陀螺仪（测角速度）两种传感器都会有零点漂移。
对于加速度传感器，我们不能保证其平放的时候就一定测出一个竖直向下的加速度，而是会有一定的偏移量，而且这个偏移量也会有浮动。
对于陀螺仪，即使当物体处于完全静止状态的时候，也不能保证陀螺仪的读数为0。即使物体无角速度，也会被传感器读出一个较小的角速度。
我么知道了这两种传感器都会产生误差，那么具体这两者的误差有什么区别呢。
我们的姿态最终要反映为角度，加速度传感器的读数能直接通过分解与姿态建立关系，但是其无法区分重力加速度还是运动加速度，因此其会产生一个高频误差（误差主要来自高频运动噪声）。
而陀螺仪则与之不同了，陀螺仪测出的是角速度，它与角度呈一阶微分关系，因此陀螺仪需要经过积分环节才能转换为姿态，会产生积分误差。因此其误差主要反映在由零飘导致的误差，即会产生低频误差（误差主要来自低频零飘噪声）。
加速度传感器高频误差较大，陀螺仪低频误差大，因此我们要将其融合。这也是我们所要研究的
二、姿态的表示方法
描述一个物体的姿态有很多种方法，最常见的如四元数法和欧拉角法。
1. 欧拉角
（1）简介
欧拉角指的是物体绕三个轴依次旋转相应角度，表示当前的姿态。优点是十分直观。
一种欧拉角为绕固定坐标系的，另一种是绕自身（载体）坐标系的。一般通过绕自身坐标系来描述姿态。我们一般用小写字母xyz表示固定坐标系，用大写字母XYZ表示自身坐标系欧拉角的3个字母的顺序表示旋转顺序。
可以证明：绕固定坐标系的
xyz欧拉角相当于绕自身坐标系的ZYX欧拉角（角度不变，旋转顺序相反）
（2）缺陷：万向节死锁
2. 四元数
（1）四元数则描述了三维空间的旋转旋转
四元数一种是用一个四维向量表示三维空间姿态的旋转的方法。
考虑矢量旋转：
矢量绕法轴旋转：
向量
a
\boldsymbol{a}
a在某平面内旋转了
θ
\theta
θ角得到向量
b
\boldsymbol b
b，则
a
=
(
cos
?
θ
+
n
sin
?
θ
)
b
\boldsymbol{a}=(\cos \theta + \boldsymbol n\sin \theta)\boldsymbol b
a=(cosθ+nsinθ)b（其中等号右侧的运算为向量的“直乘”；
n
\boldsymbol n
n为与该平面垂直的单位向量（转轴），方向与向量旋转方向满足右手定则关系）
矢量旋转的四元数表示：
向量
a
\boldsymbol{a}
a以单位向量
e
n
\boldsymbol e_n
en?为轴旋转了
θ
\theta
θ角得到向量
b
\boldsymbol b
b，则
b
=
u
a
u
?
1
\boldsymbol b = \boldsymbol u\boldsymbol a \boldsymbol u^{-1}
b=uau?1其中
u
\boldsymbol u
u为描述此旋转的四元数，且有
u
=
cos
?
θ
2
+
e
n
sin
?
θ
2
\boldsymbol u =\cos \frac{\theta}{2} +\boldsymbol e_n \sin \frac\theta2
u=cos2θ?+en?sin2θ?
u
?
1
\boldsymbol u^{-1}
u?1为
u
\boldsymbol u
u的逆，为u的共轭除以u的“模的平方”，因为此时u为单位四元数，模为1，故u的逆即为u的共轭。
（2）四元数与欧拉角的关系
对于ZYX欧拉角（绕自身坐标系ZYX分别旋转
α
β
γ
\alpha \beta \gamma
αβγ，或绕固定座标xyz分别旋转
α
β
γ
\alpha \beta \gamma
αβγ），四元数
q
=
(
q
0
,
q
1
,
q
2
,
q
3
)
q=(q_0,q_1,q_2,q_3)
q=(q0?,q1?,q2?,q3?)，有
三、姿态融合：根据加速度和角速度计算四元数
1. 使用一阶龙格——库塔法求四元数
使用一阶龙格——库塔法求四元数
[
q
0
q
1
q
2
q
3
]
t
+
Δ
t
=
[
q
0
q
1
q
2
q
3
]
t
+
Δ
t
2
[
?
ω
x
q
1
?
ω
y
q
2
?
ω
z
q
3
ω
x
q
0
+
ω
z
q
2
?
ω
y
q
3
ω
y
q
0
?
ω
z
q
1
+
ω
x
q
3
ω
z
q
0
+
ω
y
q
1
?
ω
x
q
2
]
\left[\begin{matrix}q_{0}\\q_{1}\\q_{2}\\q_{3}\end{matrix}\right]_{t+\Delta t} = \left[\begin{matrix}q_{0}\\q_{1}\\q_{2}\\q_{3}\end{matrix}\right]_{t} + \frac{\Delta t}2 \left[\begin{matrix}{-\omega_x q_1 - \omega_y q_2-\omega _zq_3}\\{\omega_xq_0+\omega_zq_2-\omega_yq_3}\\{\omega_yq_0-\omega_zq_1+\omega_xq_3}\\\omega_zq_0+\omega_yq_1-\omega_xq_2\end{matrix}\right]
?????q0?q1?q2?q3???????t+Δt?=?????q0?q1?q2?q3???????t?+2Δt???????ωx?q1??ωy?q2??ωz?q3?ωx?q0?+ωz?q2??ωy?q3?ωy?q0??ωz?q1?+ωx?q3?ωz?q0?+ωy?q1??ωx?q2???????
其中 ω x , ω y , ω z \omega_x, \omega_y, \omega_z ωx?,ωy?,ωz?分别为自身坐标系xyz下的分角速度， Δ t \Delta t Δt为两次结果的时间
因此我们要通过角速度来更新四元数，但是角速度传感器的值是不能拿来直接用的，因为陀螺仪存在零飘。因此我们要用加速度传感器的值来修正。
2. 修正角速度
通过加速度来修正角速度
因此我们设计一个系统，输入是当前的加速度，输出为角速度的修正值。
首先将读取到的加速度归一化，即3个方向同时除以原模长使得其模变为为单位长度1，这样我们会得到一个总加速度单位向量
a
0
=
[
a
x
a
y
a
z
]
a_0= \left[\begin{matrix}a_x\\a_y\\a_z\end{matrix}\right]
a0?=???ax?ay?az?????
即
a
0
=
(
a
x
a
x
2
+
a
y
2
+
a
z
2
,
a
y
a
x
2
+
a
y
2
+
a
z
2
,
a
z
a
x
2
+
a
y
2
+
a
z
2
)
T
a_0=(\frac{a_x}{\sqrt{a_x^2+a_y^2+a_z^2}}, \frac{a_y}{\sqrt{a_x^2+a_y^2+a_z^2}}, \frac{a_z}{\sqrt{a_x^2+a_y^2+a_z^2}})^T
a0?=(ax2?+ay2?+az2??ax??,ax2?+ay2?+az2??ay??,ax2?+ay2?+az2??az??)T
通过上一时刻四元数，估计出上一刻姿态下重力加速度的值，同样归一化，得到下述式子
v
=
[
V
x
V
y
V
z
]
=
[
2
(
q
1
q
3
?
q
0
q
2
)
2
(
q
2
q
3
+
q
0
q
1
)
1
?
2
(
q
1
2
+
q
2
2
)
]
v = \left[\begin{matrix}V_x\\V_y\\V_z\end{matrix}\right]= \left[\begin{matrix}2(q_1q_3-q_0q_2)\\2(q_2q_3+q_0q_1)\\1-2(q_1^2+q_2^2)\end{matrix}\right]
v=???Vx?Vy?Vz?????=???2(q1?q3??q0?q2?)2(q2?q3?+q0?q1?)1?2(q12?+q22?)????
上式可通过用四元数表示的旋转矩阵，将重力的单位方向向量转化到自身坐标系上而得到，本文不再赘述
两个向量的叉乘视为误差（想想叉乘的几何意义：以两个向量为邻边的平行四边形面积）
误差
e
t
=
a
0
×
v
=
[
a
y
V
z
?
a
z
V
y
a
z
V
x
?
a
x
V
z
a
x
V
y
?
a
y
V
x
]
t
e_t = a_0 \times v=\left[\begin{matrix}a_yV_z - a_zV_y\\a_zV_x-a_xV_z\\a_xV_y-a_yV_x\end{matrix}\right]_t
et?=a0?×v=???ay?Vz??az?Vy?az?Vx??ax?Vz?ax?Vy??ay?Vx?????t?
对误差进行积分
∑
e
t
0
=
∑
t
=
0
t
0
e
t
×
Δ
t
=
(
∑
t
=
0
t
0
?
Δ
t
e
t
)
+
(
e
t
0
×
Δ
t
)
\begin{aligned}\sum e_{t_0} &= \sum_{t=0}^{t_0} e_t \times \Delta t \\&= \left(\sum_{t=0}^{t_0- \Delta t} e_t\right) + \left(e_{t_0} \times \Delta t\right)\end{aligned}
∑et0???=t=0∑t0??et?×Δt=(t=0∑t0??Δt?et?)+(et0??×Δt)?
修正后的角速度
ω
′
=
ω
+
K
p
∑
e
t
0
\omega' = \omega + K_p \sum e_{t_0}
ω′=ω+Kp?∑et0??
其中
K
p
K_p
Kp?为积分系数
若想更进一步修正误差，还可以引入微分环节
ω ′ = ω + K p ∑ t = 0 t 0 e t + K i Δ e t \omega' = \omega + K_p \sum_{t=0}^{t_0} e_{t} + K_i {\Delta}e_{t} ω′=ω+Kp?t=0∑t0??et?+Ki?Δet?其中 K i K_i Ki?为微分系数
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();
}