四轴III——飞控算法 | Cry̵̴̷̧̛͠stal P̷̢͢͢͜͝uNK's Blog

type

status

date

slug

summary

整理思路

想让黑鹳平稳的飞起来？最简单的想法就是让四个旋翼同时以同样大的速度转就好了嘛。这样，四个轴有相同大小的力，合力刚好和重力抵消不久悬停在空中了吗？但是实际上并没有这么简单：飞机的重心并不一定精准和几何中心重合；桨叶间气流可能互相干扰；飞行环境中不可能完全没风；飞起来时会奇妙的自旋……总而言之，如果我们的目的是让飞机稳定的悬停在空中（暂且不考虑定点悬停，只是在空中让他保持姿态稳定）的话，那我们就需要从两方面下手：一是让飞机拥有控制自己的能力；二是让飞机知道自己目前的姿态，让它与目标姿态对比，它才知道往哪个方向控制自己。如果从物理学做功的角度思考，不仅需要有力，这个力还需要找对方向才能高效的准确的做功。这样一来，这学期的任务点思路清晰了起来。

首先是让飞机拥有控制自己的能力：

控制些什么东西呢？

不动、向前、向后、向上、向下、向左、向右、转向

保持不动其实可以分解为不停的向前、向后、向上、向下、向左、向右、转向

运动学上的向前、向后、向上、向下、向左、向右，从动力学上分析就需要使得合力向前、向后、向上、向下、向左、向右。而四轴飞行器的框架设计决定其只能通过“侧身”——改变其俯仰角、横滚角来取得目标运动方向的合外力。

很简单，比如向前的话，我们需要让飞机向前倾斜。第一个要改变的是，让后面两个轴转的比前面两个轴快些，自然就向前倾了；第二个要改变的是，需要调大四个旋翼的总推力，因为此时需要推力的铅直方向的分力与重力平衡，如果不增大总推力，那么前倾的时候就会掉高度。

转向呢？目标是保持机身与水平面平行的同时，使机身绕着过质心的z轴自旋。四轴并没有单独设计一个推进器来完成这个动作——我们可以利用四个高速旋转的旋翼带来的转动惯量。

首先需要满足飞机不要一起飞就自旋——合转动惯量要为0。解决这个问题很简单：我们让处在一条对角线上的两个电机顺时针旋转，处在另一条对角线上的两个电机逆时针旋转即可。
想要机身顺时针自旋，那么让顺时针的电机转的比逆时针快点就好，反之亦然。

其次是让飞机知道自己的姿态

飞机怎么知道自己的姿态？——听说GY86这款传感器就是收集姿态信息的诶！

说了这么久姿态，但是tmd怎么描述姿态？

如同前文那样“前倾”、“自旋”，未免显得太不精确了点，而且计算机读不懂呀！我们又没在上面跑ChatGPT，它听不懂人类的自然语言的！计算机的本质，即用数学描述世界，解决问题。自然而然，我们便需要一套数学方法来描述“姿态”——刚体运动、欧拉角、轴角法与四元数

我们找到方法来描述姿态了！那我们需要将GY86传来的数据 “转换” 为使用这套方法描述的姿态！——姿态结算

GY86感觉到的是什么？

Digital-output X-, Y-, and Z-Axis angular rate sensors (gyroscopes) with a user-programmable full-scale range of ±250, ±500, ±1000, and ±2000°/sec
Digital-output triple-axis accelerometer with a programmable full scale range of ±2g, ±4g, ±8g and ±16g
12-Bit ADC Coupled with Low Noise AMR Sensors Achieves 5 milli-gauss Resolution in ±8 Gauss Fields

怎么把加速度、角速度等元数据转换为用某套方法描述的 “姿态” ？

高斯牛顿迭代、梯度下降等数据处理
误差怎么办？

这下飞机知道自己的姿态了。但是目标姿态是什么？—— 遥控器！

每个通道控制什么？油门？欧拉角？安全措施？
遥控器给出的姿态怎么转换成用某套方法描述的 “姿态” ？
不用人来决定？——具身智能

知道了自己的姿态和目标的姿态，差了多少？怎么调整？——PID控制器

….

好了，这下把逻辑理清楚了。开始逐个突破吧！

1. 怎么描述姿态？

1.1 刚体运动 /Rigid Body

1.1.1 参考系与坐标系

参考系是参照物的数学抽象，有几种经典的坐标系：

直角坐标系

很经典的坐标系，分为左手系和右手系

大拇指指向的方向为x轴正方向，食指为y，中指为z

自然坐标系

在已知运动轨迹上任取一点为坐标原点，用质点距离原点的轨道长度来确定质点任意时刻的位置，以轨迹切向和法向的单位矢量作为其独立的坐标方向，这样的坐标系，称为自然坐标系，称为自然坐标。

极坐标系

在一固定直线上选取一点作为坐标原点，以点为端点做射线，称由射线、原点和固定直线构成的坐标系为极坐标系，通常称射线为极轴。

1.1.2 质点与刚体

回忆大学物理：

staff.ustc.edu.cn

http://staff.ustc.edu.cn/~dinggj/lectures/mechanics/Chapter8.pdf

质点模型：当物体的线度（大小和几何形状）对所研究物体运动状态的影响可以忽略不计时，用一个集中了物体所有质量的数学点来代表物体的运动状态，该点称为质点。

刚体模型：当物体的形变对其运动状态的影响可以忽略不计时，将物体看作为一个不发生形变的几何体。

可以窥见，刚体相比质点多了一个物体本身的几何形状，这也是我们大学学习的刚体运动和高中学习的质点运动的核心区别。对于机械运动的研究，只局限于质点的情况是很不够的。物体是有形状大小的，它可以作平动、转动，甚至更复杂的运动。如果物体在外力的作用下形状体积改变很小时，形变可以忽略。我们就得到实际物体的另外一个抽象模型——刚体。

刚体：在任何外力作用下，形状大小均不发生改变的物体。

刚体运动包含了平移运动和定轴转动，分别对应着几组概念：质量与转动惯量、速度与角速度、加速度与角加速度——前者属于线量、后者属于角量。

刚体平动：刚体运动时，连接刚体内任意两点的直线在任意时刻都保持平行的运动

定轴转动：刚体运动时，刚体上的两点固定不动，根据刚体的定义可知，这两点连线上的所有点也静止不动，过这两点的直线称为转轴，这种运动称为定轴转动

选择质点运动模型来描述无人机的运动显然是不足的——无法描述无人机绕x、y、z轴的定轴转动现象。

1.1.3 描述一个刚体的状态

平面：两个平动参数、和一个转动参数，需要三个自由度

空间：三个平动参数、、和三个转动参数、、，需要六个自由度

自由度（Degree of Freedom）：决定某系统在空间中位置所需要的独立坐标的数目

更数学化的描述

利用各个自由度的微分，将位移（displacement）和姿态（orientation）转换到速度（velocity）和加速度（acceleration）等运动状态。一次微分获得速度，二次微分获得加速度。

如何搭建人类感知的 “前倾” 与芯片处理的二进制代码之间的桥梁？答案是数学。数学刻画世界，代码表达数学。用数学描述它的第一步，那便是将其放到坐标系里了。

取一个初始状态吧：我们使用空间直角坐标系进行建模，设我们无人机的初始坐标为，初始方向为。

第一，我们犯了一个经验上的错误。

我们不能直接将以前在二位空间中使用单个向量来表达刚体在三位空间中的向量：假设一个刚体类似于一架飞机。你可以用飞机的机头方向（如指向的向量）来表示飞机的前进方向（如“指向北方”），但这并不能告诉你飞机是否“翻滚”了（即绕着机头方向旋转）。飞机可以机头指向北方，但它可能正倒飞、水平飞行或翻滚飞行。这说明仅仅知道一个向量（机头方向）不足以描述飞机的完整姿态。

第二，有没有办法统一的描述刚体的平动和转动呢？我们可以不针对平动和转动分别写两个式子吗？

方案：直接在刚体这里建一个坐标系，常将其建立在质心上。平动，由坐标系的原点位置判定；转动，由坐标系的姿态判定。

参考系（固定系）又称实验室参考系，是静止不动的坐标系（固连系）则固定于刚体，随着刚体的旋转而转动

想要描述好这个不可忽视几何的刚体，我们得把无人机看作是一个独立的三维坐标系才行。毕竟无人机是一个三维物体，我们得用三维坐标系才能描述清楚无人机。无人机的各种旋转，也就是这个坐标系相对于大地参考系的旋转。

下面，我们先来讨论用数学来描述刚体旋转的几个数学方案。

1.2 使用向量来描述刚体平动

以向量来描述坐标系 B 相对于 A 的状态。

比如下图，我们使用一个具有三个分量的向量来表达其相较于参考系的位移。

1.3 使用欧拉角描述刚体转动 Euler Angles

莱昂哈德·欧拉用欧拉角来描述刚体在三维欧几里得空间的取向。对于任何参考系，一个刚体的取向，是依照顺序，从这参考系，做三个欧拉角的旋转而设定的。所以，刚体的取向可以用三个基本旋转矩阵来决定。换句话说，任何关于刚体旋转的旋转矩阵是由三个基本旋转矩阵复合而成的。

对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现。

1.3.1 旋转顺序&顺归与本归（外旋与内旋）

在旋转问题中，有两个关键因素决定了旋转结果

旋转顺序是指在三维旋转中，绕不同坐标轴进行的旋转操作的执行顺序。在三维空间中，物体的旋转通常是通过多个轴的组合旋转来实现的。不同的旋转顺序会导致不同的最终结果。

旋转顺序

例如，假设你有一个物体，你可以依次绕 x、y、z 轴旋转。你可以选择不同的顺序来执行这些旋转，如：

XYZ 顺序：先绕 x 轴旋转，再绕 y 轴旋转，最后绕 z 轴旋转。

ZYX 顺序：先绕 z 轴旋转，再绕 y 轴旋转，最后绕 x 轴旋转。

每种旋转顺序会导致不同的最终姿态，因此选择旋转顺序是非常重要的。

顺归本归

顺归旋转（extrinsic rotation）：也称为外旋，是指相对于固定的全局坐标系进行的旋转。每次旋转都围绕全局坐标系的固定轴（例如固定的 x、y、z 轴）。
本归旋转（intrinsic rotation） ：也称为内旋，是指相对于物体自身的局部坐标系进行的旋转。每次旋转后，物体的坐标系会随之更新，下一次旋转是相对于物体自身的新坐标系进行的。

下面介绍两种最常见的欧拉角。

1.3.2 ZXZ顺归的欧拉角

参阅右图。设xyz轴为参考系的参考轴，XYZ轴为物体自身的坐标轴，xoy平面与XOY平面的相交线为交点线，用（N）表示。zxz顺规的欧拉角可以静态地这样定义：

（进动角）是x-轴与交点线的夹角

（章动角）是z-轴与Z-轴的夹角，

（自旋角）是交点线与X-轴的夹角。

对于夹角的顺序和标记，夹角的两个轴的指定，并没有任何常规。科学家对此从未达成共识。每当用到欧拉角时，我们必须明确的表示出夹角的顺序，指定其参考轴。

实际上，有许多方法可以设定两个坐标系的相对取向。欧拉角方法只是其中的一种。此外，不同的作者会用不同组合的欧拉角来描述。因此，使用欧拉角前，必须先做好明确的定义。

这种欧拉角常用于天体运动、图形学、摄像机操控等，在这些领域中，旋转通常是相对于全局坐标系进行的。

三个欧拉角： ,其中蓝色为xyz-轴，红色的轴为XYZ-坐标轴，绿色的线是交点线(N)。

可以想象成两个圆平面初始为重合（xy和XY），其中一个先在这个平面内自旋，得到，然后开始绕x轴翻转角度，随后再绕z自旋角度。这样能很直观的得到三个欧拉角的范围：的范围是 , 值的范围为。即 Z-X'-Z'' 的旋转顺序（zxz顺归）

1.3.3 ZYX本归的欧拉角

在机器人运动学和姿态控制中，通常使用的是ZYX本归旋转（也称为 Yaw-Pitch-Roll 顺序的本归旋转）。这是因为这种旋转顺序和表示方式适合描述机器人在三维空间中的实际运动，尤其是机械臂、无人机、移动机器人的姿态变化。我们令机头方向为 x 轴正方向，垂直机体向上为 z 轴正方向的空间直角坐标系，zyx本规的欧拉角可以静态地这样定义：

（Yaw，偏航角）是绕自身 z 轴的旋转

（Pitch，俯仰角）是绕自身 y 轴的旋转

（Roll，横滚角）是绕自身 x 轴的旋转

横滚、俯仰、偏航 多用于航空航天、无人机、机器人和车辆控制领域，提供了更为直观的姿态描述。

旋转是围绕物体自身的轴进行的。每次旋转后，物体的坐标系会发生变化，下一次旋转是基于物体当前的姿态（即局部坐标系）进行的。

1.3.4 欧拉角的优缺点

优点：

很直观，也便于手动设定姿态。

缺点：

万像锁问题

从初始态到目标态的转动方案并不唯一。存在多种转动方案来达到目标姿态，假如从状态A到状态B，其中俯仰角的变化达到时，那么其实可以只用改变横滚角，就能达到同时改变横滚角和偏航角的效果。换句话说，z 轴被架空了！绕不绕 z 轴其实无所谓！使得系统失去了绕某一轴独立旋转的自由度。再换句话说，当系统调节三个角度时，却只有两个轴向受到了影响。

【Unity编程】欧拉角与万向节死锁（图文版）_万向锁-CSDN博客

文章浏览阅读5.7w次，点赞178次，收藏465次。本文详细探讨了Unity中欧拉角与万向节死锁问题，通过陀螺仪的工作原理进行解释，并通过小程序演示了死锁过程。在特定的欧拉角旋转顺序下，会出现自由度丢失，即万向节死锁。建议在编程时谨慎直接修改Transform的欧拉角，以避免不可预测的结果。

https://blog.csdn.net/andrewfan/article/details/60981437

不适合复杂旋转

欧拉角表示旋转时，旋转顺序非常重要，因为三维旋转是非交换的（即旋转的顺序会影响最终结果）。例如，绕 x 轴、y 轴、z 轴的旋转顺序不同，物体的最终姿态也不同。这种旋转的非交换性使得欧拉角在处理多个旋转时变得复杂。
不同的旋转顺序会导致完全不同的姿态。为了正确计算每次旋转对物体姿态的影响，必须精确地追踪旋转后的坐标系，而这在欧拉角表示中并不是那么直接和方便。
欧拉角的每次旋转都依赖于之前的旋转结果，这意味着我们无法简单地将多个旋转角度相加来得到最终的旋转结果。如果想要将两组欧拉角的旋转组合起来，不能简单地将每个轴的旋转角度相加。每次旋转会改变物体的局部坐标系，因此你需要重新计算每个旋转角所依赖的轴。

1.4 使用轴角法描述刚体转动 Angle-axis

轴角法（Axis-Angle Representation）是一种用于描述三维空间中刚体旋转的表示方法。它通过一个旋转轴和一个旋转角度来唯一地定义一个旋转。轴角法非常直观且简洁，是描述刚体旋转的经典方法之一，广泛应用于计算机图形学、机器人学、航天器姿态控制等领域。

1.4.1 轴角法的基本概念

在三维空间中，任意一个旋转都可以看作是绕某一个固定轴旋转一个角度。轴角法通过以下两个部分来描述旋转：

旋转轴：一个单位向量，表示旋转发生的方向。这是一个单位向量，即 ∣n∣=1。

旋转角度：一个标量 θ，表示物体绕该轴旋转的角度，单位通常是弧度（radians），但在某些应用中也可以使用度数（degrees）。

因此，轴角法的旋转表示可以写为：

其中表示旋转的轴，是一个方向向量，表示绕该轴的旋转角度。

1.4.2 具体细节

右手坐标系

在轴角的表示方法中，一个旋转的定义需要使用到四个变量：旋转轴 u 的 𝑥, 𝑦, 𝑧 坐标，以及一个旋转角 θ，也就是我们一共有四个自由度 (Degree of Freedom)．这很明显是多于欧拉角的三个自由度的．实际上，任何三维中的旋转只需要三个自由度就可以定义了，为什么这里我们会多出一个自由度呢？

其实，在我们定义旋转轴的 𝑥, 𝑦, 𝑧 坐标的同时，我们就定义了的模长（长度）．然而，通常情况下，如果我们说绕着一个向量 u 旋转，我们其实指的是绕着所指的方向进行旋转．回忆一下向量的定义：向量是同时具有大小和方向的量，但是在这里它的大小（长度）并不重要．我们可以说绕着这个轴进行旋转，也可以说绕着旋转．虽然这两个向量完全不同，但是它们指向的都是同一个方向（即 𝑧 轴的方向）：

为了消除旋转轴 u 模长这个多余的自由度，我们可以规定旋转轴 u 的模长为，也就是说是一个单位向量．这样一来，空间中任意一个方向上的单位向量就唯一代表了这个方向．我们其实可以将模长规定为任意的常量，但是规定能为我们之后的推导带来很多的便利，这也是数学和物理中对方向定义的惯例。

在实际代码的实现中，你也可以让用户输入一个非单位长度的旋转轴向量，但是在进行任何的计算之前你必须要记得先将它转化为一个单位向量：

四元数与三维旋转.pdf

442.2KB

1.4.3 轴角法的优点

直观性：轴角法直接描述了旋转的轴和角度，非常直观。我们在物理上经常见到这种旋转：比如地球绕地轴旋转，飞轮绕轴旋转等。

简洁性：轴角法只需要 4 个参数（3 个轴向量分量 + 1 个旋转角度），相比于旋转矩阵的 9 个参数更加简洁。

无冗余：旋转矩阵的 9 个元素中有冗余，而轴角法没有冗余，能够唯一地描述三维旋转。

无万向锁问题：欧拉角在处理某些姿态时会遇到万向锁问题，导致系统失去一个自由度。轴角法避免了这个问题，适合处理复杂的旋转。

易于转换：轴角法可以轻松转换为旋转矩阵、四元数等其他旋转表示形式。

1.4.3 轴角法的缺点

不适合连续复合旋转：轴角法适合描述单次旋转，但在处理多次复合旋转时，旋转矩阵或四元数更方便，因为它们可以通过矩阵乘法或四元数乘法直接组合多个旋转。

数值不稳定性：当旋转角度非常小时，轴角法在数值上可能会不稳定，尤其是在计算旋转轴时可能出现数值误差。它不适合用来做插值，因为用它得到的插值结果会导致插值不平滑，甚至产生跳跃，这一点与欧拉角描述时相同

在三维旋转的计算和动画中，插值（Interpolation）是一种常见的技术，用于平滑地过渡物体在两个姿态（旋转状态）之间的运动。插值的目标是生成一系列中间状态，使物体从初始姿态平滑地旋转到目标姿态，而不会出现突兀的跳跃或不自然的运动。

1.5 使用旋转矩阵描述刚体转动 Rotation Matrix

1.5.1 旋转矩阵的概念

转动：描述 {B} 相对于 {A} 的姿态——Rotation Matrix

刚体坐标系的三个基向量的方向可以唯一确定刚体在某空间中的姿态。那么我们做的第一步就是把这三个基向量一列一列的。排在一起，变成一个矩阵。这个矩阵就是坐标系{B}相对于{A}的旋转矩阵：

这里的指的是，刚体坐标系 {B} 的 X 轴方向的基向量，放在参考系{A}里后的方向。也就是求基向量的三个分量相对于参考系{A}变成多少，也就是求 相对与坐标系{A}三个轴的投影：

【例】：{B} 相对于 {A} 的姿态是多少？

{B} 的 x 轴为 {A} 的 z 轴反向

{B} 的 y 轴与 {A} 的 y 轴重合

{B} 的 z 轴与 {A} 的 x 轴重合

蓝色：参考系{A}；红色：坐标系{B}

因此, {B} 相对于 {A} 的姿态为：

1.5.2 旋转矩阵的特性

旋转矩阵具有以下几个重要特性：

{B}对{A}的旋转矩阵等于{A}对{B}的旋转矩阵的转置

前面提到：

而求投影使用的点乘具有可交换顺序的特性，于是让每一项的前后向量互换：

也就是：

旋转矩阵是一个正交矩阵 Orthogonal Matrix

旋转矩阵是正交矩阵，即其转置等于其逆。这意味着，其中是单位矩阵。

Always invertible:
Columns:

每个列向量的模长均为 1
每个列向量之间都是互相正交的

因此，这个虽然有9个数字，但却隐含了6个隐藏条件，其实只有3个数字是完全互相独立不受干扰的，也就是说，只有3个自由度，这与三维空间中转动具有3个DOF相符

的行列式为 1

旋转矩阵的行列式恒等于 1，这保证了旋转变换不会改变空间的体积或方向。

1.5.3 旋转矩阵的作用

描述一个坐标系相对于另一个坐标系的姿态

将一点在某坐标系的表达转换到在另一个坐标系中表达

将一向量在同一个坐标系中进行转动

1.5.4 旋转矩阵与欧拉角的转换

空间中的转动具有3个自由度，那要如何把一般旋转矩阵所表达的姿态拆解成3次旋转角度，以对应到3个自由度？

两个拆解方式

顺归（外旋）
本归（内旋）

多种旋转顺序

【例】ZYX 本归 Euler Angles

我们讨论一个关于轴旋转度的情况（偏航角）：

其中右下角指旋转轴，后面括号里的是旋转角度。从 z 轴正方向向下俯视，逆时针旋转为正方向，即右手螺旋。

我们再讨论下关于轴旋转度的情况（俯仰角）：

我们可以用投影的思想，很简单的口算出每一列的向量值。

我们最后讨论关于轴旋转度的情况（横滚角）：

这一套下来，就分别完成了三次绕轴旋转。

接下来，我们来完成一次完整的ZYX本归旋转：先绕自身 z 轴转度，再绕自身 y 轴转度，最后绕自身 x 转度：

那么最后坐标系{B}相对于参考系{A}的旋转矩阵可以表达为：

其中，第一个右下角的 X’、Y’、Z’ 标明了旋转顺序，与括号内的旋转角度按次序一一对应。需要注意的是，在等式的右边相乘顺序与旋转顺序相同，是Z-Y-X的顺序，这是因为每一步都需要右乘的缘故。我们依次乘开：

它的本质是“1.5.3 旋转矩阵的作用 ”中提到的“将一点在某坐标系的表达转q换到在另一个坐标系中表达”，这也是其每一步旋转都右乘的依据。

【例】XYZ 顺归

我们先让它绕参考系 x 轴旋转度，再绕参考系 y 轴旋转度，最后绕参考系 z 轴旋转度：

那么最后坐标系{B}相对于参考系{A}的旋转矩阵可以表达为：

其中，第一个右下角的 XYZ 标明了旋转顺序，与括号内的旋转角度按次序一一对应。需要注意的是，在等式的右边相乘顺序与旋转顺序相反，是Z-Y-X的顺序，这是因为每一步都需要左乘的缘故。我们依次乘开：

它的本质是“1.5.3 旋转矩阵的作用 ”中提到的“将一向量在同一个坐标系中进行转动”，这也是其每一步旋转都左乘的依据。

最后，我们惊讶的发现 ZYX本归和XYZ顺归竟然得到了同样的！从几何上实际转动，也得到了同样的旋转结果：

于是我们从现象中归纳出了一个结论：对于同一种旋转拆解方式（如本归），同样的角度，不同的顺序，结果不同。而对于一对拆解方式（如本归对顺归），同样的角度，相反的顺序，能得到相同的旋转结果。从数学上，也能得到解释：本归对应变换坐标系基底，应依次右乘得到；顺归对应变化坐标系中向量，应为依次左乘得到。一个ZYX，一个XYZ，本来相反的旋转顺序被相反的计算顺序给“正”过来了。

1.5.5 旋转矩阵的优缺点

优点

多次连续旋转，直接把相乘就行

缺点

参数冗余。用了9个参数来描述3个自由度，白白增大了计算量
和欧拉角一样，同样存在万向锁问题：为90度时，你会发现分母变成 0了。

1.6 使用四元数描述刚体转动 Quaternion

直接架在虚数基础上，从数学原理上解决了为90度时分母变成0的问题。

1.6.1 四元数&三维旋转

正如复数是对二维空间的扩展，四元数则是对三维空间的扩展。百年前的四元数如今正在图像处理、Robitics、量子力学等领域大放异彩。

核心：复数与旋转变换的关系：

——它能表示了 2D 空间内的旋转。

四元数？所有的四元数 ( H 代表四元数的发现者 William Rowan Hamilton）都可以写成下面这种形式：

其中

为实部，为三个虚部。显然，对于一个四元数来说有四个自由度：，用来描述 3D 旋转甚至还多了一个自由度（根据前文欧拉角的讨论，最少仅用3个自由度即可确定三维空间中的刚体姿态）。那么用四个参数表示四个自由度来描述三维空间中的姿态是否造成计算资源浪费？

首先，上面用九个参数表示三个自由度的旋转矩阵明显浪费的多；

其次，仅用三个自由度描述的欧拉角，存在万向锁问题。

很自然的可以联想到，使用轴角法（1.4 使用轴角法描述刚体转动 Angle-axis ）来描述姿态的方案，恰好利用四个参数表示四个自由度（轴的方向三个，旋转角度一个）来描述三维空间中的刚体姿态。该方法不仅能唯一确定姿态，还不存在万向锁的问题，那我们自然可以用四元数描绘轴角法。

与复数类似，因为四元数就是对基的线性组合，四元数也可以写成向量的形式：

除此之外，我们经常将四元数的实部与虚部分开，并用一个三维的向量来表示虚部，将它表示为标量和向量的有序对形式:

梦回大一学线代，太有意思了。

四元数更多的性质，参加下面这份 pdf。

四元数与三维旋转.pdf

442.2KB

换句话说，如果我们有, 那么可以将沿着旋转度。所有旋转四元数的实部都只是一个角度的余弦值，假设有一个单位四元数 , 那么可以直接得到旋转的角度：

想要获得旋转轴？把虚部的每一项都除以即可。

在实际的应用中，我们可能会需要将旋转与平移和缩放进行复合，所以需要用到四元数旋转的矩阵形式．它可以很容易地从上面这个公式推导出来。参见上附的pdf：左乘一个四元数等同于下面这个矩阵：

而右乘等同于这个矩阵

所以，我们可以用这两个公式将写成矩阵形式，假设我们就能得到

因为 , 能进一步化简为

矩阵最外圈并不会对进行任何变换，我们可以将他压缩成3*3矩阵

虽然3D旋转的矩阵形式可能不如四元数简单，而且占用更多的空间，但是对于大批量的变换，使用预计算好的矩阵是比四元数乘法更有效率的。

2.怎么解算姿态？

2.1 姿态数据采集与误差的校准

我们从GY86处采集得来加速度计、陀螺仪、磁力计和气压计的数据。基于我们去年的工作，我们已能通过 IIC 通信协议实时获取其寄存器中的值。之后，我们要如何处理这些传感器的数据呢？

将寄存器的值转换为实际具有物理意义的值

很简单，查一下手册。MPU6050的测量值均为16位有符号数，拿去除一下量程即可。

想办法降低各种误差

上电一看，静止在地面上Z轴加速度竟然为, 三个方向的角加速度竟然都为，很明显，这个误差也太大了点，需要我们修正。

通过数学，由此求出其他物理量，得到姿态

由测得的加速度，角速度导出速度，位移，及表示姿态的四元数。

2.1.1 姿态传感器误差模型

GY-86包含了MPU6050这款姿态传感器，它含有加速度计以及陀螺仪，分别能测量加速度以及角速度。下面是姿态传感器的几种误差：

零偏误差 Zero-g offset

通常是指室温25°C时，某一轴与重力方向垂直放置时该轴输出值相对于0的偏差。注意zero-g offset也会随着温度的变化而变化，对于温度范围比较大的应用的话，也需要关注zero-g offset over temperature参数。当在实验室测量传感器 zero-g offset时，可以采用转台(rate table或者叫gyro table)或者校准过的大理石平台(marble platform)来保证高精度的水平状态。
产生zero-g offset的原因主要有两点：一是由于MEMS sensor敏感的微机械结构老化导致，二是由于传感器供电电压变化导致电路产生的electrical offset drift导致。

高斯白噪声 Noise

MEMS加速度计噪声定义为当传感器输入量固定时，输出量波动的量化值。其产生的根本原因是由于传感器内部分子布朗运动和电路放大器产生的噪声所致。噪声属于非确定性信号，MEMS传感器的噪声通常表现为高斯白噪声，用noise功率谱密度来定量衡量噪声水平，且在整个频域范围内功率谱密度为常数。加速度计datasheet上噪声参数为Spectral noise density，单位μg/√Hz(有时也使用RMS noise表示，即通过对目标带宽内噪声功率谱密度积分后开根号得到。spectral noise density和RMS noise可以互相转换)；简单计算也可用峰峰值和RMS值来表示。下图是一个加速度计静止时采集400ms的数据，可以看到其数据统计服从正态分布。

轴偏误差 Cross axis error

理想情况下加速度计的三个轴是理想正交的，两两相差是90度。由于实际生产时MEMS三轴间结构往往会存在误差不满足绝对正交，所以交叉轴灵敏度（cross axis sensitivity）指沿一个轴方向加速度信息耦合到其他轴的程度。比如加速度计水平放置，Z轴输出1g，但X轴输出10mg（理论是0g），那么X与Z轴的交叉灵敏度就是10ms/1g=1%

刻度误差 Scale error

刻度误差：指传感器在一定激励输入下，其输出值与输入值的比值。比如我们将飞控放在一个以 500°/s 恒定角速度运动的转台上，而陀螺仪实际测量得到的角速度输出为 495°/s，500 / 495 ≈ 1.01 便是该陀螺仪的刻度误差。

除此之外，还存在姿态传感器所在位置与飞机重心不重合的问题，也需要转换。

在这里，建模为线性误差模型。对于某一个物理值：

其中代表校准后的值，代表测量值，代表轴偏误差，代表刻度误差，则包含了零偏误差和随机噪声。我们带入到IMU实际的情况，那就变成了矩阵：

我们仅讨论比较简单的情况，仅考虑刻度误差（）与零偏误差（），不考虑轴偏误差（）。这样一来，对于加速度，角速度和磁力计，就各有六个参数（三轴刻度误差和三轴零偏误差）:

2.1.1 姿态传感器误差标定&最小二乘法

问题来了。如何确定这六个参数，使得校准后的测量值尽可能接近真实值呢？我将飞机正置于水平地面，测量出z轴加速度为1.23g，三个方向的角速度均为21度/s，这很明显存在着不同程度的刻度误差和零偏误差。

如何求得这六个参数？幸运的是，我们能建立一些约束方程，来求解这六个参数。

加速度计的误差标定

有一个纯天然的方程，那就是合加速度测量值应等于重力加速度：

即

很显然，这个方程组拥有六个变量，如果能取得六组，那这六个参数也就解出来了。但是问题并没有这么简单！这六组加速度测量值均带有一定程度的噪声。比如，我们在取第一组加速度测量值时，将飞机水平静止，但测量值却在小幅度变换，你该取哪一时刻的值作为此姿态的观测值？取哪一个都不准确，如果使用任意六组观测值直接求解，解出的参数会被噪声严重影响，结果可能偏离实际。

为了克服直接求解的局限性，我们希望找到一组能使测量值整体误差最小的优化解，而不是依赖某几组观测值的精确性。我们只能通过最小化“损失函数”，求得最优解。我们建立如下的优化目标

这个优化目标的构建细节值得注意：

最小化误差的平方和：

使用平方和可以避免正负误差相互抵消，使得目标函数的值真实反映误差的大小。
平方函数对大误差更敏感，可以减少异常观测值的影响。

优化求解的连续性：

目标函数构建为平方和形式，使其在参数空间中通常是连续可导的。
这为采用数值优化方法（如梯度下降或非线性最小二乘法）提供了便利。

带入测得的N组测量值，使用某种方法，取得一组参数使得目标函数最接近0，那么可以认为这组参数为当下的最优解。

目标函数（或误差标定中的平方和形式）和人工智能尤其是机器学习中的损失函数在本质上非常相似。两者的目标都是通过优化某种数学形式来找到一组最优参数，使得系统的预测值或校准值尽可能接近真实值（ground truth）。我们构建的这个误差标定损失函数和机器学习中的MSE（均方误差）十分类似：

在人工智能中，损失函数衡量模型的预测值与真实值的差异。如果损失函数值较小，说明模型的预测值更接近真实值。

在标定问题中，目标函数衡量校准后的加速度值是否满足物理规律。如果目标函数值较小，说明六个标定参数更准确。

在人工智能中，我们通过优化算法（如梯度下降、Adam 等）来最小化损失函数，从而找到最优的模型参数（如神经网络的权重和偏置）。优化过程通常是迭代的，目标是找到使损失函数最小的参数。

在加速度计标定中，目标是优化六个标定参数，使得目标函数最小。这个过程同样可以通过优化算法（如非线性最小二乘法或梯度下降）实现。

因此，标定问题中的目标函数在形式和优化思路上，与人工智能中的损失函数极为相似。

为什么不使用其他损失函数？

绝对值损失函数（如 Mean Absolute Error, MAE）是另一种常用形式：

它对异常值（outliers）不敏感，但不连续可导。虽然可以减少异常值的影响，但优化过程可能会变得困难，且结果的平滑性较差，因此在数值优化中不如平方和形式方便。

通过以上分析，可以更深入理解标定问题和人工智能中的损失函数的相似性：

标定问题	机器学习
目标函数：最小化加速度测量与真实值的偏差	损失函数：最小化模型预测与真实标签的偏差
参数：比例因子、零偏	参数：神经网络权重和偏置
优化算法：梯度下降、最小二乘法	优化算法：梯度下降、Adam 等
数据：多组加速度测量值	数据：训练集中的输入与标签

那么现在，我们就来探索一下优化方法吧！之前，我们用类似于MSE均方误差的方法定义了目标优化函数，这种类似于MSE均方误差的定义方法被称为最小二乘法。

最小二乘法是一种数学方法，用于解决参数估计问题，它的目标是最小化预测值和真实值之间的误差平方和。目标函数为：

其中：

：模型的预测值，依赖于参数。

：观测值（真实值）。

：目标函数（损失函数），即误差的平方和。

最小二乘法的基本思想是通过调整参数，使得误差平方和最小。根据模型预测函数又可以分为线性和非线性最小二乘：

线性最小二乘

线性最小二乘法用于解决模型是参数的线性函数时的拟合问题。目标函数可以表示为：

其中：

：真实观测值。
：输入特征向量。
：待求的参数向量。
：模型预测值，是参数 θ 的线性组合。

线性最小二乘问题的目标函数是一个二次凸函数，可以通过直接解析解得到最优解。将误差平方和展开，目标转化为：

其中：

是观测值向量。
是设计矩阵，包含所有输入特征向量的堆叠形式。

最优解为：

推导过程：

将目标函数写为矩阵形式：

对 θ 求梯度并令其为 0：

解得：

特点也是比较多的：

解析解：线性最小二乘问题具有闭式解，可以直接通过矩阵运算求解。
高效：当数据规模适中时，求解过程计算开销较低。
唯一解：由于目标函数是凸的，解是唯一的。
局限性：模型必须是线性的，不能处理复杂的非线性关系。

非线性最小二乘

非线性最小二乘法用于解决模型是参数的非线性函数时的拟合问题。目标函数可以表示为：

其中：

：模型预测值，是一个关于参数的非线性函数。

例如，指数模型就是非线性的。非线性最小二乘问题无法直接通过解析解求解，需要使用数值优化方法。常见的求解方法包括：

高斯-牛顿方法

通过对残差函数做一阶泰勒展开，近似目标函数为二次形式。

Levenberg-Marquardt 方法

是高斯牛顿的改进版，加入正则化项，避免雅可比矩阵病态或不可逆的问题。

梯度下降方法

经典的通用方法，直接利用目标函数的梯度，逐步更新参数，梯度下降方法较为通用，但收敛速度可能较慢：

特点和线性最小二乘颇有不同：

无解析解：目标函数是非线性的，必须通过迭代优化方法逼近最优解。
初值敏感性：迭代优化依赖于初值选择，初值不当可能导致收敛到局部最优解。
计算复杂度高：每次迭代需要计算梯度或雅可比矩阵，计算开销大。
不一定有全局最优解：非线性目标函数可能是非凸的，优化可能停留在局部最优点。

编号	任务点
挑战一	刚体运动及姿态描述（欧拉角、轴角法）
挑战二	四元数与欧拉角的转换
挑战三	四轴飞行器运动机制与模型
挑战四	姿态数据采集及处理（加速度计与高斯牛顿迭代）
挑战五	陀螺仪和地磁数据采集及处理
挑战六	梯度下降及数据处理
挑战七	PID反馈控制与数值模拟
挑战八	姿态解算-I （AHRS：MADGWICK）
挑战九	数据补偿、扩展卡尔曼滤波（EKF）
挑战十	PID控制器设计、动能分配与系统调试
挑战十一	姿态解算-II（MAHONY……)
挑战十二	DARTS实时软件设计方法及研讨
挑战十三	系统重构与优化