[发明专利]一种四旋翼飞行器保持机体平衡的方法

权利要求书

1.一种四旋翼飞行器保持机体平衡的方法，包括姿态解算算法和姿态控制算法，其特征在于包括以下步骤：

姿态解算算法，

步骤一：首先定义姿态速度、计算周期、定义姿态解算误差的积分、初始化四元数，利用姿态解算函数分别对三个轴的角速度和加速度原始数据进行计算归一化，得到单位加速度；

步骤二：把四元数换算成方向余弦矩阵中的第三列的三个元素，根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，就是当前的机体坐标参照系上，换算出来的重力单位向量；

步骤三：在机体坐标参照系上，加速度计测出来的重力向量与陀螺积分后的姿态来推算出的重力向量的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差；误差向量仍旧是位于机体坐标系上，误差向量大小与陀螺积分误差成正比；

步骤四：将误差向量积分，用误差向量来修正陀螺零偏，通过调节加速度计修正陀螺仪积分姿态的速度参数，可以控制加速度计修正陀螺仪积分姿态的速度；

姿态控制，

采用两个串联起来的PID控制器构成串级PID控制系统，其中主调节器的输出作为副调节的给定值，主调节器的控制环称为外环，副调节器的控制环为内环，外环的控制周期为内环控制周期的两倍。

2.根据权利要求1所述的一种四旋翼飞行器保持机体平衡的方法，其特征在于计算周期为姿态解算时间的一半，姿态速度为500HZ。

3.根据权利要求1所述的一种四旋翼飞行器保持机体平衡的方法，其特征在于所述姿态控制中，姿态稳定时其姿态速度为零。

4.根据权利要求1所述的一种四旋翼飞行器保持机体平衡的方法，其特征在于所述串级PID控制系统采用角度外环和角速度内环组成的串级PID控制器。