人机协作是未来生产生活的主流方式. 跟随机器人能够自主跟随用户，帮助解放用户双手，进一步提升效率. 跟随机器人的实现重点在于辨认目标并实现鲁棒且安全的跟踪^[1]，如何使用成本低、可移植性强且通用的跟随方法，使移动机器人安全、鲁棒地在人的引导下实现自主跟随功能是研究的重点对象之一.

在机器人进行目标跟随时，由于传感器视野固定，快速移动的目标容易走出视野，导致跟踪失败. 研究者提出的多种对跟踪视野进行优化的解决方法可以分为2类. 1）增加传感器的数量以辅助定位目标. Dam等^[2]使用手机IMU加速度信息进行辅助运动推断. Tee等^[3]使用穿戴主动红外发射机构，基于接受信号强弱进行辅助定位. 部分研究者使用多传感器融合方法扩大跟踪视野，包括鱼眼全景相机^[4]、相机与激光雷达融合^[5]、外部环境相机^[6]等. 这些方法虽然能够实现更大视野范围内跟踪目标的跟踪，但是传感器数量增加使数据量变多，算法的复杂度也相应提升；同时多传感器间须联合标定，这增加了机器人系统的硬件与使用成本. 2）在不额外增加传感器的条件下，将相机置于与机器人底盘运动分离的云台，单独进行随动控制，这样可以基于视觉伺服的方法扩大视野，实现对运动目标的持续跟踪^[7]. 类2）的相关研究较少，且通常是对目标进行视觉伺服跟踪，没有关注视野内的障碍物信息. 这类解决方法须搭配深度传感器进行辅助避障，因此无法实现降低机器人硬件成本的目的.

鉴于跟随机器人的任务需求，其传感器不仅要提供丰富的特征信息以实现对跟随用户的识别，提供视野内的深度信息以识别、检测可通行区域与障碍物，还要有低成本、可移植性强的特点. RGB-D传感器可以获得具有丰富特征的彩色图像信息,常用于行人目标的检测与跟踪^[8–10]. 它通过计算飞行时间测距得到视野内目标的深度信息，与其他传感器相比场景适用范围更大^[11-12].

本研究提出基于自适应随动机构的行人跟随方案. 将RGB-D传感器置于可以单独进行规划控制的机器人云台，基于彩色图像跟踪跟随目标，使用深度图像进行避障与路径规划，结合实时调整相机视野的自适应规划算法，使用单一传感器实现机器人在复杂环境下的行人跟随功能.

实现行人目标的自主跟随功能须先确定跟随目标的位置. 本研究提出基于二维检测结果的实时行人位置跟踪算法.

基于点云的三维检测方法虽能获得目标三维位置，但该方法算力要求较高，在机器人计算平台上较难达到实时检测要求. 本研究选用基于卷积神经网络的YOLOv3^[13]算法对彩色图像进行实时目标检测.

YOLOv3算法是YOLO系列的第3代改进算法. 在主体网络结构上YOLOv3采用具有53个卷积层的Darknet-53特征提取网络，相比YOLOv2所使用的含有19个卷积层的Darknet-19网络，准确率大幅提高. 网络结构能够加深的主要原因是Darknet-53引入He等^[14]提出的残差网络结构. YOLOv3算法中使用的残差网络结构如图1所示，该结构将前后不同层级输出相加，以避免网络层数过深导致梯度消失无法收敛的问题.

为了解决YOLO算法对小物体检测精度低的问题，YOLOv3算法引入多尺度特征图检测方法。在主干网络上分别生成32倍、16倍和8倍的降采样特征图，在不同尺度的特征图上均进行检测框预测，以实现不同尺寸物体的相同高精度检测效果. 相比其他网络模型，YOLOv3的量级轻，资源要求低，且满足跟随任务中的行人检测需求.

检测算法能够获得目标在图像坐标系下的位置，在进行跟随时，跟随机器人还需要获得跟踪目标相对于底盘的三维坐标. 本研究基于针孔成像模型与深度信息解算目标三维位置.

基于相机针孔成像模型，可以推导出三维位置 ${\left[ {X,Y,Z} \right]^{\text{T}}}$与成像坐标 $ {\left[ {u,v} \right]^{\text{T}}} $的对应关系为

(1) ${\begin{array}{*{20}{c}} {X = }{\dfrac{{u - {c_x}}}{{{f_x}}}Z\;,}\;\; {Y = }{\dfrac{{v - {c_y}}}{{{f_y}}}Z.} \end{array}} $

式中： $ {f}_{x}、{f}_{y}、{c}_{x}、{c}_{y} $均为相机内参， $ X、Y $为目标在相机坐标系的三维位置， ${u、v} $为目标在图像坐标系的像素坐标， $Z$为目标深度.

假设检测框的中心点坐标为目标所在像素点，查询RGB-D传感器深度图像中对应点的深度值，通过式(1)计算目标在相机坐标系的三维坐标，结合相机外参转换到底盘坐标系并进行后续跟踪. 三维坐标解算的计算流程如图2所示.

传统多目标跟踪算法在图像坐标系下对检测框的运动状态进行跟踪^[15–17]。为了改善在相机随动情况下的跟踪效果，本研究使用目标在底盘坐标系下的位置信息作为Kalman跟踪器状态变量. 跟踪器采用5维状态量设计，即

(2) $ {\boldsymbol{X}} = {\left[ {x,y,z,\dot x,\dot y} \right]^{\text{T}}}. $

式中： $ x、y、z $为目标在底盘坐标系下的位置， $ \dot{x}、\dot{y} $为沿运动平面X、Y轴的速度.

跟踪器的运动模型选择在目标平面的X轴和Y轴分别采用匀速运动模型，得到状态更新矩阵

(3) $ {\boldsymbol{A}} = \left[ {\begin{array}{*{20}{c}} 1&0&0&{\Delta T}&0 \\ 0&1&0&0&{\Delta T} \\ 0&0&1&0&0 \\ 0&0&0&1&0 \\ 0&0&0&0&1 \end{array}} \right] .$

式中： $ \Delta T $为2次状态更新的时间间隔. 由于存在目标走出视野或未被检测到的情况，滤波器更新频率并不固定。采用动态更新矩阵的方法，每次滤波器更新前根据实际间隔对 $ {\boldsymbol{A}} $进行更新.

在数据匹配阶段，使用在运动平面上的欧式距离作为匹配度量，计算公式为

(4) $ c\left( {i,j} \right) = \sqrt {{{\left( {{{\hat x}_i} - {x_j}} \right)}^2} + {{\left( {{{\hat y}_i} - {y_j}} \right)}^2}} . $

式中： $ c\left( {i,j} \right) $为第 $i$个跟踪器与第 $j$个检测结果的匹配权重， ${\hat{x}}_{i}、{\hat{y}}_{i}$为跟踪器预测目标的先验位置， $ {x}_{j}、{y}_{j} $为三维目标检测结果.

对每帧检测结果，计算得到所有匹配权重后，使用Kuhn-Munkras算法^[15]求解检测集与跟踪集的最优匹配. 根据匹配结果更新跟踪器状态，并管理跟踪器的生成与删除. 跟踪器匹配与更新算法的具体实现流程如下. 1）使用跟踪器预测结果与三维目标检测结果，计算匹配权重矩阵 ${{\boldsymbol{C}}_{N \times M}}$，矩阵中每个元素为式(4)定义的匹配权重. 2）初始化匹配对集合 $M$为空集，初始化未匹配检测集合 $ \mu $为输入检测结果集合 $D$，初始化未匹配跟踪集合 $\nu $为输入跟踪器结果集合 $ T $. 3）使用Kuhn-Munkras算法基于 ${{\boldsymbol{C}}_{N \times M}}$计算跟踪集合 $T$与检测集合 $D$间的最优匹配，存储在矩阵 ${{\boldsymbol{P}}_{N \times M}}$中. 当 $ {p_{i,j}} = 1 $时，表示第i个跟踪器与第j个检测结果为最优匹配对. 4）使用矩阵 $ {\boldsymbol{P}} $更新匹配对集合 $M$， $M$中存储了成对的跟踪器与检测结果. 5）使用 $ {\boldsymbol{P}} $更新 $\;\mu $，从 $\;\mu $中删去已匹配的检测结果. 6）使用 $ {\boldsymbol{P}} $更新 $\nu $，从 $\nu $中删去已匹配跟踪器. 7）对于 $M$中的所有匹配对，使用检测结果对跟踪器状态进行更新，并将更新后的跟踪器储存在 $T$中. 8）对于 $\mu $中未匹配的检测结果，基于检测结果初始化新的跟踪器，并添加到 $ T $中. 9）对于 $\nu $中未匹配的跟踪器，检查其连续未成功匹配的次数，若大于阈值 $ {A_{\text{m}}} $，则从 $T$中删除该跟踪器. 10）返回更新后的 $T$.

传感器角度规划算法的研究较少，同时目标跟随、角度规划问题均是实际场景下的应用任务，较难进行准确的数学建模，因此该方向缺少普遍承认的评价指标，用于对比不同的传感器角度，这样除主观评价外很难对不同策略的优劣进行分析. 结合任务需求与运动规划领域研究成果，设计多项评价指标对跟随过程中随动传感器的视野进行全面评价，包含以下几个部分。

传感器的角度应使跟随目标保持在视野中央，以免当目标快速移动时出现丢失目标的情况.设置跟踪目标位置评价指标的计算公式为

(5) $ {S}_{\text{t}}=\left\{\begin{array}{l}0,\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;未检测到目标;\\ -\dfrac{1}{2}{\left(\dfrac{{\theta }_{{\rm{f}}}}{{\theta }_{\text{m}}}\right)}^{2}+1,\;\;\;\;\;\;\;\;\;检测到目标.\end{array} \right.$

式中： ${\theta _{\rm{f}}}$为跟随目标在视野内的角度； ${\theta _{\text{m}}}$为相机最大视野角，基于视野内是否检测到目标进行区分.

对于每帧检测结果，判断相机画面中是否成功检测到跟踪目标. 若未检测到目标，则 $ {S_{\text{t}}} = 0 $；若成功检测到目标，则获得0.5分的基础分，其余0.5分按照目标距离视野中心距离以二次函数关系下降，目标离视野中心越近评分越高.

传感器须感知视野内的可通行区域，其区域占比应尽可能大，以便规划后续的运动路径. 可通行区域占比评价指标的计算公式为

(6) $ {S}_{\text{p}}=\frac{{{\displaystyle \sum }}_{i=1}^{N}{d}_{i}\text{Δ}\theta }{{{\displaystyle \sum }}_{i=1}^{N}{d}_{\text{m}}\text{Δ}\theta  }. $

式中： ${d_i}$为深度扫描得到的深度， ${d_{\text{m}}}$为传感器最大测量深度， ${{\Delta }}\theta $为相邻扫描数据的角度增量， $N $为传感器深度扫描数目.

仅考虑可通行区域大小的评价指标，可能会导致机器人忽略前进路线上的障碍物. 结合路径规划领域的相关评价方法^[18]，加入视野内的障碍物数量 ${N_{\text{o}}}$作为传感器障碍物感知效果的评价指标. ${N_{\text{o}}}$通过统计深度数据中可通行区域与障碍物区域的切换次数得到.

对上述3项评价指标进行加权求和，即可根据当前传感器数据对规划角度进行实时量化评分. 综合评分 ${S_{\text{a}}}$的计算公式为

(7) $ {S_{\text{a}}} = {S_{\text{t}}} + {S_{\text{p}}} + \gamma {N_{{\text{o}}}}. $

式中： $\gamma $为可调参数，代表每个检测到的障碍物所带来的评分增益，设定 $\gamma $=0.05.

对不同角度规划算法的行人跟随效果进行综合对比评价，统计相同算法在相同仿真场景多次实验的平均跟随时间.

(8) $ {\overline T_{\text{f}}} = \frac{1}{N}\mathop \sum \limits_{i = 1}^N {\kern 1pt} T_{\text{f}}^i. $

式中： $ T_{\text{f}}^i $为单次跟随时间，即机器人连续丢失目标超过一定阈值时间(5 s)所对应的仿真时长.

从随动机构控制效果考虑，规划算法所生成的角度轨迹应保持连续平滑. 参考运动指令规划领域的相关评价指标^[19]，对规划算法的所有目标角度进行平滑性度量：

(9) $ {f_{\rm{s}}} = \frac{1}{{\left( {N - 2} \right){{\Delta }}{T^2}}}\mathop \sum \limits_{i = 2}^{N - 1} {\left( {{{\Delta }}{\theta _{i + 1}} - {{\Delta }}{\theta _i}} \right)^2}. $

式中： ${{\Delta }}{\theta _i}$为相邻目标指令间的角度增量， ${{\Delta }}{\theta _i} = {\theta _i} - {\theta _{i - 1}}$； $ {{\Delta }}T $为角度规划周期. ${f_{\rm{s}}}$越小对应算法的角度轨迹越平滑.

基于仿真环境设计并对比多种角度规划策略

固定传感器即传感器相对于底盘静止、不进行单独规划控制的策略. 以该策略为基准说明随动式感知机构的优势：

(10) $ {\theta _{\text{c}}} = 0. $

式中： ${\theta _{\text{c}}}$为传感器随动机构相对底盘的角度，式（10）表示传感器相对底盘静止且指向前方.

目标伺服策略不考虑视野内的障碍物信息，让跟踪目标始终处于传感器视野中央，可以最大化跟踪效果，避免当目标快速移动时传感器丢失目标.

(11) $ {\theta _{\text{c}}} = {\theta _{\text{t}}}. $

式中： ${\theta _{\text{t}}}$为机器人跟随目标相对底盘的角度。式（11）表示传感器始终指向目标所在方向.

针对目标伺服策略在转弯场景中容易忽视前方道路和障碍物的问题，提出结合底盘方向角度规划策略. 取目标角度 $ {\theta _{\text{t}}} $与底盘运动角度 ${\theta _{\text{p}}}$的加权和作为规划角度，可以在较大角度范围兼顾目标跟踪与障碍物检测的效果.

(12) $ {\theta _{\text{c}}} = \lambda {\theta _{\text{t}}} + \left( {1 - \lambda } \right){\theta _{\text{p}}}. $

式中： $\lambda $为权重系数，作用是调节目标跟踪、障碍物检测任务在视野内占比. 实验表明，当 $  \lambda $=0.6~0.8时，对应的传感器角度规划效果较好， $  \lambda $过大或过小都会导致某一项任务的效果变差. $\lambda $的取值还应结合实际场景的跟随效果进行具体分析.

结合底盘方向角度规划策略虽然相比目标伺服策略有一定改进，能够兼顾目标跟踪与障碍物检测的需求，但是机器人运动时的底盘方向一方面依赖于底盘结构与驱动方式，另一方面又依赖于路径规划与控制算法，因此得到规划效果的泛用性与可移植性较差.本研究提出基于视野域加权的自适应规划策略，使用传感器深度信息对当前视野内角度进行加权求和作为运动方向的估计，并将估计角度 $ {\theta _s} $与目标角度 ${\theta _t}$加权作为规划角度，实现基于传感器数据的相机视野自适应调整.

(13) $ {\theta _{\text{s}}} = \frac{1}{{\displaystyle\mathop \sum \nolimits_{i = 1}^N d\left( {{\theta _i}} \right)}}\displaystyle\mathop \sum \limits_{i = 1}^N {\theta _i}d\left( {{\theta _i}} \right), $

(14) $ {\theta _{\text{c}}} = \lambda {\theta _{\text{t}}} + \left( {1 - \lambda } \right){\theta _{\text{s}}}. $

式中： $d\left( {{\theta _i}} \right)$为角度 ${\theta _i}$对应的传感器扫描深度.

以RoboMaster开源机器人平台为基础，实现搭载随动机构的跟随机器人平台. RoboMaster机器人平台采用具备4个麦克纳姆轮的全向底盘与双自由度机械云台设计，可以实现云台相对底盘的独立控制. 机器人使用固定在云台的Azure Kinect作为随动式RGB-D传感器，使用固定于底盘的Jetson TX2开发板作为嵌入式硬件计算平台. 跟随实验中使用的RoboMaster机器人平台如图3所示.

为了辅助算法开发与仿真验证，本研究基于Gazebo搭建行人跟随任务的仿真平台. 机器人在Husky Robot模型^[20]基础上进行修改，添加Kinect传感器并将其置于旋转关节，实现随动传感器功能. 仿真地图在playpen静态地图^[20]的基础上，加入运动行人对象作为跟随目标与动态障碍物. 基于Gazebo平台的仿真机器人与地图如图4所示.

使用Ubuntu 18.04平台运行ROS Melodic 作为移动机器人感知与运动控制系统的软件框架.系统架构按照不同功能层级划分为应用层、框架层、系统层和硬件层，每个模块均通过ROS发布和订阅不同的消息话题，以实现不同进程间的通信与配合. 基于ROS的软件架构与功能模块如图5所示.

本研究使用Darknet框架^[13]部署YOLOv3算法，基于roscpp编写目标跟踪与角度规划模块，结合ROS提供开源运动规划模块move_base，实现机器人完整的跟随功能.

在ROS平台上部署YOLOv3-tiny目标检测网络，并分别在仿真环境与实际场景中进行测试实验. 在Gazebo仿真环境中，通过添加openni_kinect插件实现对Kinect深度相机的仿真，并添加动态行人actor对象作为检测目标. 实际场景中分别进行单人、多人场景下的数据采集与实验，测试数据通过Azure Kinect传感器在彩色相机720p、深度相机NFOV 2x2 binned工作模式下采集. 使用YOLOv3算法在仿真与实际场景下的行人检测结果如图6所示. 实验表明在仿真与实际场景下，YOLOv3算法均可以实现对视野内不同尺寸目标的准确检测.

在仿真环境中可以获得目标的真实位置，本研究基于Gazebo仿真平台，对单目标跟踪与坐标解算效果进行实验测试，得到仿真平台中单目标跟踪轨迹与真实轨迹对比如图7所示. 可以看出，本研究所使用的方法能够正确解算出目标三维位置，但检测结果存在一定的测量噪声，经Kalman滤波后的跟踪轨迹中噪声得到改善，且跟踪轨迹与真实轨迹的误差不超过20 cm，满足行人跟随任务的要求.

基于Azure Kinect传感器采集到的数据对实际场景下多目标跟踪效果进行实验，得到多目标匹配与跟踪结果、多目标跟踪轨迹分别如图8、9所示. 可以看出，本研究所提多目标跟踪算法能够实现对多行人位置的持续跟踪。当目标在视野范围内运动时能够保持相同的身份码(identity, ID)，从而区分不同的行人目标，实现对目标轨迹的持续跟踪. 多目标跟踪算法使得机器人在当视野内存在多个行人目标时，仍保持对单一目标的持续跟随. 此外本研究所提算法的运算速度较快，坐标解算与跟踪模块在Jetson TX2平台上的计算速度能够达到100 Hz，满足移动机器人进行实时行人位置跟踪的需求.

对于传感器角度规划任务，本研究基于仿真平台对上述4种传感器角度规划策略进行的仿真测试. 在相同仿真场景进行多次仿真测试后，统计得到的不同传感器角度规划策略各项评价指标的平均评分如表1所示. 可以看出，本研究所提权重视野规划策略可以通过传感器数据对传感器角度进行自适应调整，兼顾了目标跟踪与障碍物检测需求，在多项指标中都取得最优评分. 由此也证明了本研究所提自适应角度规划策略的有效性，这与实验观察中得到的结果相吻合.

结合机器人的感知规划与硬件控制模块功能，本研究在仿真与实物机器人平台实现行人跟随功能，并进行实验测试，仿真与实物机器人的行人跟随实验场景如图10所示. 可以观察到，本研究所提基于自适应随动机构的跟随方法极大地改善了机器人目标跟随的成功率。当目标在视野范围移动时，随动机构能够及时根据目标与环境信息自适应调整云台角度，从而保证对目标的持续跟踪. 在多人场景跟随实验测试中，基于多目标跟踪算法，机器人在当视野存在多个行人目标时，能够实现对单一行人目标的持续跟随, 证明本研究所提方法具有较强的鲁棒性.

如图11所示为机器人跟随实验中跟随目标与机器人的运动轨迹. 可以看出，本文所提方法能够实现机器人对运动目标稳定的轨迹跟随效果. 其中仿真场景的机器人轨迹跟随效果更好，原因是仿真中行人目标匀速运动，机器人容易跟随且传感器的数据质量更好. 在实物场景中由于目标真实位置较难获得，用机器人目标跟踪结果进行近似，得到目标轨迹的噪声较大，但依然可以看出机器人对目标轨迹稳定的跟随效果.

（1）针对相关研究较少的随动式感知机构角度规划问题，从任务需求出发设计多项指标进行综合评价，提出基于底盘方向的规划策略与基于权重视野域的规划策略，改善跟随任务中的传感器角度规划效果.

（2）针对行人跟随场景，提出基于YOLOv3网络、三维坐标解算与位置度量匹配的多目标位置跟踪算法. 该算法实现了对视野内行人目标位置良好的实时检测与跟踪效果，满足机器人视觉目标感知的需求.

（3）将视觉感知与角度规划模块应用于ROS平台，结合开源机器人平台实现机器人的行人跟随功能. 实验结果表明，本研究设计的行人跟随框架能够实现机器人对行人运动轨迹的持续跟随效果.

（4）本研究设计的随动机构角度规划策略基于预先设定规则，需要手动调整参数，对不同场景的规划效果存在差异. 后续研究将考虑基于优化理论以及强化学习的角度规划算法，实现对不同场景更加鲁棒的角度规划效果. 后续计划将本研究所提随动感知机构在无人车辆平台进行部署测试，研究不同运动平台基于随动机构的目标跟随效果.