本发明是四旋翼飞行器的位姿控制器，其结构包括姿态角度控制器、姿态角速度控制器、控制量融合模块、规划模块、姿态检测模块；本发明的优点：（1）该控制器能完成路点飞行、悬停等多种飞行任务，抗干扰能力较强；（2）该控制器主要用程序软实现，硬件结构简单，易于实现，成本低廉，运行可靠；（3）本控制器程序均采用模块化设计，具有较好的可移植性，缩短了系统软硬件的开发周期。
1.四旋翼飞行器的位姿控制器，其特征是包括2个反馈控制回路，姿态角度控制器、姿态角速度控制器，规划模块，控制量融合模块；所述2个反馈控制回路是姿态角度和姿态角速度这2个3维向量回路；所述姿态角度控制器和姿态角速度控制器采用PID控制算法，分别控制四旋翼飞行器的姿态角度和姿态角速度，规划模块根据四旋翼机当前飞行状态和飞行任务要求，调节这两类控制器输出权值和被控量期望值，最后再由控制量融合模块计算出四个旋翼电机的最终控制量；
所述的姿态角度控制器的信号输出端接控制量融合模块的第一信号输入端，规划模块的第一信号输出端接姿态角度控制器的信号输入端，规划模块的第二信号输出端接控制量融合模块的第二信号输入端，规划模块的第三信号输出端接姿态角速度控制器的信号输入端，姿态角速度控制器的信号输出端接控制量融合模块的第三信号输入端，控制量融合模块的信号输出端接四旋翼飞行器的信号输入端，四旋翼飞行器的信号输出端接姿态检测模块的信号输入端，姿态检测模块的第一信号输出端接姿态角度控制器的第二信号输入端，姿态检测模块的第二信号输出端接姿态角速度控制器的第二信号输入端。
2.根据权利要求1所述的四旋翼飞行器的位姿控制器，其特征是所述的规划模块根据四旋翼飞行器的任务要求，确定角度控制器的期望值，从而完成各类飞行动作；其次，当飞行器处于起飞或路点飞行状态时，优先进行角度控制，相应提高权重值 ，当飞行器处于降落或振动状态时，优先进行姿态角速度控制，相应提高权重值 ，当飞行器处于稳定悬停状态时，实行均衡控制 。
3.根据权利要求1所述的四旋翼飞行器的位姿控制器，其特征是所述的四旋翼机机体坐标系 ，四旋翼的参考点选为机体中心位置，在全局坐标系 中的坐标记为，该点与 的坐标原点重合；其中，轴方向为四旋翼机头方向，绕 轴旋转的角度记为 ，称为滚转角；绕 轴旋转的角度记为 ，称为俯仰角；绕 轴旋转的角度记为 ，称为偏转角；
4.根据权利要求1所述的四旋翼飞行器的位姿控制器，其特征是所述的姿态角度控制器和姿态角速度控制器均采用位置式离散PID算法，与滚转、俯仰、偏转角对应，姿态角度控制器由3个角度的PID控制器组成，姿态角速度控制器与之类似，由3个角速度的PID控制器组成；由姿态检测模块mpu6050芯片可直接检测当前角速度 ，进而运用四元数法解算出当前角度 ；规划模块根据当前飞行任务要求，给定姿态角度和姿态角速度期望值；例如，完成悬停任务时， 均为0；路点飞行任务时，初始阶段 不为0，设置 关闭角速度控制通道；下面以 轴方向滚转角 为例，令，其角度控制器第k个周期的输出值为， （1）
其中， 分别为姿态角度控制器的比例、积分、微分系数；同理可得 、轴方向角度控制器的输出值 ，最终可得 ；
姿态角速度控制器与姿态角度控制器结构类似，仍以 轴方向为例，令 ，其角速度控制器第k个周期的输出值为， （2）
其中， 分别为姿态角速度控制器的比例、积分、微分系数；同理可得 、轴方向姿态角度控制器的输出值 ，最终可得 ；
根据反馈控制回路的权重值 ，调节角度和角速度控制器输出，
经加权计算出基于姿态角的综合控制量 ，最后结合高度控制量alt，计算出4个旋翼电机的控制量；
四旋翼飞行器的位姿控制器
技术领域
[0001] 本发明是一种四旋翼飞行器的位姿控制器，可对四旋翼位姿（位置和姿态）进行控制，进而完成路点飞行、悬停等飞行任务，属于飞行器控制的技术领域。
背景技术
[0002] 无人驾驶飞机（Unmanned Aerial Vehicle）是一种以无线电遥控或由自身程序控制的不载人飞机，与载人飞机相比，它具有体积小、造价低、对飞行环境要求低、空中生存能力强等特点。不仅可以节省大量的人力和物力资源，而且使用起来更加安全方便，这些独特的优势使无人机在工农业生产和军事领域备受青睐，应用前景广阔。
[0003] 四旋翼(Quadrotor)是一类典型的无人驾驶的旋翼式飞行器，由4个围绕机体中心对称分布的旋翼构成的飞行器。它是一种结构简单、可以垂直起降，它在总体布局形式上属于非共轴式碟形飞行器，具有独特的飞行控制方式。同其他结构的直升机相比，四旋翼直升机的主要有以下两点：
[0004] (1)有效载荷更大，与常规旋翼式飞行器相比，四只旋翼使它可以产生更大的升力，因此拥有更大的载重量；
[0005] (2)控制系统简单，控制四个旋翼的转速就可以实现姿态、水平/垂直移动，不需要配备专门的反扭矩桨，四只旋翼可互相抵消各自产生的反扭力矩。
[0006] 近年来，四旋翼的应用和研究广泛受到广泛重视。在军事领域，四旋翼可作为空中侦察平台执行侦察监视、目标定位、激光制导、通信中继、电子干扰、战斗评估等任务。在民用领域，四旋翼可应用于场区监控、电力线路巡查、气象探测、公路巡视、勘探测绘、电影特技、航空摄影、交通管理、森林火灾救防等。
发明内容
[0007] 本发明提出的是一种四旋翼飞行器的位姿控制器，选用ARM芯片作为主控芯片，设计一种硬件结构简单、易于实现、抗干扰能力强的四旋翼位姿控制器，使得四旋翼能够胜任多种飞行任务。该控制器的构造要点包括采用姿态角和姿态角速度双反馈回路，特别设计了规划模块（在主控芯片中软件方式实现），根据飞行器的飞行状态和飞行任务要求，适时调节双PID控制器输出。
[0008] 本发明的技术解决方案：四旋翼飞行器的位姿控制器，其特征是包括2个反馈控制回路，姿态角度控制器、姿态角速度控制器，规划模块，控制量融合模块；所述2个反馈控制回路是姿态角度和姿态角速度这2个3维向量回路；所述姿态角度控制器和姿态角速度控制器采用PID控制算法，分别控制四旋翼飞行器的姿态角度和姿态角速度，规划模块根据四旋翼机当前飞行状态和飞行任务要求，调节这两类控制器输出权值和被控量期望值，最后再由控制量融合模块计算出四个旋翼电机的最终控制量。
[0009] 本发明的优点：
[0010] （1）该控制器能完成路点飞行、悬停等多种飞行任务，抗干扰能力较强；
[0011] （2）该控制器主要用程序软实现，硬件结构简单，易于实现，成本低廉，运行可靠；
[0012] （3）本控制器程序均采用模块化设计，具有较好的可移植性，缩短了系统软硬件的开发周期。
附图说明
[0013] 图1是四旋翼坐标系示意图。
[0014] 图2是四旋翼飞行器的位姿控制器结构图。
[0015] 图3是角度和角速度控制器结构示意图。
[0016] 图4是控制量融合模块结构示意图。
[0017] 图5是控制器功能结构示意图。
具体实施方式
[0018] 四旋翼飞行器的位姿控制器，其结构包括2个反馈控制回路，姿态角度控制器、姿态角速度控制器，规划模块，控制量融合模块；所述2个反馈控制回路是姿态角度和姿态角速度这2个3维向量回路；所述姿态角度控制器和姿态角速度控制器采用PID控制算法，分别控制四旋翼飞行器的姿态角度和姿态角速度，规划模块根据四旋翼机当前飞行状态和飞行任务要求，调节这两类控制器输出权值和被控量期望值，最后再由控制量融合模块计算出四个旋翼电机的最终控制量。
[0019] 下面结合附图进一步描述本发明的技术方案：
[0020] 如图1所示，全局坐标系 ，四旋翼机体坐标系 ，四旋翼的参考点选为机体中心位置，在 中的坐标记为 ，该点与 的坐标原点重合。其中，轴方向为四旋翼机头方向，绕 轴旋转的角度记为 ，称为滚转角；绕 轴旋转的角度记为 ，称为俯仰角；绕 轴旋转的角度记为 ，称为偏转角。四旋翼位姿的向量形式记为 。
[0021] 如图2所示，四旋翼飞行器的位姿控制器，其结构是包含2个反馈控制回路，同时角度控制器、角速度控制器分别控制四旋翼的角度和角速度，规划模块根据四旋翼当前飞行状态和飞行任务要求，调节控制器权值和控制器期望值，最后再由控制量融合模块通过加权计算出四个旋翼电机的最终控制量，由于受外界不确定因素如风、气流的干扰，当姿态角偏离期望值幅度过大时，应优先进行角度控制，相应提高权重值 ；反之，当姿态角处于正常范围，姿态角速度偏离期望值幅度过大时，应优先进行角速度控制，相应提高权重值 。
[0022] 表1 双控制器输出权重表
[0023]飞行状态
起飞状态 1:0
稳定悬停状态 1:1
机体振动状态 1:3
路点飞行状态 3:1
降落状态 1:2
[0024] 规划模块根据四旋翼当前飞行状态和飞行任务要求，确定角度和角速度控制器权重和控制器期望值。如表1所示，起飞/降落，悬停和路点飞行状态可根据遥控器/地面控制站的命令加以识别；飞行过程中对角速度采样值的绝对值进行滑动平均（滑动窗中含128个点），如果角速度均值大于120，则飞行器处于振动状态，规划模块将修改双控制器的权重；反之，飞行器处于稳定悬停状态或路点飞行状态。路点飞行状态需外接GPS模块构成反馈通道，规划模块根据位置偏差确定姿态角期望值，进而完成路点飞行任务。
[0025] 所述的角度控制器的信号输出端接控制量融合模块的第一信号输入端，规划模块的第一信号输出端接角度控制器的信号输入端，规划模块的第二信号输出端接控制量融合模块的第二信号输入端，规划模块的第三信号输出端接角速度控制器的信号输入端，角速度控制器的信号输出端接控制量融合模块的第三信号输入端，控制量融合模块的信号输出端接四旋翼飞行器的信号输入端，四旋翼飞行器的信号输出端接姿态检测模块的信号输入端，姿态检测模块的第一信号输出端接角度控制器的第二信号输入端，姿态检测模块的第二信号输出端接角速度控制器的第二信号输入端。
[0026] 如图3所示，姿态角度控制器和姿态角速度控制器均采用位置式离散PID算法，与滚转、俯仰、偏转角对应，角度控制器由3个角度的PID控制器组成。角速度控制器与之类似，由3个角速度的PID控制器组成。姿态检测模块（mpu6050芯片）可直接检测当前角速度，进而运用四元数法解算出当前角度 。规划模块根据当前飞行任务要求，给定角度和角速度期望值 。例如，完成悬停任务时， 均为0；路点飞行任务时，初始阶段 不为0，设置 关闭角速度控制通道。下面以 轴方向滚
转角 为例，令 ，其角度控制器第k个周期的输出值为，
[0027] （1）
[0028] 其中， 分别为角度PID控制器的比例、积分、微分系数。同理可得 、轴方向角度控制器的输出值 ，最终可得 。
[0029] 角速度控制器与角度控制器结构类似，仍以 轴方向为例，令 ，其角速度控制器第k个周期的输出值为，
[0030] （2）
[0031] 其中， 分别为角速度PID控制器的比例、积分、微分系数。同理可得 、轴方向角度控制器的输出值 ，最终可得 。
[0032] 如图4所示，控制量融合模块，规划模块根据当前飞行状态，确定合适的权重值，调节角度和角速度控制器输出，经加权计算出基于姿态角的综合控制量 ，最后结合高度控制量alt，计算出4个旋翼电机的控制量。
[0033] （3）
[0034] （4）
[0035] 其中 ， ，alt为四旋翼高度控制量。 是4个旋翼电机的控制量。
[0036] 如图5所示，该控制器采用STM32f4076VET芯片为主控芯片。传感器模块由气压计、磁场传感器、陀螺仪和加速度计构成，其中陀螺仪和加速度计集成于MPU6050芯片中，用于检测飞行器的姿态角和姿态角速度，是飞行器的核心传感器；气压计MS5611用于测量飞行器的高度，磁场传感器HMC5983用于控制飞行器机头方向。遥控器通信接口可外接2.4G无线接收器，方便向飞行器发送命令或进行紧急控制；串行和无线通信接口用于数据的双向传输，既可以将命令由地面控制站（上位机）发送到飞行器，也可以将飞行器数据（高度、姿态角等）发送到地面控制站。电源模块采用两级供电模式，一级稳压模块将锂电池的电压降为5V，给二级稳压模块和遥控器接收机供电，二级稳压模块将5V降为3.3V，给主控芯片和传感器芯片等供电。
[0037] 控制器硬件设计思想如下，首先，主控制芯片具备以下特点：（1）内核强大：具有168MHz的CortexTM-M4内核（具有浮点单元），支持浮点运算。（2）资源丰富：6个高速USART串口、3个45Mb/s的SPI总线和3个硬件I2C总线，这些接口足够连接传感器、GPS等设备；定时器多达17个，可用于遥控器数据读取和电机输出等（3）存储器容量大：具有512KB Flash和
192KB SRAM，具有充足的内存运行操作系统和处理数据。
[0038] 为了避免电机的大电流对控制系统造成不利影响，控制器采用了2级稳压给控制器供电。锂电池的供电电压范围是10.5V 12.6V，一级稳压电源采用开关电源，辅以大电解~电容，稳定电压在5V，此电压将供电给二级稳压模块和遥控器接收机。二级稳压由于需要低噪稳定电源，采用低压降的线性稳压芯片进行稳压，将电源稳压在3.3V供给主控芯片和传感器芯片等。一级稳压电源采用的是MP2307DN开关电源，二级稳压芯片采用的是AMS1117线性稳压芯片。MP2307DN开关电源具有以下特性：（1）3A大电流持续输出，足以满足控制系统供电和接收机供电，并且为以后的机载设备提供5V电源；（2）4.75V 23V宽电压输入；（3）~
95％的 电压转换效率；（4）具有过载保护功能，更好的保护了控制系统安全。AMS1117线性稳压电源具有以下特性：（1）超低噪音供电，给控制系统的无线模块供电提供保障；（2）快速的线性负载响应，确保控制系统的电源更稳定；（3）宽输入电压，低电源压降。
[0039] MPU6050传感器芯片目前使用相当广泛，内部整合了3轴加速度计和3轴陀螺仪，并且还扩展了I2C接口，并利用传感器自带的DMP（Digital Motion Processor）硬件加速引擎，通过主I2C接口，向主控芯片输出姿态数据。MS5611气压传感器，其具有以下特点：（1）高分辨率模块，测量精度可达10cm；（2）转换时间快至1ms；（3）1μA低功耗；（4）内置数字压力传感器（24位AD转换）；磁场传感器采用霍尼韦尔的HMC5983芯片，它是一个自带温度补偿的三轴集成数字罗盘，内置了HMC118X系列的高分辨率磁阻传感器和ASIC，再集成有自动消磁带、放大器、偏置带以及12位ADC，使得通过HMC5983获得的航向角精确度达到了1 2度。~
[0040] 串口通信接口采用CP2102芯片作为串口控制器，由于其内置了USB2.0的全速功能控制器和USB收发器，因此可以直接通过USB连接PC机进行通信。无线通信接口与主控芯片的SPI总线相连，方便外接nRF24L01等通信模块。遥控器通信接口与主控芯片的定时器引脚相连，方便捕获遥控器发出的PWM。
[0041] 采用STM32f4076VET芯片为主控芯片。传感器模块包括气压计、磁场传感器、陀螺仪和加速度计共4类传感器，其中陀螺仪和加速度计集成于MPU6050芯片中，检测飞行器姿态角和姿态角速度，气压计MS5611用于测量飞行器的高度，磁场传感器HMC5983用于控制飞行器机头方向。通信接口包括遥控器、串口、无线通信接口共3类接口，既可以将命令由地面控制站（上位机）发送到飞行器，也可以将飞行器数据（高度、姿态角等）发送到地面控制站。电源模块采用2级供电模式，一级稳压模块将锂电池的电压降为5V，给二级稳压模块和遥控器接收机供电，二级稳压模块将5V降为3.3V，给主控芯片和传感器芯片等供电。
