道控制器参数的值通道俯仰滚转航向时间幅值姿态角控制阶跃响应俯仰角滚转角航向角图姿态角控制效果小结本章首先介绍了四旋翼无人直升机的建模方法，并对四旋翼无人直升机模型的些参数进行辨识。接下来在平台上搭建四旋翼无人直升机的系统仿真模型。在本章的最后设计了最常用的控制器来控制系统的姿态角。从仿真图像上看出，本章设计的控制器可以实现对姿态角控制，改善了系统系能。软件系统设计引言控制系统软件共分为六个模块系统初始化模块传感器数据采集模块数据处理模块导航模块控制模块和无线通讯模块。本章主要内容是软件系统的设计与实现，在总流程之后分别介绍各子模块的功能和实现方法。软件系统总流程四旋翼无人直升机的软件系统的总流程如下图所示开始系统初始化自检测试传感器数据测试电机的状态循环发送各种数据半分钟时间到否正常是否自动导航解算高度姿态控制结束是否手动控制是结束否中断处理中断处理数字罗盘数据周期中断改变声纳触发口电平无线接收中断提取控制指令中断采样值存储中断提取声纳高电平时间图软件系统总流程图由上图可以看出，整个软件系统分为主流程和中断处理两部分。中断处理部分的作用是解决低速外设和高速之间的通信问题。主流程负责整个系统各部分的协调工作上电之后，系统进行初始化自检。如果系统各器件工作正常，则判断飞行模式，如果选择手动控制，则导航解算和自动控制程序将不执行。若是选择自动飞行，系统根据控制目标，自动完成飞行动作。图所示的流程图与小结中的软件总体各个软件模块是相对应的。其中系统初始化自检操作是由系统初始化模块传感器数据采集模块和无线通信模块共同完成导航解算操作由导航模块完成高度姿态控制是由控制模块完成中断处理部分由传感器数据采集模块和控制模块共同完成。系统初始化自检模块整个控制器只采用单片进行数据采集处理和飞行控制。因此，对性能要求比较高，同时要合理利用的系统资源。软件系统初始化软件初始化操作包含以下几方面头文件引用系统全局变量定义及初始化函数的声明等。硬件系统初始化硬件系统初始化包括外设的启动与寄存器配置中断设置等等。另外对于硬件系统初始化应该有以下四点注意事项硬件系统初始化必须按照定的顺序进行，如果顺序不当，硬件系统有可能工作不正常。如图所示，般硬件初始化步骤如下初始化系统控制包括锁相环看门狗和外设时钟等初始化关全局中断④初始化中断控制寄存器清除中断标志寄存器和中断使能寄存器初始化中断向量表开中断初始化外设使能中断和中断⑩使能全局中断详细信息可参考相关文献或者官方的例程。图硬件系统初始化流程图系统暂时使用不到的外设，比如本文未使用的外设有接口接口和口等，都可以在硬件系统初始化时关闭相应的外设时钟，以降低功耗。这点常常被般开发者忽略。如果采用官方提供的软件系统框架编写程序。在使用烧写时，应避免使用文件中的汇编函数如，因为这些函数是专门为编写的，和的内存分配方式不同，运行该汇编程序会使程序跑飞，跳到个未知的内存地址。采用三级中断机制，分别为外设级级和级。系统初始化的时候应该正确设置各级中断，否则有可能不能进入中断服务子程序。比如本文使用的中断，在外设级中应当将寄存器的置在级中，将使能寄存器的位置在级中，开启第组中断。不同功能模块都要使用的外设，应该只初始化次，否则有可能造成误操作。如的中断触发源是模块的时基，同时模块负责驱动两路直流电机。因此对于的设置和修改需要谨慎，否则有可能造成。数据采集模块和无线通信模块第三章介绍了控制系统选用的各种传感器，由于每种传感器的传输速度通信方式和工作原理都各不相同。因此在采集相关数据时的操作也并不样。般情况下，响应外设的方式有中断和查询两种。的系统频率可以达到上百兆赫兹，远远超过了般传感器的速度。所以，为了保证控制系统的实时性，采用查询方式是不切实际的。因此，对于信号的采集全都使用中断方式。当没有中断发生时，系统可以正常运行导航数据处理等程序。中断到来之后，保存断点，执行中断操作执行完毕，重新载入断点，继续运行刚才的程序。数字罗盘信息采集无线通讯模块是控制器和其他设备通讯的途径。由第三章相关内容可知，无线模块和数字罗盘都采用接口通信。由图可知，两个器件都是采用中断方式读取数据的原理是相同的。由参考文献可知，数字罗盘上电后会自动发送数据形式如下的码字符串,,,,其中发送的数据分别表示航向角，俯仰角，滚转角。数据之间以逗号隔开，另外还注意到信息精确到小数点后位。根据以上数据的特点，可以通过检验相应的标志如逗号进行姿态信息提取。统中编写如下图所示的高效的串口接收中断服务子程序。在到之间，接收标志置位数组最后位写更新姿数组设置相应寄存器退中断服务子程序进入串口接收中断接收到字符给写入字符数组否否否否否否是是是是是是图数字罗盘中断服务子程序流程图在中断服务子程序中，除了读到小数点之外，每进入次中断都会执行多种有效地操作，可以实现数据的边接收边运算的功能，提高了效率。为了提高响应速度，中断服务子程序的要求尽量短小，以免影响其他程序的执行。从图中可以看出，判断的值之后，最长的操作只有步，对系统的影响比较小。高度声纳的信息获取小节介绍了高度声纳的工作原理，因此只有获取声纳返回高电平的时间，才能求出声纳到地面或者障碍物之间的距离。的增强捕捉单元可以捕捉引脚上电平的跳变，然后将捕捉的计数器的值锁存起来。增强捕捉单元在系统频率情况下，位时基精度达到为。软件实现中，为了提高捕捉精度减少误差，设置的四个捕捉事件分别为捕捉上升沿下降沿上升沿和下降沿，即捕捉两个高电平时间。当四次捕捉事件发生时，产生中断事件，然后将捕捉数据取均值便求得高度声纳的高电平的时间。系统的捕捉操作是由硬件自动执行，并不需要软件干预，只有捕捉中断发生时才产生中断时间，所以执行效率非常高。惯性传感器参数的获取陀螺仪和加速度的输出信号是模拟信号，需要通过转换。的采样由模块定时触发，采样频率为，触发过程由硬件自动完成。当中断到来后，将结果寄存器的数据存储，然后将中断产生标志置位。主程序查询中断标志，当产生中断之后进行滤波降噪处理。无线通讯模块无线通讯模块和数字罗盘都采用异步串行通讯方式，所以数据接受的软件处理方式和数字罗盘相同。控制器通过无线发送相对简单，只需将需要发送的数据转换成字串保存到数组当中，将需要发送的字符依次写入中。为了保证稳定传输，每次写数据之前查询发送寄存器状态位，当发送寄存器为空时才写入数据。的软件校正观测信号通常是失真或不完整的，而且受到噪声的干扰，所以抑制噪声和除去信道失真是信号处理系统的重要任务。结束清中断标志开全局中断采样中断中断标记读取采样数据开始是运行其他程序否均值和滤波读取基准电压的转换值计算增益和失调误差计算校正增益和校正失调校正其他通道清除中断标志结束图通道偏差的软件校正算法如图所示，偏差的软件校正共分两部分采样中断负责读取采样数据，同时将中断发生标记置位主程序每个程序循环查询中断标记，当中发生时，进行校正工作。如小结所述，当主程序查询到中断标志置位后，首先进行滤波等预处理接下来读取基准电压转换值，然后按照式和式计算实际增益误差和失调误差接下来计算校正增益和校正失调使用计算的校正增益和校正失调来校正其他通道最后在退出校正子程序之前将中断标计清零，等待下次中断将其置位。数字滤波算法小结介绍了两种数字滤波算法，软件实现如图所示结束是否是是是是是是是是否否否否否否否否否图数字滤波器算法流程数字滤波算法由两个部分组成，分别是中值平均滤波和滤波。系统初始化中定义全局变量用来存放采样数据，定义为数组当前位置，另外定义数组中最大和最小两个值分别为。图所示，首先将转换结果的值赋给变量，更新数组和值和位置，接下来判断的值是否是最大和最小的值，如果是则更新相应的数值。最后，将数组之和减去最大和最小的两个值取均值便完成了中值平均滤波算法。滤波器实现可以利用中值平均算法的数据。首先计算权值向量，当采样频率不变的情况下权值向量是不变的。数据中去除最大和最小值之后，剩余的个值加权求和便可求出滤波之后的数据。捷联惯导算法由小节分析可知，在满足定的条件之后，惯性导航算法可以近似简化，流程如下图所示系统初始化开始读取陀螺仪数据四元数更新四元数归化坐标变换阵更新读入加速度计数据姿态更新数据输出结束结束输出数据图简化惯性导航算法流程图首先根据当前的数字罗盘的输出姿态，采用式求出四元数的初值。将式转化成差分方程，然后使用四阶龙格库塔方程解算四元数进行四元数更新。四元数归化采用式。将更新的四元数代入式，更新坐标变换阵。平台的姿态更新主要依据式和表和表。小结本章首先介绍了控制软件的总流程。然后根据软件子系统的功能介绍了系统初始化自检模块数据采集模块数字滤波和校正模块和捷联惯性导航算法。总之，好的算法是实现软件功能的前提，正确简洁的代码编写是达到控制目标的保证。检验软件系统那个和硬件系统的性能好坏，最好的方法就是实际系统的检验。系统调试引言前面各章分别介绍了控制系统的软硬件实现导航和姿态控制等内容。以上各部分需要经过实际系统的调试检验。系统调试平台如下图所示图调试平台上图中，左边为系统的控制器部分，右边是安装在机架上的电机驱动部分。系统调试按照先供电后信号，先数字后模拟的顺序进行。电源调试将焊接好的电路连接电源。使用万用表测量各电源芯片电平如表所示表电源芯片电平元器件名称输入电压输出电压输出电压由上表可以看出，电源供电满足表所列出的电平需求。最小系统调试最小系统包括芯片时钟模块口复位电路还有调试。用示波器量晶振的输出波形如下图所示，晶振频率为因此时钟电路工作正常。图晶振波形图然后将系统断电，连接仿真器，上电之后使用下载测试程序，并执行，经检验，最小系统工作正常。数字罗盘和无线通信调试数字罗盘采用通信方式。上电后通过示波器观查的发送引脚的波形，如图所示。图数字罗盘输出波形由图可知数字罗盘工作正常。然后连接仿真器运行数字罗盘接收程序，通过观察窗查看接收的数据。如图所示，表明数据接收正确。无线通信和数字罗盘的通信方式相同。测试程序使用无线模块循环发送的字符串，上位机动过串口调试助手接收受到的数据。如图所示，其中左边为发送的数据，右边为串口调试助手受到的数据。图获取的数字罗盘信息图无线通信数据声纳调试声纳的原理是给声纳至少的高电平，激发声纳产声个脉冲波。当超声脉冲遇到障碍物返回。声纳的输出引脚会有段高电平，高电平持续时间就是超声发出到返回所需的时间。使用仿真器下载超声波测试程序，通过观察口测量口捕捉的高电平的时间。如下图和图所示，可以看出，超声传感器工作正常。图距离障碍物时的捕捉时间图距障碍物时的捕捉时间加速度计和陀螺仪调试静态情况下使用万用表测量陀螺仪和加速度计的输入输出电平见表表惯