基于深度测距的移动机器人自适应跟随

杨 彪，王 狄，沈绍博，杨长春+，刘小峰

(1.常州大学 微电子与控制工程学院，江苏 常州 213164；2.河海大学 物联网工程学院，江苏 常州 213022)

0 引 言

室内外环境的复杂性为移动机器人的精准跟随提出了挑战。为了更好地完成行人跟随任务，需要精确测量人-机距离[1，2]，根据距离和偏航角调整机器人运动状态，从而鲁棒地跟随行人。

为了准确测量前人距离，研究人员使用多传感器融合的方法来检测和跟踪行人[3，4]。Tripathi等[5]使用超声波和红外传感器跟随指定行人。随着激光雷达技术的发展，可以实现远距离人体目标的测距[6，7]。Chebotareva等[8]基于激光测距仪和单目相机的数据融合方法，提出了人体跟踪算法。Zhu等[9]使用毫米波雷达和单目相机的信息感知环境，使机器人以预期的距离跟踪人体目标。毫米波雷达和激光雷达可以直接获取目标位置，但是两者成像分辨率低，难以区分物体边界，而多传感器融合方法因其高昂的售价难以在移动机器人中普及[10]。

本文基于低成本的RGB-D相机，搭建了移动机器人平台，提出了一种基于MRSD测距的移动机器人跟随系统，主要工作在于：①本文引入Mask R-CNN算法[11]对行人进行实例分割，根据行人掩膜从深度图像中提取深度像素点，测量行人距离。②针对深度伪影问题，当前工作多使用传统滤波算法[12]，导致测距精度低。本文引入S2R-DepthNet网络[13]从彩色图像中推理深度，替换相机深度图像中的无效点，提高测距精度。③针对深度融合的测距信息，建立基于Sage-Husa自适应滤波器的测距模型，抑制量测异常值的影响，从而使得移动机器人能够更加鲁棒地跟随行人。

1 算法理论基础

1.1 实例分割算法

实例分割是计算机视觉领域经典任务之一。在移动机器人领域，实例分割可以分析场景，检测场景中的不同实例并完成像素层面的分类。随着深度学习技术的发展，科研工作者相继提出了Faster R-CNN[14]、Cascade R-CNN[15]和YOLO系列[16，17]网络模型。相比于传统目标检测算法，上述网络模型在准确性有着显著优势。但是，上述算法的目标检测框无法准确分离物体的前景和背景，从而降低深度测距精度。因此，本文引入Mask R-CNN模型，检测场景中的行人目标，获得行人掩膜。

通过预训练的RPN获得行人目标检测框、使用ROIPool从检测框中提取特征，在进行分类和边界框回归的同时输出二进制掩膜。在具体训练过程中，分类损失函数Lc和边界框回归损失函数Lb的运算如下

(1)

(2)

其中，s是分类器预测的softmax概率分布，u为目标真实类别标签，tu为预测回归参数，v为真实目标边界框参数。

Mask的损失函数Lm为

Lm=-ylog(p(x))-(1-y)log(1-p(x))

(3)

式中：p(x) 是模型输出，y是真实标签。

与单任务网络相比，多任务网络共享特征提取层参数，从而减小模型参数和计算量，完成检测和分割任务。

1.2 单目深度估计算法

单目深度估计网络的任务是预测彩色图像中像素的深度值。S2R-DepthNet模型借鉴了人对场景的三维感知的特点，重点关注彩色图像物体的空间结构，结合深度注意力模块预测彩色像素的深度信息。S2R-DepthNet模型可以分离彩色图像结构信息并提取互补的高层语义，抑制与深度无关的信息，从而增加深度推理的准确性。

具体过程如下：输入彩色图像，通过结构提取模块分离彩色图像中不变的结构信息，生成结构图，真实深度为T，损失函数如下所示

(4)

式中：T′是预测的深度值，p是深度像素索引，Δx和Δy是深度图的水平和垂直算子，α和β是超参数。

结构图中包含了大量与深度无关的低层信息，会降低深度估计的精度。而利用深度注意力模块产生的深度注意力图，优化结构图，输出预测深度图，从而提高深度估计质量。注意力图和深度预测的损失函数为

(5)

2 基于MRSD测距的跟随系统

本文搭建了一个移动机器人作为行人跟随的平台，提出的基于MRSD测距的移动机器人跟随系统的运行流程，如图1所示，主要包含MRSD测距算法和行人跟随控制两个部分。首先对输入的彩色图像进行目标检测；然后深度融合测距模块获取人-机距离，根据目标距离建立Sage-Husa自适应卡尔曼预测和测量方程输出距离信息；最后，当人-机距离与设定距离不等时，通过PID控制器输出相应角速度和线速度，驱动小车跟随行人移动至设定距离后，若行人继续运动，小车持续跟随行人，否则，小车停止运动。

图1 机器人跟随系统流程

2.1 MRSD测距算法结构

本文提出的MRSD测距算法包含两个部分：行人检测和深度测距，如图2所示。具体流程为：将彩色图像输入Mask R-CNN和S2R-DepthNet网络，前者用于检测行人，获取行人前景掩膜；后者用于推理深度图像。根据掩膜从深度图像中提取行人区域深度点，使用推理深度图像替代相机深度图像中的无效点，从而精确计算人-机距离。下面分别介绍MRSD测距算法的两个模块。

图2 MRSD测距算法结构

2.1.1 行人检测

行人检测既是深度融合测距的前提，又是移动机器人感知环境的基础[18]。YOLOv5虽然可以快速检测行人，但目标检测框会选中图像的背景像素，导致深度测距精度降低。因此，准确表示行人区域，对于测量人-机距离至关重要。本文引入Mask R-CNN网络准确分割行人区域并输出前景掩膜。定义输入彩色图像X，则掩码Y的公式如下

Y=Mask(X，Ws)

(6)

式中：Mask()表示预训练的Mask R-CNN网络，Ws表示权重矩阵。

Mask R-CNN结合Faster R-CNN和FCN的功能，前者用于提取特征，随后在特征图上，利用分类器鉴别的候选ROI并检测行人；后者利用卷积和去卷积操作，对每个像素分类后得到行人掩膜，从而准确分割行人前景和背景。同时为了降低误判率，本文设置算法的行人置信度为0.92。

2.1.2 深度融合测距

行人运动速度的动态变化，导致人-机距离也随之变化。若移动机器人能精确测量人-机距离，就能通过控制算法控制其运动状态，准确跟随行人。在跟随行人的过程中，由于行人运动状态的不确定性和RGB-D相机的成像限制，移动机器人不可避免地产生距离误差。深度测距误差原因通常来自于两个方面：不准确的目标检测框以及深度图像中无效像素值，前者可以通过Mask R-CNN输出行人掩膜解决；后者指深度图像中距离为0的深度像素点。

为了降低无效深度点对测距精度的影响，本文首先定位相机深度图像中深度无效点的位置，然后提取单目深度估计网络推理的深度图像对应位置的距离，最后完成像素替换，从而提高测距精度，完成行人跟随任务。定义结构提取模块的编码器、解码器，前者将彩色图像X映射到潜在空间[19]，后者生成结构图Ms。由于结构图中存在大量无用的低维信息，S2R-DepthNet将注意力机制与结构图结合，消除低维信息对深度估计的影响，进一步建立彩色像素和深度像素的映射关系。

深度注意力模块中的编码器使用扩展残差网络[20]，用于增加编码器网络的感受野。在解码器后增加sigmoid函数，输出深度注意力MA。S2R-DepthNet的推理深度为

Tt=f(Ms×MA)

(7)

式中：f()表示深度预测模块。

RGB-D相机的深度图像存在深度伪影的固有问题，若直接使用相机深度图像测量人-机距离，会降低测距精度，进而降低移动机器人跟随的稳定性。因此本文引入单目深度估计网络，弥补相机深度图像中的无效像素点。在获得行人掩膜后，融合相机深度和推理深度，计算人-机距离。定义Pli表示相机深度图像的行人区域第i个像素点的距离，Pti表示S2R-DepthNet网络推理深度图像的行人区域第i个像素点的距离，测量人-机距离Edis的公式如下所示

(8)

(9)

其中，Pi表示融合后行人区域第i个像素的距离；n表示行人掩膜区域的深度像素的总数量。

2.1.3 偏航角计算

偏航角φ表示为行人目标偏离相机视野中间位置的角度，如图3所示。在行人跟随过程中，人-机距离Edis和偏航角φ共同决定移动机器人的运动状态。在相机的成像平面中，行人目标偏移距离所占像素PQ误差小，相机焦距f为固定值，由两者计算得到的偏航角具有较高的准确性[21，22]。

图3 偏航角计算模型

定义P是人体中心点，其坐标为(Px，Py)，O是相机光心，AQ是彩色图像宽度的一半，ζ是AO与QO之间的角度，则偏航角φ为

(10)

2.2 跟随与运动控制

2.2.1 系统模型建立

在行人跟随过程中，距离信息受光照变化、行人和移动机器人速度的影响较大，导致测量距离存在较大波动的问题。基于此，本文使用卡尔曼滤波算法对测距值进行平滑处理，使得测量距离更加趋近真实值，从而驱动移动机器人更加平稳地跟随前人。卡尔曼滤波算法将加速度建模为随机噪声，使用人-机距离、速度和加速度作为状态变量，其中人-机距离p由观测得到，速度为v，加速度为a，那么移动机器人在k时刻的状态向量

X=[pva]T

(11)

系统的状态方程和观测方程如下

(12)

式中：Xk是目标k时刻的状态向量；Zk是观测向量；Fk，k-1是状态转移矩阵；Hk是观测矩阵；Wk-1和Vk分别是状态预测过程和观测过程的高斯噪声且相互独立。状态转移矩阵F为

(13)

式中：T代表采样时间。

由于本文只有人-机距离一个观测量，故设置观测矩阵H为

(14)

2.2.2 Sage-Husa自适应滤波器

测距系统常采用卡尔曼滤波算法进行运动控制，但在滤波过程中，若噪声特性时变，易发生滤波发散和精度降低的现象。因此，本文引入Sage-Husa自适应滤波器(SHAKF)，减小时变噪声的影响，测距系统的状态空间方程见式(12)。

常规卡尔曼滤波假设系统的量测噪声具有稳定性，但在实际应用中，时变的量测噪声对滤波效果影响较大。若能够实时推理系统的量测噪声协方差矩阵Rk，就可以防止滤波发散，提高滤波精度。系统的新息公式为

(15)

由上式可得新息协方差矩阵为

(16)

(17)

式中：b为渐消因子，一般取0.9，故量测噪声协方差矩阵Rk为

(18)

联合上式，Sage-Husa自适应滤波公式为

(19)

式中：Pk-1和Pk，k-1分别是k-1时刻和k时刻的状态估计协方差；Qk是k时刻的过程噪声协方差；Kk是k时刻的卡尔曼增益。针对距离信息受光照条件和行人速度变化大的问题，Sage-Husa自适应滤波能够有效降低测距误差，提高距离测量的精度，从而增加移动机器人跟随的平顺性。

2.2.3 行人跟随控制

当移动机器人获得行人的距离和偏航角后，需要控制器驱动移动机器人，完成行人跟随任务。

PID控制器能够减小测量位置和设定位置之间的误差，使得移动机器人能够快速稳定跟随行人运动。同时，PID控制器能够避免机器人移动速度过快导致人-机碰撞危险。因此，本文选择PID控制器作为移动机器人的运动控制模块。

PID控制器以测量距离和偏航角作为输入，以线速度和角速度作为输出，驱动车轮电机跟随行人。本文设置跟随距离为0.6 m，设置偏航角为0°。本文所使用的离散式PID控制器的计算公式为

(20)

(21)

其中，v和w分别表示PID输出的线速度和角速度；ed(k) 和ew(k) 分别为k时刻的距离偏差和角度偏差；ed(k-1) 和ew(k-1) 分别为k-1时刻的距离偏差和角度偏差；KP、KI和KD分别为速度PID的比例系数、积分系数和微分系数；Kp、Ki和Kd分别为角度PID的比例系数、积分系数和微分系数。

当距离和偏航角与设定值产生误差时，PID控制器中的比例控制快速响应，积分控制用于消除过去累计的误差，微分控制用于提高系统动态性能指标，输出线速度和角速度。PID控制器能够使得移动机器人与目标人体保持合适的相对位置，达到理想的跟随效果。

3 实验结果

3.1 实验平台搭建

本实验的硬件平台由乐视LeTMC-520RGB-D相机、NVIDIA Jetson Xavier、STM32控制器和驱动电机组成，如图4所示。本文选择乐视RGB-D相机作为视觉传感器，通过深度测距使得机器人能以设定的距离跟随目标移动。将MRSD算法部署在NVIDIA Jetson Xavier上，既可以配合RGB-D相机测量距离和偏航角，又可以连接STM32控制直流电机，完成行人跟随任务。

图4 移动机器人硬件

测试数据：本文使用RGB-D相机在室内环境中采集图像数据和深度数据，采集的图像分辨率为640×480，行人和相机间的真实距离由卷尺手工测量。由于无法实时获得人体目标和移动机器人的真实距离，本文设置移动机器人和人体目标保持静止状态。每隔0.6 m测量人体目标的实际距离，并保存此位置的20张彩色图像和深度图像。由于深度相机存在距离越远误差越大的限制，本文将距离测量范围设置为0.6 m至4.8 m，并将测量范围划分为7个区间，方便后续距离误差测试。

评价指标：本文使用位移误差Derr(displacement error)评价机器人和目标之间距离的精度。位移误差由实际距离Rdis(real distance)和测量距离Edis计算可得

Derr=Rdis-Edis

(22)

3.2 销蚀实验

本文提出了一种基于MRSD测距的移动机器人跟随系统，引入MaskR-CNN实例分割算法和S2R-DepthNet单目深度估计网络，提高深度测距的精度。为评价不同网络模块的作用，在测试数据上进行销蚀实验。MRSD测距算法销蚀实验结果见表1，与目标检测算法相比，实例分割算法能大幅提高深度测距精度。在加入单目深度估计算法后，在测试数据上的平均误差相较于之前降低了40.6%，取得更好的测距效果。

表1 MRSD测距算法销蚀实验结果

3.3 人-机测距定量分析

测量人-机距离是移动机器人跟随系统领域的热点话题。传统方法通过YOLOv5算法检测行人，由于无法准确获得行人区域的深度点，导致人-机距离测量不精确。本文以深度图像为基础，结合行人检测和深度融合算法，提高人-机距离的测量精度。

为了验证本文方法的测距性能，选取了如下方法对比：YL和YLX表示YOLOv5和YOLOX[23]的目标检测框的测距方法；IS表示Mask R-CNN的实例分割的测距方法；Fu等[12]表示使用传统数字滤波处理深度图像的实例分割的测距方法；IS_S2R和IS_RMS表示S2R-DepthNet和R-MSFM[24]的深度融合的测距方法；Ours表示本文提出的测距方法。

本文使用测试数据上8个位置进行实验，采用位移误差指标衡量人-机测距的精度，如表2所示，本文算法由于引入了Mask R-CNN网络和S2R-DepthNet单目深度估计网络，抑制深度无效点对深度融合测距的影响，提高了人-机测距的精度。

表2 人-机距离误差比较

3.4 Mask R-CNN测距方法定性分析

本文引入Mask R-CNN算法，获得行人掩膜，并根据掩膜信息提取行人区域的深度像素点，计算人-机距离。因此，检测算法的输出形式决定了行人区域的表示准确度，从而影响人-机测距的精度。YOLOv5的深度测距方法误差较大，且随着距离的增加误差逐渐增大，如图5所示。本文引入的Mask R-CNN在人-机距离较近时，深度测距的精度较高。但在人-机距离较远时，受限于RGB-D相机成像素质和深度图像中的无效深度点，测距误差逐渐增大。

图5 不同算法测量距离与实际距离对比结果

受到光线的影响，YOLOv5算法误检和漏检，无法准确选中行人，影响深度测距的精度，如图6所示。Mask R-CNN由于应用了ROlAlign策略，可以实现像素级别的分割，使其能够获得较高的行人检测准确率。所以本文引入Mask R-CNN算法分割人体目标的效果较好。

图6 YOLOv5和Mask R-CNN行人检测

3.5 深度融合测距方法定性分析

深度可视化结果如图7所示，图7(a)场景中相机深度图像出现无效点，而S2R-DepthNet算法因加入了结构一致性模块，其推理图像具有深度连续性，避免出现无效深度点。图7(b)场景中深度相机由于受到室外的强烈光照和人-机距离过远的影响，其深度图像像素点的距离为0，而S2R-DepthNet推理深度图像能够获得人-机距离。但相比于相机深度图像，推理深度的距离并不精确。在室外场景中，通过深度融合算法可获得大致人-机距离信息，从而提高移动机器人跟随系统的鲁棒性。因此，本文在近距离使用Mask R-CNN的测距方法；在远距离时使用深度融合测距的方法，将单目深度估计后的深度图像替换无效深度点进行深度测距，最终提高深度测距的精度。

图7 室内室外彩色图像和深度图像可视化

3.6 Sage-Husa自适应滤波算法评估

在室外走廊环境中进行行人跟随实验并采集人-机距离，采样频率为7 Hz，历时142 s，共采集994个图像序列。由于光照强度变化，乐视深度相机采集的距离信息存在野值。以ZED相机输出的距离信息作为真实值，对比传统卡尔曼滤波算法、检测野值的卡尔曼滤波算法和SHAKF算法的测距精度。

本文将不同卡尔曼滤波算法输出距离数据与真实值对比，采用均方根误差RMSE来量化算法的性能指标，公式为

(23)

式中：n是图片帧的数量；Mi是第i个图像帧的距离估计值；Gi是第i个图像帧的距离真实值。

3种算法在行人跟随过程中第500～600帧的距离对比曲线如图8所示，第39帧距离信息发生异常，传统卡尔曼算法出现了滤波拖尾的现象，影响下一帧的距离估计值。而当过滤异常值后，SHAKF算法可以快速计算量测噪声协方差，获得更为精确的距离信息，提高移动机器人跟随系统的鲁棒性。

图8 500～600帧距离对比

3种算法在行人跟随过程中第500～600帧的距离均值误差对比曲线如图9所示。与传统卡尔曼滤波和过滤野值的卡尔曼滤波相比，SHAKF算法的均值误差更小，可以提高算法的测距精度，进而使得移动机器人能够更加鲁棒地跟随行人，对应的RMSE见表3。

表3 3种算法的均方根误差

图9 500～600帧距离均值误差对比

在本文的实验场景中，实验环境为Ubuntu18.04，软件环境为ROS机器人操作系统，硬件平台由阿克曼小车、乐视RGB-D相机和NVIDIA Jetson Xavier等硬件搭建。移动机器人首先将RGB-D相机采集的彩色图像和深度图像送入深度融合测距模块中，计算粗略的人-机距离；然后通过Sage-Husa自适应滤波算法估计当前时刻的人-机距离，减小测距误差；最后通过PID控制算法驱动移动机器人跟随前人。

移动机器人在室内室外的跟踪结果如图10所示。在室内场景中，移动机器人使用RGB-D相机采集彩色图像和深度图像，并根据深度图像获得行人位置信息，以此调整人-机相对位置。在室外场景中，移动机器人的视野范围更广，但是室外走廊光线条件较为复杂，可以验证其在复杂光照环境的行人跟随能力。移动机器人能够在室内环境稳健地跟踪行人目标，且在光照强度变化剧烈的室外环境中依然具有跟随的鲁棒性。

图10 机器人跟随

4 结束语

本文提出了一种基于MRSD测距的移动机器人跟随系统，主要包含3项工作：①引入Mask R-CNN检测行人目标获取掩膜信息，并根据行人掩膜从深度图像中提取深度信息。②引入S2R-DepthNet网络从RGB图像估计深度信息，融合相机深度图像解决无效深度点的问题，从而准确测量目标行人的距离和偏航角。③基于该距离，建立了基于SHAKF的测距模型，降低量测异常值的影响，同时设计了PID控制器，能够以设定的距离鲁棒跟随前方行人。本文在真实环境中进行了实验，实验结果表明：本文提出的基于MRSD测距的机器人跟随系统能够快速准确检测前人距离并跟随目标人物。