CN103268616A - 多特征多传感器的移动机器人运动人体跟踪方法 - Google Patents

Abstract

本发明属于计算机视觉与智能机器人的交叉领域，公开了一种新的多特征多传感器的移动机器人运动人体跟踪方法。包括：步骤一，利用RFID系统对在其附近携带无源标签的人体进行粗定位；步骤二，利用基于头肩特征的自适应模板匹配算法对图像中人体进行初定位；步骤三，利用基于多特征的mean-shift算法对图像中人体精确定位；步骤四，利用扩展卡尔曼滤波算法预测人体运动状态；步骤五，采用双层协作定位机制对获取到的目标位置信息进行筛选；步骤六，利用机器人跟随控制算法控制机器人跟随人体运动。本发明可实现对不同位姿人体的跟踪，解决了目标突然转弯及被遮挡对跟踪的影响问题，使机器人能够精确、稳定、连续地跟踪运动人体。

Description

多特征多传感器的移动机器人运动人体跟踪方法

技术领域

本发明属于计算机视觉与智能机器人的交叉领域，涉及一种机器人目标跟踪技术，尤其涉及一种多特征多传感器的移动机器人目标跟踪方法。 

背景技术

运动人体跟踪技术是机器视觉领域的重要问题之一，它广泛应用于智能交通、人体行为识别、智能监控系统等领域。虽然相关研究己有数十年的历史，但这个问题目前仍是计算机视觉、模式识别，特别是机器人领域的一个研究热点。近几年，随着运动人体跟踪技术在服务机器人、反恐机器人等领域的广泛应用和发展，它逐渐成为计算机视觉领域中最为前沿的研究方向之一。 

均值漂移(mean-shift)算法因其实时性较好，且易与其他算法集成，被广泛应用于目标跟踪算法中。该算法是一种基于无参数密度的估计方法，在1975年被Fukunag首次提出。1995年，Cheng将其发展并引入到计算机视觉领域。Comaniciu在他们的基础上首次将mean-shift算法应用到目标跟踪系统中，采用颜色直方图描述目标特征实现目标识别与定位。但是，经典mean-shift算法中颜色直方图对目标特征描述能力不强、跟踪窗口及模型缺乏必要的更新，当背景与目标颜色相近时会导致误跟踪；当目标被遮挡时，基于mean-shift的跟踪算法往往会出现定位偏差较大或跟丢的现象。针对上述问题，相关文献提出了许多改进的mean-shift算法，但无论采用哪种算法，单一的视觉特征均无法取得良好的跟踪效果，因此通过多种互补特征跟踪目标逐渐被许多学者所采用。 

近年来，许多学者通过提取多种目标描述特征或融合多传感器信息的方法提高复杂环境下机器人目标跟踪精度。立体视觉系统通过恢复场景三维信息为系统提供目标的有效位置，同时受光照等环境因素影响小，因此广泛应用于一些复杂的跟踪算法中。R.Cipolla与M.Yamamoto首先提出利用立体视觉实现目标跟踪，但该算法需要处理大量信息从而影响了系统的实时处理能力。为了在保证跟踪精度的同时满足系统的实时性要求，D.Grest和R.Koch采用基于粒子滤波算法并结合深度信息实现对运动人体实时跟踪。在利用单一传感器实现目标跟踪的基础上，许多研究采用多传感器互补信息以提高跟踪精度和系统鲁棒性。J.Cui融合激光和视觉信息，提取人体腿部几何特征实现机器人准确跟踪运动目标。 

近几年，随着无线传感器网络的发展，射频识别技术(Radio Frequency Identification，简称RFID)逐渐显示出了巨大的应用潜力和研究价值。RFID具有非视距数据通信、标签ID唯 一等特点，它在提供室内定位的同时，还可以辅助服务机器人进行物体识别。但很少有学者将RFID技术引入到目标跟踪系统中，特别是利用RFID系统与视觉系统的互补信息实现机器人目标跟踪。 

对于服务移动机器人，针对复杂的环境，设计一个连续稳定的目标跟踪控制算法是必要的。Ouadah.N根据机器人运动特性与机械结构，采用PID控制器调整机器人运动状态以跟随人体连续稳定运动。Thombre结合了机器人运动特性与跟踪目标的运动状态设计了一种智能调速算法，但由于没有考虑人机位置关系，很难取得良好的跟踪效果。因此，如何控制机器人跟踪目标运动也是目前亟待解决的一大热点问题。 

发明内容

为了克服上述跟踪运动人体方法中存在的问题，本发明提供了一种多特征多传感器的移动机器人运动人体跟踪方法。 

本发明采用的技术方案如下： 

利用RFID系统对携带标签的目标人体进行粗定位，并确定视差图中的感兴趣区域(ROI)，有效地结合了RFID粗定位结果与图像信息。在ROI内进行精确定位，提出基于头肩模型的自适应模板匹配算法，以实现跟踪不同位姿人体。提出多特征自适应均值漂移算法的目标跟踪，在传统基于核函数mean-shift算法的基础上，利用头肩特征定位结果获得人体躯干侧影进而改进核函数，消除背景颜色及纹理对目标模型的影响。同时，利用头肩尺寸调整跟踪窗口大小，降低了由于目标尺寸变化而导致跟踪失败的可能性。在跟踪过程中，多特征互补信息提高了算法对目标特征的描述能力。根据选择性更新策略，自适应更新目标模型既有助于消除跟踪过程中的累积误差，又避免了过度更新模型造成的模型偏移。提出双层协作定位机制，当目标突然转弯以致消失在相机视野中时，利用RFID系统粗定位结果为机器人运动提供导向，有助于机器人快速跟踪到目标。当目标人体被遮挡或正常跟踪时，系统采用EKF(Extend Kalman Filter，扩展卡尔曼滤波器)预测目标运动状态，解决了目标短暂遮挡情况下的跟踪问题。同时，设计基于模糊规则的智能调速控制器，根据人机运动状态，通过模糊化、模糊推理及解模糊化，实时自动调整基准线速度和转弯增益，并结合智能调速算法，保证机器人连续稳定地跟随目标运动。 

多特征多传感器的移动机器人运动人体跟踪方法，其特征在于包括以下步骤： 

步骤一，利用RFID系统对在其附近携带无源标签的人体进行粗定位。如果检测到了标签，转下一步；否则，转步骤四。 

首先在室内环境中建立RFID传感器模型，然后采用贝叶斯定理结合传感器模型，计算RFID系统前方区域每个栅格中标签存在的概率，将概率高于阈值的区域作为粗定位结果，从 而实现对携带标签的人体的粗定位。具体方法如下： 

(1)建立RFID传感器模型 

首先，将天线固定在室内某一位置，同时携带无源标签的机器人在天线前方一固定区域内任意运动，记录每一采样间隔内天线的检测数据与机器人位置。通过对实验结果的统计学分析，确定单根天线的检测区域，然后将两根天线并排装载到移动机器人上，根据单根天线的检测区域划分两个天线的检测区域。 

(2)进行目标粗定位 

将机器人天线前方的固定区域划分成大小相等的栅格区域，根据利用贝叶斯公式计算每个栅格被检测到标签存在的概率P_t(E|O)： 

P_t(E|O)∝P(O|E)P_t-1(E)                       (1) 

式中，O表示标签被检测到事件，E表示标签存在事件，P(E)表示标签存在的概率。 

初始化栅格地图中每个栅格的P(E)，在跟踪过程中，机器人装载RFID系统以检测出现在机器人前方不同位置携带标签的人体。根据RFID天线新的观测信息，利用式(1)对栅格地图中每个栅格的概率值P_t(E|O)进行迭代更新。最后将概率高于阈值的区域作为粗定位结果。 

步骤二，利用基于头肩特征的自适应模板匹配算法对图像中人体进行初定位。如果检测到了人体，转下一步；否则，转步骤五。 

本发明提出一种模板匹配算法，该算法从二值图像中提取头肩模型，利用垂直投影直方图表征人体头肩的生理结构特征，根据提取的头肩模型及模板尺寸自适应地对二者进行归一化，以达到最佳匹配效果，并利用欧氏距离度量二者相似度，将相似度高于阈值的区域认定为人体位置。方法如下： 

(1)设投影直方图轮廓曲线分别为x＝f_hs(y)，x^*＝f_t(y^*)，其中f_hs(y)为从图像中提取的头肩模型的垂直投影直方图轮廓，H为当前头肩模型高度，f_t(y^*)为模板头肩模型的垂直投影直方图轮廓，H_t为模板头肩高度。为了得到合适的模板，根据头肩模型高度对模板直方图沿y轴方向进行压缩。 

(2)对提取的头肩模型及模板直方图按下式进行归一化： 

$a_y=\frac{1}{\mathrm{\Σhs}}\underset{x=0}{\overset{f_{\mathrm{hs}}\left(y\right)}{\mathrm{\Σ}}}I(x,y)$

$b_{y_t^{*}}=\frac{1}{\mathrm{\Σtemplate}}\underset{x^{*}=0}{\overset{f_t\left(y_t^{*}\right)}{\mathrm{\Σ}}}I(x_t^{*},y_t^{*})$

式中，

I(x,y)为当前头肩模型直方图面积，即像素总数，a_y为头肩模型不同位置的宽度，即某一y值上像素总数；

为压缩后模板面积，即像素总数，

为模板不同位置的宽度，即某一

值上像素总数。 

归一化后的头肩模型垂直投影集合为A＝{a_y|y＝0,1,...,H}，归一化后的模板垂直投影集合为 $B=\left\{b_{y_t^{*}}\right|y_t^{*}=\mathrm{0,1},...,H\}.$

(3)利用欧氏距离，度量集合A、B之间的相似性，以判断从候选区域中提取的目标是否为人体。 

步骤三，利用基于多特征的mean-shift算法对图像中人体精确定位。 

本发明提出一种基于多特征的均值漂移算法。该算法首先利用人体躯干侧影改进模型核函数。同时，根据头肩尺寸调整跟踪窗口大小。采用目标颜色特征与纹理特征建立目标联合相似度函数，通过迭代计算以寻求与模型相似度最大的位置。根据选择性模型更新策略，自适应地更新目标模型中匹配贡献度低的特征分量。具体方法如下： 

(1)建立目标颜色特征模型 

将RGB图像转换到HSV颜色空间以提取色度分量，将目标的色度特征离散分布为m_c个区间，假定目标所在区域的点集为{x_i}，i＝1,2,...,n，区域中心点为x₀，则对于中心位于x₀的目标颜色特征描述模型为： 

${\hat{q}}_u=C_{q_u}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{k}_{\mathrm{new}}\left({\left|\right|\frac{x_i-x_0}{h}\left|\right|}^2\right)\mathrm{\δ}[b\left(x_i\right)-u]$

式中，为使

的归一化常数，h为跟踪窗口宽度，δ(x)为Delta函数，其作用是判断x_i的灰度值是否属于第u个量化等级。 

由于周围环境的影响，相对于目标外围区域像素，目标中心区域像素更具有辨识价值，因此选择Epanechnikov核函数为目标区域内每个像素分配权值，即赋予接近中心的像素一个较大权值、远离中心的像素一个较小的权值。为了进一步消除远离目标外围区域像素点对目标模型的影响，利用从视差图中基于头肩特征提取的人体上半身躯干侧影对核函数按下式进行改造： 

$k_{\mathrm{new}}\left({\left|\right|\frac{x_i-x_0}{h}\left|\right|}^2\right)=k\left({\left|\right|\frac{x_i-x_0}{h}\left|\right|}^2\right)\×B_{\mathrm{hs}}\left(x_i\right)$

$B_{\mathrm{hs}}\left(x_i\right)=\left\{\begin{array}{ccc}1& \mathrm{if}& x_i\∈\mathrm{headshoulder}\\ 0& \mathrm{if}& x_i\∉\mathrm{headshoulder}\end{array}\right.$

式中，

为改造后的核函数，

为Epanechnikov核函数，B_hs(x_i)为二值化掩码函数。 

改造后背景图像处核函数值为零，人体躯干侧影处其值与Epanechnikov核函数一致。相较于只采用Epanechnikov核函数，新的核函数进一步消除了目标外围像素对目标模型的影响，加强了对目标的特征描述。 

(2)建立目标纹理特征模型 

纹理特征反映图像区域中像素灰度级的空间分布的属性，它是图像区域的一种重要属性，是人们区分不同区域的重要线索之一。局部二进制模式(Local binary patterns,LBP)作为一种有效的纹理描述算子，由于其对图像局部纹理特征的卓越描绘能力而获得了广泛应用。采用一种扩展的LBP模式，称之为均匀纹理模式，定义为： 

式中，gray_c表示中心像素点的灰度值，α和β表示目标像素的局部区域，A为邻域内像素的数目；gray_α表示以中心点为圆心，半径为β的邻域内均匀分布的α像素点的灰度值。函数

和Q(LBP_A,β)的定义为： 

函数Q(LBP_A,β)返回的是LBP_A,β模式下，二进制位串中从“0”到“1”或者从“1”到“0”的跳变数。对于均匀纹理模式，这种跳变不超过两个。在随后的直方图计算过程中，只为均匀纹理模式分配单独的直方图收集箱(bin)，这样对于8邻域像素的LBP算子仅存在9种特征模式。统计目标像素点纹理特征模式，进而获得目标的纹理特征直方图。为了更好地消除目标外围区域像素点纹理的影响，同样利用人体躯干侧影改造目标纹理模型。假定目标的纹理特征空间中特征值v分布在m_t个区间，则对于中心位于x₀的目标纹理特征分布模型为： 

${\hat{q}}_v=C_{q_v}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{k}_{\mathrm{new}}\left({\left|\right|\frac{x_i-x_0}{h}\left|\right|}^2\right)\mathrm{\δ}[t\left(x_i\right)-v]$

式中，

为目标纹理特征模型，

为目标纹理直方图模型标准化常量，t(x_i)为映射函数，δ[t(x_i)-v]的作用是判断像素点x_i的纹理特征是否属于第v个量化等级，i＝2,3,4,...,t,...。 

(3)由头肩特征定位结果确定中心为y₀的候选区域，建立候选区域标颜色、纹理模型 

${\hat{p}}_u\left(y\right)=C_{h_u}\underset{i=1}{\overset{n_h}{\mathrm{\Σ}}}{k}_{\mathrm{new}}\left({\left|\right|\frac{y-x_i}{h}\left|\right|}^2\right)\mathrm{\δ}[b\left(x_i^{*}\right)-u]$

${\hat{p}}_v\left(y\right)=C_{h_v}\underset{i=1}{\overset{n_h}{\mathrm{\Σ}}}{k}_{\mathrm{new}}\left({\left|\right|\frac{y-x_i}{h}\left|\right|}^2\right)\mathrm{\δ}[t\left(x_i^{*}\right)-v]$

式中，

为颜色特征直方图模型中的标准化常量，为纹理特征直方图模型中的标准化常量。 

利用头肩尺寸按下式更新跟踪窗口大小： 

$\left\{\begin{array}{c}W=W_{\mathrm{hs}}\\ H=H_{\mathrm{hs}}\×1.5\end{array}\right.$

式中，W、H分别为跟踪窗口大小，W_hs、H_hs分别为人体头肩宽度和高度。 

(4)建立联合相似度函数 

为了更好地结合目标颜色及纹理信息，本发明采用颜色纹理联合相似度函数，以寻求与目标模型相似度最大的位置，函数表达式为： 

$\widehat{\mathrm{\ρ}}\left(y\right)={\widehat{\mathrm{\ρ}}}_u{\widehat{\mathrm{\ρ}}}_v=\underset{u=1}{\overset{m_c}{\mathrm{\Σ}}}\sqrt{{\hat{p}}_u\left(y\right){\hat{q}}_u}\underset{v=1}{\overset{m_t}{\mathrm{\Σ}}}\sqrt{p_v\left(y\right)q_v}$

式中，为颜色纹理联合相似度函数，

分别为颜色、纹理相似度函数。将头肩模型定位结果中心y₀作为当前帧的初始候选区域中心，将上式在y₀处按泰勒级数展开，近似为： 

$\widehat{\mathrm{\ρ}}\left(y\right)\≈\frac{C_h}{2}\underset{i=1}{\overset{n_h}{\mathrm{\Σ}}}{w}_i\left({\left|\right|\frac{y-x_i}{h}\left|\right|}^2\right)---\left(2\right)$

$w_i=\underset{u=1}{\overset{m_c}{\mathrm{\Σ}}}\sqrt{{\hat{p}}_u\left(y\right){\hat{q}}_u}\underset{v=1}{\overset{m_t}{\mathrm{\Σ}}}\sqrt{\frac{{\hat{q}}_v}{{\hat{p}}_v\left(y_0\right)}}\mathrm{\δ}(t\left(x_i\right)-v)+\underset{v=1}{\overset{m_t}{\mathrm{\Σ}}}\sqrt{{\hat{p}}_v\left(y\right)q_v}\underset{u=1}{\overset{m_c}{\mathrm{\Σ}}}\sqrt{\frac{{\hat{q}}_u}{{\hat{p}}_u\left(y_0\right)}}\mathrm{\δ}(c\left(x_i\right)-u)---\left(3\right)$

式中，w_i为跟踪窗口内各像素点权值，c_h为标准化常量，m_c为目标的颜色特征分量数目，n_h为跟踪窗口内像素点数量，u为颜色特征空间中的特征分量，

为候选区域颜色特征模 型。 

为了寻求目标与候选区域相似度最大化，将(2)式两边对y求偏导，并令其为零，从而得到每次迭代重新定位的候选区域中心y₁： 

$y_1=\frac{\underset{i=1}{\overset{n_h}{\mathrm{\Σ}}}{x}_i{w}_{i}g\left({\left|\right|\frac{{\hat{y}}_0-x_i}{h}\left|\right|}^2\right)}{\underset{i=1}{\overset{n_h}{\mathrm{\Σ}}}{w}_{i}g\left({\left|\right|\frac{{\hat{y}}_0-x_i}{h}\left|\right|}^2\right)}$

式中，g(x)＝-k′(x)。 

(5)采用式(3)跟踪窗口内各像素点权值w_i。 

(6)计算候选区域质心y₁。若||y₁-y₀||＜ε则进行下一步；否则，将y₁作为新的候选区域中心，返回步骤(5)，以寻找满足条件的位置。 

(7)采用一种选择性模型更新策略，即对匹配贡献度高的特征分量不进行更新，仅更新贡献度低的特征分量。匹配贡献度函数为： 

${\mathrm{DOC}}_{\mathrm{\λ}}=\frac{\sqrt{{\hat{p}}_{\mathrm{\λ}}\left(y\right){\hat{q}}_{\mathrm{\λ}}}}{\mathrm{\Σ}\sqrt{{\hat{p}}_{\mathrm{\λ}}\left(y\right){\hat{q}}_{\mathrm{\λ}}}},\mathrm{\λ}=u,v$

利用颜色、纹理相似度系数与

决定模型更新时机。当二者相似度均高于阈值时，表明跟踪效果良好，不进行模型更新。当其中某种特征相似度系数较小时，表明跟踪效果受到影响，若相似性系数在[d₁,d₂]范围内，根据匹配贡献度对该特征空间中的特征分量进行排序，前γ个贡献度高的特征分量表示当前模型与目标模型较为匹配，所以不进行更新，只选择后m-γ个匹配贡献度较差特征分量进行更新，避免了因为过度更新而造成的模型偏差。若两种特征相似度系数均低于d₁，表明目标被遮挡，不更新目标模型，系统将利用EKF先验估计跟踪目标。具体规则为： 

$\left\{\begin{array}{cc}{\hat{q}}_{{\mathrm{\λ}}_j}^{t+1}={\hat{q}}_{{\mathrm{\λ}}_j}^{t}r+{\hat{p}}_{{\mathrm{\λ}}_j}^t\left(y\right)(1-r),& j>\mathrm{\γ},\widehat{\mathrm{\ρ}}\∈[d_1,d_2]\\ {\hat{q}}_{{\mathrm{\λ}}_j}^{t+1}={\hat{q}}_{{\mathrm{\λ}}_j}^t,& j\≤\mathrm{\γ},\widehat{\mathrm{\ρ}}\∈[d_1,d_2]\\ {\hat{q}}^{t+1}={\hat{q}}^t,& \widehat{\mathrm{\ρ}}\∉[d_1,d_2]\end{array}\right.$

式中，r为更新因子。 

步骤四，利用扩展卡尔曼滤波算法预测人体运动状态。 

EKF作为一种最优状态估计算法通过提取目标运动特征，不但能够利用其后验估计确定人体3D位置，而且当目标被其它人体短暂遮挡时，还能利用其先验估计预测目标运动状态提高跟踪系统鲁棒性。本发明利用EKF这一优势，有效确定目标位置，为机器人跟随人体运动 提供有效的保障。具体方法如下： 

在机器人坐标系下EKF的状态向量如下： 

x_r＝[X_r Y_r Z_r X_r Y_r] 

式中，[X_r,Y_r,Z_r]与[X_r,Y_r]分别是人在机器人坐标系的3D位置和水平移动速度。根据机器人的运动模型，机器人坐标系下的状态方程可被描述如下： 

$x_r^{t+1}=f_t(x_t,{\mathrm{control}}_t)+R_t{w}_t$

$f_t(x_r^t,{\mathrm{control}}_t)=\left[\begin{array}{c}(X_r^t+\mathrm{\Δt}{X}_r^t-\mathrm{\Δ}{X}_r)\cos \mathrm{\Δ\θ}+(Y_r^t+\mathrm{\Δt}{Y}_r^t-\mathrm{\Δ}{Y}_r)\sin \mathrm{\Δ\θ}\\ -(X_r^t+\mathrm{\Δt}{X}_r^t-\mathrm{\Δ}{X}_r)\sin \mathrm{\Δ\θ}+(Y_r^t+\mathrm{\Δt}{Y}_r^t-\mathrm{\Δ}{Y}_r)\cos \mathrm{\Δ\θ}\\ Z_r^t\\ X_r^t\cos \mathrm{\Δ\θ}+Y_r^t\sin \mathrm{\Δ\θ}-v\\ -X_r^t\sin \mathrm{\Δ\θ}+Y_r^t\cos \mathrm{\Δ\θ}\end{array}\right]$

式中，控制向量为control_t＝[v_l,v_r]，ΔX_r，ΔY_r分别代表t时刻人在机器人坐标系X轴和Y轴方向上的位移，Δθ为转动的角度,w_t是过程噪声。 

传感器观测到的人体3D位置为

观测量为相机观测到的人体3D位置，观测方程如下： 

$y_r^t=H_t{x}_r^t=p_t$

式中，p_t为观测噪声， $y_r^t=\left[\begin{array}{c}{X}_r^t\\ Y_r^t\\ Z_r^t\end{array}\right].$

步骤五，采用双层协作定位机制对获取到的目标位置信息进行筛选。 

利用双层协作机制将RFID对标签的粗定位结果与立体视觉获取的目标信息进行筛选、分析和处理，实现外层(RFID粗定位)与内层(基于头肩、颜色、纹理及运动特征的精确定位)协作定位，以提高复杂环境下机器人目标跟踪的鲁棒性。定位结果为： 

$q=q_{\mathrm{stereo}}{u}_v+q_{\mathrm{rfid}}{u}_{\mathrm{rfid}}+q_p\left(\stackrel{\‾}{u_v+u_{\mathrm{rfid}}}\right)$

式中，q为目标定位结果，q_stereo为立体视觉内层定位结果，q_rfid为RFID系统外层粗定位结果，q_p为EKF先验估计结果。 

正常跟踪过程中，外层定位结果协助内层精确定位目标，即u_v＝1,u_rifd＝0，机器人利用内层定位结果跟踪目标。当目标突然转弯时，RFID系统仍能检测到标签信号，内层定位失败，即u_v＝0,u_rfid＝1，目标定位结果由外层粗定位确定，为机器人跟随目标提供导向，使机器人快速跟踪目标。当目标被遮挡时，RFID系统无法接收来自标签发出的信号，u_v＝0,u_rfid＝0，利用EKF的先验估计作为定位结果。 

步骤六，利用机器人跟随控制算法控制机器人跟随人体运动。 

首先设计两个模糊控制器，分别调整机器人运动的基准线速度及转弯增益。基准线速度模糊控制器中，将人机垂直距离，即当前时刻人体在机器人坐标系的垂直坐标X_r，与人机垂直距离变化率v_px为控制器输入，基准线速度v为该控制器输出；转弯增益模糊控制器中，将人机水平距离即当前时刻人体在机器人坐标系的水平坐标Y_r与人机水平距离变化率v_py作为控制器输入，输出为转弯增益K，通过模糊化、模糊推理及解模糊化，得到机器人运动的基准线速度与转弯半径。在此基础上，结合智能调速算法，计算得到机器人左、右轮速度，从而控制机器人跟随目标运动。具体方法如下： 

(1)确定隶属函数、模糊集进行模糊化 

直线函数能快速调整较大的人机距离与人机距离变化率，曲线型函数变化平滑，有利于控制的平稳性。当人机距离和人机距离变化率较大时，采用三角形隶属函数，保证机器人跟随要求；当人机距离在安全范围内时，主要考虑平稳性，采用高斯型隶属函数。 

模糊化的作用是将输入的精确量转化成模糊化量,取X_r的模糊子集，在其论域中均划分为5个集合：“最近(VN)”、“近(N)”、“正常(ZE)”、“远(F)”、“最远(VF)”；取v_px的模糊子集，在其论域中均划分为5个集合：“负大(NB)”、“负小(NS)”、“正常(ZE)”、“正小(PS)”、“正大(PB)”；取v的模糊子集，在其论域中均划分为5个集合：“很小(VL)”、“小(L)”、“中(M)”、“大(H)”、“很大(VH)”；取Y_r、v_py的模糊子集，在其论域中均划分为5个集合：“负大(NB)”、“负小(NS)”、“正常(ZE)”、“正小(PS)”、“正大(PB)”；取K的模糊子集，在其论域中均划分为5个集合：“很小(VL)”、“小(L)”、“中(M)”、“大(H)”、“很大(VH)”。通过试验得到参数的有效论域：X_r∈[0,3]，v_px∈[-1,1]，v∈[0,200]，Y_r∈[-1,1]，v_py∈[-1,1]，K∈[0,3]。 

(2)建立控制规则 

控制规则用模糊条件语句来描述为： 

R_1i：if Q₁=A_i and Q₂＝B_i,then v=C_i； 

R_2i：if Q₃=D_i and Q₄＝E_i,then K=F_i。 

R_1i为基准线速度模糊控制器控制规则，R_2i为转弯增益模糊控制规，Q₁表示人机垂直距离语言变量，Q₂表示人机垂直距离变化率的语言变量。Q₃表示人机水平距离语言变量，Q₄表示人机水平距离变化率的语言变量，v和K分别表示基准线速度和转弯增益语言变量。它们的语言值在相应论域中的模糊子集分别为A_i、B_i、C_i、D_i、E_i、F_i。根据基准线速度模糊 控制器调整机器人线速度，当人机垂直距离大于安全距离，为快速跟随目标，系统将增大机器人运动线速度；当人机垂直距离小于安全距离，减小线速度以保证人机之间的安全距离；当人机垂直距离过小，机器人停止运动以防人机碰撞。根据转弯增益模糊控制器调整转向增益，当人机水平距离过大时，转向增益增大，转弯半径减小，通过快速调整转向以保证目标在视野中心位置。 

(3)智能调速 

根据两个模糊控制器的输出v和K，并结合机器人运动模型，机器人左、右轮速度分别计算如下： 

$v_l=v(1-2d{\mathrm{KY}}_r/(X_r^2+Y_r^2))$

$v_r=v(1+2d{\mathrm{KY}}_r/(X_r^2+Y_r^2))$

式中，2d为机器人两轮间距。 

本发明的有益效果是：针对现有算法跟踪效果易受环境影响的问题，提出基于头肩特征的自适应模板匹配算法，实现对不同位姿人体跟踪。同时，提出基于多特征的mean-shift算法，该算法既能有效解决传统算法中目标描述不完整的问题，又能自动更新目标模型及跟踪窗口，提高了算法的跟踪精度。提出双层协作定位机制，有效地解决了跟踪过程中可能出现的目标突然转弯以及目标被遮挡的问题，并合理地利用了RFID系统与立体视觉系统目标定位结果，明显地提高了跟踪方法的鲁棒性。 

附图说明

图1为本发明所涉及硬件结构示意图； 

图2为本发明所涉及的方法流程图； 

图3为基于头肩特征的自适应模板匹配过程； 

图4为应用本发明中基于头肩特征的自适应模板匹配算法人体检测实验结果； 

图5为应用本发明中基于多特征的mean-shift算法人体检测实验结果； 

图6为应用本发明所涉及的机器人跟踪运动人体实验情况：（a）为运动人体跟踪实验过程，（b）、（c）为人机垂直距离变化曲线。 

具体实施方式

本发明所涉及硬件结构如图1(b)所示。实验采用的立体视觉系统由两台Point Grey公司的FLEA2相机搭建而成，相机成像的最大分辨率为1024×768，焦距长为3.5mm。RFID系统采用一台超高频读卡器与两根8DBi圆极化天线组成，目标人体携带无源RFID标签，实验硬件平台为Pioneer3-DX型机器人。 

多特征多传感器的移动机器人运动人体跟踪方法流程图如图2所示，包括以下几个步骤： 

步骤一，利用RFID系统对在其附近携带无源标签的人体进行粗定位。如果检测到了标签，转下一步；否则，转步骤四。 

具体方法如下： 

(1)建立RFID传感器模型 

建立RFID天线模型：将天线固定在室内某一位置，同时携带无源标签的机器人在天线前方4m×4m的区域内任意运动，记录每一采样间隔内天线检测数据与机器人位置。通过对实验结果的统计学分析，确定单根天线检测区域，如图1(a)。图中A区域为RFID天线的主要检测区域，长约为2.6米，宽约为1.8米，设定检测区域内标签存在概率为0.9，未知区域内标签存在概率设定为0.5。 

划分天线检测区域：系统装载两根8DBi圆极化天线以提高检测范围和精度。检测角度约为55°，检测宽度约为3m。根据单根天线检测区域，划分两根天线检测区域，如图1(c)所示。1号区域与3号区域分别是两根天线单独检测区域，2号区域是两根天线公共检测区域。 

(2)目标定位 

将机器人前方4m×4m的区域划分成64个50cm×50cm的栅格区域。根据式（1）计算每个栅格被检测到标签存在的概率P_t(E|O)。初始化栅格地图中的每个栅格P(E)＝0.5，在跟踪过程中，机器人装载RFID系统(如图1(c))以检测出现在机器人前方不同位置携带标签的人体，当标签被右侧天线检测到时，机器人前方右侧区域即图1(c)中3号区域栅格P(O|E)＝0.9，其他区域栅格P(O|E)＝0.5；同理，当标签被两个天线同时检测到时，机器人前方中间区域即图1(c)中2号区域栅格P(O|E)＝0.9，其他区域栅格P(O|E)＝0.5；当标签被左侧天线检测到时，机器人前方左侧区域即图1(c)中1号区域栅格P(O|E)＝0.9，其他区域栅格P(O|E)＝0.5。根据RFID天线新的观测信息，利用式(1)对栅格地图中每个栅格的概率值P_t(E|O)进行迭代更新。最后将概率高于阈值的区域作为粗定位结果，一定程度上降低了RFID系统的定位误差。 

步骤二，利用基于头肩特征的自适应模板匹配算法对图像中人体进行初定位。如果检测到了人体，转下一步；否则，转步骤五。其过程如图3所示。 

步骤三，利用基于多特征的mean-shift算法对图像中人体精确定位。 

步骤四，利用扩展卡尔曼滤波算法预测人体运动状态。 

步骤五，采用双层协作定位机制对获取到的目标位置信息进行筛选。 

步骤六，利用机器人跟随控制算法控制机器人跟随人体运动。 

下面给出应用本发明所述方法在室内真实环境下进行机器人运动目标跟踪的实验实例。 

图4为应用本发明中基于头肩特征的自适应模板匹配算法人体检测实验结果图。实验中 共采集426帧图像，其中每幅图像尺寸800×600像素，从14帧起人体不停运动，经统计采用本文提出的算法对人体检测正确率达98.6%。实验结果表明本发明利用基于头肩特征自适应模板匹配算法能准确检测不同位姿人体。 

图5为应用本发明中基于多特征的mean-shift算法人体跟踪实验结果图。实验共采集78帧图像，图像尺寸为800×600。从第2帧开始人体开始不停地旋转、平移运动，同时跟踪效果并没有受到相近颜色、纹理及非目标人体的干扰。实验结果表明由于在目标模型核函数中引入头肩特征定位结果，有效地消除了背景对目标模型及跟踪效果的影响；结合纹理与颜色特征的目标模型增强了算法对目标特征的描述能力，提高了跟踪方法的准确性。 

图6为应用本发明所提跟踪方法的实验结果图。目标跟踪实验共采集70帧图像，每幅图像尺寸为800×600像素，图6(b)(c)为在跟踪过程中人机距离变化曲线。当目标突然转弯时，机器人减慢运动速度，并根据粗定位结果为机器人提供导向以快速跟踪到丢失的目标。当非目标进入相机视野时，RFID系统无法检测标签信息造成跟踪失败，而目标仍继续运动，此时利用EKF先验估计预测目标的最佳位置，重新跟踪到了目标。由图6(b)(c)可以看出，基于模糊控制策略的智能调速算法不仅能根据人体运动状态准确稳定跟踪人体，而且有效地控制了人机距离处于正常范围内。实验结果表明，本发明所提跟踪方法在解决目标被短暂遮挡及快速转弯问题的同时，能根据人体运动状态准确稳定跟踪人体，而且有效地控制了人机距离处于正常范围内，提高了系统的鲁棒性与智能化水平。 

以上所述，仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。 

Claims (4)

1.一种多特征多传感器的移动机器人运动人体跟踪方法，其特征在于包括以下步骤： 

步骤一，利用RFID系统对在其附近携带无源标签的人体进行粗定位；如果检测到了标签，转下一步；否则，转步骤四；具体方法如下： 

(1)建立RFID传感器模型 

首先，将天线固定在室内某一位置，同时携带无源标签的机器人在天线前方一固定区域内任意运动，记录每一采样间隔内天线的检测数据与机器人位置；通过对实验结果的统计学分析，确定单根天线的检测区域，然后将两根天线并排装载到移动机器人上，根据单根天线的检测区域划分两个天线的检测区域； 

(2)进行目标粗定位 

将机器人天线前方的固定区域划分成大小相等的栅格区域，根据利用贝叶斯公式计算每个栅格被检测到标签存在的概率P_t(E|O)： 

P_t(E|O)∝P(O|E)P_t-1(E)                (1) 

式中，O表示标签被检测到事件，E表示标签存在事件，P(E)表示标签存在的概率； 

初始化栅格地图中每个栅格的P(E)，在跟踪过程中，机器人装载RFID系统以检测出现在机器人前方不同位置携带标签的人体；根据RFID天线新的观测信息，利用式(1)对栅格地图中每个栅格的概率值P_t(E|O)进行迭代更新；最后将概率高于阈值的区域作为粗定位结果； 

步骤二，利用基于头肩特征的自适应模板匹配算法对图像中人体进行初定位；如果检测到了人体，转下一步；否则，转步骤五； 

步骤三，利用基于多特征的mean-shift算法对图像中人体精确定位； 

步骤四，利用扩展卡尔曼滤波算法预测人体运动状态，具体方法如下： 

在机器人坐标系下EKF的状态向量如下： 

x_r＝[X_r Y_r Z_r X_r Y_r] 

式中，[X_r,Y_r,Z_r]与[X_r,Y_r]分别是人在机器人坐标系的3D位置和水平移动速度； 

根据机器人的运动模型，机器人坐标系下的状态方程可被描述如下： 

式中，控制向量为control_t=[v_l,v_r]，ΔX_r，ΔY_r分别代表t时刻人在机器人坐标系X轴和Y轴方向上的位移，Δθ为转动的角度,w_t是过程噪声； 

传感器观测到的人体3D位置为

观测量为相机观测到的人体3D位置，观测方程如下： 

式中，p_t为观测噪声，

步骤五，采用双层协作定位机制对获取到的目标位置信息进行筛选； 

步骤六，利用机器人跟随控制算法控制机器人跟随人体运动，具体方法如下： 

(1)确定隶属函数、模糊集进行模糊化 

当人机距离和人机距离变化率较大时，采用三角形隶属函数，保证机器人跟随要求；当人机距离在安全范围内时，主要考虑平稳性，采用高斯型隶属函数； 

取X_r的模糊子集，在其论域中均划分为5个集合：“最近(VN)”、“近(N)”、“正常(ZE)”、“远(F)”、“最远(VF)”；取v_px的模糊子集，在其论域中均划分为5个集合：“负大(NB)”、“负小(NS)”、“正常(ZE)”、“正小(PS)”、“正大(PB)”；取v的模糊子集，在其论域中均划分为5个集合：“很小(VL)”、“小(L)”、“中(M)”、“大(H)”、“很大(VH)”；取Y_r、v_py的模糊子集，在其论域中均划分为5个集合：“负大(NB)”、“负小(NS)”、“正常(ZE)”、“正小(PS)”、“正大(PB)”；取K的模糊子集，在其论域中均划分为5个集合：“很小(VL)”、“小(L)”、“中(M)”、“大(H)”、“很大(VH)”；通过试验得到参数的有效论域：X_r∈[0,3]，v_px∈[-1,1]，v∈[0,200]，Y_r∈[-1,1]，v_py∈[-1,1]，K∈[0,3]； 

(2)建立控制规则 

控制规则用模糊条件语句来描述为： 

R_1i：if Q₁=A_i and Q₂=B_i,then v=C_i； 

R_2i：if Q₃=D_i and Q₄=E_i,then K=F_i； 

R_1i为基准线速度模糊控制器控制规则，R_2i为转弯增益模糊控制规，Q₁表示人机垂直距离语言变量，Q₂表示人机垂直距离变化率的语言变量；Q₃表示人机水平距离语言变量，Q₄表示人机水平距离变化率的语言变量，v和K分别表示基准线速度和转弯增益语言变量；它们的语言值在相应论域中的模糊子集分别为A_i、B_i、C_i、D_i、E_i、F_i； 

根据基准线速度模糊控制器调整机器人线速度，当人机垂直距离大于安全距离，为快速 跟随目标，系统将增大机器人运动线速度；当人机垂直距离小于安全距离，减小线速度以保证人机之间的安全距离；当人机垂直距离过小，机器人停止运动以防人机碰撞；根据转弯增益模糊控制器调整转向增益，当人机水平距离过大时，转向增益增大，转弯半径减小，通过快速调整转向以保证目标在视野中心位置； 

(3)智能调速 

根据两个模糊控制器的输出v和K，并结合机器人运动模型，机器人左、右轮速度分别为： 

式中，2d为机器人两轮间距。 

2.根据权利要求1所述的一种多特征多传感器的移动机器人运动人体跟踪方法，其特征在于，步骤二利用基于头肩特征的自适应模板匹配算法对图像中人体进行初定位的方法如下： 

(1)设投影直方图轮廓曲线分别为x＝f_hs(y)，x^*＝f_t(y^*)，其中f_hs(y)为从图像中提取的头肩模型的垂直投影直方图轮廓，H为当前头肩模型高度，f_t(y^*)为模板头肩模型的垂直投影直方图轮廓，H_t为模板头肩高度；为了得到合适的模板，根据头肩模型高度对模板直方图沿y轴方向进行压缩； 

(2)对提取的头肩模型及模板直方图按下式进行归一化： 

式中，

为当前头肩模型直方图面积，即像素总数，a_y为头肩模型不同位置的宽度，即某一y值上像素总数；

为压缩后模板面积，即像素总数，为模板不同位置的宽度，即某一

值上像素总数； 

归一化后的头肩模型垂直投影集合为A＝{a_y|y＝0,1,...,H}，归一化后的模板垂直投影集合为

(3)利用欧氏距离，度量集合A、B之间的相似性，以判断从候选区域中提取的目标是否为人体。 

3.根据权利要求1所述的一种多特征多传感器的移动机器人运动人体跟踪方法，其特征 在于，步骤三利用基于多特征的mean-shift算法对图像中人体精确定位中，不仅利用了目标的颜色、纹理特征，还考虑了头肩特征，具体方法如下： 

(1)建立目标颜色特征模型 

将RGB图像转换到HSV颜色空间以提取色度分量，将目标的色度特征离散分布为m_c个区间，假定目标所在区域的点集为{x_i}，i＝1,2,...,n，区域中心点为x₀，则对于中心位于x₀的目标颜色特征描述模型为： 

式中，

为使

的归一化常数，h为跟踪窗口宽度，δ(x)为Delta函数，用于判断x_i的灰度值是否属于第u个量化等级； 

由于周围环境的影响，相对于目标外围区域像素，目标中心区域像素更具有辨识价值，因此选择Epanechnikov核函数为目标区域内每个像素分配权值，即赋予接近中心的像素一个较大权值、远离中心的像素一个较小的权值；为了进一步消除远离目标外围区域像素点对目标模型的影响，利用从视差图中基于头肩特征提取的人体上半身躯干侧影对核函数按下式进行改造： 

式中，

为改造后的核函数，

为Epanechnikov核函数，B_hs(x_i)为二值化掩码函数； 

改造后背景图像处核函数值为零，人体躯干侧影处其值与Epanechnikov核函数一致；相较于只采用Epanechnikov核函数，新的核函数进一步消除了目标外围像素对目标模型的影响，加强了对目标的特征描述； 

(2)建立目标纹理特征模型 

采用一种扩展的局部二进制模式(Local binary patterns,LBP)来描述目标的纹理特征，称之为均匀纹理模式，定义为： 

式中，gray_c表示中心像素点的灰度值，α和β表示目标像素的局部区域，Α为邻域内像素的数目；gray_α表示以中心点为圆心，半径为β的邻域内均匀分布的α像素点的灰度值；函数和Q(LBP_A,β)的定义为： 

函数Q(LBP_A,β)返回的是LBP_A,β模式下，二进制位串中从“0”到“1”或者从“1”到“0”的跳变数；对于均匀纹理模式，这种跳变不超过两个；在随后的直方图计算过程中，只为均匀纹理模式分配单独的直方图收集箱(bin)，这样对于8邻域像素的LBP算子仅存在9种特征模式；统计目标像素点纹理特征模式，进而获得目标的纹理特征直方图；为了更好地消除目标外围区域像素点纹理的影响，利用头肩特征定位结果获得人体躯干侧影以改造目标纹理模型；假定目标的纹理特征空间中特征值v分布在m_t个区间，则对于中心位于x₀的目标纹理特征分布模型为： 

式中，为目标纹理特征模型，

为目标纹理直方图模型标准化常量，t(x_i)为映射函数，δ[t(x_i)-v]的作用是判断像素点x_i的纹理特征是否属于第v个量化等级，i＝2,3,4,...,t,...； 

(3)由头肩特征定位结果确定中心为y₀的候选区域，按下式建立候选区域标颜色、纹理模型

式中，

为颜色特征直方图模型中的标准化常量，为纹理特征直方图模型中的标准化常量； 

利用头肩尺寸按下式更新跟踪窗口大小： 

式中，W、H分别为跟踪窗口大小，W_hs、H_hs分别为人体头肩宽度和高度； 

(4)建立联合相似度函数 

采用颜色纹理联合相似度函数，表达式为： 

式中，

为颜色纹理联合相似度函数，

分别为颜色、纹理相似度函数；将头肩模型定位结果中心y₀作为当前帧的初始候选区域中心，将上式在y₀处按泰勒级数展开，近似为： 

式中，w_i为跟踪窗口内各像素点权值，c_h为标准化常量，m_c为目标的色度特征分量数目，n_h为跟踪窗口内像素点数量，u为颜色特征空间中的特征分量，

为候选区域颜色特征模型； 

为了寻求目标与候选区域相似度最大化，将(2)式两边对y求偏导并令其为零，从而得到每次迭代重新定位的候选区域中心y₁： 

式中，g(x)＝-k′(x)； 

(5)采用式(3)跟踪窗口内各像素点权值w_i； 

(6)计算候选区域质心y₁；若||y₁-y₀||＜ε则进行下一步；否则，将y₁作为新的候选区域中心，返回步骤(5)，以寻找满足条件的位置； 

(7)采用一种选择性模型更新策略，即对匹配贡献度高的特征分量不进行更新，仅更新贡献度低的特征分量；匹配贡献度函数为： 

利用颜色、纹理相似度系数

与

决定模型更新时机；当二者相似度均高于阈值时，表 明跟踪效果良好，不进行模型更新；当其中某种特征相似度系数较小时，表明跟踪效果受到影响，若相似性系数在[d₁,d₂]范围内，根据匹配贡献度对该特征空间中的特征分量进行排序，前γ个贡献度高的特征分量表示当前模型与目标模型较为匹配，不进行更新，只选择后m-γ个匹配贡献度较差特征分量进行更新，避免了因为过度更新而造成的模型偏差；若两种特征相似度系数均低于d₁，表明目标被遮挡，不更新目标模型，系统将利用EKF先验估计跟踪目标；具体规则为： 

式中，r为更新因子。 

4.根据权利要求1所述的一种多特征多传感器的移动机器人运动人体跟踪方法，其特征在于，步骤五采用双层协作定位机制对获取到的目标位置信息进行筛选的方法如下： 

利用双层协作机制将RFID对标签的粗定位，结果为： 

式中，q为目标定位结果，q_stereo为立体视觉内层定位结果，q_rfid为RFID系统外层粗定位结果，q_p为EKF先验估计结果； 

正常跟踪过程中，外层定位结果协助内层精确定位目标，即u_v＝1,u_rifd＝0，机器人利用内层定位结果跟踪目标；当目标突然转弯时，RFID系统仍能检测到标签信号，内层定位失败，即u_v＝0,u_rfid＝1，目标定位结果由外层粗定位确定，为机器人跟随目标提供导向，使机器人快速跟踪目标；当目标被遮挡时，RFID系统无法接收来自标签发出的信号，u_v＝0,u_rfid＝0，利用EKF的先验估计作为定位结果。 

Images