四旋翼姿态解算原理

姿态结算原理

旋转矩阵的建立

如果要进行四旋翼姿态分析，那么需要进行座标系与座标系之间的转换，地理座标系与四旋翼座标系之间的联系如图2-6所示。四旋翼(Quad-rotor)分别绕三个轴转动的角度变化如图2-7 所示。

图2-6 机体座标与地面座标的关系

                                                

图2-7 滚转角、俯仰角、偏航角示意图

根据图2-6、图2-7中的关系，建立如下矩阵：

                 

                  

如向量P 依次绕Z、Y、X旋转，那么旋转后的向量与之前的向量的关系为：

                     

其中，就是地理与机体座标之间的旋转关系，如下： 

    

四元数计算法

四元数在四旋翼姿态角计算中用的比较多，利用四元数计算姿态角较简单，同时，利用四元数解算姿态角可以避免万向节死锁。四元数的表示方法如下：

              

    其中，theta/2是旋转的角度，n是四元数表示的旋转轴的方向，q₀是实部，其余的是不同方向的虚部。且表示的三个方向都相互垂直。在四元数中，可以用两个向量的乘积表示一个四元数向量绕另一个向量旋转。

设四元数A和B，如果要表示A以B为转轴旋转后的结果，则可以表示为四元数A和B的乘积。表示方法如下：

    设一个四元数Q没有实部,且是单位四元数。则有|Q|=1，。如果四元数R(x,y,z)以Q为转轴旋转，那么旋转后的向量可表示为，,也就是。旋转后的向量的计算过程如下：

       

    设四元数中的旋转矩阵是R_q ，那么R_q就可以表示为：

  又由

则有

 

                

可得

   由公式（2-16）、（2-17）和（2-18）可得姿态角通过四元数进行计算的公式。对四元数关于时间微分，可以得到四元数与四旋翼角速度相关的更新公式，四元数更新后，进行姿态角计算。在这里本文不进行推导直接给出计算公式如下：

      

.3 互补误差融合

由于陀螺仪长时时间工作存在较大的漂移，但是陀螺仪短时间内测得的数据比较准确，而加速度计长时间内测得的数据比较准确，所以利用四旋翼加速度与四旋翼的重力加速度的差作为陀螺仪测得的角速度的补偿量,进行姿态误差补偿。设四旋翼座标系上的重力加速度为，加速度计测得的加速度为，计算过程如下：

 

则可得：

             

融合一定的PI参数，将融合后的误差对角速度进行补偿，如下：