机器人跟随人的任务要求移动机器人检测并跟踪选定的目标人，并与目标保持适当的距离。在实际应用中，机器人必须在考虑意外障碍物的情况下不断地估计目标位置，以保持跟随人控制的稳定。本文提出将目标跟随控制与避障相结合，使机器人在避障的同时跟随目标。在本设计中，利用RGB-D摄像机获取障碍物和人体特征。深层神经网络(DNN)模型用于在环境中检测和识别用户。将改进的人工势场算法应用于机器人运动规划中，将目标人的位置信息和障碍物信息结合起来。在移动机器人上的实际实验验证了该方法的有效性。在室内环境中，机器人可以在躲避障碍物的同时稳定跟随用户。
各种各样的机器人正在出现，在日常生活中提供各种服务。在许多应用程序场景中，例如在机场或购物中心中，移动机器人需要跟随用户[1]与用户进行交互。跟随人是移动服务机器人的一个有用功能。人体检测和人体跟踪控制的方法有很多种。机器人跟踪的主要挑战是如何在复杂环境下稳定地跟踪所选择的目标。移动机器人在跟随人的过程中遇到静态或动态障碍，需要在有效跟踪用户的同时避开障碍物。
保持被跟踪者在机器人的视场(FOV)内是很重要的。通过记录历史信息来估计可能的位置，机器人可以检索目标并继续跟随用户。对于人体检测，设计师经常采用基于特征分析[2]-[4]和骨架检测策略[5]。考虑到环境因素的影响，采用超声波传感器、二维激光雷达、RGB摄像机、RGB- d摄像机等多种环境传感器进行人体检测。超声波传感器相对便宜，但它很容易受到干扰，角度分辨率差。Afghani等人[6]利用超声波传感器检测环境障碍物信息，然后对红外传感器的感知范围进行分割，判断是避开障碍物还是跟踪目标。Song等[7]使用二维激光雷达检测人体腿部特征和环境信息，计算出避障所需的角速度，然后使用共享控制结合以下运动命令。相机可以提供彩色图像，充分代表对象的特征。RGB- D相机可以提供RGB图像和深度图像。但是相机很容易受到环境光照条件的影响。Yun et al. [8]利用RGB摄像机检测目标，利用二维激光雷达检测环境障碍物，然后利用势场法(PFM)计算出最合适的路径方向。Lee等人[9]使用RGB-D摄像机对用户进行检测和识别，并通过轨迹预测和反射控制生成平滑安全的路径。
本文利用RGB-D摄像机将目标信息与环境障碍物信息相结合，设计了一种鲁棒的人类目标跟随避障系统。该系统能够在复杂环境中避开障碍物，同时稳定有效地跟踪人类目标。
图1显示了所提出的的人类目标跟踪系统。一个RGB-D摄像头被安装在机器人的前面，用来检测用户和环境中的障碍物。提出的系统结构由三个部分组成。第一部分是用户跟踪模块。第二部分是障碍物检测模块。第三部分是带避障的人体跟随运动规划与控制。这种设计的优点是提供了机器人能够在避开障碍物的同时持续跟随用户。用户跟踪模块采用[10]中的用户识别和定位算法。在本设计中，训练了两种深度学习模型用于人类分类和识别。首先采用Inception[11]作为检测器，采用three-stream暹罗架构进行具体的人员识别。这些模型允许机器人实时识别预先注册的用户。通过深度信息得到用户相对于机器人的位置。基于先前获得的用户位置和里程计信息进一步采用扩展卡尔曼滤波器(EKF)进行鲁棒跟踪。
利用RGB-D摄像机获取三维障碍物信息。将三维信息转换为二维障碍物位置信息，以向量表示。矢量作为避障控制的基础。利用人工势场(APF)实现人跟随与避障行为的结合。在APF方法中，目标产生引力，障碍物产生排斥力。在这个势场中的机器人，由于这两个势场的合力，会做出相应的运动。吸引场是根据被跟踪目标与机器人的相对位置产生的，而排斥场则是根据环境配置从多个不同的方向产生的。然而，为了实现更理想的跟随行为，人的跟随应该与避障有效结合。这种组合应该允许机器人进行充分的运动规划和避障控制。
我们采用[10]和开源[12]的Re-ID方法来构建机器人的人体识别功能。通过深度神经网络，为图像中选定的人物生成一个边界框。将边界框图像与预注册的用户图像进行相似度评分。如图2所示，将检测到的图中绿色标记的边界框作为区分用户的依据。目标位置
P
u
v
=
[
u
t
,
v
t
,
1
]
T
\bold{P}_{uv}=[u_t,v_t,1]^T
Puv=[ut,vt,1]T推导如下:
u
u
l
,
u
l
r
,
v
u
l
,
v
l
r
u_{ul},u_{lr},v_{ul},v_{lr}
uul,ulr,vul,vlr 是边界框在像素坐标中的左上和右下坐标。b为边界框内目标点高度比。
如图3所示。通过针孔相机模型将像素坐标转换为三维坐标。目标点在三维空间中的坐标
P
=
[
x
,
y
,
z
]
T
\bold{P}=[x,y,z]^T
P=[x,y,z]T通过
P
u
v
\bold{P}_{uv}
Puv计算得到：
P
=
z
K
−
1
P
u
v
(3)
\bold P=z\bold K^{-1} \bold{P}_{uv}\tag{3}
P=zK−1Puv(3)
K
∈
R
3
×
3
\bold K \in R^{3\times3}
K∈R3×3是相机的内参矩阵。由于摄像机是固定在机器人上的，所以这里不需要考虑外部参数。摄像机坐标是主坐标。
为了持续跟踪用户，我们记录下机器人在每一时刻的位置信息，以估计下一时刻的信息。机器人与用户两个连续瞬间的关系如图4所示。在机器人坐标系中，定义机器人在第k时刻的位置为
r
k
=
[
x
r
,
y
r
,
1
,
1
]
T
\bold{r}_k=[x_r,y_r,1,1]^T
rk=[xr,yr,1,1]T，从
r
k
\bold{r}_k
rk到
r
k
+
1
\bold{r}_{k+1}
rk+1的传递矩阵为
W
k
∈
R
4
×
4
\bold W_k \in R^{4\times4}
Wk∈R4×4
△
θ
,
l
x
,
l
y
\triangle \theta,l_x,l_y
△θ,lx,ly是机器人的
x
,
y
x,y
x,y在下一时刻的旋转和平移，如下：
v
r
,
ω
\bold v_r,\bold\omega
vr,ω是机器人的速度，
△
t
△t
△t是任意两帧的时间间隔。下一时刻的位置为
h
k
+
1
=
[
x
h
(
k
+
1
)
,
y
h
(
k
+
1
)
,
1
,
1
]
\bold h_{k+1}=[x^{(k+1)}_h,y^{(k+1)}_h,1,1]
hk+1=[xh(k+1),yh(k+1),1,1]，公式如下：
v
h
x
,
v
h
y
\bold v_{hx},\bold v_{hy}
vhx,vhy是用户在x、y方向的速度，
△
t
△t
△t是每一刻之间的时间间隔，用户在k +1时刻与机器人的相对位置
h
^
k
+
1
=
[
x
^
h
(
k
+
1
)
,
y
^
h
(
k
+
1
)
,
1
,
1
]
\bold{\hat{h}}_{k+1}=[\hat x^{(k+1)}_h,\hat y^{(k+1)}_h,1,1]
h^k+1=[x^h(k+1),y^h(k+1),1,1]
h ^ k + 1 = h k + 1 W k − 1 (11) \bold{\hat{h}}_{k+1} = \bold h_{k+1} \bold W_k^{-1}\tag{11} h^k+1=hk+1Wk−1(11)
利用上述预测模型，我们可以利用EKF估计目标位置。根据第k时刻人在机器人坐标中的位置和速度，定义状态
x
k
x_k
xk为
通过结合用户预测模型(11)和机器人运动
u
k
=
[
v
r
,
ω
r
]
T
\bold u_k=[v_r,\omega_r]^T
uk=[vr,ωr]T,状态方程表示如下
w
k
w_k
wk是过程噪声
机器人坐标中第k个时刻观察到的用户位置记为
y
k
\bold y_k
yk。观测方程表示为
v
k
v_k
vk是测量噪声
通过不断更新和修正EKF的状态方程和观测方程，可以估计出更可能的目标位置。该机器人可以通过提出的估计方案将用户从其他人中分离出来。
如果机器人想要避开障碍物，机器人需要知道环境信息。通过深度相机，我们得到了一个深度图像与相机的FOV大小。为了方便运动规划,我们将3 d信息转化为一个二维S = (d,ϕ)的信息,这通过ROS自带函数可以实现[11]。原理如图5所示。公式如下
r
→
i
j
{\mathop r \limits^\rightarrow}_{ij}
r→ij 是带有方位角
φ
i
\varphi_i
φi和极角
θ
j
\theta_j
θj的障碍物向量。然后简化障碍物信息
d
s
d_s
ds，设计如下[14]:
其中
D
s
a
f
e
D_{safe}
Dsafe为给定的机器人与障碍物之间的安全距离，
d
i
d_i
di为小于障碍物向量
D
s
a
f
e
D_{safe}
Dsafe的距离，
a
a
a为小于1的值，用于增大较近距离时障碍物的影响。
当机器人跟随用户时，用户路径附近可能会出现障碍物。为了实时躲避障碍物并保持跟踪，机器人需要进行相应的处理。为此，我们选择APF算法作为基础[15]。与势函数对应的引力和斥力函数可以表示为：
k
a
k_a
ka是吸引常数，
k
r
k_r
kr是排斥常数。
x
x
x是机器人当前位置，
x
g
x_g
xg是用户当前位置。
d
o
d_o
do为障碍物的碰撞半径。
在这个设计中，过多的障碍物会导致排斥力场过大，导致机器人移动缓慢，远离用户。因此，为了更好的满足跟随人的目的，我们添加了用户信息来改进原来的公式。权值参数
G
f
G_f
Gf设计如下:
其中，
θ
h
\theta_h
θh为用户相对于机器人的角度，而
α
\alpha
α的取值为正值，目的是为了增加用户偏离机器人时的影响。
G
f
G_f
Gf是机器人当前时刻朝着正确方向前进的指示器，其最小值为0。通过
G
f
G_f
Gf调整虚拟力的大小，使避障行为与跟随行为相结合，使机器人在环境中行走更加灵活。虚力
F
F
F表示为：
当机器人由于障碍物排斥力而偏离用户时，
G
f
G_f
Gf会增加，导致吸引力增加，排斥力减少。环境的影响将会减少。机器人减少了不必要的避障行为，并能快速回到用户身后继续跟随。通常机器人在跟随用户时保持安全距离，并使用线性加减速。线速度
v
f
v_f
vf设计如下：
其中，
d
h
d_h
dh为用户相对于机器人的距离。
v
c
v_c
vc是机器人的线速度。
D
s
s
D_{ss}
Dss为开始线性变化的距离，
D
s
e
D_{se}
Dse为机器人与用户之间的安全距离。角速度
ω
f
ω_f
ωf设计如下:
式中，
θ
h
\theta_h
θh为用户相对于机器人的角度。
ω
c
\omega_c
ωc是机器人的角速度。
θ
s
\theta_s
θs是开始线性变化的角度。本文采用两种独立的驱动轮设计。两个轮子的速度由下式计算:
式中
v
l
f
v_{lf}
vlf为左轮速度，
v
r
f
v_{rf}
vrf为右轮速度，
L
L
L为两轮之间的距离。
为了验证所提出的方法，我们使用一个实验室移动机器人进行了两个实验。机器人前端安装了一个RGB-D摄像头，用于人跟随和避障。几个有趣的实验结果表明，机器人可以连续跟踪用户，同时避免意外障碍。
本实验的目的是验证Human-Following行为的有效性。实验场地为约6m x 2.4m的矩形区域。在一个走廊。用户走在走廊里，机器人跟在后面。通过在移动机器人上实现的激光雷达SLAM记录轨迹。
如图6 (a) - ©所示，机器人通过预先注册的用户数据识别相机FOV中的特定用户。机器人注视着相似性得分最高的那个人，开始跟着他走。如图6 (d) - (f)所示，机器人基于用户信息稳定跟随用户。在图6 (g)中，用户两侧出现了两个来自不同方向的行人。如图6 (h)-(i)所示，机器人利用用户的历史信息通过EKF预测下一时刻的距离和角度，然后选择最接近预测结果的人跟随。从图中可以看出，机器人的运动完全依赖于用户。图7描绘了记录的机器人轨迹以及用户与机器人之间的关系。红色实线是机器人的轨迹。红色的三角形是记录下来的机器人姿势，黄色的笑脸是用户。蓝色的菱形和绿色的椭圆形表示行人的轨迹。
这个实验的目的是为了验证机器人在避开意想不到的障碍物时是否会继续跟随用户。在实验中，用户在环境中行走，机器人在后面跟着。有其他行人靠近用户，但机器人受到其他人的影响，一直跟随同一个人。当用户通过障碍物时，机器人会避开障碍物并返回继续有效地跟随用户。
实验结果如图8所示。
如图8 (a)所示，用户面前有几个障碍。如图8 (b)-(f)所示，机器人在跟随用户的同时遇到障碍物并避开障碍物。在避开障碍物后，机器人通过本文提出的人跟随避障设计成功地回到用户的后面，并一直跟随用户。在多人同时出现的情况下，如图8 (g)-(k)所示，机器人利用EKF预测用户应该在哪里，然后避开其他人继续跟随用户。如图8 (l)所示，机器人会停在距离用户安全的距离。用户与机器人的关系如图9所示。红线是机器人的轨迹。红色的三角形是机器人，黄色的笑脸是用户。黑色的矩形是障碍物。蓝色的菱形是人。
如图10所示，在k =5和k =8时，机器人由于障碍物的排斥力而产生避障行为。由于用户从机器人的FOV中消失，机器人通过用户预测位置产生引力抗衡障碍物的排斥力。利用EKF预测的用户位置来产生吸引力。
本文提出了一种基于RGB-D摄像机的人体跟踪和避障运动控制系统。使用RGB-D摄像机通过深度神经网络来对用户进行检测，利用深度图像估计用户位置和环境障碍。EKF用于预测用户的位置，防止用户在多人同时出现时离开机器人的FOV。通过设计的权重参数，将避障与人类目标跟随相结合，实现环境中的跟踪行为。实验结果表明，该机器人能够在复杂环境下稳定地跟随用户。然而，在公共区域，可能同时有多个人，用户的检测将变得更加具有挑战性。在未来，我们将加强人体检测，以实现更可靠的人体跟踪。
