基于Pixhawk 飞控的硬件在环仿真系统设计①

吴和龙 向政蓉 高宇航 杨剑锋 蔡茗茜 吴其琦

(∗工业和信息化部电子第五研究所 广州511370)

(∗∗广西科技师范学院机械与电气工程学院 来宾546199)

(∗∗∗广西科技大学自动化学院 柳州545006)

0 引言

目前无人机(unmanned aerial vehicle,UAV)的应用越来越广泛,其工作环境中影响飞行的不确定因素越来越多,这些工况对飞行控制系统(以下简称飞控)的设计与开发提出了更高的要求[1]。Pixhawk 是一款开源、可二次开发的飞控,支持多旋翼、固定翼等多种类型的无人机,在工业、农业、民用等领域得到广泛使用[2-4]。因此,针对Pixhawk 飞控设计飞行仿真系统,用于测试无人机的新功能与新航线,或者用于无人机出厂前的性能测试,都具有积极的现实意义。对于飞控的设计,验证逻辑正确性和稳定性显得尤为重要。如果使用全软件仿真对飞控系统进行验证[5-6],由于仿真环境理想化,无法反映无人机飞行时的真实情况;如果使用真实无人机测试,由于待测飞控系统的不确定性,可能会出现坠机危险,试验风险大。为此,本文设计了基于Pixhawk飞控的硬件在环仿真系统。

目前,国内外已经有多所科研单位以及高校对飞控硬件在环仿真系统进行研究。吕永玺等人[7]结合无人机数学模型和xPC 实时系统设计了无人机飞控半实物仿真系统。李瑞等人[8]采用上位机、仿真计算机、飞控3 部分相结合的方式设计了基于VxWorks 的四旋翼半实物仿真平台。Prabowo 等人[9]通过飞控硬件控制无人机模型,实现了硬件在环仿真系统,并使用FlightGear 显示仿真飞行画面。以上仿真系统在专用平台上获得了较好仿真效果,但由于使用了闭源的飞控,不能对广泛应用的开源Pixhawk 飞控仿真,存在一定的局限性。为了在Pixhawk 飞控上实现硬件在环仿真,设计了新的仿真系统,能够对多旋翼、固定翼或直升机等无人机进行仿真。本文以四旋翼为例,建立了四旋翼机体模型,在Pixhawk 飞控中嵌入传感器数据重构算法、传感器驱动程序、仿真通信协议,最终实现传感器行为级别的仿真,获得真实度较高的仿真效果。

1 硬件在环仿真设计方案

Pixhawk 飞控的工作原理如图1 所示。飞控利用传感器测量无人机机体的位置、姿态与角速度(为便于描述统称为位姿数据),在内核中使用位姿数据与航线计算旋翼电机当前的转速,控制无人机沿着航线飞行。根据上述原理,为了在飞控中实现仿真功能,需要完成如下2 点关键设计:(1)建立无人机机体的数学模型,即把图1中的无人机机体进行数字化;(2)在飞控中完成位姿数据至传感数据的变换,即把图1 中的物理传感器进行软件化。

具体的硬件在环仿真系统设计方案如图2 所示。仿真系统主要包括Pixhawk 飞控和仿真计算机2 部分。飞控作为硬件部分接入仿真系统中,仿真计算机显示四旋翼机体模型在虚拟场景中飞行的三维画面。仿真通信协议充当两部分之间串口通信的桥梁[10],机体模型输出位姿态数据送至飞控中运算得到转速数据,转速再送至计算机中控制机体模型,形成闭环控制。使用Mission Planner 地面站[11]并通过MavLink 通信协议连接飞控[12],用于控制与观测无人机。仿真计算机运行Matlab 软件,在Simulink环境中运行机体模型[13-14],同时嵌入运行FlightGear视景窗口软件,使用位姿数据显示仿真过程中无人机的飞行画面;Pixhawk 运行的飞控软件为ArduPilot,在原有软件基础上设计仿真通信协议、模拟传感器数据重构程序、软件传感驱动程序与转速转换程序。

工作过程如下:飞控通过仿真通信协议获得位姿数据,首先经过传感器数据重构程序处理后转换为模拟传感器数据;再通过传感驱动程序把传感器数据传至飞控内核,内核输出代表转速的脉冲宽度调制(pulse width modulation,PWM)信号,经过转速转换程序转换为转速数据;最后通过仿真通信协议把转速数据传回计算机,整个系统形成控制闭环回路。在完成硬件在环飞行仿真之后,使用参数切换真实传感系统的数据至内核,再对真实无人机中进行实飞测试。

2 四旋翼无人机机体模型

四旋翼的机体模型采用北航可靠飞行控制研究组发布的开源模型,如图3 所示,以电机的PWM 控制量为输入,以多旋翼的状态和传感器信息为输出,四旋翼无人机机体模型主要由动力单元模型、控制效率模型和刚体控制模型组成。动力单元模型是由无刷电机、电子调速器和螺旋桨组成的动力机构,输入量是PWM 信号,输出量是螺旋桨转速;控制效率模型的作用是将螺旋桨旋转所产生的拉力和反扭力矩计算出来。刚体控制模型包括动力学模型和运动学模型,动力学模型计算出的拉力方向始终与机体Z轴的复方向一致,运动学模型可计算出机体位置、速度、姿态和角速度等参量。

图3 四旋翼无人机机体模型

四旋翼无人机模型的输入量是4 个电机的转速信号,输出量是三轴位置PosE、姿态角AngEuler、三轴旋转速率(陀螺仪数据)AngRateB、4 个旋翼的转速和旋转矩阵(dimensional constraint manager,DCM),该模型在Matlab/Simulink 中经过软件在环仿真验证,模型输出值正确,可将该模型作为飞控硬件在环仿真系统中的无人机机体模型,模型设计过程可参考文献[15]。

3 飞控仿真程序设计

飞控仿真程序的设计基于开源飞控软件ArduPilot。这是一款应用广泛与功能强大的自动驾驶软件,支持多旋翼、固定翼与直升机等无人机,具备手动飞行、自动飞行、故障返航等各功能[16]。ArduPilot 软件兼容Pixhawk 飞控,是基于Pixhawk 原生代码开发的上层应用[17-18]。系统在ArduPilot 原生代码上嵌入了模拟传感器数据重构程序、软件传感器驱动程序、转速转换程序和仿真通信程序。

3.1 模拟传感器数据重构程序设计

飞控工作所需的传感器数据来源于陀螺仪、加速度计、磁力计、气压计和全球定位系统(global positioning system,GPS)。由第2 节可知,四旋翼机体模型的输出值只能提供陀螺仪数据AngRateB(xyz轴角速度)与GPS 数据PosE(经纬度与高度),而加速度计、磁力计与气压计数据需要根据模型的输出值重构,重构算法如下。

加速计测量加速度值。在北-东-地球坐标系中,当加速计水平放置且Z轴垂直向上时,可以测量Z轴上重力加速度值-g,X轴和Y轴的值为0,记录为(0,0,-g)。当无人机旋转一定的姿态时,重力加速度将在加速度计的3 个轴上产生相应的分量,这本质上是从大地坐标系中的(0,0,-g)到无人机坐标系的转换。无人机坐标系和大地坐标系之间相差一个旋转矩阵DCM,无人机坐标系下的三轴加速度计值为

其中,θ是俯仰角,ϕ是横滚角,φ是偏航角。

磁力计测量机体所处位置的磁场强度。当无人机静止且X轴朝北向时,无人机的X轴与北方重合。此时,只有X轴上能测量到磁场强度,Y轴和Z轴的磁场强度为0。假设磁场强度为B,则三轴磁场强度为(B,0,0)。当无人机旋转一定姿态时,磁场强度会在磁力计的3 个轴上产生相应的分量,这本质上是从大地坐标系中的(B,0,0)到无人机坐标系的转换。无人机坐标系和大地坐标系之间相差一个旋转矩阵DCM,无人机坐标系下的三轴磁力计值为

气压计用于测量无人机的高度,在Ardupilot 中主要参考气压计高度而不是GPS 高度。气压测高方法是在起飞前测量起飞点的气压P0和地面温度T,在飞行过程中再测量所处高度的气压P,把P0、P与T代入气压测高公式计算相对于起飞点的高度h。在Ardupilot 官方代码中使用式(3)计算高度:

当重构气压数据时,对式(3)反变换得到高度至气压的变换关系:

传感器数据重构程序的逻辑结构如图4 所示,从仿真通信协议中获得位姿数据。图中,ax、ay、az分别表示x轴、y轴和z轴的加速度,mx、my、mz分别表示x轴、y轴和z轴的磁场值。利用姿态角横滚、俯仰与偏航重构加速度计与磁力计数据;利用高度重构气压计数据。陀螺仪数据直接使用xyz轴角速度;GPS 数据直接使用位置数据经度、纬度与高度。以上传感器数据通过软件传感器驱动程序传输到飞控内核之中。

图4 传感器数据重构程序逻辑结构图

3.2 软件传感器驱动程序设计

ArduPilot中驱动程序的结构如图5所示。驱动程序包括驱动前台接口、驱动后台接口和传感器驱动程序。其中驱动前台接口负责将传感器数据传至飞控内核;驱动后台接口负责管理传感器,一类传感器对应一种驱动程序,另一类传感器中包括各型号传感器,每个驱动接口对应一种型号的传感器。

图5 Ardupilot 驱动程序结构

为了把数据重构程序输出的传感器数据传输至飞控内核,需要为各类传感器设计相应的驱动程序,在此称为软件传感器驱动,而传递真实传感器数据的驱动称为硬件传感器驱动。软件传感器驱动包括加速度计、陀螺仪、磁力计、气压计和GPS 等5 部分,如图6 所示。每一类传感器对应一个后台接口,每个接口包括硬件驱动和软件驱动。硬件驱动是系统中原有的驱动,用来接收真实传感器信号;软件驱动用于接收传感器数据重构程序输出的模拟传感器数据。当使用真实无人机飞行时,后台接口读取硬件驱动的数据;当使用机体模型飞行时,后台接口读取软件驱动的数据,实现了把机体模型位姿数据传输至飞控内核的功能。

图6 传感器驱动设计结构

3.3 仿真通信协议

为了保证仿真计算机与飞控之间数据传输的正确性与传输效率,需要根据传输数据的特性设计专用的通信协议。协议以数据包为基本单位,包含包头、类型、参数长度、参数、循环冗余校验码(cyclic redundancy check,CRC)5 个要素,各个要素如表1所示。

表1 数据包要素

数据包各个要素的描述如下。(1)包头:用于标识数据包的开始。(2)类型:用于表明通信载荷的属性。其中姿态类型是1;位置类型是2;角速度类型是3;电机转速数据类型是4。(3)参数长度:类型附带参数的长度,数值范围0～255。(4)参数:类型附带的参数,长度0～255。(5)CRC 校验码:用于校验数据帧是否存在错误。校验的数据是类型、参数长度与参数3 个要素,采用的CRC 多项式为

各个数据包的内容如表2 所示。

表2 传输数据参数

姿态数据包传输无人机横滚、俯仰与偏航3 个长度为4 字节的角度数据;位置数据包传输经度、纬度与高度长度为4 字节的位置数据;角速度数据包传输xyz轴的角速度数据,转速数据包传输4 个长度为2 字节的电机转速数据。仿真通信协议有2 个版本,分别运行在Ardupilot 与Simulink 中。

4 仿真测试与分析

无人机硬件在环仿真系统实物如图7 所示,包括四旋翼无人机、仿真计算机与地面站3 部分。无人机使用的飞控是重新设计的Pixhawk 飞控,飞控通过串口与仿真计算机相连,计算机中的机体模型依据图中四旋翼无人机建构。在进行硬件在环仿真时,首先设置飞控参数以指定使用软件传感系统的传感器数据,并输入起飞点的位置、气压气温、磁场强度等数据。然后运行Simulink 中的机体模型,与飞控进行位姿数据与转速数据的交互通信,并使用Mission Planner 地面站控制与观测无人机;使用FlightGear 视景窗口显示三维飞行画面,飞行界面如图8 所示。完成硬件在环仿真飞行之后,通过修改飞控参数使用真实传感器数据,即可进行真实无人机的飞行测试。

图7 硬件在环无人机仿真系统实物

图8 无人机仿真飞行界面

仿真效果测试的目的是验证硬件在环仿真的真实度,测试方法是使用相同的飞行航线分别进行硬件在环仿真飞行与真实无人机飞行,然后计算飞行数据的相似度。飞行航线如图9 所示,航线中总共设有7 个航点,无人机从初始位置航点H起飞,高度设定为20 m,飞到2 号航点(2 号航点与6 号航点重合),按照航点顺序绕航线飞行,飞到6 号航点后自动返回初始航点,至此完成一次飞行。

图9 无人机飞行航线

无人机按照上述航线飞行后,分别得到仿真飞行与真实飞行数据(下文简称仿真数据和实飞数据)曲线。飞控的飞行日志单元记录飞行数据,并以bin 文件存储在TF 卡(trans-flash card)中。利用Matlab 软件并遵照日志格式把bin 文件导入到计算机中进行绘图与分析。由于仿真飞行与真实飞行在起飞时刻上的不同步,为了提高分析准确度,对两组数据进行了时间上的对齐处理。选取有代表性的姿态角、加速度、陀螺仪、磁力计、高度气压与经纬度数据作为分析对象,如图10～15 所示,其中仿真数据冠以仿真-前缀以区别实飞数据。

图10 无人机姿态角曲线

通过比较仿真和实飞的数据,从直观上可知2种数据的吻合度较高,进一步使用式(5)计算2 种数据之间的线性相关系数r[19]。

式中x是仿真数据,y是实飞数据,每组数据中有n个数据量,¯、¯分别是x与y的平均值。每组数据对应的线性相关系数如表3 所示。

表3 传感器数据线性相关系数

线性相关系数越接近于1 表明相似程度越高。表3 中数据都大于0.8,大部分大于0.85,表明仿真数据接近实飞数据,仿真效果较好。在图10 的姿态角曲线中,融合加速度、陀螺仪与磁力计数据后解算得到姿态角,相关系数约为0.85。仿真曲线与实飞曲线交织变化,表明变化趋势一致,差别主要表现在瞬时数据变化上,原因是真实飞行中受到环境风力、机械振动与电磁干扰等因素的影响,而这些影响在仿真环境中无法模拟,故造成了数据差异。在图11的加速度曲线与图12 的陀螺仪曲线中,仿真曲线较平滑,而实飞曲线中包含了电机转动引起的噪音,故相关系数较低。在图13 的磁力计曲线中,电流引起的磁干扰是造成数据差异的主要因素。飞行过程中磁力计xy轴曲线大幅变化,磁干扰占比小,故相关系数较大;z轴曲线几乎不变,磁干扰占比大,故相关系数较小。在图14 的高度与气压曲线中,仿真曲线较平滑,而环境风力与气压变化造成实飞曲线的噪音较大。在图15 的经纬度曲线中,因为航线的飞行范围小,飞行过程中经纬度微量变化,故相关系数很高。

图11 无人机加速度计曲线

图12 无人机陀螺仪曲线

图13 无人机磁力计曲线

图14 无人机气压计曲线

图15 无人机经纬度曲线

5 结论

本文基于Pixhawk 飞控设计无人机仿真系统,实现了硬件在环仿真功能,只需要在无人机中安装仿真固件就能进行硬件在环仿真,在Pixhawk 飞控得到广泛应用的民用无人机领域,具有较好的应用价值。仿真飞行与真实飞行的数据比较表明,仿真系统的仿真相似程度较高;由于仿真飞行在计算机上的虚拟环境中进行,不存在坠机风险,安全性得到极大保证;仿真测试全程在室内进行,高效的测试效率、优越的便利性与低廉的测试成本都得到了充分体现。通过以上分析,该硬件在环仿真系统具有较好的综合性能。

从本文设计的仿真系统结构可知,在飞控软件中额外运行了软件传感器驱动、数据传感器重构与仿真通信协议等程序,尽管这些程序运行在独立进程中,已经最小限度改动原有软件结构,但仍然占用了一定的飞控运行时间和资源,造成仿真效果与真实飞行仍然有一定差距,如果通过其他方式解决该问题,仿真真实度还可以进一步提升。