姿态篇：一.初识姿态估计

[深入浅出多旋翼飞控开发][姿态篇][一][初识姿态估计]

作者:Sky

一.什么是姿态

在学习姿态估计之前，我们先来了解一下，什么是 “姿态”。

想象一架飞机准备起飞，于是它在机场跑道上进行一段加速助跑，达到一定速度后，机头抬升15度，腾空而起，离开了地面。然而由于起飞时的方向和目标方向相差甚大，于是飞机调转机头，最终往北偏东30度的方向飞去。

这个过程中，根据日常生活经验，我们如此说明飞机在起飞过程中的发生的变化：“抬升15度”，“北偏东30度”。也就是说，通常我们会使用 “角度”，来表示一个物体的姿态。那这个角度大小是如何得来的？其中，“15度”指飞机机身与水平地面的夹角，“北偏东30度”则指飞行方向与正北方向的夹角。**也就是说，日常生活中我们描述物体的姿态，是以脚下的大地作为参考的。**通常人类在太空中会失去方向感，是因为地球上的重力加速度的存在让我们始终能够以大地作为参考物，而失重环境下失去了参考物，我们便无法感知自身以及周围物体的姿态了。

为了方便描述物体之间的方位关系，我们定义了坐标系。在上面的例子中，我们可以定义成两个坐标系，如图1：

• 大地坐标系O-xyz
• 机体坐标系P-vuw

坐标系定义

于是，我们将姿态定义为：

姿态，即表示O-xyz与P-vuw两个坐标系之间的关系。

通常，坐标系会有多个，但必须选择一个坐标系作为参考系，大部分时候我们会选取大地坐标系作为参考系，比较符合人类的直觉与习惯。

了解了姿态的定义后，我们又面临着一个问题，如何使用数学方式来描述姿态，以便下一步的计算？

二.欧拉角

欧拉角是最直观的一种姿态描述方式，其定义为：

一个坐标系到另一个坐标系的变换，可以通过绕不同坐标轴的3次连续转动来实现。这三次的转动角度统一称之为欧拉角

但有一点值得注意的是，欧拉角并不直接等同于我们常说的姿态角。

欧拉角由三次绕轴旋转组成，而这三次转动顺序任意，且转动轴可以是参考系的，也可以是机体系的，因此共有24种转动方式。

而通常飞控上所使用的姿态角，指的是航空领域主要应用的航空次序欧拉角，也叫卡尔丹角（Tait-Bryan angles），定义欧拉角的转动顺序为Z-Y-X。其中绕Z轴转动为偏航角（Yaw），绕Y轴转动为俯仰角（Pitch），绕X轴转动为横滚角（Roll），其具体意义如下：

• 偏航角（Yaw）
  机体系x轴投影到水平面与参考系x轴的夹角，顺时针旋转为正。

• 俯仰角（Pitch）
  机体系x轴与水平面的夹角，抬头为正。

• 横滚角（Roll）
  机体坐标系z轴与通过机体系x轴的铅垂面间的夹角，机体右旋为正。

图2表示了姿态欧拉角的转动方向
Z-Y-X次序欧拉角

上面提到了，欧拉角意义上并不直接等于姿态角，它只是描述了三次绕轴转动。而我们使用的姿态角：偏航角、横滚角与俯仰角实际上是机体坐标系与参考坐标系（大地坐标系）之间关系的表述。大致清楚姿态角的概念后，接下来便了解一下，如何使用传感器测量姿态角。

三.姿态的测量

1.加速度计测量俯仰与横滚角

假设飞控处于静止状态，此时加速度计仅受到重力的作用。当飞控与水平面完全平行时，此时加速度计的输出理论值为$[0,0,g]$（g为重力加速度的大小）。而当飞控以一定角度倾斜于水平面时，其输出值可能为$[0.37g,-0.5g,0.78g]$，且其模值为1g。实际上，静止状态下飞控倾斜时，加速度计所测量到的数据，便是重力加速度在机体坐标系下的投影。

于是根据加速度计的测量值与三角函数关系，我们便能计算出飞控的姿态角。不过由于偏航角定义为机体系x轴投影到水平面与参考系x轴的夹角，而重力加速度正好完全正交于水平面，因此加速度计测量值中并没有包含偏航角信息。
加速度与姿态角之间的三角函数关

姿态角与重力加速度投影之间的三角函数关系，这里就不具体解释了，直接给出结论，在本文的下一节中我们再从数学的角度来推导出姿态角公式。

设当前有归一化的加速度向量$a=[a_x,a_y,a_z]$，飞机横滚角为$angl{e}_x$，俯仰角为$angl{e}_y$，有如下关系：
$$\{\begin{array}{l}angl{e}_x=-arcsin(a_y)\\ angl{e}_y=arctan2(a_x,a_z)\end{array}$$

在天穹飞控的vector3.c文件中定义了AccVectorToRollPitchAngle函数来实现该转换：

void AccVectorToRollPitchAngle(Vector3f_t* angle, Vector3f_t vector)
{
   
	angle->x = -asinf(vector.y);            //横滚角
	angle->y = atan2f(vector.x, vector.z);  //俯仰角
}

2.磁力计测量偏航角

上面我们利用加速度计测得飞机的横滚与俯仰姿态角，但还有一个偏航角是未知的。偏航角有许多方式可以获得，但是最简单和最低成本的方式，还是通过磁力计进行测量。

假设当前有磁力计测量得到的磁场强度向量$m=[m_x,m_y,m_z]$，然后计算得到其在大地坐标系下的投影$m_n=[m_{nx},m_{ny},m_{nz}]$（具体的转换方式请阅下一节）。

设飞机的偏航角为$angl{e}_z$，有：
$$angl{e}_z=-arctan2(m_{ny},m_{nx})$$

该公式的推导请参阅下节。同样，在天穹飞控中定义了MagVectorToYawAngle函数来实现该转换：

void MagVectorToYawAngle(Vector3f_t* angle, Vector3f_t vector)
{
   
	angle->z = -atan2f(vector.y, vector.x);     //偏航角
}

3.关于陀螺仪

至此，我们已经了解完毕如何使用加速度计和磁力计来测量飞机的姿态角。但你可能发现，飞控中最重要的一个传感器：陀螺仪，还没有登场。既然使用上述两个传感器就已经能得到飞机的姿态，那还需要陀螺仪做什么呢？

实际上，使用加速度计和磁力计计算姿态的前提是，飞机处于一个理想的静止状态下。然而现实中这是不可能的，那么当飞机处于飞行状态时，加速度计测量得到的数据，便不仅仅是重力加速度了，可能还包含了飞行过程中所产生的运动加速度，以及一些有害加速度，通常由于机身震动，还会引入额外的震动噪声（也属于线性加速度）。此时直接使用加速度数据计算姿态，最终得到的是带有了巨大误差以及噪声的值，对于飞控来说是完全不可用的。同样，磁力计的实际测量噪声一般也比较大，而且在许多环境下还会存在磁场干扰，从而导致直接使用磁力计计算出来的偏航角存在大量误差而不可用。

而陀螺仪是一种可以测量旋转角速度的传感器，对角速度值进行离散化数值积分，便得到某段时间内的姿态变化量，这就是我们使用陀螺仪来计算飞机姿态的方式。但需要注意，使用陀螺仪只能计算出飞机姿态的相对变化量，而无法直接测量绝对姿态值，因此使用陀螺仪来更新飞机姿态时，必须先得知初始姿态值。

相比加速度计和磁力计，陀螺仪几乎不受外界环境干扰，且对线性加速度（震动）不敏感，这些特性使得陀螺仪成为了飞行器中最核心的传感器，甚至在某些极端情况下（比如火箭的起飞加速阶段），只有陀螺仪可用，这时陀螺仪的精度便至关重要了。实际上，尽管陀螺仪拥有如此优秀的特性，却也不是完美的，自身会存在各种误差，而在角速度积分阶段，陀螺仪误差会被累积，从而使得姿态逐渐偏离真实值。

经验告诉我们，使用单一传感器来计算获取飞机的姿态是不现实的，所以实际应用中，需要同时使用多种传感器来计算姿态。上面我们已经了解了使用加速度计和磁力计来计算姿态角的方法，下面便接着学习如何使用陀螺仪来更新姿态。在这之前，必须要先介绍姿态的另一种描述方式：方向余弦矩阵。

四.方向余弦矩阵

1.方向余弦矩阵的定义

第二节中我们定义姿态欧拉角的转动顺序为Z-Y-X。

根据天穹飞控中坐标系和旋转方向的定义，将姿态角的单次转动转化为矩阵的形式，有：

• 绕Z轴转动（偏航角$z$）的旋转矩阵

$$C_z=\left[\begin{array}{cccc}cosz& sinz& 0& \\ -sinz& cosz& 0& \\ 0& 0& 1& \end{array}\right]$$

• 绕Y轴转动（俯仰角$y$）的旋转矩阵

$$C_y=\left[\begin{array}{cccc}cosy& 0& siny& \\ 0& 1& 0& \\ -siny& 0& cosy& \end{array}\right]$$

• 绕X轴转动（横滚角$x$）的旋转矩阵
  $$C_x=\left[\begin{array}{cccc}1& 0& 0& \\ 0& cosx& -sinx& \\ 0& sinx& cosx& \end{array}\right]$$

将上面的3个旋转矩阵以Z-Y-X的转动顺序进行连乘，我们便得到了一个可以表示此次欧拉转动的旋转矩阵$C$，计算过程如下：

$$C=C_z\ast C_y\ast C_x$$
$$=\left[\begin{array}{cccc}cosz& sinz& 0& \\ -sinz& cosz& 0& \\ 0& 0& 1& \end{array}\right]\left[\begin{array}{cccc}cosy& 0& siny& \\ 0& 1& 0& \\ -siny& 0& cosy& \end{array}\right][\begin{array}{ccc}& & \\ 1& 0& 0\\ 0& cosx& \\ 0& sinx& \end{array}$$