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

说明书

技术领域

本发明涉及控制领域，特别是涉及一种四旋翼飞行器保持机体平衡的方法。

背景技术

四旋翼飞行器是一种新型的可以垂直起降的小型无人飞行器，它具有呈十字对称交叉结构形式的四个螺旋桨，并且具有非线性、强耦合性、时变性的动力学特征以及系统本身的不确定性等特点。它通过改变四个对称分布的旋翼转速来调整机体的姿态与位置，具有可悬停、机动性好、结构简单等优点。四旋翼飞行器能垂直起降、自主悬停,使用灵活方便,特别适合于获取点状或带状目标的航拍影像,越来越多地被应用于山区输电线路的日常检测，因而这类四旋翼飞行器成为了当前的研究热点。

四旋翼飞行器在空中飞行的稳定性和可靠性是由姿态角解算的速度和精度来决定的。而现有的姿态解算主要有卡尔曼滤波算法和互补滤波算法两种。而飞行器在空中飞行主要是通过陀螺仪、加速度计和电子罗盘测量得到数据，因为这几种器件受到技术的限制或多或少都存在一定缺陷，不能再整个频率范围内都表现出良好的动态响应特性，使得最终影响测试效果，而最终解算出的结果就会影响到飞行器在空中平衡性，使得飞行器不能平稳可靠的飞行。

发明内容

本发明的目的是在现有算法的基础上提出一种新的算法，包括姿态解算算法和姿态控制算法，解决飞行器在空中不能平稳可靠飞行的问题。

为实现上述目的，本发明采用如下技术方案：

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

姿态解算算法，

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

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

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

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

姿态控制，

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

在上述技术方案中，计算周期为姿态解算时间的一半，姿态速度为500HZ。

在上述技术方案中，所述姿态控制中，姿态稳定时其姿态速度为零。

在上述技术方案中，所述串级PID控制系统采用角度外环和角速度内环组成的串级PID控制器。

本发明与现有技术相比，其优点在于：采用本发明的姿态解算和姿态控制使得飞行器在空中飞行明显比现有的控制方式更具有稳定性，飞行器在空中非常平稳，不会出现摇摆不定的情况，且控制效率明显高于现有的控制方式。

附图说明

无。

具体实施方式

现在进一步对本发明的方案做出说明。

四旋翼飞行器保持飞机平衡的算法，主要包括姿态解算算法和姿态控制算法。首先是姿态解算，当飞机从传感器获得数据后，我们需要对数据进行处理和解算，这里使用四元数的方法进行姿态解算和串级PID的控制算法进行控制。

一，姿态解算算法

先定义控制加速度计修正陀螺仪积分姿态的速度和计算周期，计算周围是姿态解算时间的一半。此处解算姿态速度为500HZ，因此计算周期为0.001。

初始化四元数，定义姿态解算误差的积分，参数gx，gy，gz分别对应三个轴的角速度，单位是弧度/秒;参数ax，ay，az分别对应三个轴的加速度原始数据；

由于加速度的噪声较大，此处应采用滤波后的数据，将加速度的原始数据，归一化，得到单位加速度。

把四元数换算成“方向余弦矩阵”中的第三列的三个元素。根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。所以这里的vx、vy、vz，其实就是当前的机体坐标参照系上，换算出来的重力单位向量。