CN104460685A - 一种四旋翼飞行器的控制***及其控制方法 - Google Patents

Abstract

本发明公开了一种四旋翼飞行器的控制***，包括主控制器以及分别与之连接的惯性测量单元、地磁仪、测距传感器和GPS模块，还包括摄像头模块、图像处理单元、电机驱动模块和4个电机，摄像头模块经图像处理单元与主控制器连接，主控制器经电机驱动模块分别与4个电机连接，所述4个电机分别带动四旋翼飞行器的4个螺旋桨。本发明还包括前述控制***的控制方法，包括姿态控制算法、定高控制算法和数据融合算法。本发明采用惯性测量单元与定位***的数据融合的控制结构与方法，提高了***的稳定性和可靠性。

Description

一种四旋翼飞行器的控制***及其控制方法

技术领域

本发明属于飞行器领域，特别涉及了一种四旋翼飞行器的控制***及其控制方法。

背景技术

无人飞行器是指不需要驾驶员操作，通过无线电遥控或自身程序控制，利用空气动力承载飞行并可回收重复使用的飞行器。无人飞行器出现在二十世纪二十年代，最早出现时是作为靶机供放空火炮部队进行打靶使用。在海湾战争后，由于无人机在战争中的优异表现，世界各国都争相进行无人机的研发，先进的无人机可以携带各种探测设备执行侦察与监视任务，甚至可以装备攻击型武器执行打击任务。

无人飞行器分为固定翼与旋翼式两种，旋翼无人机与固定翼无人机相比具有多方面的优势，旋翼无人机可垂直起降，拥有良好的低空低速飞行性能并可在空中悬停，在空中变向灵活并且对起飞和着陆场地要求小。在军用领域，旋翼无人机可以执行侦察、监视、诱馆、攻击和通信中继站等任务。在民用和警用领域，旋翼无人机可以广泛应用于交通监控、航空测绘、灾害评估及救援、环境保护和电力线路巡线等领域。四旋翼飞行器是一种电动的、能够垂直起降的多旋翼式遥控/自主飞行器，属于非共轴式碟形飞行器。与常规旋翼式飞行器相比，四旋翼飞行器不需要调整螺旋桨倾角来改变姿态，而是通过改变四个螺旋桨的转速来改变飞行器的姿态，因此结构紧凑。由于螺旋桨数量的增加，载重也随之变大，特别是其四只旋翼对称分布，产生的反扭力矩相互抵消，因此不需要额外的反扭矩尾桨，与常规布局的直升机相比，四旋翼飞行器机械结构简单，成本较低，易于维护。其四个螺旋桨对称分布，使得四旋翼飞行器的机动能力更强，静态盘旋的稳定性更好，也更容易实现机型的微小型化，因此特别适合在室内、城区和丛林等近地面环境开展监视、侦察等任务，具有良好的军事和民用前景。

现有的小型四旋翼飞行器普遍使用惯性测量单元来测量姿态，从而控制飞行器的飞行姿态，定位***来获得飞行器的位置信息，很少将惯性测量单元与定位***进行信息融合。

发明内容

为了解决上述背景技术提到的技术问题，本发明旨在提供一种四旋翼飞行器的控制***及其控制方法，采用惯性测量单元与定位***的数据融合的控制结构与方法，提高了***的稳定性和可靠性。

为了实现上述技术目的，本发明的技术方案为：

一种四旋翼飞行器的控制***，包括主控制器以及分别与之连接的惯性测量单元、地磁仪、测距传感器和GPS模块，还包括摄像头模块、图像处理单元、电机驱动模块和4个电机，摄像头模块经图像处理单元与主控制器连接，主控制器经电机驱动模块分别与4个电机连接，所述4个电机分别带动四旋翼飞行器的4个螺旋桨；所述惯性测量单元、地磁仪、测距传感器和GPS模块分别检测四旋翼飞行器的惯性数据、方位角数据、高度数据和位置数据并将这些数据传送给主控制器，主控制器根据前述数据产生油门信号并将油门信号传送给电机驱动模块，电机驱动模块根据油门信号驱动4个电机运转，所述摄像头模块采集位置图像信号并通过图像处理单元得到位置信息并将位置信息传送给主控制器。

其中，上述控制***还包括蓝牙模块，所述蓝牙模块连接主控制器，主控制器通过该蓝牙模块实现其与地面控制站的数据交互。

其中，上述惯性测量单元的型号为MPU6050，它包括三轴加速度传感器和三轴陀螺仪；地磁仪的型号为HMC5983；测距传感器为HC-SR04超声波传感器；摄像头模块的型号为OV7670；GPS模块的型号为HOLUX M-89；主控制器的型号为TIVA M4。

本发明还包括基于上述一种四旋翼飞行器的控制***的控制方法，包括姿态控制算法，所述姿态控制算法采用级联PID算法，级联PID算法包括外环和内环，外环是将惯性测量单元和地磁仪采集的姿态数据转换为四元数，再将四元数转换为欧拉角，将该欧拉角与期望的欧拉角进行PID运算，输出角速度，内环是将外环PID运算输出的角速度作为期望的角速度，与惯性测量单元采集的角速度进行PID运算，输出4个电机的PWM信号。

本发明还包括基于上述一种四旋翼飞行器的控制***的控制方法，包括定高控制算法，所述定高控制算法采用级联PID算法，级联PID算法包括外环和内环，外环是将测距传感器采集的高度值与期望的高度值进行PID运算，输出速率，内环是将外环PID输出的速率作为期望的速率，与惯性测量单元采集的加速度经一阶积分得到的速率进行PID运算，输出4个电机的PWM信号。

本发明还包括基于上述一种四旋翼飞行器的控制***的控制方法，包括数据融合算法，通过联邦滤波器进行数据融合，所述联邦滤波器包括1个主滤波器和第一～第三子滤波器，惯性测量单元作为公共参考***，惯性测量单元与GPS模块采集的数据输入第一子滤波器进行位置数据融合，惯性测量单元与测距传感器采集的数据输入第二子滤波器进行高度数据融合，惯性测量单元与地磁仪采集的数据输入第三子滤波器进行方位数据融合，再将3个子滤波器输出的数据送给主滤波器进行数据融合，其具体步骤如下：

(1)初始化时主滤波器将公共状态向子滤波器分配一次，然后各子滤波器单独工作；

(2)各子滤波器按照各自的***方程和量测方程独立进行卡尔曼滤波，即进行时间更新和量测更新，各子滤波器将同一时刻的滤波结果送入主滤波器；

(3)对各子滤波器送来的滤波结果，主滤波器只把公共状态和相应的估计误差协方差直接进行融合，保留融合后的值，直到下一融合时刻。

采用上述技术方案带来的有益效果：

(1)本发明中各传感器在主控制器的协调下，相互协作，稳定四旋翼飞行器的各个姿态参数，从而使得飞行器可以稳定高效完成各种动作任务，在***初始化时，利用GPS模块采集的信息对惯性测量单元进行初始对准，使之工作在最佳状态；

(2)本发明采用数据融合方法，并通过构建联邦滤波器，使得采集到的数据更加准确可靠；

(3)本发明的控制方法包括姿态控制算法、定高控制算法等，这些控制算法采用级联的PID控制算法，使得对***的控制更加稳定。

附图说明

图1为本发明的***结构框图；

图2为本发明的姿态控制算法示意图；

图3为本发明的定高控制算法示意图；

图4为本发明的数据融合算法示意图。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

如图1所示本发明的***结构框图，一种四旋翼飞行器的控制***，包括主控制器以及分别与之连接的惯性测量单元、地磁仪、测距传感器和GPS模块，还包括摄像头模块、图像处理单元、电机驱动模块和4个电机，摄像头模块经图像处理单元与主控制器连接，主控制器经电机驱动模块分别与4个电机连接，所述4个电机分别带动四旋翼飞行器的4个螺旋桨；所述惯性测量单元、地磁仪、测距传感器和GPS模块分别检测四旋翼飞行器的惯性数据、方位角数据、高度数据和位置数据并将这些数据传送给主控制器，主控制器根据前述数据产生油门信号并将油门信号传送给电机驱动模块，电机驱动模块根据油门信号驱动4个电机运转，所述摄像头模块采集位置图像信号并通过图像处理单元得到位置信息并将位置信息传送给主控制器。

在本实施例中，控制***还包括蓝牙模块，所述蓝牙模块连接主控制器，主控制器通过该蓝牙模块实现其与地面控制站的数据交互。惯性测量单元的型号为MPU6050，它包括三轴加速度传感器和三轴陀螺仪；地磁仪的型号为HMC5983；测距传感器为HC-SR04超声波传感器；摄像头模块的型号为OV7670；GPS模块的型号为HOLUX M-89；主控制器的型号为TIVA M4。惯性测量单元MPU6050在工作之前需要进行初始对准，GPS模块HOLUX M-89获得四旋翼飞行器的初始位置、速度和姿态信息并直接传递给MPU6050，MPU6050根据这些信息进行初始对准。

本发明还包括基于上述四旋翼飞行器控制***的控制方法，包括姿态控制算法、定高控制算法、定位控制算法和数据融合算法。

在分析姿态控制算法之前，先建立数学模型，需要做如下假设：

①四旋翼飞行器为均匀对称的刚体；

②惯性坐标系E的原点与飞行器几何中心及质心位于同一位置；

③四旋翼飞行器所受阻力和重力不受飞行高度等因素影响，总保持不变；

④四旋翼飞行器各个方向的拉力与推进器转速的平方成正比例。

再做如下定义：

偏航角ψ：Ox在OXY平面的投影与X轴夹角；

俯仰角θ：Oz在OXZ平面的投影与Z轴夹角；

翻滚角φ：Oy在OYZ平面的投影与Y轴夹角。

根据牛顿第二定律和坐标转换公式等，推导出动力学方程：

式(1)中，[x y z]^T为四旋翼飞行器在导航坐标系下的线位移，为运动加速度，m为飞行器质量，θ,φ分别为机体的偏航角、俯仰角和横滚角，l为旋翼面中心到四旋翼飞行器质心的距离，I_X，I_Y，I_Z为轴向惯性主矩。通过上述动力方程在加上对动量矩的描述方程，可以得到算法直接依赖的方程。

$\left[\begin{array}{c}{U}_1\\ U_2\\ U_3\\ U_4\end{array}\right]=\left[\begin{array}{c}{F}_1+F_2+F_3+F_4\\ F_4-F_2\\ F_3-F_1\\ F_2+F_4-F_3-F_1\end{array}\right]=\left[\begin{array}{c}{k}_t\underset{i=1}{\overset{4}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^2\\ k_t({\mathrm{\ω}}_4^2-{\mathrm{\ω}}_2^2)\\ k_t({\mathrm{\ω}}_3^2-{\mathrm{\ω}}_1^2)\\ k_d({\mathrm{\ω}}_1^2-{\mathrm{\ω}}_2^2+{\mathrm{\ω}}_3^2-{\mathrm{\ω}}_4^2)\end{array}\right]---\left(2\right)$

式(2)中，U₁为垂直速度控制量，U₂为翻滚输入控制量，U₃为俯仰控制量，U₄为偏航控制量，ω为旋翼转速，F_i为旋翼所受拉力(i＝1,2,3,4)。由于油门(即输出PWM占空比)与F_i成近似的线性关系，即F_i＝k*ai(ai为四个电机PWM占空比)，则得到了控制量ai与U1，U2，U3，U4的之间关系。此数学模型直接建立了四通道PWM占空比对飞行姿态的控制，是姿态控制的理论依据。

如图2所示本发明的姿态控制算法示意图，姿态控制算法采用级联PID算法，级联PID算法包括外环和内环，外环是将惯性测量单元和地磁仪采集的姿态数据转换为四元数，再将四元数转换为欧拉角，将该欧拉角与期望的欧拉角进行PID运算，输出角速度，内环是将外环PID运算输出的角速度作为期望的角速度，与惯性测量单元采集的角速度进行PID运算，输出4个电机的PWM信号，从而调整四旋翼飞行器的姿态。其中，四元数(x,y,z,w)是简单的超复数,是3D图形学中常用的表示旋转的数，利用四元数可以将原始的姿态数据转换为一个四元数的矩阵，方便储存姿态数据，减小储存空间。四元数只是一个中间量。在笛卡尔坐标系下可以得到四元数和欧拉角的转换公式为：

如图3所示本发明的定高控制算法示意图，高控制算法采用级联PID算法，级联PID算法包括外环和内环，外环是将测距传感器采集的高度值与期望的高度值进行PID运算，输出速率，内环是将外环PID输出的速率作为期望的速率，与惯性测量单元采集的加速度经一阶积分得到的速率进行PID运算，输出4个电机的PWM信号，从而确定四旋翼飞行器的飞行高度。

由于加速度传感器不能捕捉匀速运动给位置带来的变化，故引进OV7670摄像头，辅助定位。OV7670摄像头采集图像数据，并将其送给图像处理单元，图像处理单元分析图像数据，得到四旋翼飞行器当前位置信息，将位置信息送给主控器，主控器进行PID运算，调整姿态角的期望，从而使四旋翼飞行器到达稳定的位置。

如图4所示本发明的数据融合算法示意图，通过联邦滤波器进行数据融合，该联邦滤波器包括1个主滤波器MF和3个子滤波器LF1、LF2、LF3，惯性测量单元作为公共参考***，惯性测量单元与GPS模块采集的数据输入子滤波器LF1进行位置数据融合，惯性测量单元与测距传感器采集的数据输入子滤波器LF2进行高度数据融合，惯性测量单元与地磁仪采集的数据输入子滤波器LF3进行方位数据融合，再将3个子滤波器输出的数据送给主滤波器进行数据融合，

其中，3个子滤波器采用普通的卡尔曼滤波器即可，而主滤波器直接将3个子滤波器输出的信息进行融合，不需要滤波，其具体步骤如下：

(1)初始化时主滤波器将公共状态向子滤波器分配一次，然后各子滤波器单独工作：初始分配一次信息，由于惯导***同时参与了三个子滤波器的滤波，惯导状态为公共状态，其信息按照信息守恒原理在各个子滤波器直接进行分配：

$\left\{\begin{array}{cc}{\hat{X}}_{\mathrm{ci}}={\hat{X}}_{\mathrm{cg}}& \\ P_{\mathrm{ci}}^{-1}={\mathrm{\β}}_i{P}_{\mathrm{cg}}^{-1}& i=\mathrm{1,2,3}\\ Q_{\mathrm{ci}}^{-1}={\mathrm{\β}}_i{Q}_{\mathrm{cg}}^{-1}& \end{array}\right.---\left(4\right)$

其中，β₁、β₂和β₃为信息分配系数，满足β₁+β₂+β₃＝1，其值可以根据实际情况选择。Q为公共状态的干扰噪声(过程噪声)方差强度阵，下标“c”表示公共状态，即为惯导***状态。“g”表示全局估计。

以下介绍信息分配系数的取值原则即公共参考信息的分配原则。

设在联邦滤波器中，参与组合的导航子***除惯导外，还有N个非相似导航子***。由于惯导是公共参考***，它参与了由该N个子***和惯导构成的N个子滤波器的滤波，所以惯导的信息应在这N个子滤波器之间进行分配。根据信息守恒原理，分配系数应满足β_i为第i个子滤波器获得的信息分配系数。设X的估计误差的均方误差阵为P，P描述了对X的估计质量，而P^-1为信息矩阵。P越大，X的估计质量就越差，信息矩阵P^-1就越小；反之，P越小，X的估计质量就越好，信息矩阵P^-1就越大。由式可以看出，对惯导***的信息作分配，实质上就是将参与第i个子滤波器滤波的惯导的估计均方误差阵Pc扩大1/β_i倍。从中可以看出，β_i越小，则Pc扩大的倍数就越大。由于卡尔曼滤波器能自动根据信息质量的优劣作权重不同的利用，所以β_i越小，对惯导信息的利用权重就越低，该子滤波器的滤波精度主要取决于第i个子***的信息质量，而惯导***的输出信息所起的作用相对降低；反之亦然。

综上所述，惯导信息在子滤波器分配的一般原则：子***精度越差，惯导信息的分配系数应该越大；子***精度越高，惯导信息的分配系数应适当取得小些。因为子***精度越高，相应子滤波器滤波精度受β_i的影响就越小，此时，β_i取得小些可使总量有限的惯导信息在较低精度子***所在的子滤波器中能充分发挥作用。

(2)各子滤波器按照各自的***方程和量测方程独立进行卡尔曼滤波，即进行时间更新和量测更新，各子滤波器将同一时刻的滤波结果送入主滤波器。

(3)对各子滤波器送来的滤波结果，主滤波器只把公共状态和相应的估计误差协方差直接进行融合，保留融合后的值，直到下一融合时刻。主滤波器数据融合周期与子滤波器滤波周期相同，将3个子滤波器每步滤波的结果中，取出公共状态P_ci，i＝1,2,3，传递给主滤波器。主滤波器按照式(5)完成信息的最优综合，形成全局***的综合信息P_cg：

$\left\{\begin{array}{c}{P}_{\mathrm{cg}}^{-1}=P_{c1}^{-1}+P_{c2}^{-1}+P_{c3}^{-1}\\ {\hat{X}}_{\mathrm{cg}}\left(k\right)=P_{\mathrm{cg}}[P_{c1}^{-1}{\hat{X}}_{c1}\left(k\right)+P_{c2}^{-1}{\hat{X}}_{c2}\left(k\right)+P_{c3}^{-1}{\hat{X}}_{c3}\left(k\right)]\end{array}\right.---\left(5\right)$

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (6)

1.一种四旋翼飞行器的控制***，其特征在于：包括主控制器以及分别与之连接的惯性测量单元、地磁仪、测距传感器和GPS模块，还包括摄像头模块、图像处理单元、电机驱动模块和4个电机，摄像头模块经图像处理单元与主控制器连接，主控制器经电机驱动模块分别与4个电机连接，所述4个电机分别带动四旋翼飞行器的4个螺旋桨；所述惯性测量单元、地磁仪、测距传感器和GPS模块分别检测四旋翼飞行器的惯性数据、方位角数据、高度数据和位置数据并将这些数据传送给主控制器，主控制器根据前述数据产生油门信号并将油门信号传送给电机驱动模块，电机驱动模块根据油门信号驱动4个电机运转，所述摄像头模块采集位置图像信号并通过图像处理单元得到位置信息并将位置信息传送给主控制器。

2.根据权利要求1所述一种四旋翼飞行器的控制***，其特征在于：还包括蓝牙模块，所述蓝牙模块连接主控制器，主控制器通过该蓝牙模块实现其与地面控制站的数据交互。

3.根据权利要求1所述一种四旋翼飞行器的控制***，其特征在于：所述惯性测量单元的型号为MPU6050，它包括三轴加速度传感器和三轴陀螺仪；所述地磁仪的型号为HMC5983；所述测距传感器为HC-SR04超声波传感器；所述摄像头模块的型号为OV7670；所述GPS模块的型号为HOLUX M-89；所述主控制器的型号为TIVA M4。

4.基于权利要求1所述一种四旋翼飞行器的控制***的控制方法，其特征在于：包括姿态控制算法所述姿态控制算法采用级联PID算法，级联PID算法包括外环和内环，外环是将惯性测量单元和地磁仪采集的姿态数据转换为四元数，再将四元数转换为欧拉角，将该欧拉角与期望的欧拉角进行PID运算，输出角速度，内环是将外环PID运算输出的角速度作为期望的角速度，与惯性测量单元采集的角速度进行PID运算，输出4个电机的PWM信号。

5.基于权利要求1所述一种四旋翼飞行器的控制***的控制方法，其特征在于：包括定高控制算法，所述定高控制算法采用级联PID算法，级联PID算法包括外环和内环，外环是将测距传感器采集的高度值与期望的高度值进行PID运算，输出速率，内环是将外环PID输出的速率作为期望的速率，与惯性测量单元采集的加速度经一阶积分得到的速率进行PID运算，输出4个电机的PWM信号。

6.基于权利要求1所述一种四旋翼飞行器的控制***的控制方法，其特征在于：包括数据融合算法，通过联邦滤波器进行数据融合，所述联邦滤波器包括1个主滤波器和第一～第三子滤波器，惯性测量单元作为公共参考***，惯性测量单元与GPS模块采集的数据输入第一子滤波器进行位置数据融合，惯性测量单元与测距传感器采集的数据输入第二子滤波器进行高度数据融合，惯性测量单元与地磁仪采集的数据输入第三子滤波器进行方位数据融合，再将3个子滤波器输出的数据送给主滤波器进行数据融合，其具体步骤如下：

（1）初始化时主滤波器将公共状态向子滤波器分配一次，然后各子滤波器单独工作；

（2）各子滤波器按照各自的***方程和量测方程独立进行卡尔曼滤波，即进行时间更新和量测更新，各子滤波器将同一时刻的滤波结果送入主滤波器；

（3）对各子滤波器送来的滤波结果，主滤波器只把公共状态和相应的估计误差协方差直接进行融合，保留融合后的值，直到下一融合时刻。