一种四旋翼飞行器保持机体平衡的方法
【技术领域】
[0001]本发明涉及控制领域，特别是涉及一种四旋翼飞行器保持机体平衡的方法。
【背景技术】
[0002]四旋翼飞行器是一种新型的可以垂直起降的小型无人飞行器，它具有呈十字对称交叉结构形式的四个螺旋桨，并且具有非线性、强耦合性、时变性的动力学特征以及系统本身的不确定性等特点。它通过改变四个对称分布的旋翼转速来调整机体的姿态与位置，具有可悬停、机动性好、结构简单等优点。四旋翼飞行器能垂直起降、自主悬停，使用灵活方便，特别适合于获取点状或带状目标的航拍影像，越来越多地被应用于山区输电线路的日常检测，因而这类四旋翼飞行器成为了当前的研究热点。
[0003]四旋翼飞行器在空中飞行的稳定性和可靠性是由姿态角解算的速度和精度来决定的。而现有的姿态解算主要有卡尔曼滤波算法和互补滤波算法两种。而飞行器在空中飞行主要是通过陀螺仪、加速度计和电子罗盘测量得到数据，因为这几种器件受到技术的限制或多或少都存在一定缺陷，不能再整个频率范围内都表现出良好的动态响应特性，使得最终影响测试效果，而最终解算出的结果就会影响到飞行器在空中平衡性，使得飞行器不能平稳可靠的飞行。
【发明内容】
[0004]本发明的目的是在现有算法的基础上提出一种新的算法，包括姿态解算算法和姿态控制算法，解决飞行器在空中不能平稳可靠飞行的问题。
[0005]为实现上述目的，本发明采用如下技术方案:
一种四旋翼飞行器保持机体平衡的方法，包括姿态解算算法和姿态控制算法，包括以下步骤:
姿态解算算法，
步骤一:首先定义姿态速度、计算周期、定义姿态解算误差的积分、初始化四元数，利用姿态解算函数分别对三个轴的角速度和加速度原始数据进行计算归一化，得到单位加速度；
步骤二:把四元数换算成方向余弦矩阵中的第三列的三个元素，根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，就是当前的机体坐标参照系上，换算出来的重力单位向量；
步骤三:在机体坐标参照系上，加速度计测出来的重力向量与陀螺积分后的姿态来推算出的重力向量的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差；误差向量仍旧是位于机体坐标系上，误差向量大小与陀螺积分误差成正比；
步骤四:将误差向量积分，用误差向量来修正陀螺零偏，通过调节两个速度参数，可以控制加速度计修正陀螺仪积分姿态的速度；
姿态控制， 采用两个串联起来的PID控制器构成串级PID控制系统，其中主调节器的输出作为副调节的给定值，主调节器的控制环称为外环，副调节器的控制环为内环，外环的控制周期为内环控制周期的两倍。
[0006]在上述技术方案中，计算周期为姿态解算时间的一半，姿态速度为500HZ。
[0007]在上述技术方案中，所述姿态控制中，姿态稳定时其姿态速度为零。
[0008]在上述技术方案中，所述串级PID控制系统采用角度外环和角速度内环组成的串级PID控制器。
[0009]本发明与现有技术相比，其优点在于:采用本发明的姿态解算和姿态控制使得飞行器在空中飞行明显比现有的控制方式更具有稳定性，飞行器在空中非常平稳，不会出现摇摆不定的情况，且控制效率明显高于现有的控制方式。
【附图说明】
[0010]无。
【具体实施方式】
[0011 ] 现在进一步对本发明的方案做出说明。
[0012]四旋翼飞行器保持飞机平衡的算法，主要包括姿态解算算法和姿态控制算法。首先是姿态解算，当飞机从传感器获得数据后，我们需要对数据进行处理和解算，这里使用四元数的方法进行姿态解算和串级PID的控制算法进行控制。
[0013]一，姿态解算算法
先定义控制加速度计修正陀螺仪积分姿态的速度和计算周期，计算周围是姿态廣算时间的一半。此处解算姿态速度为500HZ，因此计算周期为0.001。
[0014]初始化四元数，定义姿态解算误差的积分，参数gx，gy，gz分别对应三个轴的角速度，单位是弧度/秒；参数ax，ay, az分别对应三个轴的加速度原始数据；
由于加速度的噪声较大，此处应采用滤波后的数据，将加速度的原始数据，归一化，得到单位加速度。
[0015]把四元数换算成“方向余弦矩阵”中的第三列的三个元素。根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。所以这里的vx、vy、vz，其实就是当前的机体坐标参照系上，换算出来的重力单位向量。
[0016]这里说明一点，加速度计由于噪声比较大，而且在飞行过程中，受机体振动影响比陀螺仪明显，短时间内的可靠性不高。陀螺仪噪声小，但是由于积分是离散的，长时间的积分会出现漂移的情况，因此需要将用加速度计求得的姿态来矫正陀螺仪积分姿态的漂移。
[0017]在机体坐标参照系上，加速度计测出来的重力向量是ax、ay、az ;陀螺积分后的姿态来推算出的重力向量是vx、vy、vz;它们之间的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差。
[0018]向量间的误差，可以用向量积(也叫外积、叉乘)来表示，eX、ey、eZ就是两个重力向量的叉积。这个叉积向量仍丨日是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
[0019]用叉乘误差来做修正陀螺零偏，通过调节加速度计修正陀螺仪积分姿态的速度的两个参数，可以控制加速度计修正陀螺仪积分姿态的速度。
[0020]二，姿态控制算法:
姿态解算后出来三个角度，即横滚角(roll)、俯仰角(pitch)、偏航角(yaw)。横滚角(roll)是飞行器在YZ平面绕X轴转动产生的角度；俯仰角(pitch)是飞行器在XZ平面绕Y轴转动产生的角度；偏航角(yaw)是飞行器在XY平面绕Z轴转动产生的角度。
[0021]所谓的姿态控制，就是通过控制四轴飞行器的姿态，就可以达到让四轴飞行器前后、左右、顺时针、逆时针飞行的目的。
[0022]解算出的姿态需要通过控制电机的转动来让四轴飞行器稳定飞行。控制电机较为经典的是采用PID算法，但四轴飞行器是个需要稳定性很强的系统，单闭环PID控制器稳定性不强，会使四轴飞行器飞行效果不好，故本设计引入串级PID控制器。
[0023]串级PID控制系统由两个串联起来的PID控制器构成，其中主调节器的输出作为副调节的给定值，主调节器的控制环称为外环，副调节器的控制环称为内环。外环的控制周期一般为内环控制周期的两倍。由于串级PID引入了两个PID控制器，能使两个控制器都起作用，因此串级PID控制器能够改善过程的动态特性，提高系统的控制质量，对于进入副回路的扰动能够迅速克服。
[0024]为了使四轴飞行器稳定飞行，应该使飞行器姿态稳定的时候，姿态速度也应为零，故采用角度和角速度串级PID控制器。由于姿态速度响应的速度比姿态响应的速度快，四轴飞行器飞行的时候引起的姿态速度变化也比姿态速度变化大，故姿态速度控制作为内环，姿态控制作为外环，也就是角度外环和角速度内环组成的串级PID控制器。
[0025]本说明书中公开的所有特征，除了互相排斥的特征以外，均可以以任何方式组合。以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
【主权项】
1.一种四旋翼飞行器保持机体平衡的方法，包括姿态解算算法和姿态控制算法，其特征在于包括以下步骤: 姿态解算算法， 步骤一:首先定义姿态速度、计算周期、定义姿态解算误差的积分、初始化四元数，利用姿态解算函数分别对三个轴的角速度和加速度原始数据进行计算归一化，得到单位加速度； 步骤二:把四元数换算成方向余弦矩阵中的第三列的三个元素，根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，就是当前的机体坐标参照系上，换算出来的重力单位向量； 步骤三:在机体坐标参照系上，加速度计测出来的重力向量与陀螺积分后的姿态来推算出的重力向量的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差；误差向量仍旧是位于机体坐标系上，误差向量大小与陀螺积分误差成正比； 步骤四:将误差向量积分，用误差向量来修正陀螺零偏，通过调节加速度计修正陀螺仪积分姿态的速度参数，可以控制加速度计修正陀螺仪积分姿态的速度； 姿态控制， 采用两个串联起来的PID控制器构成串级PID控制系统，其中主调节器的输出作为副调节的给定值，主调节器的控制环称为外环，副调节器的控制环为内环，外环的控制周期为内环控制周期的两倍。2.根据权利要求1所述的一种四旋翼飞行器保持机体平衡的方法，其特征在于计算周期为姿态解算时间的一半，姿态速度为500HZ。3.根据权利要求1所述的一种四旋翼飞行器保持机体平衡的方法，其特征在于所述姿态控制中，姿态稳定时其姿态速度为零。4.根据权利要求1所述的一种四旋翼飞行器保持机体平衡的方法，其特征在于所述串级PID控制系统采用角度外环和角速度内环组成的串级PID控制器。
【专利摘要】本发明公开了一种四旋翼飞行器保持机体平衡的方法，包括姿态解算算法和姿态控制算法，通过定义姿态速度、计算周期、定义姿态解算误差的积分、初始化四元数，利用姿态解算函数求解；采用两个串联起来的PID控制器构成串级PID控制系统实现姿态控制；采用本发明的姿态解算和姿态控制使得飞行器在空中飞行明显比现有的控制方式更具有稳定性，飞行器在空中非常平稳，不会出现摇摆不定的情况，且控制效率明显高于现有的控制方式。
【IPC分类】G05B13/02
【公开号】CN105259750
【申请号】CN201510705125
【发明人】刘刚强
【申请人】四川豪斯特电子技术有限责任公司
【公开日】2016年1月20日
【申请日】2015年10月27日