本文章 来自原创专栏《ESP32教学专栏 (基于ESP-IDF)》，讲解如何使用 ESP-IDF 构建 ESP32 程序，发布文章并会持续为已发布文章添加新内容！ 每篇文章都经过了精打细磨！
↓↓↓通过下方对话框进入专栏目录页↓↓↓
CSDN 请求进入目录 _ O x
是否进入ESP32教学导航(基于ESP-IDF)?
确定
文章目录完整项目地址：（ESP32 + Android双端源代码）
https://gitee.com/augtons-zhengxie/esp32-wind-swing
- 一、姿态解算算法简介
- 1. 为什么要至少两种传感器
- 2. 传感器的零点漂移问题
- 二、姿态的表示方法
- 1. 欧拉角
- （1）简介
- （2）缺陷：万向节死锁
- 2. 四元数
- （1）四元数则描述了三维空间的旋转旋转
- （2）四元数与欧拉角的关系
- 三、姿态融合：根据加速度和角速度计算四元数
- 1. 使用一阶龙格——库塔法求四元数
- 2. 修正角速度
- 3. C语言代码完成更新四元数
姿态解算，也叫做姿态分析，姿态估计，姿态融合。是指通过传感器的数据（如加速度，角速度）推算出物体当前姿态的过程。
本期项目使用MPU6050传感器测量加速度和角速度，进而解算出姿态。
1. 为什么要至少两种传感器有人可能会有疑问，只用加速度传感器不就能测出姿态了吗。直接对重力加速度分解到3个坐标轴上不就读取出来了吗。
其实这是不正确的，因为物体并非就一定处于平衡状态。想象一下当物体被静止悬挂的时候，确实可以直接使用加速度传感器读取出当时的姿态，即俯仰角度和横滚角度。但是当物体在运动状态呢，尤其是有外部作用力驱动的时候呢（如风力摆，将采用风力作为驱动），加速度传感器的值就会变成重力加速度和运动加速度的矢量和了，显然这样做是不准确的。
2. 传感器的零点漂移问题加速度传感器和陀螺仪（测角速度）两种传感器都会有零点漂移。
对于加速度传感器，我们不能保证其平放的时候就一定测出一个竖直向下的加速度，而是会有一定的偏移量，而且这个偏移量也会有浮动。
对于陀螺仪，即使当物体处于完全静止状态的时候，也不能保证陀螺仪的读数为0。即使物体无角速度，也会被传感器读出一个较小的角速度。
我么知道了这两种传感器都会产生误差，那么具体这两者的误差有什么区别呢。
我们的姿态最终要反映为角度，加速度传感器的读数能直接通过分解与姿态建立关系，但是其无法区分重力加速度还是运动加速度，因此其会产生一个高频误差（误差主要来自高频运动噪声）。
而陀螺仪则与之不同了，陀螺仪测出的是角速度，它与角度呈一阶微分关系，因此陀螺仪需要经过积分环节才能转换为姿态，会产生积分误差。因此其误差主要反映在由零飘导致的误差，即会产生低频误差（误差主要来自低频零飘噪声）。
加速度传感器高频误差较大，陀螺仪低频误差大，因此我们要将其融合。这也是我们所要研究的
二、姿态的表示方法描述一个物体的姿态有很多种方法，最常见的如四元数法和欧拉角法。
1. 欧拉角 （1）简介欧拉角指的是物体绕三个轴依次旋转相应角度，表示当前的姿态。优点是十分直观。
一种欧拉角为绕固定坐标系的，另一种是绕自身（载体）坐标系的。一般通过绕自身坐标系来描述姿态。我们一般用小写字母xyz表示固定坐标系，用大写字母XYZ表示自身坐标系欧拉角的3个字母的顺序表示旋转顺序。
（2）缺陷：万向节死锁 2. 四元数 （1）四元数则描述了三维空间的旋转旋转可以证明：绕固定坐标系的
xyz欧拉角相当于绕自身坐标系的ZYX欧拉角（角度不变，旋转顺序相反）
四元数一种是用一个四维向量表示三维空间姿态的旋转的方法。
考虑矢量旋转：
矢量绕法轴旋转：
向量
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θ+ensin2θ
u
−
1
\boldsymbol u^{-1}
u−1为
u
\boldsymbol u
u的逆，为u的共轭除以u的“模的平方”，因为此时u为单位四元数，模为1，故u的逆即为u的共轭。
对于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)，有
使用一阶龙格——库塔法求四元数
[
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]
⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+Δt=⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+2Δt⎣⎢⎢⎡−ωxq1−ωyq2−ωzq3ωxq0+ωzq2−ωyq3ωyq0−ωzq1+ωxq3ωzq0+ωyq1−ωxq2⎦⎥⎥⎤
其中 ω 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=⎣⎡axayaz⎦⎤
即
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=⎣⎡VxVyVz⎦⎤=⎣⎡2(q1q3−q0q2)2(q2q3+q0q1)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=⎣⎡ayVz−azVyazVx−axVzaxVy−ayVx⎦⎤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∑t0et×Δt=(t=0∑t0−Δtet)+(et0×Δt)
修正后的角速度
ω
′
=
ω
+
K
p
∑
e
t
0
\omega' = \omega + K_p \sum e_{t_0}
ω′=ω+Kp∑et0
其中
K
p
K_p
Kp为积分系数
3. C语言代码完成更新四元数若想更进一步修正误差，还可以引入微分环节
ω ′ = ω + 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} ω′=ω+Kpt=0∑t0et+KiΔet其中 K i K_i Ki为微分系数
转换成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();
}
欢迎分享，转载请注明来源：内存溢出
微信扫一扫
支付宝扫一扫
评论列表（0条）