一种四旋翼飞行器的控制系统及其控制方法
【专利摘要】本发明公开了一种四旋翼飞行器的控制系统，包括主控制器以及分别与之连接的惯性测量单元、地磁仪、测距传感器和GPS模块，还包括摄像头模块、图像处理单元、电机驱动模块和4个电机，摄像头模块经图像处理单元与主控制器连接，主控制器经电机驱动模块分别与4个电机连接，所述4个电机分别带动四旋翼飞行器的4个螺旋桨。本发明还包括前述控制系统的控制方法，包括姿态控制算法、定高控制算法和数据融合算法。本发明采用惯性测量单元与定位系统的数据融合的控制结构与方法，提高了系统的稳定性和可靠性。
【专利说明】一种四旋翼飞行器的控制系统及其控制方法
【技术领域】
[0001] 本发明属于飞行器领域，特别涉及了一种四旋翼飞行器的控制系统及其控制方 法。
【背景技术】
[0002] 无人飞行器是指不需要驾驶员操作，通过无线电遥控或自身程序控制，利用空气 动力承载飞行并可回收重复使用的飞行器。无人飞行器出现在二十世纪二十年代，最早出 现时是作为靶机供放空火炮部队进行打靶使用。在海湾战争后，由于无人机在战争中的优 异表现，世界各国都争相进行无人机的研发，先进的无人机可以携带各种探测设备执行侦 察与监视任务，甚至可以装备攻击型武器执行打击任务。
[0003] 无人飞行器分为固定翼与旋翼式两种，旋翼无人机与固定翼无人机相比具有多方 面的优势，旋翼无人机可垂直起降，拥有良好的低空低速飞行性能并可在空中悬停，在空中 变向灵活并且对起飞和着陆场地要求小。在军用领域，旋翼无人机可以执行侦察、监视、诱 馆、攻击和通信中继站等任务。在民用和警用领域，旋翼无人机可以广泛应用于交通监控、 航空测绘、灾害评估及救援、环境保护和电力线路巡线等领域。四旋翼飞行器是一种电动 的、能够垂直起降的多旋翼式遥控/自主飞行器，属于非共轴式碟形飞行器。与常规旋翼式 飞行器相比，四旋翼飞行器不需要调整螺旋桨倾角来改变姿态，而是通过改变四个螺旋桨 的转速来改变飞行器的姿态，因此结构紧凑。由于螺旋桨数量的增加，载重也随之变大，特 别是其四只旋翼对称分布，产生的反扭力矩相互抵消，因此不需要额外的反扭矩尾桨，与常 规布局的直升机相比，四旋翼飞行器机械结构简单，成本较低，易于维护。其四个螺旋桨对 称分布，使得四旋翼飞行器的机动能力更强，静态盘旋的稳定性更好，也更容易实现机型的 微小型化，因此特别适合在室内、城区和丛林等近地面环境开展监视、侦察等任务，具有良 好的车事和民用如景。
[0004] 现有的小型四旋翼飞行器普遍使用惯性测量单元来测量姿态，从而控制飞行器的 飞行姿态，定位系统来获得飞行器的位置信息，很少将惯性测量单元与定位系统进行信息 融合。
【发明内容】
[0005] 为了解决上述【背景技术】提到的技术问题，本发明旨在提供一种四旋翼飞行器的控 制系统及其控制方法，采用惯性测量单元与定位系统的数据融合的控制结构与方法，提高 了系统的稳定性和可靠性。
[0006] 为了实现上述技术目的，本发明的技术方案为：
[0007] -种四旋翼飞行器的控制系统，包括主控制器以及分别与之连接的惯性测量单 元、地磁仪、测距传感器和GPS模块，还包括摄像头模块、图像处理单元、电机驱动模块和4 个电机，摄像头模块经图像处理单元与主控制器连接，主控制器经电机驱动模块分别与4 个电机连接，所述4个电机分别带动四旋翼飞行器的4个螺旋桨；所述惯性测量单元、地磁 仪、测距传感器和GPS模块分别检测四旋翼飞行器的惯性数据、方位角数据、高度数据和位 置数据并将这些数据传送给主控制器，主控制器根据前述数据产生油门信号并将油门信号 传送给电机驱动模块，电机驱动模块根据油门信号驱动4个电机运转，所述摄像头模块采 集位置图像信号并通过图像处理单元得到位置信息并将位置信息传送给主控制器。
[0008] 其中，上述控制系统还包括蓝牙模块，所述蓝牙模块连接主控制器，主控制器通过 该蓝牙模块实现其与地面控制站的数据交互。
[0009] 其中，上述惯性测量单元的型号为MPU6050,它包括三轴加速度传感器和三轴陀螺 仪；地磁仪的型号为HMC5983 ;测距传感器为HC-SR04超声波传感器；摄像头模块的型号为 0V7670 ;GPS模块的型号为HOLUX M-89 ;主控制器的型号为TIVA M4。
[0010] 本发明还包括基于上述一种四旋翼飞行器的控制系统的控制方法，包括姿态控制 算法，所述姿态控制算法采用级联PID算法，级联PID算法包括外环和内环，外环是将惯性 测量单元和地磁仪采集的姿态数据转换为四元数，再将四元数转换为欧拉角，将该欧拉角 与期望的欧拉角进行PID运算，输出角速度，内环是将外环PID运算输出的角速度作为期望 的角速度，与惯性测量单元采集的角速度进行PID运算，输出4个电机的PWM信号。
[0011] 本发明还包括基于上述一种四旋翼飞行器的控制系统的控制方法，包括定高控制 算法，所述定高控制算法采用级联PID算法，级联PID算法包括外环和内环，外环是将测距 传感器采集的高度值与期望的高度值进行PID运算，输出速率，内环是将外环PID输出的速 率作为期望的速率，与惯性测量单元采集的加速度经一阶积分得到的速率进行PID运算， 输出4个电机的PWM信号。
[0012] 本发明还包括基于上述一种四旋翼飞行器的控制系统的控制方法，包括数据融合 算法，通过联邦滤波器进行数据融合，所述联邦滤波器包括1个主滤波器和第一?第三子 滤波器，惯性测量单元作为公共参考系统，惯性测量单元与GPS模块采集的数据输入第一 子滤波器进行位置数据融合，惯性测量单元与测距传感器采集的数据输入第二子滤波器进 行高度数据融合，惯性测量单元与地磁仪采集的数据输入第三子滤波器进行方位数据融 合，再将3个子滤波器输出的数据送给主滤波器进行数据融合，其具体步骤如下：
[0013] (1)初始化时主滤波器将公共状态向子滤波器分配一次，然后各子滤波器单独工 作；
[0014] (2)各子滤波器按照各自的系统方程和量测方程独立进行卡尔曼滤波，即进行时 间更新和量测更新，各子滤波器将同一时刻的滤波结果送入主滤波器；
[0015] (3)对各子滤波器送来的滤波结果，主滤波器只把公共状态和相应的估计误差协 方差直接进行融合，保留融合后的值，直到下一融合时刻。
[0016] 采用上述技术方案带来的有益效果：
[0017] (1)本发明中各传感器在主控制器的协调下，相互协作，稳定四旋翼飞行器的各个 姿态参数，从而使得飞行器可以稳定高效完成各种动作任务，在系统初始化时，利用GPS模 块采集的信息对惯性测量单元进行初始对准，使之工作在最佳状态；
[0018] ⑵本发明采用数据融合方法，并通过构建联邦滤波器，使得采集到的数据更加准 确可靠；
[0019] (3)本发明的控制方法包括姿态控制算法、定高控制算法等，这些控制算法采用级 联的PID控制算法，使得对系统的控制更加稳定。
【专利附图】
【附图说明】
[0020] 图1为本发明的系统结构框图；
[0021] 图2为本发明的姿态控制算法示意图；
[0022] 图3为本发明的定高控制算法示意图；
[0023] 图4为本发明的数据融合算法示意图。
【具体实施方式】
[0024] 以下将结合附图，对本发明的技术方案进行详细说明。
[0025] 如图1所示本发明的系统结构框图，一种四旋翼飞行器的控制系统，包括主控制 器以及分别与之连接的惯性测量单元、地磁仪、测距传感器和GPS模块，还包括摄像头模 块、图像处理单元、电机驱动模块和4个电机，摄像头模块经图像处理单元与主控制器连 接，主控制器经电机驱动模块分别与4个电机连接，所述4个电机分别带动四旋翼飞行器的 4个螺旋桨；所述惯性测量单元、地磁仪、测距传感器和GPS模块分别检测四旋翼飞行器的 惯性数据、方位角数据、高度数据和位置数据并将这些数据传送给主控制器，主控制器根据 前述数据产生油门信号并将油门信号传送给电机驱动模块，电机驱动模块根据油门信号驱 动4个电机运转，所述摄像头模块采集位置图像信号并通过图像处理单元得到位置信息并 将位置信息传送给主控制器。
[0026] 在本实施例中，控制系统还包括蓝牙模块，所述蓝牙模块连接主控制器，主控制器 通过该蓝牙模块实现其与地面控制站的数据交互。惯性测量单元的型号为MPU6050,它包 括三轴加速度传感器和三轴陀螺仪；地磁仪的型号为HMC5983 ;测距传感器为HC-SR04超声 波传感器；摄像头模块的型号为0V7670 ;GPS模块的型号为HOLUX M-89 ;主控制器的型号为 TIVA M4。惯性测量单元MPU6050在工作之前需要进行初始对准，GPS模块HOLUX M-89获 得四旋翼飞行器的初始位置、速度和姿态信息并直接传递给MPU6050, MPU6050根据这些信 息进行初始对准。
[0027] 本发明还包括基于上述四旋翼飞行器控制系统的控制方法，包括姿态控制算法、 定高控制算法、定位控制算法和数据融合算法。
[0028] 在分析姿态控制算法之前，先建立数学模型，需要做如下假设：
[0029] ①四旋翼飞行器为均匀对称的刚体；
[0030] ②惯性坐标系E的原点与飞行器几何中心及质心位于同一位置；
[0031] ③四旋翼飞行器所受阻力和重力不受飞行高度等因素影响，总保持不变；
[0032] ④四旋翼飞行器各个方向的拉力与推进器转速的平方成正比例。
[0033] 再做如下定义：
[0034] 偏航角ψ :0x在OXY平面的投影与X轴夹角；
[0035] 俯仰角θ :〇z在OXZ平面的投影与Z轴夹角；
[0036] 翻滚角Φ :0y在OYZ平面的投影与Y轴夹角。
[0037] 根据牛顿第二定律和坐标转换公式等，推导出动力学方程：
[0038]
【权利要求】
1. 一种四旋翼飞行器的控制系统，其特征在于：包括主控制器w及分别与之连接的惯 性测量单元、地磁仪、测距传感器和GI^模块，还包括摄像头模块、图像处理单元、电机驱动 模块和4个电机，摄像头模块经图像处理单元与主控制器连接，主控制器经电机驱动模块 分别与4个电机连接，所述4个电机分别带动四旋翼飞行器的4个螺旋奖；所述惯性测量单 元、地磁仪、测距传感器和GI^模块分别检测四旋翼飞行器的惯性数据、方位角数据、高度 数据和位置数据并将该些数据传送给主控制器，主控制器根据前述数据产生油口信号并将 油口信号传送给电机驱动模块，电机驱动模块根据油口信号驱动4个电机运转，所述摄像 头模块采集位置图像信号并通过图像处理单元得到位置信息并将位置信息传送给主控制 器。
2. 根据权利要求1所述一种四旋翼飞行器的控制系统，其特征在于：还包括蓝牙模块， 所述蓝牙模块连接主控制器，主控制器通过该蓝牙模块实现其与地面控制站的数据交互。
3. 根据权利要求1所述一种四旋翼飞行器的控制系统，其特征在于；所述惯性测量 单元的型号为MPU6050,它包括H轴加速度传感器和H轴巧螺仪；所述地磁仪的型号为 HMC5983 ;所述测距传感器为肥-SR04超声波传感器；所述摄像头模块的型号为0V7670 ;所 述GI^模块的型号为册LUX 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个主滤波器和第 一?第H子滤波器，惯性测量单元作为公共参考系统，惯性测量单元与GI^模块采集的数 据输入第一子滤波器进行位置数据融合，惯性测量单元与测距传感器采集的数据输入第二 子滤波器进行高度数据融合，惯性测量单元与地磁仪采集的数据输入第H子滤波器进行方 位数据融合，再将3个子滤波器输出的数据送给主滤波器进行数据融合，其具体步骤如下： (1) 初始化时主滤波器将公共状态向子滤波器分配一次，然后各子滤波器单独工作； (2) 各子滤波器按照各自的系统方程和量测方程独立进行卡尔曼滤波，即进行时间更 新和量测更新，各子滤波器将同一时刻的滤波结果送入主滤波器； (3) 对各子滤波器送来的滤波结果，主滤波器只把公共状态和相应的估计误差协方差 直接进行融合，保留融合后的值，直到下一融合时刻。
【文档编号】G05D1/10GK104460685SQ201410677479
【公开日】2015年3月25日 申请日期:2014年11月21日 优先权日:2014年11月21日
【发明者】王其, 陈景研, 曾雪峰, 宁俊 申请人:南京信息工程大学