[发明专利]一种四旋翼飞行器的控制系统及其控制方法

权利要求书

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