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

1、欧拉角的求取与互补滤波器在姿态解算中的应用李天助（1）根据欧拉定理：刚体绕固定点的位移也可以是绕该点若干次有限转动的合成。（2）在欧拉转动中，将参考坐标系（绝对坐标系，实验室参考系，是静止不动的，地理坐 标系，也是就是正北正东和垂直地面，由于相对很小的四轴飞行器而言，地球可以看成是一 个无限大的平面。其原点是飞行器的旋转中心，也就是说飞行器每次进行欧拉转动时三个旋 转轴的交点就是其原点？如果是的话，那么这两个原点就是重合的了。）转动三次得到载体 坐标系（也称为机坐标系，其原点是飞行器载体的中心）。境7抽行转任到Q角图参考坐标系与载体坐标系的关系境7抽行转任到Q角图参考坐标系与载体坐标系的关系图

2、如何通过三次绕载体坐标系三个轴来进行参考坐标系到载体坐标系的转换（此图中的旋转过程好似与绕分立的三个轴旋转的过程有点不一样）欧拉角确定的姿态矩阵是三次坐标转换矩阵的乘枳。（将其分解为三次绕以载体坐标轴 的三个轴为旋转轴的旋转，总旋转的合成就是三次分立旋转矩阵的乘积。通过阅读以下的分 析也就可以知道这是合理的，因为这是一个分步的过程，所以使用相乘。）这些坐标转换矩阵都有如下标准形式：一1 00cos。0 -sin。COSW SUll/ 0R=0 cos。 sin 笋)=010sin/ cos 00 sin 仞 cos。 sin。 0 cos Q 001 公式（1）上面的三个旋转矩阵分别是绕x、y

3、、z轴旋转得到的，其具体的推算过程是通过投影和 来实现的。（具体推导过程可以参见周建龙等人编写的几何与代数中P118页例3.19）。显然，我们知道矩阵的乘法是不满足交换律的，所以轴旋转的顺序不同，比如x-y-z, 与x-z-y是完全不一样的。通过以上的分析，就可以知道姿态矩阵还有三次转动的顺序有关。为了避免万向节锁 （Gimbal Lock）,选择合适的转动顺序是十分关键的。具体如何实现可以参考路洋学长的个 人网站 巧匠的四轴飞行器教程部分（/919/）,其中有一个视 频谈论到了此问题。-s6s(pc8c(-s6s(pc8c(pc&A = R_(/)R、(夕)冬(甲)=5(psOcifr -c

4、q)si/f c(ps6cyr + s-w式中，字符”，唔”分别为kos”和“sin”的缩写形式。(r r r 、 xxxyxsin0 + sin Osin 8cos0sin 4sin 0 + cos(t)sm 8cos i/jJ ryv feg ffshupCQ6siii Osin欧心J J)、sin 0sin 巾 cow Gcos 4cos Q/公式（2）其它不同的转动顺序自然是可以自己推导出来的。这个姿态矩阵其实也可以认为是参考坐标系到载体坐标系的一个转换过程。其中的三个角可以认为是偏航角，横滚角，俯仰角，但是这三个角还是相对于载体坐标 系而言的，但好像也不是欧拉角，而是方向余弦角？既然

5、是这样的话，那干吗还要说成是 Roll,Pitch,Yaw角呢。通过与欧拉角的定义进行比较一下。通过与欧拉角定义的比较，可以肯 定这三个角就是欧拉角。其辨别的过程还是很简单的。飞行器的姿态可以通过这三个角来加 以描述。如果能得到这三个角，可以认为这是在载体坐标系下的欧拉角，但其实欧拉角应该没有 载体坐标系与参考坐标系之分，所以这得出来的就是欧拉角。由于IMU与飞行器机体固连 （焊在PCB上了），各传感器测得的数据都在载体坐标系中（不管是陀螺仪还是加速度计）， 在解算飞行器姿态时，需要先将载体坐标系内测得的数据转换到导航坐标系（也就是地理坐 标系，或是绝对坐标系）中。就从人的观看角度而言，飞行器

6、的姿态，本来就应该相对于地 面而言，也就是绝对坐标系。在得到随时间变化的方向余弦矩阵后，由公式（2）可以得到三个姿态角：W =tan巾=tanW =tan巾=tan8=昂广1（ - j）, -成号公式（3）（3）方向余弦矩阵随时间的传递在时间t内，当t为极小值的时候，姿态角的变化量d（p、de、d。皆趋近于o,将这三 个值代入公式（2）中，并通过sin。知。，cx）s6 1,可以将旋转矩阵简化：-7。幻 + 1的+ 90 /)(1-7。幻 + 1的+ 90 /，= 邛+ 丁 / -8 + 叮公式（4）其实这个矩阵不是公式（3）真正退化的结果，但其实本质上是一样的。如果二阶无穷 小量再进一步简化

7、就可以得到：公式（5）d破d破d 、1一 d。dA 1 )(1R；Q+dQ =明(，) 顿1cu.df=明1、一3皿 30公式（6）当dt趋近于0时，由公式（6）可以得到旋转矩阵的导数: 其中：上式中的3x，外，/分别为IMU （惯性测量单元）的陀螺仪测得的载体坐标系下 的机体的x,y,z轴的角速度。比较短的时间内间隔读取陀螺仪的数据，通过上述公式就可以 实现方向余弦矩阵随时间的传递，然而，MEMS （MEMS, Micro-Electro-Mechanical System, 微机电系统）陀螺仪会发生温度漂移，同时积分去处有累积误差（枳分运算的误差从哪里体 现出来？）。如果不加以补偿，在短时

8、间内就会积累很大的姿态误差，导致四轴飞行器控制 失效。所以必须采用一定的姿态解算算法，融合各种传感器的数据，进行误差补偿以实现精 确的姿态解算。（3）基于互补滤波器的姿态求解器设计陀螺仪动态响应好，但计算姿态时，会产生累积误差（枳分体现在哪里，通过积分得到 欧拉角？）。数字罗盘（也叫做电子罗盘或是三轴磁感传场感器），和加速度传感器测量姿态 没有累枳误差，但动态响应较差。因些，它们在频域上特性互补，可以采用互补滤波器材融 合这三种传感器的数据，提高测量精度和系统的动态性能。也就是即不会产生积分上的累积 误差导致偏差太大，也不会因为动态特性太差而不会丢失了实时的特性，无法做到实时的控 制。R真实姿

9、态方向余弦矩阵R表示互补滤波器计算输出的姿态方向余弦矩阵Ro代表由加速度传感器和数字罗盘观测到的姿态方向余弦矩阵Uh表刁Ro的（ j频观测噪声，则R（）= R + Uh（动态特性太差，就是高频噪声被保留了下来，导致曲线的不平滑，从信号与系统的分析角 度上来看，动态变化太大，也就是信号变化速度快，这就是信号与系统中的高频分量过多） Rc代表陀螺仪数据计算得到的姿态电一一表示K的低频累积误差，则K = R +电（低频太差，也就是信号中的低频分量太多）由上面的分析可以知道，要得到好的，精确度高的R信号，要滤除加速度计和数字罗盘 中的高频分量，还有就是陀螺仪上的低频分量。取Gl（s）= 芸,Gh（s）

10、 = 1 - Gl（s）= 芸s如果C（s）具有全通特性（你怎 么知道它有全通特性呢？）。Gl（s）具有低通滤波特性，Gh（s）具有高通滤波特性。则两者进行互补就可以得到=Rs） +Gl（s）Uh（$） +Ch（s）临（s）RE公式（7） 互补滤波器的优点就在于它同时滤除了低频和高频的干扰，方便实现传感器数据融合。 由可以得到sRe(s) =R($)/J(s)(这一过程是怎么来的，我也搞不清楚，应该是信号与系统很相关。) 将此式代入公式(7)当中，就可以得到商S)=c(s)(虬($)顼)+()邛公式(8)根据这一式子，很容易通过直接模拟框图，得到互补滤波器的模型：圈1互补滤波器模型Fig 1

11、Model of complementary filterc (s)如果取固定常数k,则设计的低通滤波器和高能滤波器的截止频率fr冷 在大于行的高频段内，陀螺仪对解算结果起主要作用；在小于方低频段，数字罗盘和加速计 对解算的结果起主要作用。此时对公式(8)进行Laplace反变换可得其在时域中的表达式：.公式(9)为了消除静态偏差，取kP的大小决定互补滤波器的截止频率，kl的大小决定了消除静态误差的时间，通常kl的大 小为 kP M 0.01M0.1 倍。加速度传感器测量载体坐标系下重力加速度矢量g的方向，以校正横滚和俯仰的姿态角; 数字罗盘测量在参考坐标系(也就是绝对坐标系)下的航向角，以校

12、正飞行器的偏航姿态角。 通过分析可以知道，通过分解重力加速度矢量到那个载体坐标系的三个轴上，是完全可以确 定翻滚和俯仰的，但是偏航是没有办法确定的，这是因为偏航实质上就是绕着绝对坐标系的 z轴在旋转，这样的话，不管偏航如何，那个重力加速度矢量到机体坐标系上的分量大小是 不会有任何的改变的。所以，引入电子罗盘的关键作用就是在于其能通过感知地磁场，来确 认绝对坐标系，通过与载机坐标系的比较，自然可以轻松的知道飞行器的偏航角。(现在感觉以上所讨论的种种其实都是为了从传感器上获取精确的欧拉角的值，而方向余弦 变换矩阵，主要的目的似乎研究的是载体坐标系与绝对坐标系之间的变换关系，而其中含有的关键量就是欧

13、拉角，欧拉角其实就包含在了方向余弦变换矩阵当中，而从方向余弦矩阵中 求解得到欧拉角也是可以完成的。）由于加速度传感器和数字罗盘的响应时间是不相同的kP和kl,最终设计的互补滤波器 的姿态求解器的结构如图：图2姿态求解器结构图Fig 2 Structure diagram of attitude solver飞行器的偏航不会影响重力加速度矢量的方向，而旦横滚角和俯仰角也不会影响飞行器 的航向，所以，两路校正之间是解耦的，不会相互干扰，分为两路是合理的。（说的就是姿 态求解器中的那两路。）四旋翼飞行器大部分时间处于悬停、匀速飞行阶段，此时加速度传感器只会机体高频振 荡的干扰，而且在起飞、降落和加减

14、速阶段，其加速度相对重力加速度较小，而且持续时间 很短，最终会被互补滤波器滤除（毛刺可能会产生的地方）。因此可以将加速度传感器的数 据作为机体重力加速度的测量结果。互补滤波器（通过加速度计和数字罗盘，配合陀螺仪计 算出来的旋转姿态矩阵）估计的重力加速度矢量为旋转矩阵R#的第3行，其与加速度传感 器测量的重力加速度矢量的偏差，即为俯仰和横滚角误差，其值为两个矢量的叉乘。（这话 说的是毛线意思？）其中，向量g就是加速度传感器测量的重力加速度矢量。飞行器的航向是定义在参考坐标系的x-y平面内的，所以偏航误差财近似为数字罗盘测 量的航向矢量与互补滤波器估计的航向矢量，投影到参考坐标系x-y平面形成的2

15、个单位矢 量的叉乘c,再转换到载体坐标系下的向量：其中,巾为数字罗盘测量的航向角。% =R cfrK=! 2（jsin 七cos） J ，rzr /则校正之后的角速度矢量为c=pi膈+扁&% +k耘 +如也电+.其中，的为陀螺仪测量的角速度矢量。将3代入公式（6）,就可以得到随时间传送的准 确的旋转矩阵，进而由公式（3）求得各个姿态角。如此一来，姿态解算就算是结束了。通 过以上分析，发现些姿态解算方法并没有使用到四元数，而只用到了方向余弦矩阵与欧拉角 之间的转换关系。比较的简单易懂。其过程就是大概可以概括为：通过方向余弦可以进行参考坐标系与机体坐标系之间的变换，而方向余弦矩阵中又可以 求解出欧

16、拉角，而欧拉角就是控制函数的输入变量。通过互补滤波器，感觉上就是对陀螺仪进行滤波，因为从方向余弦旋转矩阵角度而言， 如果说陀螺仪测得的数据是准确的话，那其实问题就解决了，但是事实是残酷的，所以 互补滤波器，就是通过数字罗盘和加速度计来补偿陀螺仪测量是所会产生的累积误差。 而通过互补滤波器之后，把得到的，新的，经过修正过的陀螺仪的值（此时的值是准确 可信的）送到方向余弦转换矩阵中去求得欧拉角。这样问题就得以解决了。（顺便再说一个，通过设计的互补滤波器来等到融合的式子，这一个过程感觉是超过所 学的知识范畴了。但可以知道，要想得到互补之后的陀螺仪的结果，就必须经过这一个 过程。）接下来就可以讨论一下PID的值的问题，如果说上面的互补滤波器得到的是传感器测量 并计算得到的一个可靠的实时的飞行器的姿态的话，那么就可以通过遥控器来给出一个 目标的姿态。其实这个目标的姿态就会决定飞行器的飞行轨迹了。而如何更加快速地到 达目标姿态，这个就是在自动控制领域中经常使用到的PID控制方式了。达到响应快的 目的。而在智能车的控制过程中，PID的作用就是使智能车通过快速地启动，灵敏的过 弯以及减速。罗鸿飞学长：陀螺