基于激光雷达的舞蹈机器人室内行人跟踪方法与流程

本发明属于机器人技术领域，具体涉及到用于舞蹈机器人的基于激光雷达的室内行人跟踪方法。

背景技术：

机器人技术在几十年间取得了惊人发展，人类生活的各种场合，如医疗、家庭、餐馆、酒店等，都有替代人或与辅助人工作的机器人存在。由于许多情况下需要与人共处，因此机器人在完成自身工作的同时要保证人的安全，其工作水平要求较高。主动协调机器人能够在多传感器下实现机器人和人的高度协调工作，提高与人共存的可能性。典型的应用例如：为了实现舞蹈机器人在与人接触的情况下和人跳舞，机器人需要观察周围的环境，并且根据行人的运动轨迹进行自身的运动协调，在保证他人安全的前提下完成舞蹈动作。因此，对周围环境中的行人进行识别与跟踪对机器人跳舞过程中的运动协调与轨迹规划具有重要意义。

机器人的行人跟踪方法有很多，基于视频的行人跟踪是目前最常用的方法，但由于受到相机视野范围、光线、行人姿态变化的影响使得利用视觉信息完成舞蹈机器人的行人跟踪变得异常困难，尤其当人距离摄像机较近时摄像机只能采集到行人局部图像。因此基于视觉的行人跟踪多用于室外或室内的远距离行人跟踪。利用激光雷达实现行人跟踪是另一种常见方法。激光雷达，是一种利用激光技术进行测量的新型测量仪表，具有精度高、速度快、测量范围广等特点。但基于激光雷达的行人跟踪方法通常成本相对较高，且容易受到算法的影响使得实时性和准确率不高。专利CN102253391A公开了一种基于多激光雷达的行人目标跟踪方法，能有效地检测出室内的行人位置并实时跟踪行人的运动轨迹。但该方法的雷达处于固定位置，需要对多个雷达进行标定，且多个雷达成本高，不适用于移动式舞蹈机器人的行人跟踪。

综上所述，目前应用于舞蹈机器人的室内行人跟踪方法存在的主要问题是：基于视频的行人跟踪方法难以满足短距离的行人跟踪，基于激光雷达的行人跟踪方法需考虑成本，并满足实时性与准确性要求，因此研发一款低成本，实时性及实用性好，跟踪准确率高的室内行人跟踪方法具有很高的应用价值。

技术实现要素：

本发明的目的在于克服现有的室内行人跟踪方法的缺陷与不足，提供一种基于激光雷达的舞蹈机器人室内行人跟踪方法，利用单个激光雷达实现不同距离不同形态的行人识别，并可对静止、步行等不同运动模式的行人进行跟踪，跟踪范围广，实时性好，识别与跟踪的准确度高。

为达到上述目的，本发明的技术方案是这样实现的，一种基于激光雷达的舞蹈机器人室内行人跟踪方法，包括下述步骤：

(1)激光数据预处理：连接到机器人主机的激光雷达，采集到某角度处扫描到的物体到雷达的距离信息；主机获取激光原始数据，对原始数据进行预处理，预处理方法包括中值滤波和坐标系转换；

(2)基于密度的激光数据聚类：对于步骤(1)得到的数据通过基于密度的聚类实现环境分割，将不同物体进行区分；

(3)基于激光采集点的行人识别：对聚类出的类簇中的点进行行人识别，去掉非行人簇，将行人簇作为行人跟踪的输入数据；

(4)图形化显示：将激光连续获取的扫描数据进行图形化显示，即将激光数据转换成视频流；对每一个采集点做一个映射，并将采集点进行图像放大，便于更高效直观地进行跟踪；

(5)基于粒子滤波的行人跟踪，包括：

(501)初始化阶段：

在完成行人识别后，将行人簇的位置作为行人跟踪的初始位置，首先初始化跟踪器，提取矩形框内的特征，区域特征通过目标区域的颜色分布概率密度描述，并对粒子集进行初始化；

(502)传播阶段：根据转移方程s_t＝As_t-1+w_t-1，计算新的粒子集并预测新粒子的位置，其中A为状态转移矩阵，w_t-1为高斯噪声；

(503)决策阶段：选取粒子的特征相似度的衡量标准，计算出来的相似度或分数即为相应粒子的权重，计算每一个粒子的权重，并将其进行归一化；

(504)状态估计阶段：根据粒子权重，对粒子的位置依照权重求和得到目标的位置输出；

(505)重采样阶段：根据重要性权重对粒子进行重采样，得到新的粒子集；

(506)重复执行步(502)至步(505)，实现运动物体的跟踪；

(6)行人运动轨迹绘制：对每一帧图像利用粒子滤波得到行人位置，计算目标中心位置，并将该中心与前一帧目标中心做连线，得到行人运动轨迹图。

进一步的，步骤(1)所述中值滤波的方法为：激光雷达的采集点属于二维数据信号，采集到的数据是点到激光的距离，对待处理的数据点邻域内的m个数据点的距离数据进行排序，将排序后的第(1+m)/2个距离代替当前数据点的距离。

更进一步的，步骤(1)所述坐标系转换的方法为：坐标系转换，原始数据经中值滤波之后去除了部分测量误差，由于原始数据为极坐标系下的，为了便于数据分析将其转换至直角坐标系，原始数据格式为：

s_k＝d_k,k＝0,1,...,N (1)，

其中，d_k为第k个采集点的距离；

将其转换至直角坐标系中的二元向量组：

u_k＝(x_k,y_k)^T,k＝0,1,...,N_e (2)，

其中，u_k为第k个激光采集点对应的直角坐标，x_k与y_k分别为X轴与Y轴坐标，N_e为有效采集点个数；；

若点P为第k个采集点，激光雷达获得的方位角为θ_k，距离为ρ_k，角分辨率为β，则θ_k＝k×β，求得P对应的直角坐标为：

P_x＝ρ_k cos(θ_k-45°) (3)，

P_y＝ρ_k sin(θ_k-45°) (4)，

其中，P_x与P_y分别为点P的X轴与Y轴坐标，(θ_k-45°)为点P与X轴的夹角。

进一步的，步骤(2)中，使用DBSCAN算法进行聚类，主要步骤包括：

(201)将所有点标记为unvisited；

(202)随机取unvisited的点p，并标记为visited；

(203)判断点p是否为核心点，若到点p距离小于ε的点的个数大于MinPts，则p为核心点，继续步(4)，否则p为噪声，返回步(2)；

(204)p为核心点，建簇C，并将p添加至C，将P_N定义为点p的ε-邻域内点的集合,继续步(5)；

(205)对于P_N中点q，重复步(6)至P_N为空；

(206)若q的标记为unvisited，首先将其标记为visited，然后判断q是否为核心点，若q为核心点，将q的ε-邻域内的点添加至P_N，若q不是任何簇的成员，则将p添加至C；

(207)输出簇C；

(208)重复步(202)至(207)至所有点标记为visited；

更进一步的，在上述DBSCAN算法进行核心点计算时，对于第k个采集点P_k，需要将P_k与其余所有点进行距离的计算，时间复杂度高，因此在进行核心点计算时，只考虑P_k的周围点与P_k的距离，即计算P_(k-MinPts)至P_(k+MinPts)范围内的点到P_k的距离。

进一步的，步骤(3)所述行人识别的具体方法如下：

(301)首先对聚类出的类簇中的点进行扫描，获得每个类簇的信息，包括类簇中采集点的个数N_c、类簇距离激光雷达的平均距离D_avg以及类簇的上下左右边缘的位置坐标；

(302)根据类簇的大小进行粗筛选；

(303)对剩余的簇进行精细的筛选，首先获取激光雷达到该簇的距离，记作r，令r＝D_avg，设行人腰部宽度为W_ped，W_ped的取值范围为W_min<W_ped<W_max；若以r为半径，将W_ped作为弧长，则有下式：

$W_{ped}=\frac{\mathrm{\&theta;}\mathrm{\&pi;}r}{180}---\left(5\right),$

计算扇形区域的对应的圆心角θ，公式如下：

$\mathrm{\&theta;}=\frac{180\&times;W_{ped}}{\mathrm{\&pi;}r}---\left(6\right),$

根据激光雷达的角分辨率β和簇对应的圆心角θ利用式(7)计算采集点的个数P_c，计算公式如下：

$P_c=\frac{180\&times;W_{ped}}{\mathrm{\&pi;}r\mathrm{\&beta;}}---\left(7\right),$

若行人簇对应的点的个数记作P_num，则P_num的范围估计值的计算方法如下：

$\frac{180\&times;W_{min}}{\mathrm{\&pi;}r\mathrm{\&beta;}}\&lt;P_{num}\&lt;\frac{180\&times;W_{max}}{\mathrm{\&pi;}r\mathrm{\&beta;}}---\left(8\right),$

对每一个聚类之后的簇根据上述方法进行判断，去掉非行人簇，将行人簇作为行人跟踪的输入数据。

相对于现有技术，本发明所述的一种基于激光雷达的舞蹈机器人室内行人跟踪方法，主要有以下优势：

(1)本发明方法利用了单个激光雷达实现室内行人跟踪，相比于基于视频的行人跟踪方法扩大了跟踪范围，相比于多激光雷达的行人跟踪方法降低了成本；

(2)本发明对原始激光数据进行了聚类，为了提升聚类的速度，根据激光数据集的分布特点，对原始DBSCAN算法进行了优化，极大的提升了聚类速度，提升了算法效率；

(3)本发明对激光雷达数据采集点数量与物体远近的关系进行了分析，提出了基于类簇到激光雷达的距离及行人身体宽度的行人识别方法，该方法识别速度快，准确率高；

(4)本发明为了准确直观地实现行人跟踪，将激光数据进行图像化显示，将离散数据点转换为视频数据进行处理，实时性好，便于更加直观高效的实现行人跟踪。

附图说明

图1为本发明基于激光雷达的舞蹈机器人室内行人跟踪方法流程图；

图2为本发明方法中坐标系转换示意图；

图3为本发明方法中不同位置不同形态的行人在激光雷达下的状态示意图；

图4为本发明方法中行人簇内采集点的个数估计示意图；

图5(a)为本发明方法中静止状态行人的跟踪路径与实际路径对比图；

图5(b)为本发明方法中直线运动行人的跟踪路径与实际路径对比图；

图5(c)为本发明方法中曲线运动行人的跟踪路径与实际路径对比图。

具体实施方式

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

下面将结合实施例以及附图来详细说明本发明。

本发明在舞蹈机器人腰部安装激光雷达，获取扫描到的距离数据，并对数据进行预处理及聚类分析，通过对类簇的分析实现行人识别，将激光数据图形化显示，并利用粒子滤波算法实现行人跟踪；图1为本发明基于激光雷达的舞蹈机器人室内行人跟踪方法流程图，所述方法包括以下步骤：

第一步，激光数据预处理：

将激光雷达放置于舞蹈机器人的腰部位置，距离地面1m，选用日本Hokuyo公司的URG激光雷达，并通过USB接口连接至主机，利用SCIP2.0数据协议进行通信，采集到的是某个角度处扫描到的物体到雷达的距离信息；主机首先通过发送协议指令获取激光原始数据，为了去除原始数据的测量误差并加快后期运算速度，对原始数据进行预处理，预处理包括中值滤波和坐标系转换；

(1)中值滤波：

激光雷达的采集点属于二维数据信号，采集到的数据是点到激光的距离，对待处理的数据点邻域内的m个数据点的距离数据进行排序，将排序后的第(1+m)/2个距离代替当前数据点的距离，其中m取5，这样便完成了该点的数据校正；

(2)坐标系转换：

原始数据经中值滤波之后去除了部分测量误差，由于原始数据为极坐标系下的，为了便于数据分析将其转换至直角坐标系，原始数据格式为：

s_k＝d_k,k＝0,1,...,N (1)，

其中，d_k为第k个采集点的距离，如图2为本发明方法中的坐标系转换示意图，A为扫描起点，C为扫描终点，B为获取到的第一个有效采集点；

将其转换至直角坐标系中的二元向量组：

u_k＝(x_k,y_k)^T,k＝0,1,...,N_e (2)，

其中，u_k为第k个激光采集点对应的直角坐标，x_k与y_k分别为X轴与Y轴坐标，N_e为有效采集点个数；

如图2中点P为第k个采集点，激光雷达获得的方位角为θ_k，距离为ρ_k，角分辨率为β，则θ_k＝k×β，求得P对应的直角坐标为:

P_x＝ρ_k cos(θ_k-45°) (3)，

P_y＝ρ_k sin(θ_k-45°) (4)，

其中，P_x与P_y分别为点P的X轴与Y轴坐标，(θ_k-45°)为点P与X轴的夹角；

第二步：基于密度的激光数据聚类：

对于第一步得到的数据通过基于密度的聚类实现环境分割，将不同物体进行区分；DBSCAN是一种基于密度的聚类算法，用于发现数据集中的稠密区域，并且不易受噪声和离群点的影响；DBSCAN算法的输入参数为邻域半径ε和稠密区域的密度阈值MinPts；如果一个点的ε-邻域内至少包含MinPts个点，则该点称为核心点，核心点及ε-邻域内的点组成一个簇，若一个簇内含有多个核心点，则将这些核心点所属的簇进行合并，得到最终聚类结果；

DBSCAN算法进行聚类的主要步骤：

(1)将所有点标记为unvisited；

(2)随机取unvisited的点p，并标记为visited；

(3)判断点p是否为核心点，若到点p距离小于ε的点的个数大于MinPts，则p为核心点，继续步(4)，否则p为噪声，返回步(2)；

(4)p为核心点，建簇C，并将p添加至C，将P_N定义为点p的ε-邻域内点的集合,继续步(5)；

(5)对于P_N中点q，重复步(6)至P_N为空；

(6)若q的标记为unvisited，首先将其标记为visited，然后判断q是否为核心点，若q为核心点，将q的ε-邻域内的点添加至P_N，若q不是任何簇的成员，则将p添加至C；

(7)输出簇C；

(8)重复步(2)至(7)至所有点标记为visited；

由于随着激光雷达扫描到的距离的增大，点的分布越来越分散，为了能更准确地进行聚类，本发明将ε设置为100mm，MinPts设置为5；若采用原始的DBSCAN聚类算法，聚类过程时间复杂度较高，达不到实时性要求，因此根据激光采集点的数据特征对DBSCAN算法进行优化；

在原始的DBSCAN算法进行核心点计算时，对于第k个采集点P_k，需要将P_k与其余所有点进行距离的计算，时间复杂度高，由于激光采集点为扇形扫描面上的点，距离越远的点属于同一物体的可能性越小，因此在进行核心点计算时，只考虑P_k的周围点与P_k的距离，即计算P_(k-5)至P_(k+5)范围内的点到P_k的距离，在不影响聚类效果的同时大大减少了计算时间，提升了聚类的速度；

第三步：基于激光采集点的行人识别：

激光雷达发射的激光束是扇面的，行人簇一般为弧形或近似直线，距离激光雷达的远近直接影响扫描到物体的点的数量；方向、角度、大小均相同但距离不同的两个物体，到激光雷达较近的物体上采集点的数量多，密度高，到激光雷达较远的物体上采集点的数量少，密度低；

除行人到激光雷达的距离影响采集点的数量外，行人在运动过程中与激光雷达的相对位置会发生变化，行人的不同形态也会对采集点的数量产生影响；如图3为不同位置不同形态的行人在激光雷达下的状态，圆角矩形代表行人的腰部轮廓，图中显示了五种不同距离不同形态下的行人在激光雷达下的状态，扫描到不同形态不同位置的行人身上的采集点也不同，但是根据行人的腰部位置的宽度信息可对不同位置不同形态下的行人身体上采集点的数量进行估计，从而在多个簇中进行行人识别；

行人识别的具体方法如下：

(1)首先对聚类出的类簇中的点进行扫描，获得每个类簇的信息，包括类簇中采集点的个数N_c、类簇距离激光雷达的平均距离D_avg以及类簇的上下左右边缘的位置坐标；

(2)根据类簇的大小进行粗筛选，将类簇中点的个数大于200的簇视为非行人簇；

(3)对剩余的簇进行精细的筛选，利用行人与激光雷达的距离与行人的身体宽度对行人身体上反射的激光采集点的个数进行估计；图4为行人簇内采集点的个数估计示意图，首先获取激光雷达到该簇的距离，记作r，令r＝D_avg，设行人腰部宽度为W_ped，W_ped的取值范围为W_min<W_ped<W_max；若以r为半径，将W_ped作为弧长，则有下式：

$W_{ped}=\frac{\mathrm{\&theta;}\mathrm{\&pi;}r}{180}---\left(5\right),$

计算扇形区域的对应的圆心角θ，公式如下：

$\mathrm{\&theta;}=\frac{180\&times;W_{ped}}{\mathrm{\&pi;}r}---\left(6\right),$

根据激光雷达的角分辨率β和簇对应的圆心角θ利用式(7)计算采集点的个数P_c，则簇对应的点的个数为P_c或P_c+1，P_c的计算公式如下：

$P_c=\frac{180\&times;W_{ped}}{\mathrm{\&pi;}r\mathrm{\&beta;}}---\left(7\right),$

若行人簇对应的点的个数记作P_num，则P_num的范围估计值的计算方法如下：

$\frac{180\&times;W_{min}}{\mathrm{\&pi;}r\mathrm{\&beta;}}\&lt;P_{num}\&lt;\frac{180\&times;W_{max}}{\mathrm{\&pi;}r\mathrm{\&beta;}}---\left(8\right),$

本发明中取W_ped的范围为200mm<W_ped<500mm，对每一个聚类之后的簇根据上述方法进行判断，去掉非行人簇，将行人簇作为行人跟踪的输入数据；

第四步：图形化显示：

由于获得的激光采集点较分散，扫描间隔为100ms，时间较长，因此相邻两次扫描得到的数据变化较大，行人身体上采集点的位置以及形变也随之变大；鉴于此，将激光连续获取的扫描数据进行图形化显示，即将激光数据转换成采集频率为10帧/s的视频流；

对每次扫描的数据经坐标系转换后得到每个采集点的直角坐标，为了加快跟踪的速度，将直角坐标系中的坐标数据缩小20倍显示，将图像中心位置作为激光雷达的位置，即坐标原点；对每一个采集点做一个映射，并将采集点进行了图像放大，便于更高效直观地进行跟踪；

第五步：基于粒子滤波的行人跟踪：

(1)初始化阶段：

在完成行人识别后，将行人簇的位置作为行人跟踪的初始位置，首先初始化跟踪器，提取矩形框内的特征，并对粒子集进行初始化，粒子个数为100；

区域特征通过目标区域的颜色分布概率密度描述，方法如下：

首先对待跟踪的区域进行离散统计得到RGB的颜色直方图；为增强颜色分布的可靠性，将距离中心较远的像素赋予较小权值，权值函数如下：

其中，r为点到目标中心的距离，设目标区域中心坐标为X_c＝(x_c,y_c)，H_x和H_y为目标区域的宽和高，则目标区域大小用描述，目标区域第i个像素的坐标为X_i＝(x_i,y_i),i＝1,…,n,则当前状态X_t的颜色分布模型为：

$p^{\left(u\right)}\left(X_t\right)=f\underset{i=1}{\overset{n}{\&Sigma;}}k\left(\frac{\left|\right|X_c-X_i\left|\right|}{a}\right)\mathrm{\&delta;}\&lsqb;h\left(X_i\right)-u\&rsqb;,(u=1,...,m)---\left(10\right),$

$f=\frac{1}{\underset{n=1}{\overset{n}{\&Sigma;}}k\left(\frac{\left|\right|X_c-X_i\left|\right|}{a}\right)}---\left(11\right),$

其中，n为当前区域的像素个数，m为直方图特征个数，其中m＝256，f为归一化因子，u为直方图的颜色等级索引，δ[h(X_i)-u]表示目标区域中X_i位置处的颜色是否属于直方图中第u个特征，若属于则该值为1，否则为0；

(2)传播阶段：根据转移方程s_t＝As_t-1+w_t-1，计算新的粒子集并预测新粒子的位置，其中A为状态转移矩阵，w_t-1为高斯噪声；

(3)决策阶段：选取粒子的特征相似度的衡量标准，计算出来的相似度或分数即为相应粒子的权重，方法如下：

每个粒子按照上述步骤(1)计算颜色直方图分布，并将其与目标区域的颜色直方图分布比较，采用Bhattacharyya距离进行衡量，计算方法如下：

$d\&lsqb;p\left(X_t\right),p_d\&rsqb;=\underset{u=1}{\overset{m}{\&Sigma;}}\sqrt{p^{\left(u\right)}\left(X_t\right){p_d}^{\left(u\right)}}---\left(12\right),$

其中，p(X_t)为第t个粒子位置处的颜色直方图分布，p_d为目标区域的颜色直方图分布；

根据Bhattacharyya距离得到各粒子的权重值：

$w_t=\frac{1}{\sqrt{2\mathrm{\&pi;}}\mathrm{\&sigma;}}\exp (-\frac{1-d\&lsqb;p\left(X_t\right),p_d\&rsqb;}{2{\mathrm{\&sigma;}}^2})---\left(13\right),$

其中，σ为高斯平方差，取值0.1，粒子对应的权值越大，则该粒子与目标相似性越大；

计算每一个粒子的权重，并将其进行归一化；

(4)状态估计阶段：根据粒子权重，对粒子的位置依照权重求和得到目标的位置输出；

(5)重采样阶段：根据重要性权重对粒子进行重采样，得到新的粒子集；

(6)重复执行步骤(2)至(5)，实现运动物体的跟踪；

第六步：行人运动轨迹绘制：

对每一帧图像利用粒子滤波得到行人位置，计算目标中心位置，并将该中心与前一帧目标中心做连线，得到行人运动轨迹图；

为了验证本发明的行人跟踪效果，对不同距离、不同形态、不同运动方式的行人进行识别及跟踪；如图5(a)为本发明方法中静止状态行人的跟踪路径与实际路径对比图，其中跟踪位置在行人附近波动，未出现较大偏差，跟踪效果良好；如图5(b)为本发明方法中直线运动行人的跟踪路径与实际路径对比图，行人运动中的姿态以及与激光雷达的相对位置不断变化，实验表明跟踪结果准确，部分路径出现较小的误差，但并未对跟踪造成大的影响；如图5(c)为本发明方法中曲线运动行人的跟踪路径与实际路径对比图，由于行人的运动方向改变较快，形态变化也较剧烈，因此图中可以看出波动较明显，但算法及时纠正了跟踪路线；由三组实验可知，本实施例的跟踪算法对运动的行人能够快速准确地进行跟踪，并可对跟踪偏差及时修正。

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