本文章 来自原创专栏《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个字母的顺序表示旋转顺序。

可以证明：绕固定坐标系的xyz欧拉角相当于绕自身坐标系的ZYX欧拉角（角度不变，旋转顺序相反）

（2）缺陷：万向节死锁

2. 四元数

（1）四元数则描述了三维空间的旋转旋转

四元数一种是用一个四维向量表示三维空间姿态的旋转的方法。
考虑矢量旋转：

矢量绕法轴旋转：
向量a\boldsymbol{a}a在某平面内旋转了θ\thetaθ角得到向量b\boldsymbol bb，则
a=(cos⁡θ+nsin⁡θ)b\boldsymbol{a}=(\cos \theta + \boldsymbol n\sin \theta)\boldsymbol ba=(cosθ+nsinθ)b（其中等号右侧的运算为向量的“直乘”；n\boldsymbol nn为与该平面垂直的单位向量（转轴），方向与向量旋转方向满足右手定则关系）

矢量旋转的四元数表示：
向量a\boldsymbol{a}a以单位向量en\boldsymbol e_nen为轴旋转了θ\thetaθ角得到向量b\boldsymbol bb，则b=uau−1\boldsymbol b = \boldsymbol u\boldsymbol a \boldsymbol u^{-1}b=uau−1其中u\boldsymbol uu为描述此旋转的四元数，且有u=cos⁡θ2+ensin⁡θ2\boldsymbol u =\cos \frac{\theta}{2} +\boldsymbol e_n \sin \frac\theta2u=cos2θ+ensin2θ
u−1\boldsymbol u^{-1}u−1为 u\boldsymbol uu的逆，为u的共轭除以u的“模的平方”，因为此时u为单位四元数，模为1，故u的逆即为u的共轭。

（2）四元数与欧拉角的关系

对于ZYX欧拉角（绕自身坐标系ZYX分别旋转αβγ\alpha \beta \gammaαβγ，或绕固定座标xyz分别旋转αβγ\alpha \beta \gammaαβγ），四元数q=(q0,q1,q2,q3)q=(q_0,q_1,q_2,q_3)q=(q0,q1,q2,q3)，有

三、姿态融合：根据加速度和角速度计算四元数

1. 使用一阶龙格——库塔法求四元数

使用一阶龙格——库塔法求四元数
[q0q1q2q3]t+Δt=[q0q1q2q3]t+Δt2[−ωxq1−ωyq2−ωzq3ωxq0+ωzq2−ωyq3ωyq0−ωzq1+ωxq3ωzq0+ωyq1−ωxq2]\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，这样我们会得到一个总加速度单位向量a0=[axayaz]a_0= \left[\begin{matrix}a_x\\a_y\\a_z\end{matrix}\right] a0=⎣⎡axayaz⎦⎤
即
a0=(axax2+ay2+az2,ayax2+ay2+az2,azax2+ay2+az2)Ta_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}})^Ta0=(ax2+ay2+az2ax,ax2+ay2+az2ay,ax2+ay2+az2az)T

通过上一时刻四元数，估计出上一刻姿态下重力加速度的值，同样归一化，得到下述式子
v=[VxVyVz]=[2(q1q3−q0q2)2(q2q3+q0q1)1−2(q12+q22)]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)⎦⎤

上式可通过用四元数表示的旋转矩阵，将重力的单位方向向量转化到自身坐标系上而得到，本文不再赘述

两个向量的叉乘视为误差（想想叉乘的几何意义：以两个向量为邻边的平行四边形面积）
误差
et=a0×v=[ayVz−azVyazVx−axVzaxVy−ayVx]te_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]_tet=a0×v=⎣⎡ayVz−azVyazVx−axVzaxVy−ayVx⎦⎤t

由于取样周期通常很小，两次取样的间隔也很短，所以上一刻的位置与此时的位置相差得不多。可以使用上一时刻的位置推断出当时的重力加速度，然后近似为此时的加速度。之后使用 根据位置推算的加速度(无运动噪声) 和 加速度传感器读取到的加速度(有运动噪声) 求误差

对误差进行积分
∑et0=∑t=0t0et×Δt=(∑t=0t0−Δtet)+(et0×Δ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)

修正后的角速度
ω′=ω+Ki∑et0\omega' = \omega + K_i \sum e_{t_0} ω′=ω+Ki∑et0
其中KiK_iKi为积分系数

若想更进一步修正误差，还可以引入微分环节
ω′=ω+Ki∑t=0t0et+KdΔet\omega' = \omega + K_i \sum_{t=0}^{t_0} e_{t} + K_d {\Delta}e_{t}ω′=ω+Kit=0∑t0et+KdΔet其中KdK_dKd为微分系数

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 gimu->lastTime = esp_timer_get_time();
}

ESP32 之 ESP-IDF 实战（一）—— 物联网风力摆控制系统（①姿态解算部分）相关推荐

基于STM32单片机的风力摆控制系统
摘要本系统采用STM32F103V开发板作为控制中心,与万向节.摆杆.直流风机(无刷电机+扇叶).激光头.反馈装置一起构成摆杆运动状态与风机速度分配的双闭环调速系统.单片机输出可变的PWM ...
RT-Thread实战笔记|MPU6050使用详解及DMP姿态解算
小伙伴们大家好,好久不更新RT-Thread实战笔记啦,今天来搞一搞MPU6050,话不多说,淦! 本章源码获取欢迎文末留言区或者公众号后台回复"MPU6050"即可获取本教程源 ...
【毕业设计】MPU6050姿态解算姿态估计 - 物联网单片机 stm32
文章目录 1 简介 2 MPU6050 3 工作原理 4 单片机与MPU6050通信 4.1 mpu6050 数据格式 4.2 倾角计算方法 5 实现代码 6 最后 1 简介 Hi,大家好,这里是丹成 ...
图文手把手教程--ESP32 OTA空中升级(阿里云物联网平台)
本文内容 1)使用ota_example_mqtt例程,通过阿里云物联网平台,进行OTA升级. 2)将例程编译成两个版本,版本1.0.0下载至ESP32开发板,版本2.0.0上传至阿里云物联网平台. ...
图文手把手教程--ESP32 MQTT连接腾讯云物联网平台及OTA固件升级
本文内容 1)在腾讯云物联网开发平台,新建项目,并在项目下新建设备"light_001". 2)下载腾讯云SDK-esp-qcloud,使用例程led_light,通过" ...
【实战】物联网安防监控项目【4】———从网页上控制A9的LED灯
前言学习了一个新知识,当然要记录一下啦.这两天学习了boa服务器.cgic标准库和html标签语言,又双叕解锁一个嵌入式的新玩法.cgic库是沟通C语言和html网页编程语言的一座桥梁,通过在lin ...
[python opencv 计算机视觉零基础到实战] 四、了解色彩空间及其详解
一.学习目标了解什么是色彩空间了解opencv中色彩空间的转换目录 [python opencv 计算机视觉零基础到实战] 一.opencv的helloworld [[python opencv ...
docker 容器 exited_Docker实战006：docker容器使用详解
Docker容器也是docker的核心成员,是docker镜像的一个运行实例.一个镜像可以创建多个容器,多个容器也可以在同一台机器上运行并与其他容器共享操作系统内核同时将应用程序与系统其它周围环境隔离 ...
OpenCV-Python实战（17）——人脸识别详解
OpenCV-Python实战(17)--人脸识别详解 0. 前言 1. 人脸识别简介 2. 使用 OpenCV 进行人脸识别 2.1 使用 OpenCV 进行人脸识别流程示例 3. 使用 dlib ...
OpenCV-Python实战（16）——人脸追踪详解
OpenCV-Python实战(16)--人脸追踪详解 0. 前言 1. 人脸追踪技术简介 2. 使用基于 dlib DCF 的跟踪器进行人脸跟踪 2.1 完整代码 3. 使用基于 dlib DCF ...

ESP32 之 ESP-IDF 实战（一）—— 物联网风力摆控制系统（①姿态解算部分）