CN111089590B - 一种视觉和激光融合的移动机器人跟踪人腿方法 - Google Patents

Abstract

本发明提供了一种视觉和激光融合的移动机器人跟踪人腿方法，主要涉及视觉跟踪、激光跟踪、多传感器融合、移动机器人等领域。为了解决移动机器人在跟踪行人腿部时，跟踪精度比较低，尤其在转弯时出现容易丢失的问题，本发明通过相机和激光雷达同时采集数据，相机通过分析人体腿部的姿态，采用腿部姿态特征提取算法，判断移动机器人是否需要转弯；采用卡尔曼滤波算法，预测人体腿部的位置；激光通过分析人体腿部的中心和移动机器人的距离，采用腿部圆弧特征识别算法，实现移动机器人对行人实时跟踪。本发明能够实现移动机器人对人体腿部的跟踪。

Description

一种视觉和激光融合的移动机器人跟踪人腿方法

技术领域

本发明涉及机器人技术的控制领域，尤其是机器人传感器融合领域，特别是一种视觉和激光融合的移动机器人跟踪人腿方法。

背景技术

在智能物流和仓储过程中，移动机器人及其执行部件需要高精度的运动控制和路径规划，目前移动机器人在室内环境的定位导航与调度还没有具备自主化操作的能力，尤其是未知或者多变环境中需要多个移动机器人协同工作情况下。智能移动机器人是一个集环境感知、动态决策与规划、行为控制与执行等多功能于一体的综合***，机器人在行走过程中需要解决三个问题：自己在何处，要往何处去，如何到达该处。为了解决这些问题，需要对多种定位导航技术进行综合，进行深度融合达到未知环境下基于自适应融合方法的自主定位导航机制，实现移动机器人对人体腿部的跟踪。

目前实现机器人跟随人腿的方法很多，其中应用较广的是基于单一视觉的机器人自动跟随方法和基于单一激光雷达的机器人自动跟随方法。基于视觉跟踪人腿的方法，受光照、相机视野范围的因素影响，跟踪精度差。基于激光雷达跟踪人腿的方法，采集数据在移动机器人转弯时候，易出现目标丢失和失灵的情况，故障程度高。单一的传感器跟踪已经不能满足移动机器人发展的需要。在这种背景下，本专利的一种视觉和激光融合的移动机器人跟踪人腿方法应运而生。

另外，现有技术中公开了一种具有目标跟踪和自动避障的载物爬楼机器人，见公开号为：CN109434795A，公开日为：2019-03-08的中国专利，该专利公开了一种具有目标跟踪和自动避障的载物爬楼机器人；包括壳体、车架机构、轮腿机构和控制***；其中壳体上具有载物平台，车架机构包括第一车架和第二车架，轮腿机构包括第一轮腿组和第二轮腿组，控制***包括供电电源、硬件电路、驱动电机、编码器、单片机、PC机、手机APP、4G通信模块、超声波距离传感器、激光雷达和摄像头；该发明提供的载物爬楼机器人具有自动避障、图像识别跟踪和手机APP远程控制功能，达到了高效爬楼和平稳送货的效果。该专利虽然也进行了目标跟踪，但并不是机器人跟踪人的行走，且采用的技术方案与本专利也不相同。

发明内容

本发明的目的在于克服传统的单一传感器的机器人跟随技术的不足，尤其是单一视觉传感器下精度低或单一激光雷达传感器下转弯失灵，提出了一种视觉和激光融合的移动机器人跟踪人腿方法。

本发明采用以下方案实现：一种视觉和激光融合的移动机器人跟踪人腿方法，所述方法包括如下步骤：

步骤S1、通过相机和激光雷达同时采集数据，相机获取人腿的姿态信息，激光雷达获取人腿的位置信息；移动机器人得到相机获取的姿态信息，则进入步骤S2；移动机器人得到激光雷达获取的位置信息，则进入步骤S4；

步骤S2、相机通过分析人体腿部的姿态信息，采用腿部姿态特征提取算法，判断行人是直行还是转弯；如果是直行的姿态，移动机器人直接跟随行人直行；如果是转弯的姿态，则进入步骤S3；

步骤S3、采用卡尔曼滤波算法获取转弯状态并通过运动控制实现移动机器人跟随行人转弯；

步骤S4、激光通过分析人体腿部的位置信息，采用腿部圆弧特征识别算法获取激光数据点中心坐标到移动机器人的实时距离，实现移动机器人对行人实时跟踪。

进一步的，所述腿部姿态特征提取算法为：移动机器人采用三角特征贝叶斯步态识别的方式，把行人人腿姿态从环境中提取出来；行人的人腿姿态特征提取为几何形状，不同的几何形状归纳为直行的姿态和转弯的姿态；移动机器人通过实时计算人腿腿部的姿态，判断行人是直行还是转弯。

进一步的，所述腿部姿态特征提取算法进一步包括如下步骤：

步骤S21、根据相机中背景的亮度来检测相机敏感性变化：

式中，a为相机中前景像素的灰度值，b为相机中背景像素的灰度值；

选择适当的阈值T，得到二值化的运动目标区域：

式中，a_xy表示(x,y)点的前景像素灰度值，b_xy表示(x,y)点的背景像素灰度值；T表示设定的阈值，D_xy表示二值化的运动目标区域；

步骤S22、用膨胀和腐蚀的方式来处理相机图像中运动目标区域的空洞、噪声点：

式中，

表示膨胀符号，F₁表示前景图像信息，F₂表示背景图像信息，F₂′表示F₂关于原点对称的集合，Φ表示空集；(F'₂)_x表示在F₂′中截取的x大小的背景图像信息作为卷积模板，

式中，

表示腐蚀符号；(F₂)_x表示在F₂中截取的x大小的背景图像信息作为卷积模板；

步骤S23、运动目标区域的空洞、噪声点进行处理后，再提取人腿区域：从上往下扫描，记录像素点为1的值；从左往右扫描，记录像素为0的值；

步骤S24、在人腿区域中，提取髋部中心位置为A、脚部中心位置为B、膝盖中心位置为C的三角形ΔABC；提取三角形的以下参数作为腿部姿态特征：

x：腿部重心坐标(x_c,y_c)的横坐标x_c；

y：腿部区域髋部线纵坐标k_r*h，h为人体的高度，k_r为髋部位置在人体高度的比例；

θ：∠BAC的值；

l₁、l₂、l₃：三角形三边线段AB、BC及AC长度；

步骤S25、计算腿部姿态特征向量，该向量由两个相邻姿态联合表示：

F＝(f_min,f_max)

f_min＝(x_min,y_min,l_1min,l_2min,l_3min,θ_min)

f_max＝(x_max,y_max,l_1max,l_2max,l_3max,θ_max)

式中，F表示腿部姿态特征向量，f_min表示最小腿部姿态的特征向量，f_max表示最大腿部姿态的特征向量；x_min和x_max表示腿部重心坐标中横坐标的最小值和最大值；y_min和y_max表示腿部区域髋部线纵坐标的最小值和最大值；l_1min和l_1max表示腿部姿态特征中ΔABC三边线段AB的最小值和最大值；l_2min和l_2max表示腿部姿态特征中ΔABC三边线段BC的最小值和最大值；l_3min和l_3max表示腿部姿态特征中ΔABC三边线段AC的最小值和最大值；θ_min和θ_max表示腿部姿态特征中ΔABC中∠BAC的的最小值和最大值；

步骤S26、计算人体侧影重心坐标：

式中，N_b是人体腿部区域像素总数，(x_i,y_i)人体腿部区域的像素；

步骤S27、已知三顶点坐标的三角形，三边距离根据两点间的距离公式得到：

其中，l为三角形的任意的三个边，即l₁、l₂、l₃；其中一个边的两点的坐标即为(x₁,y₁)、(x₂,y₂)；

步骤S28、根据F、l和cosθ的值，对移动机器人状态使用卡尔曼滤波算法进行修正。

进一步的，所述采用卡尔曼滤波算法获取转弯状态，进一步具体为：移动机器人转弯前，当前状态为直行状态，下一状态为转弯状态；通过相机中人腿转弯的姿态检测能对移动机器人进行状态修正，并提供给移动机器人以行人转弯角度的预测；通过对当前状态的修正和下一状态的预测，实现移动机器人跟随行人平稳的转弯。

进一步的，所述卡尔曼滤波算法进一步包括如下步骤：

步骤S31、根据移动机器人***中状态转移方程和移动机器人转弯前上一时刻的状态，预测下一时刻的状态：

X(k)＝AX(k-1)+BU(k)+W(k)

式中，X(k)、X(k-1)表示状态向量，A表示包含F、l和cosθ的状态矩阵，BU(k)表示相机的腿部姿态的控制输入，W(k)表示高斯随机性向量；

步骤S32、计算X(k-1)对应的协方差P(k|k-1)：

P(k|k-1)＝AP(k-1|k-1)A'+Q，其中P(k|k-1)表示X(k-1)对应的协方差，P(k-1|k-1)表示前一时刻协方差，A'表示矩阵A的逆矩阵，Q为***噪声对应的协方差；

步骤S33、根据预测结果更新当前状态：

X(k|k)＝X(k|k-1)+K_g(k)(Z(k)-HX(k|k-1))

式中，X(k|k)表示根据预测结果得到的当前状态，X(k|k-1)表示预测结果的状态，K_g(k)为卡尔曼增益，Z(k)表示受人体姿态影响的输入方程，H表示协方差状态下包含F、l和cosθ的状态矩阵；

其中，K_g(k)为卡尔曼增益，P(k|k-1)表示X(k|k-1)对应的协方差，H'表示H相关的逆矩阵，R表示预测噪声对应的协方差矩阵；

步骤S34、计算当前时刻状态对应的协方差：

P(k|k)＝(I-K_g(k)H)P(k|k-1)

式中，I表示单位矩阵，P(k|k)、P(k|k-1)件作为下一次迭代相机的腿部姿态的控制输入，如此反复，得到移动机器人估计的状态；

步骤S35、相机中人腿转弯的姿态能对移动机器人的Z(k)函数进行修正，并提供给移动机器人估计行人转弯的角度。

进一步的，所述腿部圆弧特征识别算法为：移动机器人采用层次聚类圆弧分割的方式，去除噪声点并对环境进行分割；人腿的激光数据特征在噪声处理后为圆弧状排布；移动机器人将两个激光点云形成的圆弧视作人腿，两个圆弧的中心坐标为人腿的中心坐标；移动机器人通过实时计算移动机器人坐标和人腿的中心坐标，实现移动机器人对被跟踪行人的实时跟踪。

进一步的，所述腿部圆弧特征识别算法进一步包括如下步骤：

步骤S41、对激光数据进行预处理，分成几个聚类，计算每个聚类的总长度L_k和收尾激光点的直线距离D_k：

其中class_k[n]代表聚类后的结果，k表示聚类的符号，n表示一个聚类中元素的数量，i表示一个聚类中元素的符号，class_k[i].x、class_k[i].y分别代表第k个聚类中的第i个元素的横坐标和纵坐标；

步骤S42、计算每个聚类的弯曲度Curve[k]，k表示聚类的标号：

步骤S43、判断Curve[k]＞δ_c，且L_min＜L_k＜L_max，则把该聚类当成候选人腿的Leg簇，δ_c表示最小的曲率，L_min、L_max分别代表最小和最大的聚类长度；

步骤S44、计算所有候选行人人腿的Leg簇的中心坐标：

其中x_c和y_c分别代表第k个聚类的中心的横坐标和纵坐标，将所有聚类的中心坐标标记为Leg[n](x_c,y_c)，n代表聚类的数量；

步骤S45、计算Leg[n]中每两个Leg之间的距离Dist_Leg：

其中，Leg[k].x、Leg[k].y和Leg[k+1].x、Leg[k+1].y分别表示第k个Leg和第k+1个Leg位置的横坐标和纵坐标，将dist_max表示允许的最大行人双腿之间的距离，若Dist_Leg[k]＜dist_max，则计算Leg[k]和Leg[k+1]的中心位置，视为激光点数据中行人坐标：

x_p＝(Leg[k].x+Leg[k+1].x)/2

y_p＝(Leg[k].y+Leg[k+1].y)/2

其中，x_p和y_p分别代表行人的人腿中心的横坐标和纵坐标；

步骤S46、移动机器人通过实时计算移动机器人坐标和人腿中心坐标(x_p,y_p)，实现移动机器人对行人的实时跟踪。

本发明的有益效果在于：1、本方法提升了精度、扩大了移动机器人跟随人腿的使用范围。使用单一传感器的移动机器人跟随人腿精度低，使用范围窄。例如，使用单一相机的移动机器人不能在光线较强的环境下实现跟随人腿，使用单一激光雷达的移动机器人不能在障碍物复杂的环境下实现跟随人腿。

2、本方法提升了在转弯场景下移动机器人跟随的稳定性。移动机器人在转弯时候，不仅有激光雷达传感器进行距离的检测，还有相机传感器对行人姿态进行估计，行人转弯的姿态会反馈给移动机器人做下一状态的修正。

附图说明

图1为本发明的***框图。

图2为本发明的相机中姿态提取三角特征图。

图3为本发明的激光雷达中行人腿部圆弧状数据识别示意图。

图4为本发明的腿部圆弧特征识别算法识别流程图。

具体实施方式

为了助于理解本发明，下面将参照相关附图对本发明进行更全面的描述。附图中给出了本发明的可选实施例。但是，本发明可以以许多不同的形式来实现，并不限于本文所描述的实施例。相反地，提供这些实施例的目的是使对本发明的公开内容的理解更加透彻全面。

参见图1，图1为本发明的***框图，该***框图阐述了本发明的执行步骤。需要注意的是：步骤S2和步骤S4可以同时执行；一种视觉和激光融合的移动机器人跟踪人腿方法，所述方法包括如下步骤：

步骤S1、通过相机和激光雷达同时采集数据，其中，相机固定在移动机器人，用于实时拍摄机器人前方的路面信息，获取人腿的姿态信息；激光雷达固定在移动机器人，用于实时向周围发射激光，获取人腿的位置信息。移动机器人得到相机获取的姿态信息，则进入步骤S2；移动机器人得到激光雷达获取的位置信息，则进入步骤S4；

步骤S2、相机通过分析人体腿部的姿态信息，该姿态信息为：相机中的路面信息有人腿、环境等；移动机器人视人腿为有用信息，采用腿部姿态特征提取算法，提取相机中的人腿姿态，判断行人是直行还是转弯；如果是直行的姿态，移动机器人直接跟随行人直行；如果是转弯的姿态，则进入步骤S3；所述腿部姿态特征提取算法为：移动机器人采用三角特征贝叶斯步态识别的方式，把行人人腿姿态从环境中提取出来；行人的人腿姿态特征提取为几何形状，不同的几何形状归纳为直行的姿态和转弯的姿态；移动机器人通过实时计算人腿腿部的姿态，判断行人是直行还是转弯。

步骤S3、采用卡尔曼滤波算法获取转弯状态并通过运动控制实现移动机器人跟随行人转弯；

步骤S4、激光通过分析人体腿部的位置信息，采用腿部圆弧特征识别算法获取激光数据点中心坐标到移动机器人的实时距离，实现移动机器人对行人实时跟踪。

参见图2，图2为本发明的相机中姿态提取三角特征图，该三角特征图阐述了如何提取人腿姿态的三角特征信息。

第一步，对相机中的图像进行灰度处理，得到像素点低的黑白图像；第二步，用膨胀和腐蚀的方法来处理相机图像中的空洞、噪声点，得到噪声少的灰度图；第三步，对相机从上到下扫描，得到像素点低的行人腿部区域；第四步，将腿部特征提取为长度、角度变换三角形，即当三角形满足一定条件时，移动机器人判断行人转弯；第五步，将行人转弯的腿部姿态反馈给移动机器人，修正和调整机器人的转弯路线。该所述腿部姿态特征提取算法进一步包括如下步骤：

步骤S21、根据相机中背景的亮度来检测相机敏感性变化：

式中，a为相机中前景像素的灰度值，b为相机中背景像素的灰度值；

选择适当的阈值T，得到二值化的运动目标区域：

式中，a_xy表示(x,y)点的前景像素灰度值，b_xy表示(x,y)点的背景像素灰度值；T表示设定的阈值，D_xy表示二值化的运动目标区域；

步骤S22、用膨胀和腐蚀的方式来处理相机图像中运动目标区域的空洞、噪声点：

式中，

表示膨胀符号，F₁表示前景图像信息，F₂表示背景图像信息，F₂′表示F₂关于原点对称的集合，Φ表示空集；(F'₂)_x表示在F₂′中截取的x大小的背景图像信息作为卷积模板，

式中，

表示腐蚀符号；(F₂)_x表示在F₂中截取的x大小的背景图像信息作为卷积模板；

步骤S23、运动目标区域的空洞、噪声点进行处理后，再提取人腿区域：从上往下扫描，记录像素点为1的值；从左往右扫描，记录像素为0的值；

步骤S24、在人腿区域中，提取髋部中心位置为A、脚部中心位置为B、膝盖中心位置为C的三角形ΔABC；提取三角形的以下参数作为腿部姿态特征：

x：腿部重心坐标(x_c,y_c)的横坐标x_c；

y：腿部区域髋部线纵坐标k_r*h，h为人体的高度，k_r为髋部位置在人体高度的比例；

θ：∠BAC的值；

l₁、l₂、l₃：三角形三边线段AB、BC及AC长度；

步骤S25、计算腿部姿态特征向量，该向量由两个相邻姿态联合表示：

F＝(f_min,f_max)

f_min＝(x_min,y_min,l_1min,l_2min,l_3min,θ_min)

f_max＝(x_max,y_max,l_1max,l_2max,l_3max,θ_max)

式中，F表示腿部姿态特征向量，f_min表示最小腿部姿态的特征向量，f_max表示最大腿部姿态的特征向量；x_min和x_max表示腿部重心坐标中横坐标的最小值和最大值；y_min和y_max表示腿部区域髋部线纵坐标的最小值和最大值；l_1min和l_1max表示腿部姿态特征中ΔABC三边线段AB的最小值和最大值；l_2min和l_2max表示腿部姿态特征中ΔABC三边线段BC的最小值和最大值；l_3min和l_3max表示腿部姿态特征中ΔABC三边线段AC的最小值和最大值；θ_min和θ_max表示腿部姿态特征中ΔABC中∠BAC的的最小值和最大值；

步骤S26、计算人体侧影重心坐标：

式中，N_b是人体腿部区域像素总数，(x_i,y_i)人体腿部区域的像素；

步骤S27、已知三顶点坐标的三角形，三边距离根据两点间的距离公式得到：

其中，l为三角形的任意的三个边，即l₁、l₂、l₃；其中一个边的两点的坐标即为(x₁,y₁)、(x₂,y₂)；

步骤S28、根据F、l和cosθ的值，对移动机器人状态使用卡尔曼滤波算法进行修正。

所述采用卡尔曼滤波算法获取转弯状态，进一步具体为：移动机器人转弯前，当前状态为直行状态，下一状态为转弯状态；通过相机中人腿转弯的姿态检测能对移动机器人进行状态修正，并提供给移动机器人以行人转弯角度的预测；通过对当前状态的修正和下一状态的预测，实现移动机器人跟随行人平稳的转弯。

所述卡尔曼滤波算法进一步包括如下步骤：

步骤S31、根据移动机器人***中状态转移方程和移动机器人转弯前上一时刻的状态，预测下一时刻的状态：

X(k)＝AX(k-1)+BU(k)+W(k)

式中，X(k)、X(k-1)表示状态向量，A表示包含F、l和cosθ的状态矩阵，BU(k)表示相机的腿部姿态的控制输入，W(k)表示高斯随机性向量；

步骤S32、计算X(k-1)对应的协方差P(k|k-1)：

P(k|k-1)＝AP(k-1|k-1)A'+Q，其中P(k|k-1)表示X(k-1)对应的协方差，P(k-1|k-1)表示前一时刻协方差，A'表示矩阵A的逆矩阵，Q为***噪声对应的协方差；

步骤S33、根据预测结果更新当前状态：

X(k|k)＝X(k|k-1)+K_g(k)(Z(k)-HX(k|k-1))

式中，X(k|k)表示根据预测结果得到的当前状态，X(k|k-1)表示预测结果的状态，K_g(k)为卡尔曼增益，Z(k)表示受人体姿态影响的输入方程，H表示协方差状态下包含F、l和cosθ的状态矩阵；

其中，K_g(k)为卡尔曼增益，P(k|k-1)表示X(k|k-1)对应的协方差，H'表示H相关的逆矩阵，R表示预测噪声对应的协方差矩阵；

步骤S34、计算当前时刻状态对应的协方差：

P(k|k)＝(I-K_g(k)H)P(k|k-1)

式中，I表示单位矩阵，P(k|k)、P(k|k-1)件作为下一次迭代相机的腿部姿态的控制输入，如此反复，得到移动机器人估计的状态；

步骤S35、相机中人腿转弯的姿态能对移动机器人的Z(k)函数进行修正，并提供给移动机器人估计行人转弯的角度。

参见图3，图3为本发明的激光雷达中行人腿部圆弧状数据识别示意图，该示意图阐述了如何识别激光点数据的人腿位置。所述腿部圆弧特征识别算法为：移动机器人采用层次聚类圆弧分割的方式，去除噪声点并对环境进行分割；人腿的激光数据特征在噪声处理后为圆弧状排布；移动机器人将两个激光点云形成的圆弧视作人腿，两个圆弧的中心坐标为人腿的中心坐标；移动机器人通过实时计算移动机器人坐标和人腿的中心坐标，实现移动机器人对被跟踪行人的实时跟踪。

参见图4，图4为本发明的腿部圆弧特征识别算法识别流程图；所述腿部圆弧特征识别算法进一步包括如下步骤：

步骤S41、对激光数据进行预处理，分成几个聚类，计算每个聚类的总长度L_k和收尾激光点的直线距离D_k：

其中class_k[n]代表聚类后的结果，k表示聚类的符号，n表示一个聚类中元素的数量，i表示一个聚类中元素的符号，class_k[i].x、class_k[i].y分别代表第k个聚类中的第i个元素的横坐标和纵坐标；

步骤S42、计算每个聚类的弯曲度Curve[k]，k表示聚类的标号：

步骤S43、判断Curve[k]＞δ_c，且L_min＜L_k＜L_max，则把该聚类当成候选人腿的Leg簇，δ_c表示最小的曲率，L_min、L_max分别代表最小和最大的聚类长度；

步骤S44、计算所有候选行人人腿的Leg簇的中心坐标：

其中x_c和y_c分别代表第k个聚类的中心的横坐标和纵坐标，将所有聚类的中心坐标标记为Leg[n](x_c,y_c)，n代表聚类的数量；

步骤S45、计算Leg[n]中每两个Leg之间的距离Dist_Leg：

其中，Leg[k].x、Leg[k].y和Leg[k+1].x、Leg[k+1].y分别表示第k个Leg和第k+1个Leg位置的横坐标和纵坐标，将dist_max表示允许的最大行人双腿之间的距离，若Dist_Leg[k]＜dist_max，则计算Leg[k]和Leg[k+1]的中心位置，视为激光点数据中行

人坐标：

x_p＝(Leg[k].x+Leg[k+1].x)/2

y_p＝(Leg[k].y+Leg[k+1].y)/2

其中，x_p和y_p分别代表行人的人腿中心的横坐标和纵坐标；

步骤S46、移动机器人通过实时计算移动机器人坐标和人腿中心坐标(x_p,y_p)，实现移动机器人对行人的实时跟踪。

总之，本发明提供了一种鲁棒性强、精度高的视觉和激光融合的跟踪人体腿部的方法；通过相机和激光雷达同时采集数据，相机通过分析人体腿部的姿态，采用腿部姿态特征提取算法，判断移动机器人是否需要转弯；采用卡尔曼滤波算法，预测人体腿部的位置；激光通过分析人体腿部的中心和移动机器人的距离，采用腿部圆弧特征识别算法，实现移动机器人对行人实时跟踪。本发明提高了移动机器人对人体腿部跟踪的准确性。

以上所述仅为本发明的较佳实施例，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本发明的涵盖范围。

Claims (7)

1.一种视觉和激光融合的移动机器人跟踪人腿方法，其特征在于：所述方法包括如下步骤：

步骤S1、通过相机和激光雷达同时采集数据，相机获取人腿的姿态信息，激光雷达获取人腿的位置信息；移动机器人得到相机获取的姿态信息，则进入步骤S2；移动机器人得到激光雷达获取的位置信息，则进入步骤S4；

步骤S2、相机通过分析人体腿部的姿态信息，采用腿部姿态特征提取算法，判断行人是直行还是转弯；如果是直行的姿态，移动机器人直接跟随行人直行；如果是转弯的姿态，则进入步骤S3；

步骤S3、采用卡尔曼滤波算法获取转弯状态并通过运动控制实现移动机器人跟随行人转弯；

步骤S4、激光通过分析人体腿部的位置信息，采用腿部圆弧特征识别算法获取激光数据点中心坐标到移动机器人的实时距离，实现移动机器人对行人实时跟踪。

2.根据权利要求1所述的一种视觉和激光融合的移动机器人跟踪人腿方法，其特征在于：所述腿部姿态特征提取算法为：移动机器人采用三角特征贝叶斯步态识别的方式，把行人人腿姿态从环境中提取出来；行人的人腿姿态特征提取为几何形状，不同的几何形状归纳为直行的姿态和转弯的姿态；移动机器人通过实时计算人腿腿部的姿态，判断行人是直行还是转弯。

3.根据权利要求2所述的一种视觉和激光融合的移动机器人跟踪人腿方法，其特征在于：所述腿部姿态特征提取算法进一步包括如下步骤：

步骤S21、根据相机中背景的亮度来检测相机敏感性变化：

式中，a为相机中前景像素的灰度值，b为相机中背景像素的灰度值；

选择适当的阈值T，得到二值化的运动目标区域：

式中，a_xy表示(x,y)点的前景像素灰度值，b_xy表示(x,y)点的背景像素灰度值；T表示设定的阈值，D_xy表示二值化的运动目标区域；

步骤S22、用膨胀和腐蚀的方式来处理相机图像中运动目标区域的空洞、噪声点：

式中，

表示膨胀符号，F₁表示前景图像信息，F₂表示背景图像信息，F₂′表示F₂关于原点对称的集合，Φ表示空集；(F'₂)_x表示在F₂′中截取的x大小的背景图像信息作为卷积模板，

式中，

表示腐蚀符号；(F₂)_x表示在F₂中截取的x大小的背景图像信息作为卷积模板；

步骤S23、运动目标区域的空洞、噪声点进行处理后，再提取人腿区域：从上往下扫描，记录像素点为1的值；从左往右扫描，记录像素为0的值；

步骤S24、在人腿区域中，提取髋部中心位置为A、脚部中心位置为B、膝盖中心位置为C的三角形ΔABC；提取三角形的以下参数作为腿部姿态特征：

x：腿部重心坐标(x_c,y_c)的横坐标x_c；

y：腿部区域髋部线纵坐标k_r*h，h为人体的高度，k_r为髋部位置在人体高度的比例；

θ：∠BAC的值；

l₁、l₂、l₃：三角形三边线段AB、BC及AC长度；

步骤S25、计算腿部姿态特征向量，该向量由两个相邻姿态联合表示：

F＝(f_min,f_max)

f_min＝(x_min,y_min,l_1min,l_2min,l_3min,θ_min)

f_max＝(x_max,y_max,l_1max,l_2max,l_3max,θ_max)

式中，F表示腿部姿态特征向量，f_min表示最小腿部姿态的特征向量，f_max表示最大腿部姿态的特征向量；x_min和x_max表示腿部重心坐标中横坐标的最小值和最大值；y_min和y_max表示腿部区域髋部线纵坐标的最小值和最大值；l_1min和l_1max表示腿部姿态特征中ΔABC三边线段AB的最小值和最大值；l_2min和l_2max表示腿部姿态特征中ΔABC三边线段BC的最小值和最大值；l_3min和l_3max表示腿部姿态特征中ΔABC三边线段AC的最小值和最大值；θ_min和θ_max表示腿部姿态特征中ΔABC中∠BAC的的最小值和最大值；

步骤S26、计算人体侧影重心坐标：

式中，N_b是人体腿部区域像素总数，(x_i,y_i)人体腿部区域的像素；

步骤S27、已知三顶点坐标的三角形，三边的距离根据两点间的距离公式得到：

其中，l为三角形的任意的三个边，即l₁、l₂、l₃；其中一个边的两点的坐标即为(x₁,y₁)、(x₂,y₂)；

步骤S28、根据F、l和cosθ的值，对移动机器人状态使用卡尔曼滤波算法进行修正。

4.根据权利要求1所述的一种视觉和激光融合的移动机器人跟踪人腿方法，其特征在于：所述采用卡尔曼滤波算法获取转弯状态，进一步具体为：移动机器人转弯前，当前状态为直行状态，下一状态为转弯状态；通过相机中人腿转弯的姿态检测能对移动机器人进行状态修正，并提供给移动机器人以行人转弯角度的预测；通过对当前状态的修正和下一状态的预测，实现移动机器人跟随行人平稳的转弯。

5.根据权利要求3所述的一种视觉和激光融合的移动机器人跟踪人腿方法，其特征在于：所述卡尔曼滤波算法进一步包括如下步骤：

步骤S31、根据移动机器人***中状态转移方程和移动机器人转弯前上一时刻的状态，预测下一时刻的状态：

X(k)＝AX(k-1)+BU(k)+W(k)

式中，X(k)、X(k-1)表示状态向量，A表示包含F、l和cosθ的状态矩阵，BU(k)表示相机的腿部姿态的控制输入，W(k)表示高斯随机性向量；

步骤S32、计算X(k-1)对应的协方差P(k|k-1)：

P(k|k-1)＝AP(k-1|k-1)A'+Q，其中P(k|k-1)表示X(k-1)对应的协方差，P(k-1|k-1)表示前一时刻协方差，A'表示矩阵A的逆矩阵，Q为***噪声对应的协方差；

步骤S33、根据预测结果更新当前状态：

X(k|k)＝X(k|k-1)+K_g(k)(Z(k)-HX(k|k-1))

式中，X(k|k)表示根据预测结果得到的当前状态，X(k|k-1)表示预测结果的状态，K_g(k)为卡尔曼增益，Z(k)表示受人体姿态影响的输入方程，H表示协方差状态下包含F、l和cosθ的状态矩阵；

其中，K_g(k)为卡尔曼增益，P(k|k-1)表示X(k|k-1)对应的协方差，H'表示H相关的逆矩阵，R表示预测噪声对应的协方差矩阵；

步骤S34、计算当前时刻状态对应的协方差：

P(k|k)＝(I-K_g(k)H)P(k|k-1)

式中，I表示单位矩阵，P(k|k)、P(k|k-1)件作为下一次迭代相机的腿部姿态的控制输入，如此反复，得到移动机器人估计的状态；

步骤S35、相机中人腿转弯的姿态能对移动机器人的Z(k)函数进行修正，并提供给移动机器人估计行人转弯的角度。

6.根据权利要求1所述的一种视觉和激光融合的移动机器人跟踪人腿方法，其特征在于：所述腿部圆弧特征识别算法为：移动机器人采用层次聚类圆弧分割的方式，去除噪声点并对环境进行分割；人腿的激光数据特征在噪声处理后为圆弧状排布；移动机器人将两个激光点云形成的圆弧视作人腿，两个圆弧的中心坐标为人腿的中心坐标；移动机器人通过实时计算移动机器人坐标和人腿的中心坐标，实现移动机器人对被跟踪行人的实时跟踪。

7.根据权利要求6所述的一种视觉和激光融合的移动机器人跟踪人腿方法，其特征在于：所述腿部圆弧特征识别算法进一步包括如下步骤：

步骤S41、对激光数据进行预处理，分成几个聚类，计算每个聚类的总长度L_k和收尾激光点的直线距离D_k：

其中class_k[n]代表聚类后的结果，k表示聚类的符号，n表示一个聚类中元素的数量，i表示一个聚类中元素的符号，class_k[i].x、class_k[i].y分别代表第k个聚类中的第i个元素的横坐标和纵坐标；

步骤S42、计算每个聚类的弯曲度Curve[k]，k表示聚类的标号：

步骤S43、判断Curve[k]＞δ_c，且L_min＜L_k＜L_max，则把该聚类当成候选人腿的Leg簇，δ_c表示最小的曲率，L_min、L_max分别代表最小和最大的聚类长度；

步骤S44、计算所有候选行人人腿的Leg簇的中心坐标：

其中x_c和y_c分别代表第k个聚类的中心的横坐标和纵坐标，将所有聚类的中心坐标标记为Leg[n](x_c,y_c)，n代表聚类的数量；

步骤S45、计算Leg[n]中每两个Leg之间的距离Dist_Leg：

其中，Leg[k].x、Leg[k].y和Leg[k+1].x、Leg[k+1].y分别表示第k个Leg和第k+1个Leg位置的横坐标和纵坐标，将dist_max表示允许的最大行人双腿之间的距离，若Dist_Leg[k]＜dist_max，则计算Leg[k]和Leg[k+1]的中心位置，视为激光点数据中行人坐标：

x_p＝(Leg[k].x+Leg[k+1].x)/2

y_p＝(Leg[k].y+Leg[k+1].y)/2

其中，x_p和y_p分别代表行人的人腿中心的横坐标和纵坐标；

步骤S46、移动机器人通过实时计算移动机器人坐标和人腿中心坐标(x_p,y_p)，实现移动机器人对行人的实时跟踪。

Images