四旋翼无人飞行器的飞行控制方法与流程

本发明涉及无人飞行器技术领域，具体涉及到四旋翼无人飞行器的飞行控制方法。

背景技术：

四旋翼无人飞行器(Quadrotor Unmanned Aerial Vehicle,QUAV)是一种通过无线远程遥控设备加之自身传感器实现自主飞行的不载人飞行器，其有6个自由度，4个控制输入，由4个无刷直流电机驱动产生的差动力矩实现其俯仰运动和翻滚运动，产生的反扭力矩实现偏航运动,是非线性欠驱动系统。这类飞行器广泛应用于军事和民用领域。四旋翼无人飞行器相比固定翼无人飞行器，由于能垂直起降，起飞和着陆的要求较低，灵活性高，在复杂的地势下有更强的适应性。目前使用最多的控制方法是PID控制(Proportional-integral-derivative Control)，其通过识别目标，然后探测现状与目标的差距，再用行动消除它。PID控制结构简单，控制技术成熟，鲁棒性较好。但是，在四旋翼无人飞行器飞行过程当中，控制器当中的参数难以自动地调整来适应外界变化，因而很难达到预定的目标，影响控制效果。

技术实现要素：

本发明旨在解决现有技术中存在的技术问题。

为解决PID的控制不能根据飞行器外部环境变化而自适应调制的问题，因而需要对PID的控制参数进行自适应调整来实现飞行器自动调整，本发明是基于最优-最差蚂蚁系统(Best-worst Ant System,BWAS)算法来优化PID控制参数。

本发明提出一种四旋翼飞行器的飞行控制方法，是基于最优-最差蚂蚁系统(Best-worst Ant System,BWAS)算法来优化PID控制参数的飞行控制方法，控制四旋翼无人机的飞行姿态过程，能适应外界环境变化，实现控制参数自适应调整，保持很好的抗干扰能力和鲁棒性，提高四旋翼无人飞行器的飞行品质。

所述四旋翼无人飞行器的飞行控制方法，主要由四旋翼无人飞行器的主控制器模块来执行，具体包括以下步骤：

S10：建立四旋翼飞行器的动力学模型，QUAV的动力学方程为；

其中，θ、ψ分别为四旋翼无人飞行器的翻滚角、俯仰角和偏航角，l为其质心至旋翼中心的距离，I_x、I_y、I_z为惯性主距，Ω_i为第i个旋翼转速，Fi为第i个旋翼产生的升力，旋翼产生的升力与旋翼传动速度平方成正比，I_R为转动惯量，n₁为其升力系数，n₂为反扭矩系数；

为了把四旋翼飞行器的动力学方程转换成四个独立的控制通道,定义四旋翼飞行器的控制输入为

S20：设计基于最优-最差蚂蚁系统(BWAS)算法来优化PID控制参数，再由步骤S10中的四旋翼无人飞行器的动力学方程转换成四个独立的控制通道，四个所述控制通道分别为高度BWAS-PID、翻滚BWAS-PID、俯仰BWAS-PID、偏航BWAS-PID；

其中采用最优-最差蚂蚁系统(BWAS)算法来优化PID控制参数包括以下过程

S21:PID控制

采用增量式数字PID控制，其表达式如式(3)：

式中K_p为比例系数，e(k)为本次偏差，Δu(k)对应的控制量为u(k)，T为采样周期，T_i为积分时间常数，T_d为微分时间常数，PID控制需要确定的参数分别为K_p、T_i和T_d；

S22:PID控制参数优化

采用式(4)表示评价控制系统的性能指标：

式中LP为仿真计算点数，DT为仿真计算步骤；则式子(4)连续形式表示为式(5)：

设蚂蚁总数为m，对于每一个蚂蚁，此时刻的点为i，其对应函数值为η_i，下一个可达的点j，对应函数值为η_j，则

蚂蚁会朝着信息素最多的方向移动，当没有信息素时，便会按照原来的运动方向移动，因此蚂蚁在t时刻处于i点时朝着j点移动的概率如式(7)：

式中，allowed为蚂蚁从地点i直接到达下一个路径点的集合，τ为信息素，τ_ij是路径i到路径j的信息素含量，α为信息素的相对重要程度，若α＝0，最靠近i的地点j将有被选出；β为距离信息的相对重要程度，β＝0，蚂蚁只受到信息素的影响而忽略了启发式信息带来的偏向性；如果蚂蚁移动方向有障碍物时，则随机选择其他的方向，如果有信息素指引时，则按照其指引行动，寻优时蚂蚁的移动概率根据式(7)来确定，Δη_ij(t)＜0表示蚂蚁在自身所在地点i的邻域搜索，感知并且行动；Δη_ij(t)＞0表示蚂蚁按照移动概率从其自身所在地点i的邻域移动至j的邻域；

经过n个时间单位之后，局部信息素更新策略按式(8)、(9)演变：

τ_ij(t+n)＝(1-ρ)τ_ij(t)+ρΔτ_ij (8)

其中ρ∈(0,1)是信息素挥发参数，Δτ_ij为本次循环中在路径(i,j)上的信息素增量，为蚂蚁k在本次循环中在路径(i,j)上留下的信息素；

最优-最差蚁群算法对最优解进行了更大限度的增强，对于最差解进行了削弱，使得属于最优路径和最差路径之间的信息素量差异进一步增大，这样可以使得蚂蚁的搜索更为集中在当前循环为止所找出的最优路径的范围内；

当一次迭代结束之后，对于最差蚂蚁所经过的路径进行信息素更新，即为全局信息素更新如式(10)：

其中ε是引入的参数，L_worst和L_best分别为当前循环中最差蚂蚁和最优蚂蚁的路径长度，τ(i,j)为在路径(i,j)上的信息素轨迹量；

路径的构建：

位于节点i的蚂蚁k在每次选择下一个路径前会产生一个随机数q，那么从节点i到节点j的移动规则p′按式(11)：

q是随机变量，均匀分布在区间[0,1]中，q₀∈[0,1]；

通过以上BWAS算法可以找到PID控制参数最优解。

本发明的四旋翼飞行器控制方法，根据式(8)、(9)、(10)蚂蚁来持续更新信息素，然后蚂蚁根据不同的信息素来选择不同的路径，当路径到达最远，且信息素含量最多的那条路径就是PID控制参数的最优解路径；其中最优-最差蚁群算法对最优解进行了更大限度的增强，对于最差解进行了削弱，使得属于最优路径和最差路径之间的信息素量差异进一步增大，这样可以使得蚂蚁的搜索更为集中在当前循环为止所找出的最优路径的范围内，因而能有效地利用最优路径，也就可以更快速准确的获得PID控制参数最优解，再由步骤S10中的四旋翼无人飞行器的动力学方程转换成四个独立的控制通道，进而控制四旋翼无人飞行器的四个直流电电机的转速，解决了PID控制中其控制参数不能自适应调整的缺陷，使得无人机在飞行时可以更好地适应外界环境的变化，提高了无人飞行器控制系统的抗扰性和鲁棒性，达到了理想的飞行控制效果。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1为本发明的四旋翼无人飞行器整体结构示意图；

图2为本发明的四旋翼无人飞行器主要模块构成示意图；

图3为本发明的四旋翼无人飞行器机体坐标系和惯性坐标系图；

图4为本发明的四旋翼无人飞行器控制系统结构示意图；

图5为本发明的四旋翼无人飞行器控制方法中的基于BWAS优化PID控制系统结构示意图；

图6为本发明的四旋翼无人飞行器控制方法中的基于BWAS-PID控制下与基于PID控制、AS-PID控制下的偏航角跟踪对比图；

图7为本发明的四旋翼无人飞行器控制方法中的基于BWAS-PID控制下与基于PID控制、AS-PID控制下的翻滚角抗扰性能对比图；

图8为基于PID控制下鲁棒性测试结果图；

图9为基于AS-PID控制下鲁棒性测试结果图；

图10为本发明的方法的基于BWAS-PID控制下鲁棒性测试结果图。

具体实施方式

为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。

下面参照图1-2对本发明实施例的四旋翼无人飞行器做进一步的描述。

如图1和图2所示，四旋翼无人飞行器100包括机体10和固定在机体10上的主控制器模块20，还包括固定在机体10四个悬臂上的四个无刷电机控制模块60及由无刷电机驱动的旋翼70，另外，如图2所示，四旋翼无人飞行器还包括安装在所述机体10上并分别与所述主控制器模块20连接的传感器模块40、导航模块50和水冷-风冷模块80，还包括与主控制器20通信连接的无线通信模块30；导航模块50采用高精度的GPS卫星导航系统对四旋翼无人飞行器100进行跟踪定位，并向所述主控制器模块20提供位置信息并导航，且在导航过程中修改和固化波特率，另外还可以保存波特率的设置过程；所述传感器模块40包括分别与所述主控制模块20连接的惯性测量单元、气压传感器、电子罗盘、烟雾传感器和风速传感器，所述惯性测量单元用于检测飞行器的三轴线加速度、翻滚角速率、俯仰角速率、偏航角速率和航向信息，所述气压传感器用于检测飞行器的高度，所述电子罗盘测量飞行器的航向信息，所述风速传感器对飞行器所处的位置的风速进行监测，所述烟雾传感器设置在飞行器上的PCB电路板上用于探测所述电路板出现故障产生的烟雾并将烟雾信息反馈至所述主控制器模块20；所述无线通信模块30包括遥控器、PPM解码器和PPM接收机，所述PPM编码器与所述遥控器连接，遥控器的四个通道控制信号将通过PPM编码器编码后无线传递给所述PPM接收机，所述PPM接收机与所述主控制器模块连接；所述电机控制模块60包括用于驱动飞行器上四个旋翼70的四个电机以及分别控制所述四个电机工作的电子调速器，所述电子调速器与所述主控制器模块连接以接收电机控制信号，所述烟雾传感器布置在临近飞行器上的电路板附件以探测所述电路板出现故障产生的烟雾并将烟雾信息反馈至所述主控制器模块；所述水冷-风冷模块80可有效降低主控制器模块20和电机控制模块60工作时产生的热量。

导航模块50可以提供四旋翼无人机当前的位置信息，主控制器模块20是四旋翼无人机100控制系统的核心部分，其作用是负责采集传感器检测到的三轴线加速度、翻滚角速率、俯仰角速率和偏航角速率等组成的姿态角速率和航向信息并实时解算，再根据检测到由遥控器所发出的飞行信息，结合基于BWAS-PID控制下控制方法的控制方案，计算实际的输出电机控制信号至电子调速器，然后电子调速器根据获得的控制信号控制4个电机的转速，从而实现对4个旋翼产生的升力和转矩的控制，无刷电机可以通过PWM控制其转速从而达到对每个旋翼所产生的力和力矩的大小进行控制。

具体的，所述主控制器模块20和所述电机控制模块60上分别设置有测温元件(图中为示出)，所述测温元件与所述主控制器模块20连接以实现温度采集，所述主控制器模块20根据所采集到温度信息调节所述水冷-风冷模块80的冷却功率。既当测温元件检测到的主控制器模块20和所述电机控制模块60的温度偏高时需要提升水冷-风冷模块80的冷却功率以加速冷却降温，如果主控制器模块20和所述电机控制模块60的温度偏低时降低冷却功率，如此可以保证主控制器模块20和所述电机控制模块60工作在合适的温度范围内。

具体的，水冷-风冷模块先利用水冷系统降温，当水温升高之后达到与作用对象温度接近时，排出所有的水，此时将使用风冷系统进行降温。这样可有效减少主控制器模块20和电机控制模块60工作时产生的热量所引起温升。

本实施例的四旋翼无人飞行器，遥控器将控制信号通过PWM方式无线发送给PPM接收机，PPM编码器将PPM收机所接收到的控制信号解码后输至主控制器模块20，同时，构成四旋翼无人飞行器实时姿态信息的高度、翻滚、俯仰、偏航由等由传感器模块测得并传输至主控制器模块20，结合基于BWAS-PID控制下的控制方法的控制方案，主控制器模块综合计算实时姿态信息和控制信号信息后输出电机控制信号至电子调速器，然后电子调速器根据获得的电机控制信号控制4个电机的转速，从而实现对4个旋翼产生的升力和转矩的控制，无刷电机可以通过PWM控制其转速从而达到对每个旋翼所产生的力和力矩的大小进行控制，从而实现自动适应外界环境变化，达到理想的控制效果。

所述四旋翼无人飞行器的主控制模块综合计算实时姿态信息和控制信号信息后输出电机控制信号以控制无人飞行器的方法包括以下步骤：

S10：根据如图3所示四旋翼飞行器的机体坐标系与惯性坐标系，建立四旋翼飞行器的动力学模型，QUAV的动力学方程为；

其中，设θ、ψ分别为四旋翼无人飞行器的翻滚角、俯仰角和偏航角，l为其质心至旋翼中心的距离，Ix、Iy、Iz为惯性主距，Ωi为第i个旋翼转速，Fi为第i个旋翼产生的升力，旋翼产生的升力与旋翼传动速度平方成正比，IR为转动惯量，n₁为其升力系数，n₂为反扭矩系数；

为了把四旋翼飞行器的动力学方程转换成四个独立的控制通道,定义四旋翼飞行器的控制输入为

S20：如图4所示，设计基于最优-最差蚂蚁系统算法来优化PID控制参数，再由步骤S10中的四旋翼无人飞行器的动力学方程转换成四个独立的控制通道，四个所述控制通道分别为高度BWAS-PID、翻滚BWAS-PID、俯仰BWAS-PID、偏航BWAS-PID；

每个控制通道的BWAS优化模式如图5所示，其中r(K)指的是通过遥控器的预跟踪控制目标信号的输入，即想要实现的QUAV的飞行参数，例如理想的高度值、航向角、俯仰角或翻滚角等，y(k)指的是实际信号的输出，e(k)是指输出信号与输入信号的偏差，u(k)是指BWAS优化PID控制的控制量，其中采用最优-最差蚂蚁系统(BWAS)算法来优化PID控制参数包括以下过程

S21:PID控制

采用增量式数字PID控制，其表达式如式(3)：

式中K_p为比例系数，e(k)为本次偏差，Δu(k)对应的控制量为u(k)，T为采样周期，T_i为积分时间常数，T_d为微分时间常数，PID控制需要确定的参数分别为K_p、T_i和T_d；

S22:PID控制参数优化

采用式(4)表示评价控制系统的性能指标：

式中LP为仿真计算点数，DT为仿真计算步骤；则式子(4)连续形式表示为式(5)：

设蚂蚁总数为m，对于每一个蚂蚁，此时刻的点为i，其对应函数值为η_i，下一个可达的点j，对应函数值为η_j，则

蚂蚁会朝着信息素最多的方向移动，当没有信息素时，便会按照原来的运动方向移动，因此蚂蚁在t时刻处于i点时朝着j点移动的概率如式(7)：

式中，allowed为蚂蚁从地点i直接到达下一个路径点的集合，τ为信息素，τ_ij是路径i到路径j的信息素含量，α为信息素的相对重要程度，若α＝0，最靠近i的地点j将有被选出；β为距离信息的相对重要程度，β＝0，蚂蚁只受到信息素的影响而忽略了启发式信息带来的偏向性；如果蚂蚁移动方向有障碍物时，则随机选择其他的方向，如果有信息素指引时，则按照其指引行动，寻优时蚂蚁的移动概率根据式(7)来确定，Δη_ij(t)＜0表示蚂蚁在自身所在地点i的邻域搜索，感知并且行动；Δη_ij(t)＞0表示蚂蚁按照移动概率从其自身所在地点i的邻域移动至j的邻域；

经过n个时间单位之后，局部信息素更新策略按式(8)、(9)演变：

τ_ij(t+n)＝(1-ρ)τ_ij(t)+ρΔτ_ij (8)

其中ρ∈(0,1)是信息素挥发参数，Δτ_ij为本次循环中在路径(i,j)上的信息素增量，为蚂蚁k在本次循环中在路径(i,j)上留下的信息素；

最优-最差蚁群算法对最优解进行了更大限度的增强，对于最差解进行了削弱，使得属于最优路径和最差路径之间的信息素量差异进一步增大，这样可以使得蚂蚁的搜索更为集中在当前循环为止所找出的最优路径的范围内；

当一次迭代结束之后，对于最差蚂蚁所经过的路径进行信息素更新，即为全局信息素更新如式(10)：

其中ε是引入的参数，L_worst和L_best分别为当前循环中最差蚂蚁和最优蚂蚁的路径长度，τ(i,j)为在路径(i,j)上的信息素轨迹量；

路径的构建：

位于节点i的蚂蚁k在每次选择下一个路径前会产生一个随机数q，那么从节点i到节点j的移动规则p′按式(11)：

q是随机变量，均匀分布在区间[0,1]中，q₀∈[0,1]；

通过以上BWAS算法可以找到PID控制参数最优解。

本发明的四旋翼飞行器控制方法，根据式(8)、(9)、(10)蚂蚁来持续更新信息素，然后蚂蚁根据不同的信息素来选择不同的路径，当路径到达最远，且信息素含量最多的那条路径就是PID控制参数的最优解路径；其中最优-最差蚁群算法对最优解进行了更大限度的增强，对于最差解进行了削弱，使得属于最优路径和最差路径之间的信息素量差异进一步增大，这样可以使得蚂蚁的搜索更为集中在当前循环为止所找出的最优路径的范围内，因而能更有效地利用最优路径，也就可以更快速准确的获得PID控制参数最优解，再由步骤S10中的四旋翼无人飞行器的动力学方程转换成四个独立的控制通道，进而控制四旋翼无人飞行器的四个直流电电机的转速，解决了PID控制中其控制参数不能自动调整的缺陷，使得无人机在飞行时可以更好地适应外界环境的变化，提高了无人飞行器控制系统的抗扰性和鲁棒性，达到了理想的飞行控制效果。

为了验证本发明提出的四旋翼无人飞行器飞行控制方法的控制效果，利用搭建的四旋翼无人飞行器样机进行实验，分别进行了多个方案实验，具体如下：

如图6所示，在进行的偏航角跟踪实验中，使用了三种控制方法进行对比，PID控制下的偏航角跟踪曲线的动态性能指标为：超调量为6.7％，上升时间为1.13s，峰值时间为1.71s，调节时间为4.26s；AS-PID控制下的偏航角跟踪曲线的动态性能指标为：超调量为0.1％，上升时间为2.58s，峰值时间为2.73s，调节时间为3.17s；BWAS-PID控制下的偏航角跟踪曲线的动态性能指标为：超调量为0.1％，上升时间为0.95s，峰值时间为1.12s，调节时间为1.42s。由以上数据可知，在BWAS-PID控制下，相对于PID控制和AS-PID控制而言，超调量低，调节时间短，到达峰值所述时间少，调节快速准确，全面提升了控制系统的动态性能。

如图7所示，以翻滚角为例，来测试三种控制方法控制下的抗扰性，从图中可知，BWAS-PID控制下的抗扰能力要略强于AS-PID控制，强于PID控制。图8、图9和图10为三种不同控制方法的鲁棒性测试，以俯仰角为例，在PID控制中，超调量的变化范围为±0.4％，调节时间的变化范围为±0.02s；AS-PID控制中，超调量的变化范围为±0.2％，调节时间的变化范围为±0.01s；BWAS-PID控制中，超调量的变化范围为±0.1％，调节时间的变化范围为±0.01s；根据以上分析可知BWAS-PID控制下的系统，鲁棒性最好，AS-PID控制下的系统次之。

综上，本发明提出一种四旋翼飞行器的飞行控制方法，是基于最优-最差蚂蚁系统(Best-worst Ant System,BWAS)算法来优化PID控制参数的飞行控制方法，控制四旋翼无人机的飞行姿态过程，能适应外界环境变化，实现控制参数自适应调整，保持很好的抗干扰能力和鲁棒性，提高四旋翼无人飞行器的飞行品质。

以上仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的创造性精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。