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

摘要

本发明公开了一种四旋翼飞行器保持机体平衡的方法，包括姿态解算算法和姿态控制算法，通过定义姿态速度、计算周期、定义姿态解算误差的积分、初始化四元数，利用姿态解算函数求解；采用两个串联起来的PID控制器构成串级PID控制系统实现姿态控制；采用本发明的姿态解算和姿态控制使得飞行器在空中飞行明显比现有的控制方式更具有稳定性，飞行器在空中非常平稳，不会出现摇摆不定的情况，且控制效率明显高于现有的控制方式。

主权项

一种四旋翼飞行器保持机体平衡的方法，包括姿态解算算法和姿态控制算法，其特征在于包括以下步骤：姿态解算算法，步骤一：首先定义姿态速度、计算周期、定义姿态解算误差的积分、初始化四元数，利用姿态解算函数分别对三个轴的角速度和加速度原始数据进行计算归一化，得到单位加速度；步骤二：把四元数换算成方向余弦矩阵中的第三列的三个元素，根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，就是当前的机体坐标参照系上，换算出来的重力单位向量；步骤三：在机体坐标参照系上，加速度计测出来的重力向量与陀螺积分后的姿态来推算出的重力向量的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差；误差向量仍旧是位于机体坐标系上，误差向量大小与陀螺积分误差成正比；步骤四：将误差向量积分，用误差向量来修正陀螺零偏，通过调节加速度计修正陀螺仪积分姿态的速度参数，可以控制加速度计修正陀螺仪积分姿态的速度；姿态控制，采用两个串联起来的PID控制器构成串级PID控制系统，其中主调节器的输出作为副调节的给定值，主调节器的控制环称为外环，副调节器的控制环为内环，外环的控制周期为内环控制周期的两倍。