鲁 军，魏 峰，郝永平，徐九龙
（1.沈阳理工大学 自动化与电气工程学院，沈阳 110159；
2.沈阳理工大学 装备工程学院，沈阳 110159）
人工智能技术和智能控制技术发展迅速，牵引着无人系统自主控制理论和技术的发展，并使之逐渐成为自动控制领域的研究热点和难点[1]。随着Pixhawk 开源硬件热度的上升和PX4 固件的更新，无人机（unmanned aerial vehicle，UAV）产业迎来快速发展的春天[2]。无人机技术的进步，使无人机在军事和民事领域都有广阔的应用前景[3]。能够自主跟踪机动目标的无人机在应用中要求很高，许多研究致力于无人机的自动跟踪[4]。无人机目标追踪的研究，大多集中在图像处理的视觉追踪方案或控制规律上[5-6]。寻求识别更多目标类型，准确率更高的算法，以及遇到突发情况的解决方案，如环境因素（严重遮挡、光照变化、距离突变）等影响带来的目标丢失，以及目标与自身因素（目标的几何变形、旋转和无人机姿态变换等）造成的追踪失败[7]。对如何搭建无人机目标追踪硬件系统则研究较少。尽管仿真在一定程度上可以反映出算法的可行性，但将算法应用到工程当中所展现出的效果与仿真相比还存在一定的不同。
本文以Pixhawk 飞控和树莓派4B 平台为研究对象，通过将两种平台进行硬件通信调试和目标追踪软件开发，使无人机实现目标追踪功能，可给研究人员搭建软硬件系统提供参考。
无人机的自主飞行控制和航点计算均在机载电脑中完成[8]。将计算结果通过Mavlink 协议发送到飞控上，飞控运行PX4 开源代码，连接GPS 获取当前的位置信息，实时与树莓派通过串口通信，飞控内部以uORB 的通信方式传递消息，通过混控模块将电信号输出到电调、电机上，对无人机进行位置和姿态控制。
1.1 Pixhawk4 飞控
Pixhawk4 高性能自动驾驶仪是一款32 位ARM架构的开源飞控，其硬件和软件都开源，可根据需求自行设计软硬件，适用于固定翼、多旋翼、直升机、无人车、无人船以及其他可移动机器人平台。Pixhawk4 的性能对于高校研究、业余玩家使用和公司产品研发都可以达到要求。飞控的主处理器为STM32F765，还有一片型号为STM32F100 的IO 处理器，合理分配运算与IO 控制[9]。它还集成了BMI055（加速度计/陀螺仪）和ICM-20689（加速度计/陀螺仪），采用MS5611 气压计，IST8310 磁力计，通过程序将多传感器的数据进行融合，获得更准确的飞控状态信息，保障飞行安全。将PWM（主输出）接口与电调的信号接口连接，GPS 与SPI 接口连接，对于接收机接口根据发出的信号不同（SBUS/PPM）选择对应的接口，本系统采用SBUS 信号类型的接收机。4S1P（4 节电池串联）型的锂电池工作电压为14.8 V～16.8 V，通过分电板降压后，给飞控供电。系统硬件示意图如图1 所示。
图1 系统硬件示意图Fig.1 System hardware diagram
1.2 机载电脑
树莓派4B 是一款基于ARM 架构的微型电脑主板，将系统烧录到SD 卡中运行，开机后可以看到图形界面。主板共有4 个USB 接口，将USB2 口与摄像头连接。USB 转TTl 模块的一端连接在树莓派USB1 接口，另一端连接飞控的TELEM2 接口，再通过ROS 运行脚本，实现两者的通信。树莓派运行Ubuntu18.04 操作系统，需要通过命令下载相关的安装包[10]，搭建程序的运行环境，并连接显示器才能正常工作，但作为机载电脑需被携带到天上，显示器的重量与体积不方便携带，故将显卡欺骗器插在HDMI 口上来解决这个问题。当笔记本电脑和树莓派在同一网络下，可通过NoMachine 软件实现地面端控制天空端。笔记本电脑只控制launch 程序启动，无人机追踪二维码的过程全部由飞控和树莓派计算，笔记本电脑不参与运算。电池通过降压器将电压稳定在5.2 V 左右即可保证树莓派正常运行。树莓派与其它机载电脑相比价格低廉，处理器性能合适，但美中不足的是它没有独立图形处理器（GPU），对于当下大热的深度学习程序运行有一定阻碍，但识别二维码并进行追踪，性能完全足够。
2.1 程序总体设计
首先调用ARUCO 库识别二维码，若未检测到二维码，机载电脑读取飞控的位置信息，更新当前位置并保持悬停。反之，继续判断二维码的ID 号是否正确，ARUCO 库中多个二维码都能被识别，但无人机只需要跟踪特定的二维码，故对不同的二维码进行标号，当多个二维码同时出现在摄像头前时，根据指定的ID 号追踪二维码。如果ID 号不正确，则无人机保持悬停。若检测到正确的ID 号，机载电脑获取无人机当前位置信息，计算目标的位置，两个位置通过算法处理后，得出无人机的期望位置。无人机在追踪目标的过程中存在目标丢失的现象，设置丢失阈值为10，在追踪的过程中存在连续10次目标丢失的现象，认定目标丢失。反之，认定目标存在，发送期望位置，完成追踪。本文软件系统的实时追踪流程如图2 所示。
图2 实时追踪流程Fig.2 Real-time tracking flow chart
2.2 ROS 节点程序设计
飞控与树莓派相互传输信息需要通过相应的节点来实现，使用C++编程语言编写节点程序，既可保证程序响应的速度快又可以调用OpenCV 库进行图像处理识别二维码。ROS 以节点对节点的方式编写程序，这种方式高内聚低耦合，单个节点的功能稳定单一，各个节点之间联系紧密，程序模块的可重用性和可移植性高，程序更加稳定可靠。
web_cam 节点读取摄像头的视频信息，转换为image_raw 话题并发布。aruco 节点订阅图像话题，经过计算得出二维码的编号，布尔型目标检测判断值，目标姿态（四元数）和目标坐标，并封装为aruco_det话题，发布到目标追踪节点。
object_tracking 节点计算无人机的期望位置并做出决策，发送给底层控制模块，此节点订阅了无人机的位置信息和二维码的位置信息。由式（1）～式（3）分别求出在机体系下目标位置xyz 三轴坐标。
视网膜黄斑部是人眼视觉最敏感的部位，它位于视网膜血管颞上及颞下支之间的6mm范围的区域，是眼底的后极部（约为视野中央6°），是维护中央视力的最关键部位，一旦这里发生病变，往往对患者的中心视力造成严重影响。
此公式的目的是将相机系下目标的位置坐标转换为机体系下坐标。body_pos［］数组存储的是机体系下被追踪目标的位置，前三位分别代表机体坐标系下的xyz 三轴的点。机体坐标系以无人机的质心为原点，前方为x 轴的正方向，左方为y 轴正方向，上方为z 轴正方向。aruco［］数组存放相机坐标系中目标的坐标。相机坐标系下，右方为x 轴正方向，下方为y 轴正方向，前方为z 轴正方向，原点为相机聚焦中心。cam［］数组存放的是在机体坐标系下，摄像头与飞控的相对位置，因为以摄像头为原点去计算目标的相对位置，所以需要对无人机到目标的位置进行补偿，若无人机质心与摄像头之间的距离较小cam［］参数可以设置为0。坐标系属性（机体/相机）如表1 所示。
表1 机体坐标系/相机坐标系属性表Tab.1 Coordinate system attribute table of body/camera
得到机体系下目标的坐标后，再次将坐标转化到与无人机固连的ENU（东北天）坐标系下，即body_enu［］原点位于无人机质心，yaw=0°为x 轴正方向，yaw=90°为y 轴正方向，上方为z 轴的正方向。
通过式（4）～式（6）求出ENU 坐标系下，被追踪目标的坐标。enu_pos［］数组的前三位分别是ENU坐标系下XYZ 三轴坐标。ENU 坐标系原点为无人机起飞时的位置，正东方为x 轴正方向，正北方为y轴正方向，竖直向上为z 轴正方向。drone_pos［］代表ENU 坐标系下无人机当前的位置。坐标系属性（ENU/机体固练ENU）如表2 所示。
表2 ENU 坐标系/与机体固连ENU 坐标系属性表Tab.2 Coordinate system attribute table of ENU/ENU of body
由式（7）～式（9）得出无人机的期望XYZ 三轴坐标。
uav_pos［］为ENU 坐标系下无人机的期望位置。track_distance［］代表ENU 坐标系下无人机与目标保持的距离。将需要的坐标转换到同一坐标系后，求解无人机的期望位置时，只要知道目标位置和希望保持的距离，无需无人机当前位置，使求解无人机期望位置更准确、容易。
px4_sender 节点通过二次包装mavros 话题实现对无人机位置、速度加速度的控制使其到达目标点。pos_estimator 节点订阅无人机的位置信息，若无人机使用GPS 以外的定位方式如激光雷达、t265 视觉定位，则此节点将数据进行融合获得更准确的位置信息。此处mavros 节点代表飞控自身的数据，通过各节点信息传递使其实现追踪二维码的功能。ROS 节点表如表3 所示。
表3 ROS 节点表Tab.3 ROS node table
ROS 节点图如图3 所示。
图3 ROS 节点图Fig.3 ROS node diagram
3.1 识别测试
图4 二维码识别图Fig.4 QR code identification diagram
确认识别目标后，使用rostopic list 指令查看已发布话题，使用rostopic echo/promethues/object_decetion/aruco_det 指令打印出识别节点发布的话题消息。如图5 所示成功显示二维码编号，识别状态，目标位置等消息。
图5 话题消息图Fig.5 Topic message diagram
3.2 无人机追踪测试
测试流程：
（1）通过遥控器将无人机升至一定高度并悬停；
（2）笔记本电脑通过NoMachine 软件远程控制机载电脑启动launch 文件，确保机载电脑不断向无人机发送位置信息；
（3）通过遥控器将无人机切换到offboard 模式；
（4）无人机首次识别二维码发生偏航，对坐标系进行校正；
（5）无人机再次识别二维码开始追踪。
3.3 数据分析
测试数据如图6 所示，可从曲线中可得出如下结论：
（1）由图6（a～c）可知，机载计算机发出的期望位置与无人机实际位置的曲线趋势基本一致，说明无人机接收到期望位置后，做出反应向目标点飞去。
（2）由图6（d）可知，根据3 轴的运动范围和误差可知，运动范围越大误差越大。x 轴方向的运动范围：-（1～5）m，误差大约为+0.5 m；
y 轴方向的运动范围：（2～-4）m，误差大约为（0.1～-0.5）m；
z 轴方向的运动范围：（0.5～-1）m，误差大约为±0.15 m。
图6 测试数据图Fig.6 Test data diagram
无人机实际飞行轨迹如图7 所示。
图7 实际轨迹图Fig.7 Actual trajectory
经过飞行测试及实验结果分析，本文搭建的目标追踪系统可行，能使无人机自主追踪二维码，且软硬件开源容易获取，可根据具体需求二次开发更改算法与决策，对验证无人机目标追踪程序有一定的应用价值。
猜你喜欢 飞控树莓坐标系 独立坐标系椭球变换与坐标换算导航定位学报(2022年2期)2022-04-11民用飞机飞控通电自检测安全性研究民用飞机设计与研究(2019年2期)2019-08-05基于树莓派的骑行智能头盔设计电子制作(2019年11期)2019-07-04解密坐标系中的平移变换中学生数理化·七年级数学人教版(2018年4期)2018-06-28坐标系背后的故事数学大世界(2018年1期)2018-04-12基于树莓派的远程家居控制系统的设计电子制作(2017年17期)2017-12-1802 飞控技术与机场安保引关注中国公共安全(2017年7期)2017-10-13飞控系统故障告警实时监控软件的设计与实现计算机测量与控制(2017年6期)2017-07-01响应面法优化红树莓酒发酵工艺中国酿造(2016年12期)2016-03-01极坐标系下移动机器人的点镇定中国海洋大学学报（自然科学版）(2014年8期)2014-02-28