基于光流传感器的四旋翼飞行器设计*

颜 瑞，何 亮，邓小飞，向晓燕，陈善荣

(1.吉首大学信息科学与工程学院，湖南 吉首 416000；2.国网湖南省电力有限公司湘西经济技术研究所，湖南 吉首 416000)

目前，四旋翼飞行器自主导航系统是一个热门的研究方向.四旋翼飞行器自主导航系统根据其使用环境的不同，主要分为室外自主导航系统和室内自主导航系统.由于在室外能够接收卫星信号，因此大都使用卫星定位信号来设计室外自主导航系统，如王宏凯等[1]基于GPS/BDS开发出双模定位导航系统.而在室内接收不到卫星信号，无法利用GPS和BDS设计室内自主导航系统，所以常通过光流算法来实现室内的精准自主导航系统的设计，如陈磊等[2]提出的基于稀疏光流的无人机自主导航方案.但是，一般的光流传感器价格较高且对环境要求较严格，一旦环境发生变化就会导致输出的数据产生误差，从而降低控制性能.

针对这个问题，笔者拟提出一种直接对优象光流传感器输出的位置数据进行处理，以完成室内自主导航系统的开发策略.该优象光流传感器技术成熟且价格便宜，适用于大部分的地面，对环境的要求较低.为了验证策略的可行性，首先利用MPU9250姿态传感器的数据并结合串级比例-积分-微分(Proportion Integration Differentiation，PID)控制算法实现自稳飞行，然后在此基础上，利用优象光流传感器输出的位移、速度数据及超声波的高度数据，并结合串级PID控制算法，来实现室内的自主导航定点飞行.

1 四旋翼飞行器飞行控制原理

四旋翼小型飞行器有2种飞行结构，一种是十字型，另一种是X型.为了更好地对四旋翼飞行器进行编程控制，本研究采用的是X型，如图1所示，图中箭头所指的方向为机头方向.为了平衡桨叶转动后对机身产生的反扭力，1和3号电机顺时针旋转，2和4号电机逆时针旋转.

图1 飞行结构示意Fig. 1 Schematic Diagram of the Flight Structure

下面从垂直起飞降落、前后飞行、左右飞行和偏航飞行等8个飞行动作，对四旋翼飞行器的4个电机转速的改变进行分析，以深入了解四旋翼飞行器飞行控制的原理.要控制四旋翼飞行器垂直起飞，就要求1～4号电机同时起转，只有当桨叶转动产生的升力超过飞机自身重力时，飞机才开始起飞，这时4个电机转速要一致.如果4个电机转速同时减小且升力小于飞机自身重力，飞机就开始垂直降落.向前飞行时，要求1和4号电机转速降低，同时提高2和3号电机的转速；向后飞行时的要求则与之相反.向左飞行时，要求1和2号电机转速降低，同时提高3和4号电机的转速；向右飞行时的要求则与之相反.向左偏航飞行时，要求2和4号电机转速提高，1和3号电机转速降低；向右偏航飞行时的要求则与之相反.

2 硬件组成结构

2.1 硬件平台组成

如图2所示，系统的硬件平台主要由主控芯片、传感器、接收机、无刷电机、无线通信模块、电调、飞控电池和上位机组成.传感器包括测量高度的超声波传感器、测量姿态的MPU9250和测量位移的光流传感器；主控芯片采用Texas Instruments公司的TM4C123GH6PM，用来运行飞控程序；无线通信模块采用DL-20，用于上位机与飞控之间的信息交流；电调采用好盈乐天XRotor 20A的无刷电调；无刷电机采用920 kV的2212无刷电机；飞控电池采用达普35C的航模锂电池；接收机采用富斯的iA6B遥控接收机，用来接收遥控器的命令；上位机采用匿名上位机，用来实时监控飞控运行时的各个参数，同时发送命令帧对四旋翼飞行器进行控制，改变其运行模式.

图2 硬件结构Fig. 2 Structure Diagram of Hardware

2.2 主控芯片

采用TM4C123GH6PM芯片作为系统的主控核心，其内核为ARM Cortex-M4F处理器，系统时钟最高可达到80 MHz，内有256 kB的FLASH、32 kB的SRAM和2 kB的EEPROM，还有搭载 TivaWareTM(适用于 C 系列)软件的内置ROM.该芯片不仅有丰富的外设资源，而且有大量的通信接口，因此完全能满足飞行控制系统的要求.

2.3 传感器

2.3.1 姿态传感器 姿态传感器采用MPU9250九轴传感器，传感器芯片内置三轴的加速度计、陀螺仪和磁力计，可以通过I2C与其通信，获取原始的姿态数据.其中三轴陀螺仪可通过编程来改变其量程(±250，±500，±1 000，±2 000(°)/s)，然后输出16位ADC三轴角速度数据；加速度计也可通过编程来改变其量程(±2，±4，±8，±16 g)，然后输出16位ADC三轴加速度数据.

2.3.2 超声波传感器 采用US-100超声波模块测量四旋翼飞行器的飞行高度.该超声波模块可以实现 2 cm～4.5 m 的非接触测距功能，支持2.4～5.5 V 的宽电压输入范围，静态电流低于 2 mA；可以通过自带温度传感器对测量的四旋翼飞行器高度数据进行校正；同时具有I2C，UART等多种通信方式，内带看门狗，工作稳定可靠.

2.3.3 光流传感器 湖南优象科技有限公司自主研发的优象光流与同类型产品相比，具有价格便宜、体积小、易开发和不输于其他光流的高性能等优点，因此选择它作为系统的位置传感器.该光流模块在室内无法接收 GPS 信号的情况下，可以实时检测四旋翼飞行器的水平移动距离，再通过 UART 串行总线接口输出给飞行控制系统，同时通过算法控制飞机的水平移动距离，从而实现定点悬停功能.

2.4 无线通信模块

采用DL-20无线串口透传模块作为系统的无线通信模块.该模块是基于UART接口的全双工无线透明传输模块，可以在2 400～2 450 MHz公用频段工作；采用TI的cc2530芯片，符合IEEE 802.15.4协议；工作电压为3.0～5.5 V，传输距离最远可达250 m，传输速率最高可达3 300 Bps；可以在点对点和广播2种模式下工作.

3 软件算法设计

3.1 主控算法设计

整个四旋翼控制系统采用双环控制算法，包括位置控制环和姿态控制环.姿态控制环内部采用串级PID控制器，内环为角速度PD控制器，外环为角度PD控制器；位置控制环也采用串级PID控制器，内环为速度PID控制器，外环为位移PID控制器.主控制器的结构如图3所示.

图3 主控制器的结构Fig. 3 Structure of Main Controller

算法运行时，要在上位机上选择是定点控制还是遥控器控制.若是定点控制，则先要从上位机中获取期望位移，将期望位移送入位置环调控以得到期望角度，再将期望角度送入姿态控制环，最后将姿态控制环的输出量计算为电机的控制量，从而实现四旋翼飞行器的自主飞行.若是遥控器控制，则只要将位置控制环输出的期望角度换成遥控器的期望角度，即可使用遥控器控制四旋翼飞行器的飞行.

3.2 姿态解算算法设计

四旋翼飞行器的飞控系统，是通过对姿态解算算法得到的姿态角数据进行PID控制，从而实现平稳飞行的.如今常用的四旋翼飞行器的姿态表示方法有欧拉角、方向余弦矩阵和四元数[3].四元数表示法相比其他表示法，具有计算量小、算法简单和操作方便的优势，而且避免了欧拉角的奇异性问题，因此本研究选用它来表示和更新姿态.姿态解算算法的设计思路如图4所示.

图4 姿态解算算法的设计思路Fig. 4 Structure of Posture Solution Algorithm

角速度计在短时间内测得的原始数据是比较准确的，但是时间长了之后会受温度和电压的影响，此时再对角速度计的值进行积分求角度，得到的角度值就有较大的误差.而加速度计测得的值在短时间内因噪声而有较大的误差，但是长时间测得的值是比较准确的.所以，可以将加速度计和角速度计的值进行综合，从而获得较准确的姿态角信息.

姿态解算算法的流程为：首先将地理坐标系下的重力向量转换为机体坐标系下的等效重力向量；然后将等效重力向量与单位加速度向量进行叉乘运算以求得误差，用这个误差来校正加速度计输出的数据；接着用校正后的角速度计的数据进行四元数更新并归一化；最后将经归一化的四元数转换为姿态角.

设四元数为q=a+bi+cj+dk，其中a,b,c,d为实数，i，j，k为虚数单位.姿态角、方向余弦矩阵和四元数的转换关系为

采用一阶毕卡算法对四元数进行更新，得到

3.3 定点控制算法设计

定点控制算法的流程(图5)为：先采用串口通信的方式从光流传感器中读取原始数据，再通过原始数据解析出光流的位移和时间数据，最后利用位移和时间计算出速度数据.这里采用角速度计的数据对计算得到的速度数据进行旋度补偿，从而使得飞机在只有旋转没有平移时的光流传感器输出为0.

图5 定点控制算法的流程Fig. 5 Flow Chart of Fixed Point Algorithm

补偿公式为

fx_speed_gyro_fix= ((fx_speed+LIMIT(((Gyro_deg.x)/57.3f),-1,1))*h)，

fy_speed_gyro_fix= ((fy_speed-LIMIT(((Gyro_deg.y)/57.3f),-1,1))*h).

其中：fx_speed_gyro_fix和fy_speed_gyro_fix为旋转补偿后的输出；fx_speed和fy_speed为光流角速度；Gyro_deg.x和Gyro_deg.y为角速度计输出的角速度经过一阶低通滤波后的值；h为当前超声波测得的高度；LIMIT ()为将(Gyro_deg.x)/57.3f和(Gyro_deg.y)/57.3f的数值限制在-1～1之间的限幅函数.

单一的光流速度数据由于受到外界环境的影响而会产生误差，因此将其与经坐标转换后的加速度计的数据进行融合，从而得到较准确的光流速度数据.

加速度计数据进行坐标转换的公式为：

(1)

(2)

其中：xH，yH，zH为水平坐标系下的加速度计的三轴数据；xB，yB，zB为载体坐标系下的加速度计的三轴数据.这里用姿态转换矩阵来实现加速度计数据的坐标转换，当偏航角φ=0时得到(1)式，然后由(1)式得到(2)式，(2)式可以将载体坐标系下的加速度计的三轴数据转换到水平坐标系下.

接下来对光流的位移和速度数据进行串级PID调控，以得到期望角度(图6).即在得到期望位移后，将期望位移与光流反馈的位移相减得到的差值送入位移PID控制器，输出期望速度；将期望速度与光流传感器反馈的速度数据相减得到的差值送入速度PID控制器，输出期望角度.

图6 定点PID控制算法结构Fig. 6 Structure of Fixed-Point PID Control Algorithm

3.4 姿态控制算法设计

姿态控制算法的结构如图7所示.算法的流程为：在获得遥控器或位置环的期望角度数据后，将其与经过姿态解算而得到的角度数据相减，再将差值送入角度PD控制器；将角度PD控制器输出的期望角速度与角速度计测得的角速度数据相减，再将差值送入角速度PD控制器；角速度PD控制器的输出量经过油门计算作用到无刷电机，从而实现对四旋翼飞行姿态的调控.

图7 姿态控制算法的结构Fig. 7 Structure of the Attitude Control Algorithm

4 四旋翼飞行器的制作与测试

飞行器由1副450 mm轴距的机架、2对9443自锁桨、4个2212无刷电机、4个好盈电调、1个富斯接收机和自制的PCB电路板搭配外设传感器组成.测试时，先利用上位机发送命令帧，告知飞控此次为定点飞行模式，再发送开启自主导航定点模式的命令帧，同时发送期望位移.四旋翼自主起飞，飞行到设定高度后立即执行定点悬停任务.借助Matlab软件，将无线通信模块传送到匿名上位机上监测的参数(横滚角、俯仰角和飞行高度)数据绘制成曲线，如图8所示.飞行测试时长为4 min，设定的期望位移为0，目标高度为110 cm.从图8可以看出：横滚角和俯仰角的变动范围均稳定在-1°～1°之间，最大偏差小于2.0°；实际飞行时的高度与设定的目标高度的最大误差小于3 cm.

图8 飞行参数曲线Fig. 8 Curve of Flight Parameters

5 结语

新型四旋翼飞行器采用Texas Instruments公司的TM4C123GH6PM芯片作为系统的控制核心，通过I2C总线通信的方式获得MPU9250传感器的九轴数据，通过串口通信的方式获得超声波和光流的数据，再结合位置控制器和姿态控制器，实现了室内自主导航定点控制系统的设计.对新型四旋翼飞行器进行飞行测试时发现，在室内的普通地面，飞行器可以较准确地悬停在目标点，但是在灰白条纹地面，悬停效果却不太理想.因此，笔者接下来将考虑利用对天光流来解决这个问题.