【四旋翼无人飞行器控制系统的软件设计案例分析11000字】

导论目录TOC\o"1-3"\h\u46751.1模块化设计 161431.1.1数据处理 2153341.1.2中断响应 457131.1.3电机参数 5175021.2主控流程 749221.3传感流程 8104481.4通讯流程 9312281.5复合控制算法 10182701.6姿态解算方法 161.1模块化设计整个系统软件控制流程是：首先对系统进行初始化，针对定时器来实现事件触发，同时面向整个系统实现参数配置；其次对系统运行状况进行精确判断，对系统能否运行正常、能否及时发出中断响应进行判断，对系统能否实现数据的定点采集并按照既定顺序进行操作进行判断，同时针对数据的算法分析和控制算法应用状况进行判断，对每一次操作过程的结果实施对比，实现数据的有效性检验；最后，将处理结果通过电机运行来进行反馈，将反馈数据进行接收和调节之后实现程序的循环控制，软件控制流程示意图如图4-1所示。图4-SEQ图\*ARABIC\s11嵌入式软件控制流程示意图Fig.4-1Embeddedsoftwarecontrolflowdiagram根据上图可以发现，在整个控制系统中进行主程序设计主要包括了顺序控制、执行措施以及系统接口响应等层面，主程序主要采取的是循环结构构架，通过这种模式实现数据与设备之间相对应的初始化、数据采集和处理、参数配置以及数据响应等相关操作，与此同时在系统出现数据中断的情况下，严格按照数据的中断量优先级来进行操作，进而实现整个复杂程序的循环流程。1.1.1数据处理在我们研究过程中针对数据处理模块的设计重点内容是实现数据采集和数据算法控制融合，在此基础上来进行数据解析，针对具体反馈数据进行详细对比和分析之后，采用数据处理模块就可以实现输出端响应数据的接收以及发送，这样对整个系统的数据传输过程可以进行有效控制。以此为基础，在得到反馈数据之后，针对电流信号进行A/D转换之前需要进行滤波操作，这样就能够让元器件的置信度得到进一步提升，采用卡尔曼滤波算法最终可以得到精准的反馈信号。本文在设计过程中针对数据处理算法的内容主要有以下两个方面：（1）陀螺应用本文在设计过程中结合系统具体需求选择了一种IMU，其本身内部配备相对比较简单的防噪声参数设置和均值滤波算法，在实现上电初始化的操作之后，采用数据通讯就能够实现主控芯片对于陀螺仪默认参数的改变，在此基础上就能够调试默认配置和滤波算法，针对陀螺仪采用软件可以进行简单的指令信息输入，同时针对其数据帧进行配置，进而能够针对陀螺仪进行简单数据处理。（2）反馈参数对于整个控制系统来说其反馈子系统在充分采用电流环和速度环之后形成了一个循环回路，针对系统中出现的各类信息能够实现及时的反馈解算，以此为基础系统能够充分保持一定的精度和稳定性。由于整个子循环系统中传感器控制的实时性以及控制精度存在较大差异，所以控制作用也会存在一定差别。严格按照系统的执行度采用估计值对整个反馈过程的状态量进行反馈，针对不同环节得到的反馈值可以将其当成一个量测值，随后采用卡尔曼滤波进行处理就能够得出最优化的估计值，在此基础上形成的反馈调节参数精度更高。本文在设计过程中主要是从循环校对和反馈修正两个层面来实现系统控制结构的设计，在此基础上能够保障数据处理关联性和精确性，在设计过程中选择了这种控制方法，主要是在原有传统控制基础上引入了数据冗余处理以及一种全新的控制算法来实现参数配置，使得电机的运转精度更高，同时也能够针对整个系统的反馈数据进行校正，以此实现系统控制的精确化。控制系统模型流程示意图如图4-2所示。图4-2控制系统模型流程示意图Fig.4-2Controlsystemmodelflowdiagram其中，是陀螺旋转系数，是驱动电机系数，是陀螺阻尼标定参数，是陀螺谐振角速率，是电枢驱动阻值，是电枢驱动电感，是陀螺转动惯量，是电机摩擦综合系数，是扰动等效反驱动力矩。本文采用经典摩擦力模型进行模拟，其主要表示的是前沿上升并实现速度最低情况时的静摩擦力。如果速度保持较大的情况下，其摩擦力矩的衰减过程呈指数型，最终形成的静态摩擦力可达到50%摩擦总和。在充分采用质量不平衡的作用后，能够让在弹性力模型化相对位置所作用的弹簧以及外部力矩干扰源最终作用到系统的输出。系统自身产生的震动主要包括频率段不同的振动以及低频振动两个部分，不同频率的实际所具备的振动量级存在较大差异，在此情况下就能够对反振动力进行有效抑制，该系统采用一种新型的复合控制算法可让系统整体的控制得到有效提升。在本文的设计过程中主要采取了PI2LL以及干扰观测器进行组合的方式形成了稳定的控制方法，使得系统的主动稳定效果全面提升。这种控制器本身是实现PI2（比例+积分+积分）控制器以及超前补偿器（LEAD）等有效融合之后形成的一个控制网络，以下为其具体的传递函数： （4-1）系统的高频区会受到比例增益项的影响，而低频区则主要会受到两级积分的影响，在此基础上获得的开环增益更高；针对二级积分所形成的相位范围不足的情况可以充分采用低通滤波器进行有效弥补；低通滤波器能够针对系统中的高频噪声进行有效滤出。在具体针对PI2LL控制器但各项参数进行合理配置之后能够让系统的开放增益效果更加理想。同时针对数据通过采用模型来实现解析，具体情况如下所示：系统在运行过程中主要采取的是直流力矩电机，针对该电机主要是采用其负载力矩、电压输入以及负载参数等构建起相应的数学模型，充分结合电机电枢电压平衡方程式可以最终计算出： （4-2）式中为反电动势系数。计算可得电机力矩平衡方程： （4-3）式中为电动势系数。联立可得直流力矩电机微分方程： （4-4） （4-5）控制系统中所配置的电流环针对电流的监测和数据反馈主要是依赖于电流监测内阻实现，针对系统供给的驱动电流充分采用电流反馈调节比例可以实现进一步调整，以下为其具体的反馈传递函数表达公式： （4-6）整个控制系统的速度环最终目的是要将电机运行过程中产生的干扰力矩对于系统控制性的产生的影响进行有效隔离，针对该部分可以将其与陀螺数据进行融合后统一解析，其反馈传递函数表达公式如下： （4-7）1.1.2中断响应本文按照如下表所示的响应顺序来进行中断响应设计：表4-SEQ表\*ARABIC\s11中断向量表Table4-1Interruptvectorscale编号中断项中断工作内容备注1定时器中断系统工作周期定时中断响应主系统中断2命令中断系统控制命令中断响应主系统中断3报警中断系统数值或状态报警中断响应主系统中断4阈值中断系统阈值信号超标中断响应子系统中断5电流反馈中断电流反馈数据处理中断响应子系统中断6陀螺反馈中断陀螺反馈数据处理中断响应子系统中断根据上表可以看出，本文中主程序中断和子程序中断采用中断向量表的方式来实现其优先级的判断，系统在具体运行过程中会进行主程序循环控制，当接入终端控制信号之后各类数据信息和反馈信息就能实现更新，在此基础上同时可以针对外部的相关控制指令来实施最高等级的优先处理，系统的稳定状态主要采用循环修正数据的方式来实现数据保持，当数据进入下一个运动状态的情况下会严格按照向量表顺序具体执行。在整个系统中最高的优先级是主系统中断，每一个模块中断在产生的同时都会相应产生数据处理请求，因此需要针对主程序进行判定，并最终按照中断顺序具体执行。1.1.3电机参数在无人飞行器模块化设计中，电机参数模型计算是最主要的设计环节之一，其直接影响四旋翼无人飞行器的飞行控制状态，是系统控制末端，也是无人飞行器控制的执行端，影响电机状态和型号选择的主要因素是力矩计算，下面将对控制电机的转矩进行参数计算和模型建立，实现电机驱动的模块化设计。（1）恒定转动惯量转动惯量是由电机重量、电机尺寸和电机材料等参数计算出的数值衡量，是电机运转的载荷指标，计算误差不应高于理论值的15%。（2）负载转动惯量本文采用的无人机刚体转动模型以圆柱形结构为标准，其负载转动惯量以四轴单向承载力为基础，计算公式如下所示。 （4-8） （4-9）式中，单轴向处于轴和轴的转动惯量由无人飞行器负载重量和转动半径计算得出，而单轴向处于轴的转动惯量还包含距离飞行器质心的转动轴长，本文采用的是对称的四旋翼旋转结构，所以转动平移量为0。（3）转矩标量计算转矩标量是电机运载力和驱动力数值体现标准，可通过计算确定部分指标数值，将静态误差、角速度和角加速度按照弧度转换的计算公式，如下所示： （4-10） （4-11） （4-12）式中，为转矩静态原理偏差，通常在1°范围之内，以最大偏差为计算标准；为稳定转动角速度，按照1080°转速为平均参考值；为稳定转动角加速度，按照360°转速为平均参考值，可分别计算其弧度转动计算值。（4）静摩擦转矩静摩擦转矩是一种抵抗反向作用力的推动力矩，主要保持四旋翼静止稳定时的力矩平衡，其中轴承保持定向的静止摩擦力矩是四旋翼保持稳定的前提，计算公式如下所示。 （4-13）式中，为摩擦因数，为轴承转动直径，为稳定状态时反向作用力的载荷压力，计算公式如下所示。 （4-14）式中，为轴承横向承载面积参数，为轴承纵向承载面积参数，为轴承径向可转动负载压强，为轴承轴向可转动负载压强，计算二者的平面幅值即为反作用载荷力。当四旋翼转动轴承未进行润滑，或长时飞行造成轴承磨损时，需要根据密封圈缩紧度对其摩擦产生的转矩纳入计算中，通常采用标准公式进行计算，如下所示。 （4-15）式中，为磨损造成的偏压力，为轴承外径，为轴承材料参数，计算出的数值级数为千分之一等级。（5）转动加速转矩转动加速转矩是四旋翼无人飞行器旋翼转动的本源转矩，是由四旋翼转动的加速度产生的，动力来源于电机驱动，其力矩由转动惯量参数和转动角加速度组成，计算公式如下所示。 （4-16）（6）滑动摩擦转矩滑动摩擦转矩是四旋翼无人飞行器旋翼在转动时，轴承所承受的内外径摩擦造成的误差转矩，由于四旋翼无人飞行器在高速运转时，滑动摩擦转矩造成的稳定误差较小，所以一般可采用电机参数中的滑动摩擦参数进行直接计算。同理由于其他四旋翼转动误差造成的转矩，都可归结为滑动摩擦转矩集合，其中包含绕组转矩、对准误差转矩、刚性绕动转矩和器件承接转矩等，可根据手册计算粗略转矩误差值。（7）外部环境额定转矩外部环境额定转矩即为风力影响的外作用力抵消力矩，由于四旋翼是四轴对称物体，其内部产生的自风阻互相抵消，本文主要计算是上风力造成的力矩数值，由于下风力是升力，所以和重力产生抵消，上风力主要由飞行器横截面积，风速和迎风面力矩距离组成，计算公式如下所示。 （4-17）综上所述，计算电机参数需要对其转矩综合值进行计算，保障四旋翼无人飞行器的载荷能力和稳定控制能力，匹配合适的电机。1.2主控流程在整个控制系统中，主控制器模块能够实现整个电路硬件的初始化、执行控制算法对各类数据进行处理计算。系统设计中的飞行器控制系统主控制器软件包括了时钟配置、模块驱动配置、以及模块任务管理等，整个系统重点涉及电机控制模块任务、姿态传感器任务以及电流管理任务等几个处理机制任务。不同模块互相之间采用特定的信息机制来实现交互，四旋翼无人飞行器的控制策略和反馈单元主要选择的是姿态解算模块，通过该模块能够对复合控制算法提供最新姿态信息，采用复合控制算法能够实现飞行器位置变化量的及时调整，最后通过电机驱动实现姿态调整。主控模块流程示意图如图4-3所示。图4-3主控模块流程示意图Fig.4-3Maincontrolmoduleflowdiagram主控器编程采取的是模块化设计，各个软件模块功能如下所述：（1）usbLinkRxTask模块：采用USB接口实现上位机数据的收发，在数据结束之后进行验证，数据正确后即可以将其进行打包并向下一个模块输送。（2）radioLinkRxTask模块：该模块主要是针对NRF模块输出数据处理，同时针对输出数据进行进一步调整后打包向下一个模块发送，与此同时经过数据处理后还会以ACK的形式向NRF发送响应数据，在此情况下能够及时向远程用户反馈无人飞行器的数据接收状态。（3）atKpRxAnlTask模块：其主要是针对NRF模块发出的指令数据进行处理，同时对接收数据进行解析之后发送到相对应的模块。（4）stabilizerTask模块：在整个系统中该模块属于核心部分，其主要是针对姿态传感器传输数据、电机的PID控制数据以及PWM信号调节，以及异常状态进行信号处理。（5）aptkTxTask模块：其主要作用是针对上一级模块发送的指令数据进行接收和解析，在此基础上，经过判断进一步对NRF模块进行数据打包之后传输。（6）usbLinkTxTask模块：该模块主要是针对上一级模块发出的指令进行接收和解析，最终采用USB传输模式将其发送到上位机中。主控模块功能导向示意图如图4-4所示。图4-4主控模块功能导向示意图Fig.4-4Functionguidediagramofmaincontrolmodule1.3传感流程传感流程的核心是MEMS惯性测量单元和主控芯片之间的数据通信，主控芯片和MEMS传感器在正常上电后，传感器采集无人飞行器数据信息，并将数据信息通过通讯总线传输给主控芯片，芯片接收到数据后对数据进行处理和解析，并将计算的控制变量发送给控制驱动模块，完成传感流程。从信息采集的角度来看，四旋翼无人飞行器的姿态传感器采集信息主要可以通过查询或者中断的方式获取。本文在研究过程中对系统的多任务处理和实时性进行数据综合之后，最终针对姿态传感器的数据采集选择了中断方式进行数据读取。导航系统完成上电并进行设备唤醒之后，首先需要实现姿态传感器的初始化，随后对其进行中断方式和中断处理函数的相应配置，此时采取外部引脚中断模式对主控器进行调用时，通常情况下主动接受的就是中断信号，接收中断信号之后主控会经过跳转后进入中断向量处理函数来执行后续操作，中断处理函数主要是面对本次中断状态进行清除，并在此基础上对所对应的姿态信息进行读取，姿态传感流程示意图如图4-5所示。图4-5姿态传感流程示意图Fig.4-5Schematicdiagramofattitudesensingprocess1.4通讯流程四旋翼无人飞行器的机体空中姿态主要是通过地面站和遥控器来实施控制的，在飞行过程中需要将自身的状态情况向地面站反馈，在此情况下操作人员可以对姿态进行及时调节，在本文设计过程中选择的是NRF51822作为通讯模块完成通讯流程，这种模块本身配备了1.4GHz通信基带，通过这种模式可实现远程遥控器指令传输，NRF远程控制的基本流程图如图4-6所示。图4-6NRF模块任务导向流程图Fig.4-6NRFmoduletask-orientedflowchart观察该图可以发现，NRF模块在系统完成上线之后就可以实现初始化，此时需要针对NRF模块所对应的寄存器进行配置，在完成从机初始化之后，自身设备信息就会自动广播，同时向周围主设备发出连续通知，为了避免被其他设备连接干扰，在和主设备之间需要设置相应的配对密钥来实现互相认证，只有完成认证之后才能进行连接，两者在构建连接之后可实现命令交互。从设备在整个系统中主要是进行检测和执行，同时将飞行器当前各个设备状态及时反馈到主设备中，而作为主设备最主要的功能是在接收飞行器反馈状态信号之后，将遥控信息发送给远端飞行器，实现飞行器和地面站之间的远程交互。1.5复合控制算法在无人飞行器制动过程中，控制信号无法直接对电机进行驱动，因为直接驱动电机会造成信号响应不到位，控制衰减等问题，造成的直接影响是飞行器控制产生的升力缺失，无法正常控制飞行。所以需要在控制信号端和飞行升力端建立控制关系，通过映射关系调节控制变量，使得螺旋桨升力和姿态变化一一对应，达到无人飞行器控制操作的目的，经典的PID控制算法可建立映射关系，无人飞行器姿态和操作控制关系如图4-7所示。图4-7无人飞行器姿态和操作控制关系图Fig.4-7AttitudeandoperationcontroldiagramforUAV随着无人飞行器控制精度要求的逐渐提高，传统的PID控制算法已不能满足无人飞行器的控制需求，由于其自身存在的直联特性，无法适应较大的突变问题，所以目前在工程中常采用自适应算法、模糊算法和神经算法等进行无人飞行器控制，本文集合各控制算法的优势，在不增加大量控制计算条件的基础上，提出了一种复合控制算法，可较好的适用于四旋翼无人飞行器的姿态解算流程中，提高无人飞行器的控制响应速度和控制精度。（1）传统PID算法传统PID控制算法其本质是直接对比例、积分、微分数值计算进行调控，其优势在于反应速度快，映射关系明确，结构简单，易于操作，是控制系统的核心计算方式。（2）自适应控制算法自适应控制算法其本质是对不断模式化和规律化的控制关系进行学习，以期望值的方式通过决策信息对下一时刻控制关系进行适应性推断，完成一致性规律控制的自适应状态。其核心控制内容是决策机构、辨识策略和期望值，当实际值在控制关系中进行修正时，即可根据先验信息指导控制器在作用对象中进行控制量输出，完成控制信息校验，其控制流程如图4-8所示。图4-SEQ图\*ARABIC\s18自适应控制算法流程示意图Fig.4-8Flowchartofadaptivecontrolalgorithm由上图可知，前一时刻的自适应控制反馈值会和下一时刻的控制输入量进行叠加，按照期望值和实际值的决策信息自适应修正控制冗余数值，并将映射关系带入控制器中，对存在的作用对象进行数据处理，得到自适应判决后的输出控制量，同时将本次控制结果通过设定好的自适应辨识策略反馈给实际值进行决策量循环，依次判定自适应控制状态。（3）模糊控制算法模糊控制算法其本质是对计算模式的一种推理过程，其依据的判定准则是模糊化信息知识库，通过模糊定义和隶属关系对控制量的数值进行转换，转换结果即为映射修正关系。模糊控制和自适应控制的流程类似，都是将控制输入量通过一定解析原则的控制器对作用对象进行数据处理，结果即为规则定义的控制输出量。模糊控制算法的核心是模糊推理关系的建立，其学习内容为模糊化知识库，其前后端操作即为模糊化和解模糊化，一般对模糊判决规律采用先验信息规律学习的方式进行知识库建立，其数据流程如图4-9所示。图4-SEQ图\*ARABIC\s19模糊控制算法流程示意图Fig.4-9Flowchartoffuzzycontrolalgorithm如上图所示，模糊控制算法流程可分为反馈数值累加、模糊化控制、先验知识库模糊推理、数据解模糊化控制、控制输出五部分，其中反馈数值累加和控制输出属于控制通用流程，与其他控制算法类似。模糊化是模糊控制的数值转换前端，需要将输入量按比例关系进行模糊集合处理，将其控制范围映射到集合范围中。在转换过程中，将输入量进行比例缩小后统一偏离位移距离，将位移距离进行数值积分可以得到偏移比例，将偏移比例、缩小比例和输入量进行模糊化融合，即为模糊推理。根据知识库的数据整合规则，将模糊集合进行等级划分，采用模糊辨识策略可得到模糊子集元素序列，即为负高，负中，负低，零值，正低，正中，正高7个标准阶层，标识分别为NB，NM，NS，Z，PS，PM，PB。在模糊化流程中，选择标志因数对控制范围进行规定，采用非均匀归类标准对模糊推理进行分解，可在解模糊化时进行量化转换，得到推理规则集合的数值变换比例，当采用的模糊数据集以[-15,15]范围为规范控制范围时，可直接通过解模糊化得到输入量的融合系数，所以处于模糊数据集内嵌的标准覆盖范围时，可得到各标准阶层的分割数据曲线，按照误差隶属函数、积分误差率隶属函数和输出隶属函数标准进行归类可得范围曲线如图4-10所示。图4-SEQ图\*ARABIC\s110标准阶层模糊数据集分隔变量曲线示意图Fig.4-10Variableseparationcurvediagramofstandardhierarchicalfuzzydataset设模糊化前的输入量集合为，非线性量化转换参数为，模糊化处理后的转换输入量集合为，则三者的关系可通过集合内的范围数值进行计算，得： （4-18）模糊控制的隶属函数按照标准阶层进行输入量位移偏移量、输入量积分偏移率和模糊输出量的对应关系集合数据集确定，按照模糊推理规则，可得到先验信息的模糊流程规范化曲面，则融合数据规范量化视图如图4-11所示。图4-SEQ图\*ARABIC\s111模糊推理数据集控制规则曲面视图Fig.4-11Fuzzyinferencedatasetcontrolsregularsurfaceview解模糊化其本质是一个逆向解析模糊推理的去量化范围集合数据还原，通过数据变换和量化计算，将推理后的模糊数据集进行解析，将每个标准阶层的控制变量按照非线性量化加权法进行计算，得到的数据集合即为输出量值的数据集合，计算公式如下所示： （4-19）由上式可知，为解算的输出控制变量集合。按照模糊控制流程可对模糊控制器进行simulink数值仿真，将模糊控制和PID控制进行结合，以达到改进控制模式的目的，其控制模型如图4-12所示。图4-12模糊控制仿真流程示意图Fig.4-12Schematicdiagramoffuzzycontrolsimulationprocess由上图可知，模糊控制仿真模型中最重要的控制环节即为模糊推理流程，将解模糊化的精确量作用于PID控制变量中，可得到模糊控制的终端输出量值，本文选择PID的参数值分别为2，5和0.5，并根据数据集[-15,15]配置了标准的模糊先验量化值，将模糊控制算法用于PID控制器的上层控制策略，以无人飞行器姿态信息中的俯仰角为输出终端值进行实验测试，将平稳的无人飞行器在时间轴零位进行1°的正向偏移，观察2内俯仰角的数据变化情况，则仿真结果如图4-13所示：图4-13模糊控制与真实情况对比数据仿真图Fig.4-13Fuzzycontrolandrealsituationcomparisondatasimulationdiagram由上图可知，模糊控制算法对PID控制起到正向校正作用，可以更快速的反映真实数值的变化状态，同时缩减了姿态稳定时间，但由于输入量在标准阶层的模糊推理过程中进行了降噪处理，所以会损耗控制变量，造成修正量值的减小，无法和真实姿态变化保持一致，且在校正初期变化浮动较大。（4）复合控制算法基于上述三种控制算法的原理优势，本文对其进行了数据融合和控制方法整合，采用自适应辅助修正模糊量化参数的方式进行信号控制，将自适应期望值和决策信息判定标准融入模糊推理的隶属函数标准阶层中，对量化信息采用辨识修正的方法，不仅对信号进行降噪处理，同时对信号进行自适应补偿处理，不仅缩小模糊控制数据集合范围，同时也增加自适应辨识标准信息，将解模糊化后的精确值代入传统PID控制器中，得到最终的控制输出值，可有效提高控制精度，并缩短稳定时间。复合控制算法的流程：首先对信号控制输入量进行模糊化数据转换，按照位移偏移量、位移偏移比例和隶属函数集合数据范围进行数据修正，标准阶层不变，但模糊数据集范围不固定，可随自适应期望值在决策机构中进行变化；其次在模糊推理中进行的隶属函数量化因子计算方式，可根据自适应辨识策略中的实际值输出方式进行调整，具体方法即为去噪补偿和误差降解；再次在解模糊化流程中，当不符合区域范围的模糊信息可进行归零话处理，超阈值信息反馈给模糊推理模型；最后将精控制量通过限制区域的方式作用于PID控制器中，修正原输入量的误差偏移，得到信号控制输出量，控制流程可根据PID的增减公式进行表述，如下所示。 （4-20）由上式可知，PID控制输出量由PID控制输入量，自适应模糊控制校正数值和去噪补偿、误差降解的模糊调整数值三部分组成，其中补偿值和校正值可通过原隶属函数设定范围阶层的期望值进行解算，量化值跟随校正值进行控制变量修正，自适应模糊控制隶属函数规范量化视图如图4-14所示。图4-14自适应模糊控制规则PID修正曲面示意图Fig.4-14PIDcorrectionsurfacediagramofadaptivefuzzycontrolrules根据上述复合控制算法的PID控制原理，对PID控制模型进行实际的应用测试。在系统运行1s后，对稳定的PID控制系统加入过量姿态修正变量并迅速消除，持续时间100ms，并在8s后加入固定的偏移变量后迅速消除，持续时间200ms，观察复合控制算法PID响应状态，曲线示意图如图4-15所示。图4-15复合控制PID变化曲线示意图Fig.4-15CompositecontrolPIDchangecurvediagram如上图所示，复合控制算法PID的控制量在1s时迅速反应，PID参数随状态的改变而引起幅值变化，并在300ms内趋于平稳，保持原系统的稳定状态，在8s后，积分参数由于受固定偏移量的影响产生响应波动，并在700ms内恢复系统稳定状态，说明复合控制算法的PID控制可迅速响应姿态变化量值，并在响应结束后可迅速保持稳定状态，可在环境变化较为复杂的场合保持较强的抗干扰能力，并具备较强的适应力和稳定性。将复合控制算法用于PID控制器的上层控制策略，以无人飞行器姿态信息中的俯仰角为输出终端值进行实验测试，将平稳的无人飞行器在时间轴零位进行1°的正向偏移，观察2内俯仰角的数据变化情况，将真实值和复合控制算法解算数值进行对比，则仿真结果如图4-16所示。图4-16复合控制与真实情况对比数据仿真图Fig.4-16Compositecontrolandrealsituationcomparisondatasimulationdiagram由上图可知，复合控制算法在姿态发生变化时，可以很好的对姿态变化量进行数据控制，反应速度较快，且由于自适应的修正原理，超调和失调可以很快达到控制信号平衡的状态，同时在姿态稳定后，相关于模糊控制造成的降噪处理进行了较好的数据补偿，持续保持和真实姿态变换数据一致，具有较高的工程应用价值。1.6姿态解算方法无人飞行器的姿态解算是通过MEMS传感器和姿态转换坐标系进行联立求解的，在进行姿态解算前需要对原始的传感器数据进行滤波处理，减小传感误差带来的测量偏差。将同轴向采集数据进行密集时间点分布和区域数据采集，设置一定比例采样值的数据集合，对数据集合中的传感器数值进行均值滤波处理，依次将传感器采集值和区域均值数值进行偏差判断，将绝对值最大的采集数据剔除区域数据集合，最终得到符合本区域的标准值，即为区域滤波处理后的精确采集值信息，通常初始采集频率可设置为10ms，而操作指令频率可设置为200ms，因此一个相对区域数据集合包含20个相关采集信息，姿态解算数值即为处理后的标准数值，计算公式如下所示。（4-21）（4-22）式中，即为同轴向中区域数据集的采集值，则为依次进行循环对比后的数值偏差最大值，与其对应的采集值将作为每次剔除数据，不再进行下一次循环对比，通过迭代次后即可的得到此区域的标准采集值，可作为后续姿态解算数据。同理，可对9轴传感器数据依次进行迭代滤波，减少由于采集突变和误差引起的控制姿态变化，可有效提高飞行器的控制稳定度和姿态平滑度，从数据源头减小了控制误差。本文采用四元数解算方法对姿态信息进行计算，四元数的本质是计算系统状态方程的过程参数，根据数据转换关系进行下一时刻状态估计，从而对检测得到的姿态信息进行变换计算，本文采用四元数状态方程作为系统坐标转换的计算方程，计算可得载体坐标系下的三轴姿态信息。四元数算法的本质是通过三轴方位和旋转运动对当前物理状态进行角度计算，其中三个参数表示三轴方位的旋转幅值，而另一个参数则表示三轴方位的旋转角度，通过四元数方位矩阵可解算姿态变化的滚转角roll、俯仰角pitch和偏航角yaw，其部分代码如下所示：/*****************/@brief四元数姿态解算函数/*****************/voidQuaternion::posture(floatroll,floatpitch,floatyaw){floatR =roll *radian*90;floatR =pitch*radian*90;floatY =yaw *radian*90;float