基于姿态解算和图像识别算法的四旋翼自主飞行器系统

彭昕昀，熊正辉，黄伟强，何振环，欧森源，唐武翰，陈锦平

（韶关学院 智能工程学院，广东 韶关 512005）

四旋翼自主飞行器系统具有普通飞行器的优点，又具有无人机的造价低、可重复性强以及事故代价低等特点.在军事上，可以用来进行地面战场侦察和监视，获取情报.在民用上，可用于灾后搜救、城市交通巡逻和目标跟踪等方面.在工业上，可用于安全巡检，地震洪水、高压输电线等人工不容易到达空间进行安全任务检查与搜救工作，能够对执行区域进行航拍和成图等，四旋翼自主飞行器系统具有十分广阔的应用前景.

1 系统总体设计

四旋翼自主飞行器系统采用STM32F4 系列作为飞控板，内嵌陀螺仪、加速度计和气压计；采用四路分电板经四路无刷电调降压供电，搭载OpenMV、Radio 数传模块、GPS 模块、FS-iA10B 接收模块；用富斯i6遥控器进行遥控，上位机使用MP 地面站，进行相关数据收发.电池通过四路分电板分别给电调供电，经电调的BEC 降压接口给飞控板供电，电调的三相输出口接三相电机，各个模块统一由飞控板5 V 供电，当MCU 处理好模块之间传输回来的数据时，会给电调输出一个持续的PWM 信号，从而使电机实现持续可调节的转动.系统的总体设计框图如图1 所示.

图1 系统总体设计框图

2 算法分析

2.1 姿态解算算法

姿态算法是利用四元数在三维空间内的向量表示，来反映出物体在三维空间内的姿态变换.利用欧拉角等效，求出物体实际姿态坐标和地理坐标之间的偏差，进行校正［1］.

四元数可以描述物体在三维空间内的旋转信息，可以利用四元数来推导出四元数在三维空间里的姿态矩阵.如式（1）所示，其中分别为三维空间内单位向量的分量）［2］.

物体在三维空间内的姿态变换，可以等效为物体绕X-Y-Z 轴的复合旋转，对此可以用欧拉角在三维空间姿态，推导出变换矩阵公式.如式（2）所示，其中该公式的旋转顺序为Z-Y-X，绕Z 轴旋转时产生的角度称航向角（ψ），绕Y 轴旋转时产生的角度称俯仰角（γ），绕X 轴旋转时产生的叫称翻滚角（θ）［3］.

最后得出姿态角，如式（3）所示.

系统采用STM32 的HAL 库和Python 进行编程.让系统进入姿态模式，需要对姿态角信息进行采集，通过飞行系统上的陀螺仪获取相应的角速度，以及加速度计获取的加速度，将这些数据继续四元数微分求解，得到的数值进行归一化，有利于求解相应的欧拉角，进而可以得到无人机此时的姿态角，将其与地理坐标系角度比较求差，求出偏离地理坐标系角度，再通过PID 处理，将结果返回给飞行系统的MCU，进而做出姿态修正.姿态解算流程图如图2 所示［4］.

图2 姿态解算流程图

2.2 图像处理算法

2.2.1 中值滤波

为了提高图像的质量、抑制噪声，目标的识别前需要对采集的每一帧图像做滤波处理，本系统采用中值滤波技术.中值滤波是基于排序统计理论原理的一种能有效抑制噪声的非线性信号处理技术.它是一种邻域运算，把数字图像或数字序列中突出点的值用邻域中各点值得中值代替，使周围像素灰度值比较大的像素趋向周围像素值，从而可以消除孤立的噪点.二维中值滤波输出，如式（4）所示［5］.

式中，f（x，y），g（x，y）分别为原图像和处理后图像，W 为二维模板，通常为3*3.

对图3 中的各灰度值进行排序，取中值130 替换255，得到图4 的灰度值数据分布，可有效去除噪点.

图3 模板灰度值分布图

图4 中值滤波后灰度值分布图

2.2.2 MeanShift 图像识别算法

系统采用MeanShift 图像识别算法进行目标识别，把采集的图像转换到RGB 空间形成颜色直方图，再用MeanShift 算法匹配特征值并进行标记，最终实现对目标的识别跟踪.在实际识别目标跟踪时，运动目标在运动过程中颜色信息变化不大，光线等外在因素影响较小，使MeanShift 算法具有了良好的鲁棒性.MeanShift 算法思想是假设不同蔟类的数据集符合不同的概率分布，找到任一样点密度增大的最快方向，样本密度高的区域对应于该分布的最大值，这些样本点最终会在局部密度最大值收敛.在充分采样的情况下，可以逐渐收敛于任意的密度函数，满足任意分布的数据密度估计［6］.

MeanShift 算法的基本思想及物理意义：

给定d 维空间Rd中的n 个样本点xi，其中i=1，…，n，在x 点的MeanShift 向量的基本形式定义为：

其中，Sh是一个半径为h 的高维球区域，满足以下关系的y 点的集合为：

h 表示在这n 个样本点xi中有h 个点落入Sh区域中，式（5）中（xi-x）是样本点xi点相对于点x 的偏移向量.原理由图5 所示.

图5 MeanShift 算法原理图

由图可知，大圆圈所圈定的范围为Sh，也就是目标特征点分布区域，小圆圈代表落入Sh区域内的样本点xi∈Sh，黑点就是MeanShift 的基准点x，箭头表示样本点相对于基准点x 的偏移向量.在实际的目标识别跟踪中，平均的偏移向量Mh（x）会指向样本分布最多的地方，也就是概率密度函数的梯度方向，实现飞行器始终趋向目标进行移动跟踪［7-8］.

3 硬件设计

3.1 摄像头跟踪模块

OpenMV 是一款以 STM32 为处理核心的嵌入式摄像头，本系统采用OpenMV 进行一系列的图像采集和处理任务，对目标物体进行图像处理及实时跟踪起到重要的作用. OpenMV 摄像头模块如图6 所示.

3.2 通信系统

飞行器系统采用的通信有无线电通信、跳频通信、串口通信.通过Radio 数传模块和上位机进行无线电数据收发；富斯i6 遥控器通过跳频技术通信，对搭载在飞行系统上的接收器进行信号的收发. Radio 数传模块见图7，富斯i6 接收器见图8.

图6 Openmv 摄像头模块

图7 Radio 数传模块

图8 富斯i6 接收器

3.3 辅助电源

硬件系统搭载4 个功率为120 W 的新西达无刷电机，使用12 V 的电池经4 路分电板进行同步供电；同时为满足飞行系统上的各种模块供电，系统搭载4 路无刷电调进行降压，给飞行器控制器、数据传输、接收器等各个模块进行供电.

3.4 异常报警和保护

为了防止在飞行的过程出现数据包大面积丢失造成飞行事故，加入LED 灯和蜂鸣器.当出现上电自检不正常、飞行过程电池电压过低时，会通过LED 灯闪烁方式和蜂鸣器鸣叫发出警告.当出现数据包大面积丢失时，会发出蜂鸣警告，同时飞行系统会进行定速迫降.

3.5 硬件系统实现

系统采用S500 机架为载体，搭载STM32 主控板，OpenMV 摄像头模块，经过安装制作，系统的硬件实物见图9.

4 软件设计

四旋翼自主飞行器系统是一个多通道的飞行控制系统，软件系统的主程序先进行系统初始化，之后进入自检状态，然后进入锁定模式，再通过手动解锁启动飞行，此时可根据遥控器进行姿态切换，如果飞行器出现了低压报警，则进行手动迫降.系统的主程序流程如图10 所示［9］.

系统启动飞行后，系统进入姿态模式.该模式下，可根据遥控器的通道选择，令飞行器进入追踪模式，飞行器上搭载的OpenMV 模块会初始化采集图像，并进行中值滤波，微处理器根据MeanShift 算法进行偏差计算，得到的偏差经过互补滤波，最后由系统进行校正.在姿态飞行模式下，系统会读取陀螺仪和加速度计的数据，然后进行数据积分，最后计算偏差，经过滤波后由系统开始校正［10-11］. 飞行器系统的姿态切换流程图如图11 所示.

图9 系统硬件实物图

图10 四旋翼自主飞行器系统主程序流程图

在追踪模式下，飞行器系统搭载的OpenMV 模块完成图像采集，通过微处理器对采集到的图像做处理、目标检测、目标跟踪等［12］，利用PID算法由微处理器输出PWM 方波，以此控制电机，跟踪模式示意图如图12 所示.

图11 姿态切换子程序流程图

图12 跟踪模式示意图

5 系统调试和测试

通过上位机将飞行数据配置好之后，设定遥控器通道上对应的飞行模式，即可对飞行器进行解锁飞行操作.上位机界面图如图13 所示.

图13 上位机界面图

通过飞行测试，飞行器可以稳定的上升、下降、翻滚、偏航.数据传输最大距离为800 m，遥控器接收最大距离为700 m.电池采用11.1 V 容量为3 700 mah 的锂电池，经测试电池使用大约7 min 就会低压报警.可通过上位机接收飞行器的姿态数据，其中ACC_X 为X 轴旋转角度，ACC_Y 为Y 轴旋转角度，ACC_Z 为Z 轴旋转角度.上位机接收飞行器的姿态数据图如图14 所示.

图14 上位机接收数据波形图

在图像识别中，以小球作为目标，进行识别跟踪测试.微处理器对采集的图像做RGB 三种颜色直方图转换.采用十字可标定目标中心，方框框住目标，方框与十字标随着目标的移动而移动，实现对目标的跟踪.跟踪效果如图15 所示.

6 结语

系统通过摄像头获取图像，计算出相关偏差数据，进行PID 校正.结合上位机远程监测相关飞行数据.采用四元数姿态解算算法，实现飞行器自稳状态下飞行；采用滤波算法和MeanShift 图像识别算法，实现颜色识别.

通过测试，系统实现了四旋翼飞行器的姿态飞行，可进行翻滚动作、俯仰动作，偏航动作、颜色识别和目标追踪等功能，系统飞行状况稳定良好，目标跟踪识别准确，具有良好的应用前景.

图15 目标跟踪效果图