欧拉角的求取与互补滤波器在姿态解算中的应用

公式（5） 当然也有形如以下的表达方式：
公式（6） 当 dt 趋近于 0 时，由公式（6）可以得到旋转矩阵的导数： 其中：
k 上式中的������������ ，������������ ，������������ 分别为 IMU（惯性测量单元）的陀螺仪测得的载体坐标系下 的机体的 x,y,z 轴的角速度。比较短的时间内间隔读取陀螺仪的数据，通过上述公式就可以 实现方向余弦矩阵随时间的传递，然而，MEMS（MEMS, Micro-Electro-Mechanical System， 微机电系统）陀螺仪会发生温度漂移，同时积分去处有累积误差（积分运算的误差从哪里体 现出来？） 。如果不加以补偿，在短时间内就会积累很大的姿态误差，导致四轴飞行器控制 失效。所以必须采用一定的姿态解算算法，融合各种传感器的数据，进行误差补偿以实现精 确的姿态解算。 （3）基于互补滤波器的姿态求解器设计 陀螺仪动态响应好，但计算姿态时，会产生累积误差（积分体现在哪里，通过积分得到 欧拉角？） 。数字罗盘（也叫做电子罗盘或是三轴磁感传场感器） ，和加速度传感器测量姿态 没有累积误差，但动态响应较差。因些，它们在频域上特性互补，可以采用互补滤波器材融 合这三种传感器的数据， 提高测量精度和系统的动态性能。 也就是即不会产生积分上的累积 误差导致偏差太大， 也不会因为动态特性太差而不会丢失了实时的特性， 无法做到实时的控 制。 R— — 真实姿态方向余弦矩阵 ������ — — 表示互补滤波器计算输出的姿态方向余弦矩阵 ������0 — — 代表由加速度传感器和数字罗盘观测到的姿态方向余弦矩阵 ������������ — — 表示������0 的高频观测噪声，则������0 = R + ������������ （动态特性太差，就是高频噪声被保留了下来，导致曲线的不平滑，从信号与系统的分析角 度上来看，动态变化太大，也就是信号变化速度快，这就是信号与系统中的高频分量过多） ������������ — — 代表陀螺仪数据计算得到的姿态 ������������ — — 表示������������ 的低频累积误差，则������������ = R + ������������ （低频太差，也就是信号中的低频分量太多） 由上面的分析可以知道，要得到好的，精确度高的 R 信号，要滤除加速度计和数字罗盘 中的高频分量，还有就是陀螺仪上的低频分量。 取������������
欧拉角的求取与互补滤波器在姿态解算中的应用 李天助 （1）根据欧拉定理：刚体绕固定点的位移也可以是绕该点若干次有限转动的合成。 （2）在欧拉转动中，将参考坐标系（绝对坐标系，实验室参考系，是静止不动的，地理坐 标系，也是就是正北正东和垂直地面，由于相对很小的四轴飞行器而言，地球可以看成是一 个无限大的平面。 其原点是飞行器的旋转中心， 也就是说飞行器每次进行欧拉转动时三个旋 转轴的交点就是其原点？如果是的话，那么这两个原点就是重合的了。 ）转动三次得到载体 坐标系（也称为机坐标系，其原点是飞行器载体的中心） 。
������ 2������
。
在大于������������ 的高频段内，陀螺仪对解算结果起主要作用；在小于������������ 低频段，数字罗盘和加速计 对解算的结果起主要作用。此时对公式（8）进行 Laplace 反变换可得其在时域中的表达式：
公式（9） 为了消除静态偏差，取
kP 的大小决定互补滤波器的截止频率，kI 的大小决定了消除静态误差的时间，通常 kI 的大 小为 kP 的 0.01~0.1 倍。 加速度传感器测量载体坐标系下重力加速度矢量 g 的方向， 以校正横滚和俯仰的姿态角； 数字罗盘测量在参考坐标系 （也就是绝对坐标系） 下的航向角， 以校正飞行器的偏航姿态角。 通过分析可以知道， 通过分解重力加速度矢量到那个载体坐标系的三个轴上， 是完全可以确 定翻滚和俯仰的， 但是偏航是没有办法确定的， 这是因为偏航实质上就是绕着绝对坐标系的 z 轴在旋转，这样的话，不管偏航如何，那个重力加速度矢量到机体坐标系上的分量大小是 不会有任何的改变的。所以，引入电子罗盘的关键作用就是在于其能通过感知地磁场，来确 认绝对坐标系，通过与载机坐标系的比较，自然可以轻松的知道飞行器的偏航角。 （现在感觉以上所讨论的种种其实都是为了从传感器上获取精确的欧拉角的值， 而方向余弦 变换矩阵， 主要的目的似乎研究的是载体坐标系与绝对坐标系之间的变换关系， 而其中含有
公式（1） 上面的三个旋转矩阵分别是绕 x、y、z 轴旋转得到的，其具体的推算过程是通过投影和 来实现的。 （具体推导过程可以参见周建龙等人编写的《几何与代数》中 P118 页例 3.19） 。 显然，我们知道矩阵的乘法是不满足交换律的， 所以轴旋转的顺序不同， 比如 x->y->z， 与 x->z->y 是完全不一样的。 通过以上的分析，就可以知道姿态矩阵还有三次转动的顺序有关。为了避免万向节锁 （Gimbal Lock） ，选择合适的转动顺序是十分关键的。具体如何实现可以参考路洋学长的个 人网站——巧匠的四轴飞行器教程部分（/919/） ，其中有一个视 频谈论到了此问题。 若按 z->y->x 顺序来转动的话，其最终的姿态矩阵显然就是：
图参考坐标系与载体坐标系的关系
图如何通过三次绕载体坐标系三个轴来进行参考坐标系到载体坐标系的转换 （此图中的旋转过程好似与绕分立的三个轴旋转的过程有点不一样） 欧拉角确定的姿态矩阵是三次坐标转换矩阵的乘积。 （将其分解为三次绕以载体坐标轴 的三个轴为旋转轴的旋转， 总旋转的合成就是三次分立旋转矩阵的乘积。 通过阅读以下的分 析也就可以知道这是合理的，因为这是一个分步的过程，所以使用相乘。 ） 这些坐标转换矩阵都有如下标准形式：
其中，向量 g 就是加速度传感器测量的重力加速度矢量。 飞行器的航向是定义在参考坐标系的 x-y 平面内的， 所以偏航误差������������ 近似为数字罗盘测 量的航向矢量与互补滤波器估计的航向矢量，投影到参考坐标系 x-y 平面形成的 2 个单位矢 量的叉乘 c，再转换到载体坐标系下的向量：
公式（3） （3）方向余弦矩阵随时间的传递 在时间 t 内，当 t 为极小值的时候，姿态角的变化量dφ、dϕ、dθ皆趋近于 0，将这三 个值代入公式（2）中，并通过sinθ ≈ θ，cosθ ≈ 1，可以将旋转矩阵简化：
公式（4） 其实这个矩阵不是公式（3）真正退化的结果，但其实本质上是一样的。如果二阶无穷 小量再进一步简化就可以得到：
公式（7） 互补滤波器的优点就在于它同时滤除了低频和高频的干扰，方便实现传感器数据融合。 由
可以得到
（这一过程是怎么来的，我也搞不清楚，应该是信号与系统很相关。 ） 将此式代入公式（7）当中，就可以得到
ห้องสมุดไป่ตู้
公式（8） 根据这一式子，很容易通过直接模拟框图，得到互补滤波器的模型：
C（s）如果取固定常数 k，则设计的低通滤波器和高能滤波器的截止频率������������ =
的关键量就是欧拉角， 欧拉角其实就包含在了方向余弦变换矩阵当中， 而从方向余弦矩阵中 求解得到欧拉角也是可以完成的。 ） 由于加速度传感器和数字罗盘的响应时间是不相同的 kP 和 kI，最终设计的互补滤波器 的姿态求解器的结构如图：
飞行器的偏航不会影响重力加速度矢量的方向， 而且横滚角和俯仰角也不会影响飞行器 的航向，所以，两路校正之间是解耦的，不会相互干扰，分为两路是合理的。 （说的就是姿 态求解器中的那两路。 ） 四旋翼飞行器大部分时间处于悬停、 匀速飞行阶段， 此时加速度传感器只会机体高频振 荡的干扰，而且在起飞、降落和加减速阶段，其加速度相对重力加速度较小，而且持续时间 很短，最终会被互补滤波器滤除（毛刺可能会产生的地方） 。因此可以将加速度传感器的数 据作为机体重力加速度的测量结果。互补滤波器（通过加速度计和数字罗盘，配合陀螺仪计 ������ 算出来的旋转姿态矩阵）估计的重力加速度矢量为旋转矩阵������������ 的第 3 行，其与加速度传感 器测量的重力加速度矢量的偏差，即为俯仰和横滚角误差，其值为两个矢量的叉乘。 （这话 说的是毛线意思？）
������ =
������ (������ ) ������ +������ (������ )
,������������ ������ = 1 − ������������ ������ =
������
������ +������ (������ )
,如果 C(s)具有全通特性（你怎
么知道它有全通特性呢？） 。������������ ������ 具有低通滤波特性，������������ ������ 具有高通滤波特性。 则两者进行互补就可以得到
其中，������������ 为陀螺仪测量的角速度矢量。将ω代入公式（6） ，就可以得到随时间传送的准 确的旋转矩阵，进而由公式（3）求得各个姿态角。如此一来，姿态解算就算是结束了。通 过以上分析， 发现些姿态解算方法并没有使用到四元数， 而只用到了方向余弦矩阵与欧拉角 之间的转换关系。比较的简单易懂。其过程就是大概可以概括为： 1. 通过方向余弦可以进行参考坐标系与机体坐标系之间的变换，而方向余弦矩阵中又可以 求解出欧拉角，而欧拉角就是控制函数的输入变量。 2. 通过互补滤波器，感觉上就是对陀螺仪进行滤波，因为从方向余弦旋转矩阵角度而言， 如果说陀螺仪测得的数据是准确的话，那其实问题就解决了，但是事实是残酷的，所以 互补滤波器，就是通过数字罗盘和加速度计来补偿陀螺仪测量是所会产生的累积误差。 而通过互补滤波器之后，把得到的，新的，经过修正过的陀螺仪的值（此时的值是准确 可信的）送到方向余弦转换矩阵中去求得欧拉角。这样问题就得以解决了。 （顺便再说一个，通过设计的互补滤波器来等到融合的式子，这一个过程感觉是超过所 学的知识范畴了。但可以知道，要想得到互补之后的陀螺仪的结果，就必须经过这一个 过程。 ） 接下来就可以讨论一下 PID 的值的问题，如果说上面的互补滤波器得到的是传感器测量 并计算得到的一个可靠的实时的飞行器的姿态的话，那么就可以通过遥控器来给出一个 目标的姿态。其实这个目标的姿态就会决定飞行器的飞行轨迹了。而如何更加快速地到 达目标姿态，这个就是在自动控制领域中经常使用到的 PID 控制方式了。达到响应快的 目的。而在智能车的控制过程中，PID 的作用就是使智能车通过快速地启动，灵敏的过 弯以及减速。 罗鸿飞学长：陀螺仪部分相当于一个积分器加上高通滤波器，加速度计就是个低通滤波 器，然后两个合起来。 但是加权的意思是你一共只有两个输入，一个是根据陀螺仪和上次状态估算出的值，一 个加速度计的值，你肯定相信它们的比重不一样，如果完全相信加速度计它的权重就是 1，陀螺仪的权重就是 0，反之也一样。实际上两个东西的权重由它们信号的质量决定， 所以两个都有权重。