基于机器人操作系统的机器人自主追踪系统设计

张 军， 刘先禄， 张宇山

(1.安徽理工大学机械工程学院, 淮南 232001； 2.安徽理工大学人工智能学院, 淮南 232001)

目标追踪是计算机视觉研究领域的热点之一，其中对人脸的检测追踪又是目标追踪领域的热门方向，广泛应用于视频监控，人机交互等各个领域[1-2]。当下运用较多的目标跟踪算法如核相关滤波算法(kernelized correlation filters, KCF)[3]，其是对传统相关滤波算法的改进，减少了参数量，提高了跟踪效率，然而在面对长时间的跟踪任务时其性能会有显著降低，影响正常的跟踪任务。Zhang等[4]将KCF与CamShift (continuously adaptive mean shift) 算法进行了融合，能实现对人脸目标的准确识别追踪，但其实时性较差，容易在跟踪过程中丢失目标。牛颖等[5]利用CamShift 算法融合 TLD (tracking learning detection) 框架实现了对人脸位姿的识别与追踪，但其场景是在人脸位置相对固定，背景变化不大的情况下进行的。利用移动机器人对运动目标进行实时追踪能够获得更好的人机交互体验。相较于传统的目标跟踪算法跟踪精度较低，需要手动提取跟踪目标的特征，基于深度学习的目标跟踪算法有着天然的优势，其不需要人工设计特征，只需要利用深度神经网络来进行自我学习从而达到提取特征的目的[6-7]。使得移动机器人能够更好地对环境建模，增强其对环境的理解，从而实现其在各种环境下均能对目标进行追踪[8-9]。

由于基于深度学习的目标跟踪算法的优越性能，对深度学习算法Siamese[10]进行了改进和优化，针对其对处理器的运算能力有着较高的要求，采用ROS系统的分布式计算框架[11-13]，实现对计算资源分配，加快算法的运行速度并提升训练效果，使移动机器人能够更加准确的检测并追踪目标[14-15]，实现移动机器人对目标的实时追踪。

1 追踪系统总体布局和硬件系统组成

追踪系统的总体布局如图1所示。图1中，以固定在底盘上拥有一定运算性能的树莓派为系统的主节点，提供运行机器人操作系统(robot operating system，ROS)的Master(节点管理器)的硬件环境；再辅以两个高性能的笔记本作为从机使用，并且为深度学习算法提供独立的运行本体，其间以路由器的无线Wifi实现整个系统间的通信，共同构成整个上位机系统。另一笔记本主要用作追踪过程中的可视化显示，用以实时观察系统的追踪状况。下位机以STM32为运行本体，充当底盘的控制中心，完成对上位机的速度指令解析。

图1 系统总体框架Fig.1 The total frame of the system

主要以STM32F429为移动底盘的控制中心。需要运用usart接口实现对上位机的通信；通过IIC实现对IMU(惯性测量单元)惯导模块传感器的数据读取，利用从传感器中实时获取的数据来精确地判别底盘自身的位姿信息；通过定时器实现对电机编码器的数据汇总。

使用树莓派作为实现复杂算法运行的基础。对于导航系统，需要借助传感器来感知移动机器人周围的环境信息，采用思岚科技的二维激光雷达，实现对环境的2D平面信息感知。

移动底盘的麦克纳姆轮的结构布局如图2所示，其四轮均采用全向轮，驱动方式采用四轮驱动的方式来实现底盘的运动，以车体的几何中心为坐标原点，构建平面直角坐标系xoy，以四轮中心构建相对坐标系x1o1y1、x2o2y2、x3o3y3、x4o4y4。其中ω1、ω2、ω3、ω4分别为轮1～轮4的角速度，vg1、vg2、vg3、vg4分别为麦克纳姆轮辊子的运动线速度，β为辊子轴线与轮毂轴线的偏置角，L为前后轮的中心距，W为左右轮的中心距，v为移动底盘的速度，vx、vy分别为速度v在水平和竖直方向上的分量，ω为移动底盘的角速度，L1、L2分别为轮子中心到底盘中心的纵向距离和横向距离，轮子呈现方形对称分布。此结构布局可以实现0转弯半径，只需附以一定的转速，就能够实现移动平台的全方位运动与平面转动。

图2 移动底盘示意图Fig.2 Diagram of moving classis

2 机器人导航系统开发

运动底盘导航系统包括上位机系统与下位机系统，导航系统的总体布局如图3所示。

图3 导航系统总体分布Fig.3 General distribution of navigation system

下位机采用ros_serial协议，可以实现对ROS节点编程的移植，实现在STM32内部节点编程，并且能够在ROS系统中发布与订阅消息。下位机订阅位姿话题，实现对上位机的速度解析；同时订阅比例-积分-微分(proportion integral differential,PID)数据，以方便系统可以在线调整PID的各项参数，实现对电机的精确控制；下位机系统还发布了IMU与编码器数据，以实现对底盘位姿的实时感知，同时为上位机运行的跟踪算法实时提供采集的里程计数据。

上位机系统主要运行复杂的定位算法Acml与导航算法Move_base，上位机系统架构如图4所示。

图4 上位机系统架构Fig.4 Upper computer system architecture

导航算法通过/map_server加载预先建立的地图，并且发布给move_base节点；然后，导航系统根据预先建立的地图以及lidar发布的环境信息，由/acm根据粒子滤波实时估计与定位移动机器人的

全局位姿，并且由/move_base实现避障下的轨迹规划；与此同时，算法还预留了规划轨迹目标的位姿接口/move_base_simple/goal，视觉系统将通过该接口实现与导航系统的融合，开发出具有实时追踪性能的移动机器人追踪系统框架。

3 机器人视觉系统开发

视觉系统的硬件以Kinect深度相机为基础，该相机能够感知环境的三维信息，同时实现RGB(red green blue)图像与深度点云信息的采集。这样，在获取图像的同时，也能同时感知图像每个像素的深度信息。追踪系统通过改进后的Siamese算法在RGB图像上实时跟踪目标的像素位置，然后，依据深度信息感知移动机器人的相对位姿。最终，根据相机的内外参变换，实现对目标的全局位姿感知，达到实时追踪的目的。

3.1 机器人空间状态变换

依据针孔相机模型进行分析与计算，原理图如图5所示。

f为相机的焦距；Z为目标点到相机光心的距离； h′、H′分别为成像物体在成像平面的高度和实际高度图5 针孔成像原理Fig.5 Principal of pinhole imaging

相机的位姿可由其外参数R和t来描述，则有

ZPuv=K(RPw+t)

(1)

(2)

(3)

则机器人的全局坐标为

(4)

式中：Z为目标点与相机光心之间的距离；Puv为像素坐标的齐次坐标；K为相机内参矩阵，由相机自身决定；Pw世界坐标；[u,v]T为目标点在像素坐标系下的坐标；fx、fy为焦距;[cx,cy]T为像素坐标系到成像平面之间的坐标偏移量；[x,y,z]T机器人在空间中的全局坐标；R与t分别为外参旋转矩阵与偏移向量，可根据ROS系统的TF(transform)树进行运算。

所开发的移动机器人跟踪系统的TF树如图6所示。依据跟踪系统的TF树，可以得到机器人/map到/kinect_frame_optical的空间变换矩阵R与偏移向量t，即可实现由RGB像素点到全局坐标的空间变换。

图6 跟踪系统TF树Fig.6 TF tree of tracking system

3.2 Siamese算法设计

通过孪生神经网络结构来设计实时跟踪目标的算法结构，其中算法的结构如图7所示。

图7 注意力机制的Siamese跟踪算法结构Fig.7 Visualization of tracking results

x为输入；u为主干网络卷积层输出；X为结合了权重之后的输出；c1、c2为特征通道数；h为输入图像的高；w为输入图像的宽； H为输出特征图的高；W为输出特征图的宽；Ftr(·, θ)表示将输入x转变成u；Fsq(·)表示对特征图进行压缩； Fex(·, w)表示对压缩后的特征进行excitation操作；Fscale(·)表示channel-wise multiplication(即通道乘法)图8 注意力机制网络布局Fig.8 Node distribution of system framework

所使用的目标跟踪算法是在SiamMask[16]的基础上，对网络的backbone进行改进，基于卷积神经网络Se-net通过对上一层结果进行再次特征提取，再对原先的网络结果进行权重加成，形成本局部模块网络的结果；其中基于Se-net的注意力机制网络布局原理如图8所示。利用此方法可以提升backbone对特征的提取能力，同时改善跟踪算法的跟踪性能。

改进后的跟踪算法的输出结果由3个分支组成，分别代表着对目标属于图像网格的分类分支、图像目标的回归分支、以及图像目标的分割分支。最终可以实现对图像中目标的框取，同时实现对目标的分割，使得对跟踪目标的定位更加准确。

3.3 追踪系统架构设计

图9 追踪系统框架节点分布Fig.9 Node distribution of tracking system framework

导航系统负责底盘的运动控制和路径规划，计算出最优的路径，然后实时控制机器人底盘的运动速度；同时，导航系统还能够实时的避障，以保证在追踪的过程中移动机器人能够安全高效地运动到预定的目标位置。

/AI_center由Siamese神经网络算法实现，是视觉系统的核心；其主要是在确定第一帧的模板图像后，通过实时接收深度相机以后每一帧的RGB图像，在图像中识别出跟踪目标的像素位置；能够准确形成图像人脸的分割区域，以计算出人脸区域的中心坐标；能够使用该节点以话题的形式在ROS系统中进行实时的发布。

/tf_transformer节点主要是实现像素坐标到世界全局坐标的空间变换，并且实时的为move_base提供跟踪目标的位姿信息；该节点订阅相机的深度信息与/AI_center的目标像素坐标，根据针孔相机模型准确的计算出目标像素点在相机坐标系下的坐标；然后，根据系统的TF树，实时地监听世界坐标到相机坐标的变换矩阵与偏移矩阵数据；最终，找到在map地图下的目标位置，实现对目标的实时跟踪。

4 仿真与实验

对于在固定人脸的情况下，追踪系统对人脸的追踪性能做了仿真实验，仿真环境与目标识别结果分别如图10所示。

在图10(a)的rviz可视化显示的导航系统中，成功地识别了人脸在地图中的位置，并且以箭头的形式呈现出来。同时在Siamese算法中，神经网络成功地识别了人脸的目标像素位置，以方框的形式呈现出来，如图10(b)所示。

Siamese算法实现了对RGB图像中人脸的准确定位，同时算法也实现了对人脸的分割，在图10(b)中以红色区域来显示，区域与人脸的重合度反映了算法的追踪性能；重合度越大表明算法的分割越准确，追踪精度越高。

在仿真实验中，追踪系统成功找到了目标位置，并且最终准确的运动到了目标位置，目标图像的追踪结果在rviz中的可视化显示如图10(b)所示。

（5）电机结构简单，抗冲击能力强：电动机转子无绕组和永磁体，机械强度高，能够长期耐受强冲击与强振动负载；

图10(b)中，移动机器人位置实现了与箭头目标位置的重叠，并在导航系统中实时发布目标已经达成的Goal reached信息，表明跟踪系统对固定目标的跟踪成功以及该跟踪系统的可行性。采用移动机器人进行目标追踪实验验证，跟踪图11(a)所示的真实人脸特征，实验结果如图11(b)所示。

图11 实验验证Fig.11 Experimental verification

为了评价跟踪系统的跟踪性能，采集了跟踪过程中Siamese算法对跟踪目标的定位数据，如图12所示。图12(a)表明跟踪过程中跟踪系统的定位误差，随着移动机器人与目标点距离的接近而逐渐减小；而在移动机器人和目标点相距较远时，追踪系统的定位误差往往较大，追踪系统的目标点与真实的目标点之间无法重合。究其原因总结如下。

d表示机器人与追踪目标之间的距离；x0为距离；x为迭代次数图12 实验验证结果Fig.12 Experimental verification results

(1)相机内参误差。由于相机的畸变无法准确测出，使得在像素坐标变换时无法准确地计算出像素坐标到相机坐标的变换；使得跟踪系统带有系统误差，其呈现的特点往往是目标越远离图像中心像素位置，定位误差越大，跟踪系统性能也越差。

(2)Acml算法定位误差。由于定位算法无法准确给出移动机器人的实时位置，使得外参数据不准确，从而带来了跟踪误差；此时，跟踪系统的主要问题是导航系统失灵，往往表现为失踪，无法跟踪目标，陷于困境与死循环。

(3)深度学习算法训练。深度学习算法是在训练好的参数模型下，实现对未知目标的识别与跟踪；因此，模型参数对算法性能起到决定性的作用，训练过程中，充分考虑可以给算法带来误差的各种影响因素，才能够提高算法的鲁棒性，提升算法的适用范围，带来对算法部署性能的提升。

(4)相机与目标间的距离。如图12(b)所示，相机能够准确追踪目标需要对二者之间的距离进行一定的限制。这主要与相机的视角范围有关，距离过近，将会导致相机采集的图像超出人脸的显示轮廓区域，如图13所示，此时不能将图像显示出来，但并不会造成目标的丢失，如图12(b)所示的d=1 m时的曲线，机器人仍然能够准确移动到目标区域。而距离过大则会导致图像中的目标过小，造成跟踪目标失踪现象，如图14所示的d=4 m时的曲线，目标点无法收敛于真实目标，呈现出杂乱无章现象，且跟踪过程中相机与目标的距离呈现出震荡不收敛的情况。

图13 过近距离跟踪结果Fig.13 Tracking results in too close range

图14 距离过大实验结果Fig.14 Results of the experiment with too much distance

5 结论

基于全向轮移动平台，设计了ROS系统下的人脸追踪系统框架，实现了Siamese算法在ROS系统中的部署，完成了移动机器人对人脸的实时追踪(保持二者的间距在一定范围内)。借助于ROS仿真平台，经过分析实时记录的跟踪数据，得出如下结论。

(1)基于ROS系统，开发了人脸跟踪系统的基本框架。在上位机中，采用导航与Siamese跟踪算法结合的思想，设计了人脸追踪系统。通过Siamese神经网络算法来实时检测与识别预定的模板人脸目标，为导航算法提供目标指引，使得追踪系统能够准确跟踪人脸目标。

(2)开发了硬件与仿真平台，实现了基于深度相机下的Siamese算法在ROS下的部署；使得系统基于感知的深度信息，实现机器人在深度相机下的状态变换与视觉定位，从而实现了对人脸目标的实时追踪。