1 引言
动态目标的运动轨迹蕴含着速度与空间位置等重要信息，在多种场景中有着重要的应用前景。如在医学领域需要测量病人的肢体运动轨迹以用于病理分析，在无人机导航问题中需要实时获取动态障碍物的运动轨迹[1]。现有的轨迹方法追踪往往局限于指定单类目标的追踪，限制了其应用的场景。文献[2]中基于卡尔曼滤波算法对行人进行跟踪，得到了二维图像坐标系下的行人运动轨迹。但该方法得到的是行人像素点的运动轨迹，不具有行人实际的运动距离尺度信息。文献[3]通过背景差分、阈值分割的方法追踪鱼类，并通过单目摄像头还原鱼儿的运动轨迹，由于单目相机本身存在缺乏和实际轨迹对应的尺度信息，因此该方案也无法直接得到追踪目标物的具有客观运动尺度大小的轨迹。此外，通过背景差分和阈值分割追踪的目标稳定性较差，在光照变化或者有其他动态目标干扰时容易失效。文献[4-5]利用激光雷达产生的点云进行动态目标的轨迹检测追踪，但激光雷达价格昂贵，而且计算量较大，无法在计算性能有限的移动设备上实时运行。文献[6]通过MEMS传感器对室内的消防员定位，借助人的运动特征改善了传感器本身的漂移缺陷。文献[7]使用单一GPS传感器对进行室内外的机器人定位与轨迹追踪，但在室内环境，GPS信号往往很差，存在很大定位误差。文献[8]通过YOLO、SSD[9]、Faster RCNN[10]等3种深度学习目标检测算法对网球进行轨迹追踪，但前提是网球起点位置已知并且击打到指定高度，从而获得网球的实际运动轨迹尺度大小信息。为了避免传统基于人工特征的目标检测算法的普适性和准确性缺陷[11]，本文使用基于卷积神经网络[12]的目标检测识别算法，对目标物进行分离并在二维图像中定位。可以注意到，在当前众多的轨迹还原方案中，主要存在目标检测种类单一、准确性低以及不能得到目标物实际运动尺度大小等缺点。
本文主要针对动态单个目标的三维轨迹还原问题，提出基于视觉的解决方案。采用视觉神经网络算法对目标进行检测识别，设计坐标系转换接口将目标物在二维像素中的位置转换到世界参考坐标系中。在计算能力有限的移动设备上部署，程序运行采用有序多线程，充分利用设备资源以保证实时性。并以篮球为例进行实验验证，具有良好的实时性与准确性。对无人机导航以及肢体轨迹追踪等场景具有工程实际应用借鉴意义。
2 硬件平台
本方案基于Ubuntu系统，有关硬件如图1所示。核心计算板卡采用盈驰的微型主机，主控芯片为主频2.3 GHz的酷睿I5系列CPU。具有四核心八线程，无独立显卡。传感器包含Intel的D435I实感相机和小觅智能的MYNTEYES1030相机。D435I用于检测捕捉目标物在相机视野下的轨迹。D435I相机，最远捕捉距离为10 m，在户外阳光下也可以使用。D435I可以输出像素对齐的彩色图和深度图，且具有图像畸变校正功能。分辨率为640×480，帧率最高可达90 FPS。另一个相机为小觅智能的MYNTEYE相机，用于运行视觉惯性里程计定位算法，可以将相机视野下的轨迹关联到世界坐标系。该相机内置六轴IMU传感器，与双目相机图像保持硬件时间同步，2个摄像头的分辨率均为750×480，帧率为30 FPS，IMU频率高达1 000 Hz，满足SLAM算法对稳定定位时的图像和IMU条件要求。
图1 系统硬件图
Fig.1 System hardware composition
3 系统软件处理设计
本方案采用ROS框架，总体流程如图2所示。主要由目标检测，还原目标物在相机坐标系下的轨迹，视觉惯性里程计将相机坐标系和世界坐标系关联，轨迹滤波等4个部分组成。
图2 软件总体流程框图
Fig.2 Overall flow chart of software system
为了实现在计算资源有限的嵌入式平台实现动态目标轨迹还原，综合考虑性能和精度，本方案主要利用了2种方法：基于YOLO的目标检测方法和基于视觉惯性里程计的方法。
3.1 目标检测
还原目标物在相机参考坐标系下的轨迹需先获取目标物在彩色图像中的位置信息，通过对象检测的方法实现。对象检测需要确定目标在图像中的位置和类别，目前主要有2种方法：第1种以HOG+SVM[13]为代表的传统检测方法；第2种为基于深度学习的方法。第2种方法又可以细分为以RCNN为代表的2阶段检测器和以YOLO为代表的基于回归问题的单级检测器。尽管RCNN算法准确，但比较复杂、速度很慢，不能满足在计算能力有限的平台上实时工作。相比2阶段检测法，YOLO系列算法不需要候选区域，通过端到端的方法对目标分类识别，速度更快。因此本方案采用速度更快的YOLO方法。
由于要部署在微型主机上，没有独立GPU计算单元，因而考虑采用NCNN推理框架。NCNN是腾讯优图于2017年推出的高性能神经网络前向计算框架，可不依赖GPU运行。该推理框架为实现在移动端部署做了专门的优化，支持多核并行计算加速，优化内存，直接内存零拷贝引用加载网络模型。NCNN支持YOLO网络模型。YOLO智能卷积神经网络诞生至今已有多个版本的迭代，本方案采用YOLO-Fastest版本的网络模型算法。该算法对单目标检测做了针对性优化，适合部署在计算能力有限的平台。NCNN推理YOLO-Fastest网络模型得到的目标边界框由5个预测要素组成，分别是边框左边界x1、边框右边界x2、边框上边界y1、边框下边界y2和置信度。彩色图的目标边框信息和深度图如图3所示。我们取矩形边框中心点作为轨迹还原的中心点。该中心点像素坐标为(u,v)，其计算公式为：
(1)
由图3可以看到，计算出的(u,v)目标检测框的中心坐标可以直接对齐到深度图上。
图3 彩色图的目标边框信息和深度图像素直接对齐图
Fig.3 The target border information of color map is directly aligned with the depth map pixels
3.2 还原目标物在相机视野下的轨迹
目标物在相机坐标下的轨迹点通过D435I相机的射影逆变换计算得到。相机模型的成像过程如图4所示。
图4 针孔成像模型过程示意图
Fig.4 Pinhole imaging model
设为o-x-y-z相机坐标系，坐标系原点o为相机光心。相机坐标系中z轴正向指向相机正前方，y轴正向垂直于z轴竖直向下，x轴正向指向相机右端。设空间点P为[X,Y,Z]T，其中Z为空间点到相机光心O的距离也即深度。P点穿过相机光心O并在物理成像平面上形成点P′为[X′,Y′,Z′]T。P和P′之间的坐标变换根据图5中的相似三角形关系可得。
(2)
式(2)中： f为焦距，是光心到物理成像平面的距离；为了表示倒立成像，公式添加了负号。
图5 投影相似三角形示意图
Fig.5 Schematic diagram of projection similar triangle
式(2)反映的是空间点P和其成像点P′之间的坐标关系，但本方案的实际操作都是先以像素为单位进行的。需要将物理成像平面上的点转换到像素平面上。像素坐标系定义为o-u-v，o为图片左上角原点，u和v轴分别与x轴和y轴方向相同。因此需要将物理成像平面上的点进行平移缩放，才能得到o-u-v中对应的点。设α为u轴的缩放系数， β为v轴的缩放系数，[cx,cy]T 为原点平移量。则坐标系的旋转缩放可以表示为：
(3)
式(3)中，α和β的单位为像素/m，表示单位距离上的像素个数。将相机的焦距f转化为在x，y方向上的像素度量表示。则可令fx=αf， fy=βf。代入式(3)可得：
(4)
式(4)中， f的单位为m， fx和fy的单位为像素。将式(4)写成齐次坐标的形式为：
(5)
式(5)中,矩阵K称为内参矩阵。通过该内参矩阵可以将物理世界中的点投影到相机坐标系。
在本方案系统中，[u,v,1]T为从公式得到的已知量，空间点P为目标量，对式(5)变换得到:
(6)
通过式(6)即可得到相机参考坐标系下的目标物轨迹点。
3.3 坐标系转换接口设计
经过3.1和3.2节，可以得到相机固定不动时目标物在相机视野下的轨迹。当目标物运动范围过大，超出了D435I相机固定时的视野范围时，我们需要相机跟随目标物运动，以保持目标物始终在相机视野范围内，形成对目标物轨迹的完整捕捉。为了得到相机运动时目标物在世界参考坐标系下的轨迹，考虑对相机本身的运动轨迹进行定位。本方案采用视觉惯性里程计解决相机本身的轨迹定位问题。现有的视觉里程计主要分为2类：第1类以ORB-SLAM[14]和VINS-MONO[15]为代表的基于优化的方法;第2类以MSCKF[16]为代表的基于滤波器的方法。前者通常精度更高但对设备的计算性能要求也更高。本方案最终要在微型移动设备平台落地，因此主要考虑基于滤波器的S-MSCKF[17]方法。
为了将相机坐标系下的目标物轨迹还原到世界坐标系，我们需要对相机坐标系和里程计坐标系固连，相机坐标系和S-MSCKF里程计坐标系之间的固连通过旋转平移实现。旋转平移矩阵可以反映这层变换关系。数学上用3×3的方阵R表示旋转，3×1的向量t表示平移。将二者合并，可以写成4×4的齐次方阵，即：
(7)
设备运行时坐标关系如图6所示。在进行坐标系变换之前需要先明确本系统所涉及的3个坐标系：① 以设备运行时的起始出发点作为坐标原点的全局坐标系，在图6中记为world；② S-MSCKF追踪系统设备本身运动的里程计参考坐标系，在图6中记为odom；③ 和里程计坐标系之间存在固定的旋转平移关系的相机参考坐标系，在图6中记yolo_traj_d435。
图6 设备运行时坐标关系图
Fig.6 Coordinate relationship of equipment when running
ROS自身有static_transform_publisher工具，用以发布2个参考坐标系之间的静态变换，但在本系统方案中，该工具并不适用，因为其不仅会对当前时刻轨迹点做一个坐标变换，而且也会对历史时刻的轨迹点做坐标变换，导致在世界坐标系下目标物轨迹还原方向错误，因而需要自己重新设计坐标转换接口。
令里程计坐标系用O来表示，固定参考系用W表示，相机参考系用C表示，则从相机参考系下的点转变到世界参考坐标系下的点公式依据为：
PW=WTOOTCPC
(8)
坐标转换接口程序流程如图7所示，对应的具体步骤如下：
图7 坐标系转换接口程序流程界面
Fig.7 Key code segment of coordinate system conversion interface
步骤1 在S-MSCKF的msckf_vio.cpp源文件中，我们用Eigen::Isometry3d表示式(7)的旋转平移矩阵F。式(8)中OTC表示从D435I相机坐标系到里程计坐标系的变换。程序中用T_i_yolo表示。
步骤2 式(8)中WTO表示从里程计坐标系到全局参考坐标系的旋转平移变换，通过S-MSCKF的T_i_w获得。T_i_w*T_i_yolo得到D435I相机坐标在世界参考系下的表示。
步骤3 在createRosIO()中设置personOdomCallback()话题回调函数，接收来自yolodect进程发布的相机视野下的目标点位置话题/odom_traj，对应式(8)中PC。
步骤4 T_yolo2w*d435point_translation得到式(8)中PW，也即全局参考坐标系下的目标物轨迹点。
3.4 轨迹滤波
实际测试中发现，设备还原的轨迹存在少量的无效点。这些无效点，视觉上体现为轨迹的位移突变。必须剔除这些无用的轨迹点。
经过多次实际运行发现，这些异常点通常只有当目标物处于相机视野边缘且运动速度较快时候出现。而系统正常还原轨迹的前提是保证NCNN推理出的目标物边框中心点对齐到深度图像的像素点正确。当目标物处于相机视野边缘，由于目标物的不完整以及运动速度较快不利于实时解算，因此很可能发生目标物中心点计算错误或者计算卡顿，导致NCNN计算出的边框中心点比深度图像中对应的像素点位置滞后，从而导致如图8所示的异常值点。
图8 包含异常点轨迹图
Fig.8 Trajectory with outliers
我们用Si=[Sx,Sy,Sz]T表示i时刻的点位置，相邻时刻的轨迹点间位移可表示为：
(9)
根据实际测试经验，在设备稳定还原估计速度范围内，目标物单次处于相机图像边缘位置时的异常点不超过3个。为了应对异常轨迹点更多的极端不利情况，我们维护一个从i时刻开始的元素为Δs的5元数组Veci{Δsi,Δsi+1,Δsi+2,Δsi+3,Δsi+4}。
当Δsi≥L，L为目标物外接长方体中最短棱长，此处对应为篮球直径，将Δsi存入数组，依次计算i之后的4个Δs。当异常轨迹点数量越多，Veci中的元素大于L的就越多，最不利情况下，Veci中所有元素都大于L，因此只要判定i+5时刻Δsi+5≤mL是否成立，其中m为尺寸系数，需要根据实际目标物尺寸测试调整，对于篮球，取m=0.6。当Δsi+5≤mL成立，轨迹在正常速度捕捉范围内，便可以剔除Veci中Δsi≥L的点。当Δsi+5>mL时，则目标物运动状态超出本设备速度捕捉能力范围。试验者手持篮球并走出一个近似字母q的轨迹形状，在ROS中显示如图8所示，可以看到有几处偏离实际轨迹的异常点。实际滤波效果后的轨迹图如图9所示。
图9 轨迹滤波剔除异常值点图
Fig.9 Trajectory of eliminating outliers
3.5 数据传输任务多线程设计
动态目标轨迹还原的实时性是本方案力求保证的一个重要指标。保证这一指标的核心是协同处理好系统的数据传输。如何合理配置读取摄像头数据并处理的任务是本文研究的重点。
本方案设计yolodetect进程处理D435I数据，并通过ROS话题与S-MSCKF进程实现数据交互。对于简单的数据通信，通常只需要一个话题发布者和一个话题接收回调函数。此种单线程的方式，各个回调函数以串行的方式进行，若某个图像回调函数的处理时间过长，会导致其他的回调函数被阻塞，极易造成数据来不及处理而被新数据覆盖丢失问题。多任务未经设计安排的系统资源使用情况如图10所示，其中yolodetect进程CPU使用率为36%。
图10 改进前系统资源使用界面
Fig.10 System resource usage before improvement
针对yolodect进程的性能表现，作出如下改进：
1) 分别为彩色、深度图像处理以及接收坐标转换后的轨迹点数据显示开辟任务回调线程imageCallback()、depthCallback()、yolo_trajCallback()；
2) 将NCNN解算目标物程序段位置从main()函数的while()循环处理改为在imageCallback()线程中，形成一帧彩色图像到来衔接一次解算，避免了NCNN的无效或重复解算；
3) 当彩色图像帧的目标物位置未解算完成，而新一帧的深度图像到来时，depthCallback()会被调用。为了保证depthCallback()线程调用时，对应时刻的目标物位置已经解算出来，我们采用unordered_map容器数据结构关联标记每一帧彩色图像时间戳对应的目标物位置是否解算完成，若未解算完成，则退出depthCallback()，避免做无用功。
由图11可知，由于采用了有序的多线程，yolodetect进程的总CPU使用率降低了24%，获得了显著的性能提升。内存则从30 MiB升至55.4 MiB。这是因为yolodetect进程中并行有序处理着彩色图像和深度图像，内存稍有上升。其中第一个nodelet进程由于增加了3.3节的坐标系转换接口任务，CPU使用率相比S-MSCKF源程序有所增加。大幅度性能提升将更有利于本方案系统部署在无人机、移动机器人等计算资源有限的实际工程应用中。
图11 改进后系统资源使用界面
Fig.11 System resource usage after improvement
4 实验验证与结果分析
为了验证前述动态目标追踪的效果，进行如下实验。本次实验在南京理工大学319幢一楼的走廊和楼道之间实施。实验场地如图12所示。篮球沿着红色真值点线路放置，共18个放置点。本方案系统的双目相机MYNT-EYE用于运行S-MSCKF并对设备本身定位，深度相机D435I用于捕捉篮球的离散化轨迹。为了保持篮球运动轨迹的估计值和实际位置的时间戳一致性，将篮球置于每个路径点较长时间。利用ROS系统的录制包功能，记录下设备对篮球的放置轨迹点还原数据。
图12 实验场地示意图
Fig.12 Experimental field
4.1 标定
轨迹还原中的射影变换需要获取相机内参，相机内参通过Kalibr[18]工具标定获得。标定完成后获得参数如表1、表2所示。
表1 D435I相机标定参数
Table 1 D435I Camera calibration parameters
相机参数标定结果distortion_coeffs[0.093-0.2000.0030.002]intrinsics[595.44598.85324.045238.43]
表2 MYNTEYE相机标定参数
Table 2 MYNTEYE Camera calibration parameters
相机参数标定结果T_cam0_imu-0.0150.999-0.0060.038-0.999-0.015-0.007-0.008-0.0070.0060.999-0.0140.0000.0000.0001.000éëêêêêêùûúúúúúdistortion_coeffs0-0.3380.083-0.0020.003[]Intrinsics0389.694391.632389.095244.234[]T_cam1_imu-0.0170.9990.015-0.069-0.999-0.0170.005-0.0080.005-0.0150.999-0.0120.00.00.01.0éëêêêêêùûúúúúúdistortion_coeffs1-0.3180.067-0.0010.003[]Intrinsics1385.058388.031383.340233.191[]
4.2 神经网络模型训练
为了识别篮球目标，需预先对篮球图片进行训练，训练基于Darknet框架。当损失函数值趋于平稳不变时，则认为网络模型收敛。训练过程如图13所示。
图13 网络训练过程曲线
Fig.13 Network training chart
4.3 实验结果分析
轨迹还原误差评定参数如表3所示。
从表3计算结果参数来看，对于篮球在真值总长8.1 m路径上的轨迹点还原，最小误差为0.073 m。平均误差为0.156 m，为篮球直径的63.4%，最大误差为0.305 m，结合图14三维估计点的分布情况可知，在转弯处由于S-MSCKF产生了漂移，导致误差较大，以至后面的轨迹点整体误差也变大。在纹理更为丰富的场景中，视觉惯性里程计的漂移会有所减少。
表3 轨迹还原误差评定参数(m)
Table 3 Evaluation parameters of trajectory restoration error
名称MinMaxMedianRmse总长参数0.0730.305 0.1560.1768.1
图14 轨迹点还原三维曲线
Fig.14 3D track point restoration
5 结论
针对动态目标的三维轨迹还原问题，设计了基于三维视觉和深度学习并能部署在计算资源有限的移动平台上的方案。在坐标系转换方面，设计了坐标转换接口，克服了ROS自带的坐标转换工具的缺陷。引入NCNN和视觉惯性里程计实现无需相机固定且具有实际客观尺度信息的动态目标物轨迹还原功能。对于轨迹还原过程中的异常轨迹点进行判定剔除，从而得到更符合目标物实际运动的轨迹。设计了有序的多线程任务，降低了方案的CPU资源使用率，方便部署在计算资源有限的平台上。实验表明，本文设计的动态目标轨迹还原系统可以对经过神经网络训练的目标物进行轨迹还原，可以达到30 FPS的实时效果，并在ROS中直观可视化显示其轨迹，实现了在计算能力有限的平台实时捕捉三维轨迹的应用需求。
[1] Gandhi D，Pinto L，Gupta A.Learning to fly by crashing[C]//Proc.of the 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS).IEEE，2017.
[2] 谢玮,成艳英,陈柯,等.基于图像处理技术的行人运动轨迹提取方法[J].消防科学与技术,2019,38(01):44-47.
Xie W,Cheng Y Y,Chen K,et al.Pedestrian trajectory extraction method based on image processing technology[J].Fire Science and Technology，2019,38(01):44-47.
[3] 刘星桥,张弛.基于嵌入式图像处理系统的鱼类轨迹跟踪[J].江苏农业科学,2018,46(10):203-207.
Liu X Q,Zhang C.Fish trajectory tracking based on embedded image processing system[J].Jiangsu Agricultural Sciences，2018,46(10):203-207.
[4] Azim A，Aycard O.Layer-based supervised classification of moving objects in outdoor dynamic environment using 3D laser scanner[C]//Proc.of the 2014 IEEE Intelligent Vehicles Symposium Proceedings.IEEE，2014.
[5] Brsan I A,Liu P,Pollefeys M，et al.Robust Dense Mapping for Large-Scale Dynamic Environments Layer-based supervised classificat-ion of moving objects in outdoor dynamic environment using 3D laser scanner[C]//2018 IEEE International Conference on Robotics and Auto-mation(ICRA).IEEE，2018.
[6] 王羿帆,苏彦莽,张琳,等.基于MEMS的便携式消防员室内定位研究[J].传感器与微系统,2021,40(01):63-65,70.
Wang Y F,Su Y M,Zhang L,et al.Research on indoor positioning of portable firefighters based on MEMS[J].Transducer and Microsystem Technologies，2021，40(01):63-65,70.
[7] 赵建伟,王洪燕,唐兵,等.基于移动机器人的GPS轨迹生成及定位研究[J].工矿自动化,2016,42(01):17-19.
Zhao J W,Wang H Y,Tang B,et al.Research on GPS trajectory generation and positioning based on mobile robot[J].Industry and Mine Automation，2016,42(01):17-19.
[8] Liu W，Anguelov D,Erhan D,et al.SSD:Single Shot MultiBox Detector[C]//Proc. of the Computer Vision.ECCV，2016.
[9] Ren S，He K，Girshick R，et al.Faster R-CNN:Towards Real-Time Object Detection with Region Proposal Networks[J].IEEE Transactions on Pattern Analysis & Machine Intelligence，2017，39(6):1137-1149.
[10] 吴雪,宋晓茹,高嵩,等.基于深度学习的目标检测算法综述[J].传感器与微系统,2021,40(02):4-7，18.
Wu X,Song X R,Gao S,et al.Survey of object detection algorithms based on deep learning[J].Transducer and Microsystem Technologies，2021,40(02):4-7,18.
[11] Tajbakhsh N，Shin J Y，Gurudu S R，et al.Convolutional Neural Networks for Medical Image Analysis:Full Training or Fine Tuning[J].IEEE Transactions on Medical Imaging，2016，35(05):1299-1312.
[12] Yao C，Wu F，Chen H J，et al.Traffic sign recognition using HOG-SVM and grid search[C]//Proc. of the International Conference on Signal Processing.IEEE，2015.
[13] Mur-Artal R，Montiel J M M,Tardos J D.ORB-SLAM:A Versatile and Accurate Monocular SLAM System[J].IEEE Transactions on Robotics，2015，31(05):1147-1163.
[14] Qin T,Li P L,Shen S J,et al.VINS-Mono:A Robust and Ver-satile Monocular Visual-Inertial State Estimator[J].IEEE Transactions on Robotics，2018,34(04):1004-1020.
[15] Mourikis A I，Roumeliotis S I.A Multi-State Constraint Kalman Filter for Vision-Aided Inertial Navigation[C]//Proceedings of the 2007 IEEE International Conference on Robotics and Automation.IEEE,2007.
[16] Sun K，Mohta K，Pfrommer B,et al.Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight[J].IEEE Robotics & Automation Letters，2018,3(02):965-972.
[17] Rehder J，Nikolic J，Schneider T，et al.Extending kalibr:Calibrating the extrinsics of multiple IMUs and of individual axes[C]//Proc.of the 2016 IEEE International Conference on Robotics and Automation(ICRA).IEEE，2016.