飞行控制 | 旋转矩阵推导、IMU 姿态解算与 PID 控制

原创于 2025-12-05 18:26:29 发布 · 657 阅读

4 ·

CC 4.0 BY-SA版权

文章标签：

#飞行控制

electronic engineering 专栏收录该内容

75 篇文章

订阅专栏

注：本文为 “飞行控制” 相关合辑。
图片清晰度受引文原图所限。
略作重排，未整理去重。
如有内容异常，请看原文。

MPU6050 姿态解算 1-DMP 方式

码农爱学习原创于 2020-07-29 23:18:52 发布

MPU6050 的姿态解算存在多种实现路径，包括硬件层面的 DMP 解算、软件层面的欧拉角与旋转矩阵解算、软件层面的轴角法与四元数解算。本篇优先介绍操作便捷性较高的 DMP 解算方式。

MPU6050 基本功能

3 轴陀螺仪

陀螺仪用于测量绕 $x$ 、 $y$ 、 $z$ 轴的转动角速度，通过对角速度进行积分运算可获取对应角度信息。

3 轴加速度计

加速度计用于测量 $x$ 、 $y$ 、 $z$ 三个方向的加速度。静止状态下，其测量对象为重力加速度，因此当载体发生倾斜时，可基于重力分力粗略计算倾斜角度；运动状态下，测量结果为重力加速度与运动产生的加速度的叠加值。

在这里插入图片描述

DMP 简介

DMP 是 MPU6050 内部集成的运动引擎，全称为 Digital Motion Processor（数字运动处理器），可直接输出四元数结果。该设计能显著降低外围微处理器的计算负荷，且无需额外执行繁琐的滤波与数据融合流程。

Motion Driver 是 Invensense 公司针对其运动传感器开发的专用软件包，并非完全开源，其核心算法部分已针对 ARM 处理器与 MSP430 处理器编译为静态链接库，适用于 MPU6050、MPU6500、MPU9150、MPU9250 等系列传感器。

四元数转欧拉角

四元数是表征三维空间旋转的高效数学工具，但其概念理解具有一定难度，可先通过复数类比（复数用于表征二维平面旋转）建立初步认知。

四元数的基本表示形式为 $q_0 + q_1i + q_2j + q_3k$ ，包含 1 个实部（ $q_0$ ）与 3 个虚部（ $q_1$ 、 $q_2$ 、 $q_3$ ），具体原理本篇暂不展开。

尽管四元数在旋转表征中具有优势，但形式不够直观。为便于姿态观测，需将其转换为俯仰角（pitch）、横滚角（roll）、偏航角（yaw）的表示形式。

转换公式

在这里插入图片描述

程序实现（C 语言）

pitch = asin(-2 * q1 * q3 + 2 * q0 * q2);
roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1);
yaw   = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3);

测试验证

上述方案已在 STM32F407 硬件平台与 FreeRTOS 操作系统下完成验证，测试效果如下：

波形数据

在这里插入图片描述

姿态效果

在这里插入图片描述

视频演示

视频地址：https://www.bilibili.com/video/BV18A411v7Du/

3 维旋转矩阵推导与助记

码农爱学习原创于 2020-08-17 22:48:24 发布

旋转矩阵的应用范围较广，是姿态变换、坐标变换等操作的基础。本篇首先介绍旋转矩阵的推导过程与助记方法。

旋转矩阵的旋转包含两种含义：其一为同一坐标系下向量的旋转；其二为坐标系的旋转，同一向量在不同坐标系下将对应不同坐标。

1 向量旋转

首先讨论二维平面坐标系下的旋转，再引申至三维空间。

1.1 平面二维旋转

如下图所示，XY 坐标系中，向量 $OP$ 旋转 $\beta$ 角度至 $O P^{'}$ 位置：

根据三角函数关系，可列出向量 $OP$ 与 $O P^{'}$ 的坐标表示形式：

对比上述两式，将第二式展开：

在这里插入图片描述

用矩阵形式重新表示为：

此即二维旋转的基本形式，中间矩阵为二维旋转的旋转矩阵。坐标系中某一向量左乘该矩阵后，可得该向量旋转 $\beta$ 角后的坐标。

1.2 三维旋转

三维旋转可借助二维旋转理解，三维空间中可绕任意轴旋转，为便于分析与应用，仅考虑绕 X、Y、Z 轴的旋转。

绕 Z 轴

参照上图，添加 Z 轴后，上述二维旋转即为绕 Z 轴的三维旋转：

沿用上述推导公式，补充 Z 坐标的变换关系（实际无变化），改写为矩阵形式，红色方框内为绕 Z 轴的旋转矩阵：

在这里插入图片描述

绕 Y 轴

绕 Y 轴旋转同理，此处直接变更坐标轴符号表示，需注意坐标顺序符合右手系，本文以颜色区分不同轴。最终矩阵形式需进一步调整为 XYZ 顺序，红色方框内为绕 Y 轴的旋转矩阵：

在这里插入图片描述

绕 X 轴

参照绕 Y 轴的推导，可得绕 X 轴的结果，红色方框内为绕 X 轴的旋转矩阵：

在这里插入图片描述

1.3 助记

对于单位矩阵，绕某轴旋转时，该轴对应列保持不变，将二维旋转矩阵替换至对应 4 个位置。需注意，绕 Y 轴的旋转矩阵与另外两者不同，其 $-\sin\beta$ 位于左下位置：

在这里插入图片描述

1.4 注意事项

反向旋转

若进行反向旋转，推导过程类似：

最终得到的旋转矩阵为正向旋转矩阵的逆矩阵，由于该矩阵为正交阵，故逆矩阵等于转置矩阵：

在这里插入图片描述

书写形式

上述向量坐标均以列形式书写，若改为行形式表示，则旋转矩阵形式转置，且矩阵需与行向量右乘：

在这里插入图片描述

2 坐标系旋转

2.1 平面二维旋转

如下图所示，xy 坐标系中存在向量 $OP$ ，其坐标为 $(x, y)$ ，该向量与 X 轴夹角为 $\alpha$ 。坐标系绕原点逆时针旋转 $\beta$ 角度后形成新坐标系 $x^{'} y^{'}$ ，此时 $OP$ 在新坐标系中的坐标为 $(x^{'}, y^{'})$ 。根据几何关系推导，最终得到绿色虚框内的旋转矩阵：

在这里插入图片描述

对比前述旋转矩阵可知，此处坐标系旋转的旋转矩阵与向量旋转的旋转矩阵互为转置（实际为逆矩阵，因正交阵的逆矩阵与转置矩阵相同），二者本质为相对运动，互为逆过程。

2.2 三维旋转

绕 Z 轴

在这里插入图片描述

绕 Y 轴

在这里插入图片描述

绕 X 轴

在这里插入图片描述

欧拉角旋转

码农爱学习于 2020-08-23 18:54:31 发布

欧拉角是描述三维旋转的数学方法，其计算需依托旋转矩阵理论，关于旋转矩阵的详细推导可参考前文《3 维旋转矩阵推导与助记》。

欧拉角旋转

1 静态定义

在三维空间内，对于给定的参考系，任意坐标系的取向均可通过三个欧拉角确定。

参考系：又称实验室参考系，保持静止状态，可简化理解为大地坐标系，亦称为惯性坐标系。
坐标系：固定于刚体表面，随刚体旋转同步运动，例如飞行器自身的坐标系，亦称为载体坐标系。

上图为 ZYZ 顺序旋转的欧拉角示意图，各元素定义如下：

蓝色 xyz-轴：惯性系的参考轴，即大地坐标系的三个坐标轴；
红色 XYZ-轴：载体系的参考轴，即飞行器坐标系的三个坐标轴；
交点线：xy-平面与 XY-平面的交线，用英文大写字母 N 表示。

图中角度参数说明：

$\alpha$ ：x-轴与交点线的夹角，载体坐标系先绕 Z 轴旋转 $\alpha$ 角度，取值范围为 $\sim 2\pi$ 弧度；
$\beta$ ：z-轴与 Z-轴的夹角，载体坐标系再绕当前 Y 轴旋转 $\beta$ 角度，取值范围为 $\sim \pi$ 弧度；
$\gamma$ ：交点线与 X-轴的夹角，载体坐标系最后绕当前 Z 轴旋转 $\gamma$ 角度，取值范围为 $\sim 2\pi$ 弧度；
角度正负判定遵循右手定则：以右手大拇指指向旋转轴正方向，四指弯曲方向即为该角度的正旋转方向（如 $\alpha$ 角正方向为右手大拇指指向 z-轴时四指的弯曲方向）。

其旋转过程动画演示如下：

需注意，欧拉角的旋转顺序、角度标记方式及参考轴指定无统一标准，因此使用欧拉角时必须明确上述参数定义。合法的欧拉角组合需满足“任意两个连续旋转的转动轴不同”，基于此规则，欧拉角共有 12 种合法表示形式，具体分类如下：

6 种绕三条不同轴的旋转（泰特-布莱恩角，Tait-Bryan Angle）：XYZ、XZY、YXZ、YZX、ZXY、ZYX；
6 种绕两条轴交替的旋转（正规欧拉角，Proper Euler Angle）：XYX、YXY、XZX、ZXZ、YZY、ZYZ。

2 动态定义

欧拉角的动态定义包含两种独立表述形式，具体如下：

绕固定于载体的坐标轴进行三次旋转，最终形成复合运动；
绕大地坐标系参考轴进行三次旋转，最终形成复合运动。

相较于静态定义，动态定义可更清晰地体现欧拉角在物理场景中的实际含义与应用价值。

说明：以下描述中，大写字母 XYZ 坐标轴代表随载体旋转的坐标轴；小写字母 xyz 坐标轴代表静止的大地参考轴。本节以旋转顺序为 Z → Y → X 为例，详细说明两种动态定义。

2.1 定义 A：绕 `XYZ` 坐标轴旋转（载体坐标轴）

初始状态：惯性坐标系 xyz 与载体坐标系 XYZ 的坐标轴完全重合；
第一步：绕 Z-轴旋转 $\alpha$ 角度；
第二步：绕当前 Y-轴旋转 $\beta$ 角度；
第三步：绕当前 X-轴旋转 $\gamma$ 角度。

设空间任意一点 $P_1$ 在惯性坐标系 xyz 中的坐标为 $r_1$ ，在载体坐标系 XYZ 中的坐标为 $R_1$ 。定义：

$Z(\alpha)$ ：绕 Z-轴旋转 $\alpha$ 角度的旋转矩阵；
$Y(\beta)$ ：绕 Y-轴旋转 $\beta$ 角度的旋转矩阵；
$X(\gamma)$ ：绕 X-轴旋转 $\gamma$ 角度的旋转矩阵。

则定义 A 的坐标变换关系可表示为：

注：绕载体坐标系旋转时，旋转矩阵按旋转顺序依次左乘，即变换顺序为 $X(\gamma ) \leftarrow Y(\beta ) \leftarrow Z(\alpha )$ 。

2.2 定义 B：绕 `xyz` 坐标轴旋转（大地坐标轴）

初始状态：惯性坐标系 xyz 与载体坐标系 XYZ 的坐标轴完全重合；
第一步：绕 z-轴旋转 $\alpha$ 角度；
第二步：绕 y-轴旋转 $\beta$ 角度；
第三步：绕 x-轴旋转 $\gamma$ 角度。

设空间任意一点 $P_2$ 在惯性坐标系 xyz 中的坐标为 $r_2$ ，在载体坐标系 XYZ 中的坐标为 $R_2$ 。定义：

$z(\alpha)$ ：绕 z-轴旋转 $\alpha$ 角度的旋转矩阵；
$y(\beta)$ ：绕 y-轴旋转 $\beta$ 角度的旋转矩阵；
$x(\gamma)$ ：绕 x-轴旋转 $\gamma$ 角度的旋转矩阵。

则定义 B 的坐标变换关系可表示为：

注：绕大地坐标系旋转时，旋转矩阵按旋转顺序依次右乘，即变换顺序为 $X(\gamma ) \rightarrow Y(\beta ) \rightarrow Z(\alpha )$ 。

2.3 定义等价性说明

定义 A 与前文静态定义完全等价，该结论可通过几何作图法直接验证；
定义 A 与定义 B 的等价性可通过旋转矩阵的代数运算证明，推导过程如下：

MPU6050 姿态解算 2-欧拉角&旋转矩阵

码农爱学习原创于 2020-08-23 19:10:40 发布

IMU 姿态解算

IMU（惯性测量单元）通常包含三轴陀螺仪与三轴加速度计。此前文章 [ MPU6050 姿态解算 1-DMP 方式 ] 已对 MPU6050 这款 IMU 进行简要介绍，并通过其内部 DMP 处理单元直接获取姿态解算的四元数结果。本篇将采用软件解算方式，利用欧拉角与旋转矩阵对陀螺仪和加速度计的原始数据进行姿态求解，并对两种姿态进行互补融合，最终得到 IMU 的实时姿态。

本篇姿态解算选用的旋转顺序为ZYX，即 IMU 坐标系初始时刻与大地坐标系重合，随后依次绕自身 Z、Y、X 轴旋转。现自定义各次旋转的名称与符号：

绕 IMU 的Z 轴旋转：航向角 yaw，转动 $y$ 角度
绕 IMU 的Y 轴旋转：俯仰角 pitch，转动 $p$ 角度
绕 IMU 的X 轴旋转：横滚角 roll，转动 $r$ 角度

三次旋转的示意图如下：

此外，横滚 roll、俯仰 pitch、偏航 yaw的实际含义如下图所示：

在这里插入图片描述

旋转矩阵

旋转矩阵相关知识可参阅 [3 维旋转矩阵推导与助记 ]，此处仅列出本篇所需的 3 个旋转矩阵，需注意这些矩阵为坐标变换的旋转矩阵：

欧拉角旋转

欧拉角旋转相关知识可参阅 [欧拉角旋转]，此处需说明的是，本篇采用绕自身运动轴、以 ZYX 顺序的旋转方式。

加速度计解算姿态角

加速度计测量的是其感受到的加速度。静止状态下，加速度计无加速运动，但受重力加速度作用，根据相对运动理论，其感受到的加速度与重力加速度方向相反，即读取数据为竖直向上。加速度计英文简写为 acc，下文以 首字母 $a$ 代表加速度计数据。

加速度计利用静止时感受到的重力加速度计算姿态：

当加速度计水平放置（Z 轴竖直向上）时，Z 轴可读取 $1 g$ 数值（ $g$ 为重力加速度），X 轴与 Y 轴读取值为 0，可记为 $(0, 0, g)$ 。
当加速度计旋转至某一姿态时，重力加速度会在其 3 个轴上产生相应分量，其本质是大地坐标系下的 $(0, 0, g)$ 向量在加速度计自身坐标系下的坐标，加速度计读取的 3 个数值即为该向量的新坐标。

姿态旋转采用 ZYX 顺序的三次旋转方式，上述描述可表示为：

求解该方程可得 roll 角与 pitch 角（绕 Z 轴旋转时，重力加速度感受无变化，故加速度计无法计算 yaw 角）：

在这里插入图片描述

三次旋转过程的分解如下图所示：

陀螺仪解算姿态角

陀螺仪测量绕 3 个轴的转动角速度，对角速度积分可得角度。陀螺仪英文简写为 gyro，下文以**首字母 $g$ **代表陀螺仪数据。

如下图所示，IMU 在第 $n$ 时刻的姿态角为 $r$ 、 $p$ 、 $y$ ，表示 IMU 坐标系从初始位置经绕 Z 轴旋转 $y$ 角度、绕 Y 轴旋转 $p$ 角度、绕 X 轴旋转 $r$ 角度后得到最终姿态。需计算下一时刻（ $n + 1$ ）的姿态，设 $n + 1$ 时刻姿态角为 $r+\Delta r$ 、 $p+\Delta p$ 、 $y+\Delta y$ ，该姿态同样经三次旋转得到。计算 $n + 1$ 时刻姿态时，只需在 $n$ 时刻姿态基础上叠加对应姿态角变化量，而姿态角变化量可通过角速度与采样时间周期积分得到：

此处红框内 $\mathrm{d}r/\mathrm{d}t$ 等角速度为假想角速度，用于姿态更新。姿态更新以大地坐标系为参考，而陀螺仪在第 $n$ 状态读取的角速度以自身坐标系为参考，需将读取的 gyro 数据进行变换后，方可用于计算更新第 $n + 1$ 次姿态。

$\mathrm{d}r/\mathrm{d}t$ 等角速度的物理意义可通过下图分解三次旋转理解：

角速度坐标变换

1. $\mathrm{d}y/\mathrm{d}t$ 的物理意义

$\mathrm{d}y/\mathrm{d}t$ 本质是四旋翼三次旋转过程中，绕 Z 轴的偏航角（yaw 角）角速度，是后续角速度坐标变换的起始关键量。

2. 初始坐标系下的角速度表达（状态 ①）

基准向量定义：三次旋转的初始坐标系（状态①）中，Z 轴方向的单位向量可表示为 $0\ 0\ 1]^T$ （其中 $T$ 为向量转置符号）。
角速度向量形式：基于上述基准，绕 Z 轴的角速度 $\mathrm{d}y/\mathrm{d}t$ 在状态①坐标系下，可直接表示为向量形式 $[0\ 0\ \mathrm{d}y/\mathrm{d}t]^T$ 。

3. 角速度的坐标变换逻辑

变换触发条件：状态①坐标系并非最终姿态坐标系，后续还需依次经历绕 Y 轴的旋转与绕 X 轴的两次旋转，最终到达状态③坐标系。因此，初始角速度向量 $[0\ 0\ \mathrm{d}y/\mathrm{d}t]^T$ 需同步完成两次坐标变换，以匹配最终姿态的参考系。
变换结果定义：经过两次坐标变换后， $\mathrm{d}y/\mathrm{d}t$ 在状态 ③ 坐标系中形成的等效转动角速度，用向量 $g_{x_Z}\ g_{y_Z}\ g_{z_Z}]^T$ 表示。该向量的物理意义是：状态 ① 坐标系中绕 Z 轴的角速度，在状态③坐标系下的新坐标表达。

同理， $\mathrm{d}p/\mathrm{d}t$ 需经历 1 次旋转变换，而 $\mathrm{d}r/\mathrm{d}t$ 无需旋转变换。

将 $\mathrm{d}y/\mathrm{d}t$ 、 $\mathrm{d}p/\mathrm{d}t$ 、 $\mathrm{d}r/\mathrm{d}t$ 均变换至状态 ③ 坐标系中的新坐标并求和，结果即为状态 ③ 时刻陀螺仪读取的 gyro 数据。

因此，从 $\mathrm{d}r/\mathrm{d}t$ 等角速度到陀螺仪读取角速度 $g_x$ 等的转换关系推导如下：

进一步地，将状态③理解为状态 $n$ ，则根据状态 $n$ 时刻读取的陀螺仪数据反解 $\mathrm{d}y/\mathrm{d}t$ 等角速度数据，即可更新得到状态 $n + 1$ 的姿态。反解过程即求逆矩阵：

姿态融合

由上述分析可知，加速度计在静止时可根据重力加速度计算 roll 角与 pitch 角，且角度计算仅与当前姿态相关；陀螺仪通过对时间间隔内角速度积分得到角度变化量，叠加至上一时刻姿态角得到新姿态角，可计算 roll、pitch、yaw 三个角度。

实际应用中，加速度计仅在静止时能得到较准确姿态，陀螺仪对转动时的姿态变化敏感，但陀螺仪自身误差经连续时间积分后会不断累积。因此需结合两者计算的姿态进行互补融合，此处仅能对 roll 角与 pitch 角融合（加速度计无法获取 yaw 角）：

$K$ 为比例系数，需根据实际情况调整，例如选取 0.4。

MATLAB 公式推导

上述部分推导计算过程可借助 MATLAB 辅助完成，以避免手工计算错误：

定义 3 个旋转矩阵

% 旋转顺序：Z,Y,X（从大地坐标系到 IMU 坐标系）

% 定义符号 r=roll p=pitch y=yaw
syms r p y

% 3 个旋转矩阵（坐标系旋转,注意负号位置！）
M_x = [  1,       0,      0;
         0,  cos(r), sin(r);
         0, -sin(r), cos(r)];

M_y = [cos(p),   0, -sin(p);
            0,   1,       0;
       sin(p),   0,  cos(p)];

M_z = [ cos(y),  sin(y),  0;
       -sin(y),  cos(y),  0;
            0,       0,   1];

推导陀螺仪的变换矩阵

%% 推导陀螺仪的变换矩阵

% 定义符号 drdt dpdt dydt 分别为 roll pitch yaw 对时间的导数（角速度）
syms drdt dpdt dydt

% 绕 x 轴转动 roll 的角速度
dr_t = [drdt;
           0;
           0];

% 绕 y 轴转动 pitch 的角速度
dp_t = [   0;
        dpdt;
           0];

% 绕 z 轴转动 yaw 的角速度
dy_t = [   0;
           0;
        dydt];

% [矩阵 X*矩阵 Y*yaw 角速度 (绕 Z)] + [矩阵 X*pitch 角速度 (绕 Y)] + [roll 角速度 (绕 X)]
% IMU_gyro 为 IMU 测得的 3 个陀螺仪数据
IMU_gyro = M_x*M_y*dy_t + M_x*dp_t + dr_t;
fprintf('M_x*M_y*dy_t + M_x*dp_t + dr_t=\r\n')
disp(IMU_gyro)
% roll pitch yaw 角速度组成的列向量（大地坐标系的 3 个角速度）
rpy_t = [drdt;
         dpdt;
         dydt];

% 手动分解 IMU_gyro 为矩阵 M_gyro 与列向量 rpy_t 相乘形式
% 根据 IMU_gyro 构造 M_gyro（大地坐标系角速度转换为 IMU 坐标系角速度的矩阵）
M_gyro = [ 1,     0,       -sin(p);
           0, cos(r),cos(p)*sin(r);
           0,-sin(r),cos(p)*cos(r)];
% 验证
if M_gyro * rpy_t==IMU_gyro
    fprintf('M_gyro is true\r\n');
else
    fprintf('M_gyro is false\r\n');
end
fprintf('M_gyro=\r\n')
disp(M_gyro)

% 求 M_gyro 的逆矩阵（IMU 坐标系角速度转换为大地坐标系角速度）
M_gyro_inv = inv(M_gyro);
fprintf('M_gyro_inv=\r\n')
disp(M_gyro_inv)

 % MATLAB 求解的逆矩阵需手工化简
M_gyro_inv_ =[ 1, (sin(p)*sin(r))/cos(p), (cos(r)*sin(p))/cos(p);
               0,                 cos(r),                -sin(r);
               0,          sin(r)/cos(p),          cos(r)/cos(p)];
 % 验证 M_gyro_inv_ * M_gyro 应为单位矩阵
fprintf('M_gyro_inv_ * M_gyro=\r\n')
disp(M_gyro_inv_ * M_gyro)
fprintf('M_gyro_inv_ =\r\n')
disp(M_gyro_inv_)

运行输出结果：

M_x*M_y*dy_t + M_x*dp_t + dr_t=

               drdt - dydt*sin(p)
 dpdt*cos(r) + dydt*cos(p)*sin(r)
 dydt*cos(p)*cos(r) - dpdt*sin(r)

M_gyro is true

M_gyro=

[ 1,       0,       -sin(p)]
[ 0,  cos(r), cos(p)*sin(r)]
[ 0, -sin(r), cos(p)*cos(r)]

M_gyro_inv=

[ 1, (sin(p)*sin(r))/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2), (cos(r)*sin(p))/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2)]
[ 0,                        cos(r)/(cos(r)^2 + sin(r)^2),                       -sin(r)/(cos(r)^2 + sin(r)^2)]
[ 0,          sin(r)/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2),          cos(r)/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2)]

M_gyro_inv_ * M_gyro=

[ 1,                   0, cos(r)^2*sin(p) - sin(p) + sin(p)*sin(r)^2]
[ 0, cos(r)^2 + sin(r)^2,                                          0]
[ 0,                   0,                        cos(r)^2 + sin(r)^2]

M_gyro_inv_ =

[ 1, (sin(p)*sin(r))/cos(p), (cos(r)*sin(p))/cos(p)]
[ 0,                 cos(r),                -sin(r)]
[ 0,          sin(r)/cos(p),          cos(r)/cos(p)]

程序首先计算 M_x*M_y*dy_t + M_x*dp_t + dr_t，再手工分解为 M_gyro 与 rpy_t 相乘形式，最终求得 M_gyro 的逆矩阵 M_gyro_inv_，即实现 IMU 坐标系陀螺仪角速度值到大地坐标系的转换矩阵。

推导加速度计的变换矩阵

%% 推导加速度计的变换矩阵

M_acc=M_x*M_y*M_z;
fprintf('M_acc=\r\n')
disp(M_acc)

% 重力向量
syms g
acc = [0;
       0;
       g];
IMU_acc=M_acc*acc;
fprintf('IMU_acc=\r\n')
disp(IMU_acc)

运行输出结果：

M_acc=

[                        cos(p)*cos(y),                        cos(p)*sin(y),       -sin(p)]
[ cos(y)*sin(p)*sin(r) - cos(r)*sin(y), cos(r)*cos(y) + sin(p)*sin(r)*sin(y), cos(p)*sin(r)]
[ sin(r)*sin(y) + cos(r)*cos(y)*sin(p), cos(r)*sin(p)*sin(y) - cos(y)*sin(r), cos(p)*cos(r)]

IMU_acc=

       -g*sin(p)
 g*cos(p)*sin(r)
 g*cos(p)*cos(r)

计算得到的 IMU_acc 为加速度计感受到的重力加速度向量在 IMU 最终姿态坐标系中的坐标。

加速度计解算姿态角

卢平光原创已于 2022-07-20 17:52:47 修改

姿态角（欧拉角）

欧拉角主要用于描述刚体在固定三维参考坐标系下绕三轴任意旋转运动的角度量化方式，其为相对量，表征运动的变化形式。具体可参阅欧拉角说明。

在这里插入图片描述

图中 $x yz$ 为固定参考系坐标， $X Y Z$ 为刚体自有坐标系（假定遵循右手定则：大拇指指向某轴正方向，其余手指握拳方向为姿态角变化正方向），定义：

绕固定坐标系 $z$ 轴旋转：航向角 yaw，转动 $y$ 角度
绕固定坐标系 $y$ 轴旋转：俯仰角 pitch，转动 $p$ 角度
绕固定坐标系 $x$ 轴旋转：横滚角 roll，转动 $r$ 角度

欧拉角（姿态角）为相对各轴的角度偏移，假定旋转顺规为 Z-Y-X，其旋转矩阵表示为（其中 $y$ 代表 $\alpha$ 、 $p$ 代表 $\beta$ 、 $r$ 代表 $\gamma$ ）：

在这里插入图片描述

重力倾角

IMU（惯性测量单元）通常包含三轴陀螺仪与三轴加速度计，一般定义 IMU 的 Z 轴与重力方向垂直且反向。当加速度计水平放置（Z 轴竖直向上）时，Z 轴可读取 $1 g$ 数值（ $g$ 为重力加速度），X 轴与 Y 轴读取值为 0，可记为 $(0, 0, g)$ 。倾角为以重力为参考的绝对量，例如出现如下倾角时：

请添加图片描述

根据力的分解原理，X 轴存在重力分量：

请添加图片描述

同理，当传感器与 X、Y 轴存在夹角时，三轴重力分量均会变化：

请添加图片描述

参考 ADI 加速度计说明，可根据重力分量反推夹角值：

请添加图片描述

重力倾角 $\theta$ 、 $\phi$ 、 $\psi$ 与欧拉角定义不同，其仅表征传感器最终状态相对三轴的夹角关系，无法描述转动过程。因此，由加速度计计算欧拉角需采用其他方法。

重力计算欧拉角

设 $a_x$ 、 $a_y$ 、 $a_z$ 为三轴重力分量，可视为向量 $[0, 0, g]$ 经三轴旋转所得，满足：

在这里插入图片描述

解算结果：

在这里插入图片描述

公式推导表明 yaw 角无法解算，实际中绕 z 轴旋转时各轴重力分量无变化，故加速度计无法获取 yaw 角。

陀螺仪计算欧拉角

参考

四旋翼无人机的控制原理

helloworld7741 原创于 2024-08-03 21:10:10 发布

一、四旋翼无人机飞行原理

在这里插入图片描述

四旋翼无人机中，相邻两个螺旋桨旋转方向相反。螺旋桨 M1、M3 为逆时针旋转，螺旋桨 M2、M4 为顺时针旋转。飞行时，M2、M4 产生的逆时针反作用力（反扭矩）与 M1、M3 产生的顺时针反作用力（反扭矩）相互抵消，使机身保持稳定。在保证四旋翼无人机各旋翼转速相同（总扭矩为零）的基础上，通过调整四个旋翼的转速实现各类运动。

1、垂直运动

在这里插入图片描述

各旋翼增加/减小相同转速时，无人机实现垂直上升/下降运动。转速增加使升力大于重力，无人机垂直上升；反之则下降。

2、俯仰运动

在这里插入图片描述

如图所示，增加电机 M1、M4 转速或减小 M2、M3 转速时，前两旋翼升力大于后两旋翼，四旋翼向后飞行；反之向前飞行。

3、滚转运动

在这里插入图片描述

与俯仰运动原理一致，如图所示，增加电机 M1、M2 转速或减小 M3、M4 转速时，左两旋翼升力大于右两旋翼，四旋翼向右飞行；反之向左飞行。

4、偏航运动

在这里插入图片描述

增加电机 M2、M4 转速或减小 M1、M3 转速时，M2、M4 旋翼升力提高，M1、M3 旋翼升力下降，总合力不变但顺时针转速大于逆时针转速，产生顺时针力矩使无人机向右旋转（右偏航）；反之无人机向左旋转（左偏航）。

二、四旋翼无人机控制原理

1.姿态控制

四旋翼无人机存在姿态与平动的耦合关系、模型参数不确定性及外界扰动，仅实现姿态稳定控制方可完成航迹的有效跟踪。

在四旋翼无人机自主控制系统中，姿态稳定控制是实现自主飞行的基础，其任务为控制三个姿态角（俯仰角、滚转角、偏航角）稳定跟踪期望姿态信号，并保证闭环姿态系统具备期望的动态特性。

基于四旋翼无人机姿态与平动的耦合特点，分析可知，只有保证姿态稳定控制，旋翼总升力方可在期望方向产生分量，进而控制飞行器沿期望航迹飞行。

由于通道中存在模型误差、环境干扰、观测误差等不确定因素，系统闭环性能会受到影响。因此设计无人机控制系统时，需考虑系统的抗干扰性能（闭环系统的鲁棒性），可通过设计干扰补偿器对干扰进行逼近与补偿，以实现姿态角的稳定跟踪。

在这里插入图片描述

2、PID 控制

姿态控制主要针对俯仰角、偏航角、横滚角三个角度进行，单级控制方式如下：

在这里插入图片描述

单级控制存在电机转速与升力非线性导致的失衡、跟随滞后等问题，引入针对姿态角变化率（角速度）的 PID 控制算法，可缓解单级控制引发的速度失衡问题：

在这里插入图片描述

3、PID 控制算法

PID 算法区别于传统单一比例传递，由比例、微分、积分环节共同作用：

在这里插入图片描述

比例环节（P）：对应误差的比例部分，P 单元控制无人机的动力输出，比例环节可提供大部分动力。若比例系数 $K_p$ 过小，系统调节时间 $t_p$ 过长，响应迟缓；若 $K_p$ 过大，系统易产生振荡，此时需引入微分环节抑制振荡。通常先设置较大的 $K_p$ 以增强无人机的响应能力。

积分环节（I）：对应误差的积分部分，I 单元用于纠正外界干扰或系统误差，确保最终输出符合预期。若积分系数 $K_i$ 过小，系统会产生稳态误差；若 $K_i$ 过大，系统易出现振荡。一般可将 $K_i$ 设置为较大值，使飞行器快速从误差状态恢复至目标状态。

微分环节（D）：对应误差的微分部分，D 单元通过微分方法计算运动速度，起到类似阻尼的作用以减小系统变化。若微分系数 $K_d$ 过小，系统易产生振荡；若 $K_d$ 过大，系统阻尼过大会导致响应变慢、运动迟缓。 $K_d$ 的取值需根据无人机应用场景调整：若追求运动稳定，可设置较大的 $K_d$ ；若要求响应迅速，则设置较大的 $K_p$ 与较小的 $K_d$ 。