无人机飞控仿真教学实验平台

无人机飞控仿真教学实验平台

一  概  述

   无人机飞控仿真教学实验平台主要由主控上位机、实时仿真机、三轴电动转台、电机舵机教学实验展板、飞行系统组成。平台主要应用于飞行器相关学科专业的日常教学实验中，同时兼顾对无人机飞控系统进行功能性验证。利用搭建的仿真模型对主控制器发出的控制指令进行解析和飞行姿态反馈，所具备的功能包括：

（1）无人机运动姿态仿真：能够完整地展现出无人机滚转、俯仰、偏转运动姿态，复现无人机飞行时的姿态角变化；

（2）姿态监视和调参：飞行仿真模型运行过程中，可以利用外部调试接口实时显示姿态信息和调整参数；

（3）故障注入：在进行完整半物理仿真飞行时，可以按测试需求模拟飞行过程的各种突发故障类型，对飞控系统控制算法的鲁棒性进行评估。

二、系统总体设计

为了最大限度模拟真实的动态飞行环境，无人机开源飞控教学实验平台采用数学模型来为半物理仿真系统提供机体运动状态和动态大气环境等动态飞行参数。采用Simulink进行无人机本体模型的搭建，并利用RTW自动代码工具完成目标仿真平台的嵌入式代码自动生成。该平台的总体结构2.1所示。

图2.1 平台系统结构图

图中主控上位机主要作用是在仿真前进行无人机动力学模型的构建、数字仿真、目标机代码生成，通过集成开发环境Tornado，完成VxWorks操作系统内核的生成、主机和目标机的搭接、下载目标机代码等；在仿真过程中通过RTW外部模式在线修改无人机模型中各种参数、获取各种仿真数据；还可以选择运行自行开发的仿真控制软件。

实时仿真机中运行无人机动力学模型代码，接收由舵机发送的舵偏角信号，根据无人机动力学模型完成无人机当前姿态信息的解算并将这些信息通过光纤网络发送到MPC08运动控制卡，根据收到的无人机当前姿态信息驱动三轴转台。转台上的惯性测量元件测量实际的姿态和速率信息，通过DA和串口发送到主控制器，根据获取的传感器信息解算控制律，通过DA向舵机发送控制指令，形成控制闭环。

三、平台硬件设计

3.1 系统组成

无人机开源飞控教学实验平台硬件部分主要包括三轴转台、运动控制箱、实时仿真机、MPC08控制卡、主控上位机和无人机机体结构组成。

无人机机体结构为平台姿态角实际展示部分，为一个真实的无人机航模，其中主控制器、惯性测量单元、舵机等均已安装在航模上。通过与实时仿真机通信，由舵机等执行机构能够实际展现出当前根据不同姿态角无人机航模的反应。

三轴转台是平台的姿态运动模拟器，能够模拟无人机飞行当中的姿态运动，展现无人机飞行时姿态角度的变化。MPC08控制卡安装在PC机的PCI插槽内，运动控制箱则是将电源、转台驱动器和各种接口控制电路集成与一体，与控制卡连接完成对三轴转台三个轴步进电机的控制。实时仿真机负责姿态传感器数据的获取、姿态解算算法和舵机控制算法实现等功能。上位机则是完成数据的显示和对三轴转台转动指令的发送。

3.2 主要器件的介绍

3.2.1 主控制器（STM32F103ZET6）

STM32F103ZET6控制器是一款大容量增强型芯片，内部包含丰富的外设配置，可以满足平台所需的计算速度和数据处理、存储容量，同时具备足够的扩展性，以便后续工作的补充。

3.2.1.1 技术指标

Ÿ                      内存：512k；

Ÿ                      SRAM：64k；

Ÿ                      静态存储控制器：有；

Ÿ                      定时器：通用定时器（4）、高级定时器（2）、基本定时器（2）；

Ÿ                      通信接口：SPI（3）、I2C（2）、UART（5）、USB（1）、CAN（1）、SDIO（1）；

Ÿ                      GPIO端口：112；

Ÿ                      12位ADC模块（通道数）：3（21）；

Ÿ                      CPU频率：72MHz；

Ÿ                      工作电压：2.0-3.6V；

Ÿ                      封装形式：LQFP114、BGA144

3.2.2 三轴电动转台（TT-3DM-3E-10）

如图3.1所示，三轴电动转台TT-3DM-3E-10采用UUO形铝合金框架结构，由内环横滚轴框架、外环俯仰轴框架、以及方位轴底座组成相互垂直的三维旋转坐标系，采集控制器串行接口连接上位计算机实现测量控制。

图3.1  三轴电动转台TT-3DM-3E-10

3.2.2.1 特点

Ÿ                      操作使用方便，便于搬动和携带；

Ÿ                      旋转角位置动态跟踪测量与控制；

Ÿ                      串行输出实时角位置数据 ，串行输入控制指令；

Ÿ                      位置、速率、摇摆功能。

3.2.2.2 技术指标

Ÿ                      负载尺寸重量： 5 kg；

Ÿ                      负载及夹具安装空间：120 mm×120mm×120mm；

Ÿ                      三轴转角范围：连续无限；

Ÿ                      角位置综合测量精度：±0.05º；

Ÿ                      控制到位精度：±0.01º；

Ÿ                      速率范围：0.1º/s～25 º/s；

Ÿ                      速率精度与平稳度：1%；

Ÿ                      测角数据采集频率：20Hz；

Ÿ                      用户导电滑环：8环/每环2A；

Ÿ                      台体重量：30 Kg（不含负载）

3.2.3 运动控制卡（MPC08）

MPC08控制卡是一个开放运动控制的平台，与PC机配合使用，直接插在PC机的PCI槽中，安装相应的驱动即可使用。

3.2.3.1 技术指标

Ÿ                      主接口：PCI 3.3V；

Ÿ                      控制轴数：4轴；

Ÿ                      编码器输入：4路；

Ÿ                      编码器输入计数器：四轴32bit，符号数2147483647，A/B/Z相（2M pps）；

Ÿ                      通用数字输入：DCV 24/DCV 5，光耦 16点；

Ÿ                      通用数字输出：DCV 24/DCV 5，光耦 16点；

Ÿ                      专用输入：每轴四点（正限位、负限位、原点、减速），报警；

Ÿ                      脉冲输出最大频率：4M；

Ÿ                      脉冲输出规格：每轴梯形加减速；

Ÿ                      脉冲输出计数器：每轴32bit，符号数2147483647；

Ÿ                      变速：运动中变速度；

Ÿ                      操作系统：Window 98，Window 2000，Window XP

3.2.4 运动控制箱

运动控制箱用于控制步进电机驱动的旋转平台实现多种运动。通过DB62线与MPC08控制卡相连，接收来自PC上位机的各种控制信号。同时通过RS232接口与三轴转台相连，由其内部包含的步进电机驱动进行驱动控制三轴转台的步进电机。该控制箱最多可独立或联合驱动控制三台步进电机，实现三维空间的任意方式运动。

3.2.5 仿真及测控计算机

如图3.2，测控计算机是系统的主控计算机，用于系统测试和控制，编制的专用测控软件都安装在该计算机系统中。

图3.2  仿真及测控计算机

3.2.6 惯性测量单元（3DM-E10A）

惯性测量单元3DM-E10A是一款微型的全姿态测量传感装置，它由三轴MEMS陀螺、三轴MEMS加速度计、三轴磁阻型磁强计等三种类型的传感器构成。三轴陀螺用于测量载体三个方向的绝对角速率，三轴加速度计用于测量载体三个方向的加速度，在系统工作中，主要作用是感知系统的水平方向的倾斜，并用于修正陀螺在俯仰和滚动方向的漂移，三轴磁阻型磁强计测量三维地磁强度，用于提供方向角的初始对准以及修正航向角漂移。惯性测量单元3DM-E10A可提供的输出数据有：原始数据、四元数、姿态数据等（产品外形如图3.3所示）。更详细的资料参见惯性测量单元3DM-E10A相关章节。

图3.3  惯性测量单元3DM-E10A

3.2.6.1 特 性 

Ÿ                      实时三轴惯性输出；

Ÿ                      大于100Hz的内部更新率；

Ÿ                      尺寸小、重量轻、低功耗；

Ÿ                      RS-232/RS-485A串行接口，方便连接。

3.2.6.2 技术参数

Ÿ                      输出数据格式：原始数据、姿态角、四元数；

Ÿ                      内部更新率        100        Hz；

Ÿ                      启动时间        < 1         sec；

Ÿ                      静态角度误差（俯仰、滚动） ± 0.1        degree；

Ÿ                      动态角度误差（俯仰、滚动） ± 2.0        degree；

Ÿ                      静态角度误差（航向） ± 0.5        degree；

Ÿ                      动态角度误差（航向） ± 2        degree；

Ÿ                      航向角分辨率 <0.1        degree；

Ÿ                      加速度计测量范围        ± 2g；

Ÿ                      加速度计非线性        0.2 % ；

Ÿ                      速率陀螺测量范围        ± 300°/sec ；

Ÿ                      速率陀螺非线性        0.2 %         ；

Ÿ                      磁力计测量范围        ±1.3 Gauss；

Ÿ                      磁力计非线性        0.4 %         ；

Ÿ                      短时冲击        500 (Not reinforced) g 。

3.2.6 电机、舵机控制实验板

如图3.4所示，实验板包含固定翼无人机1个、 电机舵机实验展板，含ST32ARM开发系统、实验软件一套。使用主控制器的定时器功能输出相应的PWM波即可控制舵机的转动角度。

图3.4  电机、舵机控制实验板

四、平台软件设计

基于总体方案的设计需求，从四个部分进行软件实现，包括无人机动力学模型实现、控制算法实现、下位机软件设计和上位机软件设计。最终将模型编译后加载到实时仿真机中进行仿真控制，完成整个无人机半实物仿真教学实验平台的闭环测试。

4.1 无人机动力学模型实现

参考牛顿-欧拉方程，得出表征飞机运动特性的12个微分方程，利用Simulink创建无人机六自由度仿真模型。加上由经验公式得出的气动力矩计算模块和大气模型实现对本体模型的搭建。无人机本体模型的六自由度飞行姿态解算模块如图4.1所示。

图4.1 无人机6自由度数学模型

4.2 控制算法实现

飞控算法采用双闭环PID控制结合速度前馈，其外环为角度（angle）控制，角度值是由滤波与姿态解算后得到的欧拉角，有延迟且存在误差，单闭环无法实现姿态控制过程。在此基础上引入内环，内环选择角速度（rate）控制，角速度由陀螺仪直接测量得到，误差小，响应快，延迟短。

4.3 接口模型实现与故障注入

根据仿真系统的设计需求，在MATLAB中利用S函数模板搭建仿真所需要的各设备通信接口和基本数据流，然后在Simulink中定义设备板卡的接口模型，并映射到驱动函数。然后将定义完整的设备模型添加到用户自定义的模型系统中，在系统模型需要的时候调用。最后，利用RTW模型代码自动生成工具对设备接口模型进行编译，完成各仿真系统子模块的通信交互。

接口模型的功能包括，静态模型参数调试（包括对特定的传感器模型赋值或改变某一控制位来测试飞控程序），动态模型参数调试（主要包括在动态飞行过程中改变飞行状态等）。利用此功能，可对设备接口模型注入静态故障和动态故障，将故障信息注入仿真平台。

（1）传感器故障

平台传感器组模块主要包括惯性测量单元、大气模型、无线电高度计模型和导航系统模型等。在飞行仿真过程中，对传感器组接口模型建模时预留故障注入接口，实现传感器故障状态实时注入（包括静态故障注入和动态故障注入）

（2）舵机故障

舵机接口模型的主要作用是通过串口模型将舵机输出的信号发送到实时仿真机进行相应的姿态解算。因此在对接口建模时预留故障注入接口，可实现实时模拟执行器系统故障注入。

4.3 下位机软件设计

无人机半实物仿真教学实验平台下位机部分主要完成三轴转台实时姿态解算，并将相关传感器测得的数据和姿态发送给主控制器。

姿态解算的主要传感器3DM-E10A负责获取转台的姿态数据信息，采用I2C总线驱动，与控制器STM32的管脚相连，通过STM32获取传感器的数据，继而进行后面的数据处理。其核心部分为内置加速度计、陀螺仪和磁强计数据的获取，相应的程序流程图如图4.2所示。

图4.2 数据采集流程图

对3DM-E10A的数据完成获取后，控制器STM32需要根据数据完成转台实时的姿态的估计。根据四元数法和四阶龙格库塔法解算姿态角，加上磁强计修正偏航角，便可准确实时更新姿态角。其数据融合姿态角解算流程如图4.3所示。

图4.3 数据融合姿态角解算流程

4.4 上位机软件设计

4.4.1 MPC08运动控制系统

实现对三轴转台的实时控制是上位机的一大主要功能，MPC08控制卡对三轴转台的运动控制功能取决于其内部的运动函数库。库中包含了对单轴到多轴转台的多种运动函数，包括单轴运动、多轴独立运动、运动指令执行等方式。利用VS编译软件新建工程时，加入相应的动态链接库，然后根据上位机界面的功能所需调用动态函数库中的函数体来实现转台的运动。

同时，考虑到实验室安装，调试的准确性，如图4.4所示我们还增加了“置水平位"和“归位"操作，分别能够使转台旋转至初始0度位置，和垂直于水平面的位置。这一功能能够使在学生完成实验之后，统一调整转台至同一姿态，保证实验室整齐整洁。

图4.4 高校飞行控制实验室设备图

4.4.2 上位机控制界面设计

上位机主要包括与下位机通信方式的选择、三轴运动形式的选择、姿态角指令的输入、三轴转台反馈位置的读取、姿态角指令文件和样本采样文件的数据对比、加速度角速度数据波形显示及姿态角数据仪表显示以及姿态3D显示等相关功能。其上位机界面如图4.5所示。

图4.5 上位机界面

3D姿态界面图如图4.6所示，其三维图形用长方体代替无人机。

图4.6 3D姿态显示界面

五、实验内容

可支持开设实验

5.1 控制率离散化实验

在控制系统的纯数字设计与仿真中，一般应用连续域控制器，但是在半物理仿真中，由于需要将控制算法与硬件相结合，所以必须将连续域控制器离散化。在数学仿真实验环节中，学生通过对比离散化前后的系统响应，并以主控制器的基本运行方式和涉及的相关原理为背景，有助于开启从书本中的控制系统到真实控制系统设计观念的转变

5.2 仿真模型代码生成实验

对模型代码生成规则进行配置优化。主要涉及参数包括仿真Solver参数，目标平台参Target Selection和Interface等的配置。其中Solver参数主要完成模块仿真周期，仿真时间和仿真算法的配置。Interface参数主要是配置代码生成后的函数调用接口。完成参数配置后，通过Simulink生成mdl模型代码，在利用编译器编译成仿真控制系统能识别的动态库dll文件，编译流程如图5.1所示。利用RTW工具对mdl模型文件进行处理，分析模型文件描述和信息结构以及各模块间的逻辑关系。利用解析器完成对文件的转换后生成rtw中间文件。通过目标语言编译器，RTW配置参数进行控制，生成用户定义的目标平台二进制代码。在生成C代码过程中，可通过修改联编文件对代码生成过程进行编译和链接控制，并最终综合生成可执行文件。

图5.1 模型仿真代码生成过程

5.3 半实物仿真实验

包括舵机系统硬件仿真、无人机组合导航实验、硬件闭环实验以及故障诊断与飞控算法鲁棒性评估实验。

舵机和控制板，可连接实验终端，做舵机原理及控制实验，方向舵角度开环控制实验，升降舵角度开环控制实验，副翼角度开环控制实验。

无人机组合导航实验包括GPS和IMU组合导航标定实验和无人机导航实验路径规划和轨迹数据采集。如图5.2和5.3所示，学生可以观察无人机的实际飞行轨迹。

图5.2 无人机GPS和IMU组合导航标定实验

图5.3 无人机导航实验路径规划和轨迹数据采集

硬件在环实验是在开环实验的基础上，加入控制器将所有硬件形成闭环，将实验结果与数字离散仿真结果进行对比，可以更直观地向学生解释半物理仿真中接入了串口通信、转台、传感器等实物，带来了附加的延迟、死区等非线性动力学特征的影响。

飞控算法鲁棒性评估实验是在飞行仿真中，对传感器或执行器注入一定大小的故障，观察在故障情况下，飞控算法跟踪控制指令的响应速度和误差效果，用以评估算法的鲁棒性能。