一种基于多传感器融合的移动机器人自主跟随系统及方法与流程
本发明涉及工业自动化技术领域，尤其涉及一种基于多传感器融合的移动机器人自主跟随系统及方法。
背景技术：
伴随着机器人从工业环境逐渐过渡到家庭或个人应用领域中，人与机器人之间的直接互动已经成为一个新型的研究领域，其中检测并且跟随人是一项必要技能。近几年，对于跟随机器人的研究和应用不断深入，广泛应用于医院、商场和战场等环境中，帮助普通大众在日常生活或工作环境中完成一些简单任务。机器人若要实现快速稳定的跟随行人，必须能够获得目标人员的准确位置信息。
目前常见的定位技术包括室内和室外定位技术：基于实时定位与建图(slam)技术，基于gps的室内外定位技术，基于室内信标系统的定位技术等。
目前对于人员的识别检测这一小范围的定位应用场景，使用gps成本过高并且精度不太适用，因此常采用slam的方式或者基于信标基站系统进行定位。在slam中视觉的方式通过检测图像中行人的位置来进行人员定位，成本较低而且相关应用算法成熟。但由于相机视野有一定局限性，容易出现行人脱离视野从而导致跟踪失败的现象。并且视觉传感器都有相同的痛点，对于光照条件比较敏感，并且不适用于室外环境。在slam中激光传感器有着视野宽阔的优点，可以扫描到整个360度的范围，因此在人员检测中也有很多的应用。但激光检测人员时，仅仅使用了人的轮廓信息，无法区分不同的人员，在人员较多、干扰较大的场合容易发生误识别。并且对于跟随人员被障碍物遮挡的情况也无法有效的处理。信标系统的优点在于人员可以手持一个信标，通过车载的信标基站可以获得相对的位置信息。可以应用于人员被障碍物遮挡此类视觉和激光传感器无法识别的情况中，但信标系统的信号有时会出现较大的漂移而且数据信息不够稳定。
技术实现要素：
本发明要解决的技术问题是针对上述现有技术的不足，提供一种基于多传感器融合的移动机器人自主跟随系统及方法，使移动机器人能够自主跟随目标人物。
为解决上述技术问题，本发明所采取的技术方案是：一方面，本发明提供一种基于多传感器融合的移动机器人自主跟随系统，包括上层导航单元、底层运动控制单元及供电单元；所述上层导航单元包括二维激光雷达、路由器、aoa信标系统、摄像头、工控机及ttl转usb模块；所述底层运动控制单元包括机器人本体、嵌入式开发板和光电编码器；
所述二维激光雷达用于检测固定范围内的平面控制位置信息，通过路由器的一个lan口与工控机相连，确保了激光雷达与工控机之间数据传输的稳定性、安全性和实时性；嵌入式开发板用于实现对机器人的运动控制，与路由器的第二个lan口连接；工控机用于实现上层的导航规划，与路由器的第三个lan口连接，通过路由器的有线连接，确保了工控机向底层运动控制单元发送控制指令，进而控制机器人的运动速度和运动方向；所述摄像头安装在机器人前方顶部，用于获取当前视野下的图像信息，与工控机连接，确保了实时有效的图像信息传输；所述aoa信标系统包括aoa信标基站和aoa手持信标，其中，aoa信标基站安装在机器人上，aoa手持信标在要跟随的目标人员手中，aoa信标基站获得与aoa手持信标的相对位姿；所述aoa信标基站通过ttl转usb模块与工控机连接，使得工控机实时接收aoa手持信标的信息，从而实现激光雷达和aoa信标系统信息的数据融合；所述机器人移动的车轮采用直流减速电机；所述嵌入式开发板连接电机驱动模块，电机驱动模块连接直流减速电机，光电编码器安装在机器人车轮轴处，与嵌入式开发板连接用于获得机器人车轮速度；所述工控机内置程序用于实现目标人员的检测及跟随路径的规划；所述供电单元分别连接上层导航单元和底层控制单元，为整个系统供电。
优选地，所述供电单元包括车载电池和电源管理模块，其中，电源管理模块连接车载电池，将车载电池电压转换为系统中各部件所需要的电压后与各部件进行连接，为整个系统供电。
优选地，所述机器人本体采用双轮差分小车。
优选地，所述工控机内置的程序，包括人员检测单元和跟随导航单元，用于实现以下功能：
(1)对二维激光雷达和摄像头采集的待检测人员的信息数据进行处理；
(2)融合来自二维激光、摄像头以及aoa信标系统的信息，获取一个更加精确的目标人员定位信息；
(3)为目标人员分配对应的id，将二维激光和摄像头处理后的数据分类存储，将新的匹配对象创建为跟踪对象，并进行保存；
(4)对机器人向目标人员运动的轨迹进行规划计算，选出局部路径最优的规划轨迹；
(5)向底层运动控制单元发送控制指令，进而控制机器人的运动速度和运动方向。
另一方面，本发明还提供一种基于多传感器融合的移动机器人自主跟随方法，包括以下步骤：
步骤1、通过二维激光雷达和摄像头采集待检测人员的信息数据，并对采集的数据进行处理，识别出目标人员；
所述对于二维激光雷达采集的数据进行处理的方法为：首先对激光返回的点进行聚类，小于一定阈值的点被划分为一个聚类，对这些聚类生成几何特征；所述几何特征包括激光点的数量、激光簇的宽度长度、相对激光的距离和角度；基于这些几何特征通过随机森林分类器进行分类，训练出人腿部模型的适应特征；然后将激光数据中的激光簇进行特征提取，将提取到的特征与通过随机森林训练出的适应特征做对比，以检测出周围环境的人腿；当检测到的两条腿的距离小于0.4m时，取两条腿位置的平均值作为新腿的位置；
所述对于摄像头采集的数据进行处理的方法为：对于每一帧图像采用hog特征提取的方法计算不同像素块的梯度值，然后根据计算出来的梯度值将提取出来的特征送入支持向量机(supportvectormachine，即svm)分类器中进行训练，训练出人体的适应特征；然后将视觉数据中提取到的特征与分类器训练出的特征作对比，以识别出视野中的目标人员，从而实现对于人员的识别；
步骤2、对激光和摄像头获得的行人腿部信息和行人图像信息采用匈牙利算法进行对应人员的信息匹配处理，将两者识别的信息按照对应的规则进行匹配，从而获得一组对应的视觉识别的人员和激光识别的人员的融合位置；然后采用基于卡尔曼滤波器(kalmanfilter，kf)的交互式多模型(interactingmultiplemodel，imm)滤波器融合来自二维激光、摄像头以及aoa信标系统的信息，获取一个更加精确的目标人员定位信息；
步骤3、为目标人员分配对应的id，将二维激光和摄像头处理后的数据分类存储，将新的匹配对象创建为跟踪对象，并进行保存，对于跟踪失败的目标进行移除，以区分不同的目标人员；
步骤4、采用快速行进算法(fastmarchingmethod，即fmm)生成一个目标势场，然后通过改进的动态窗口算法(dynamicwindowapproach，即dwa)增加方向梯度场的衡量指标，来约束机器人的规划轨迹，进而选出局部路径最优的规划轨迹，具体方法为：
首先使用激光传感器感知环境信息，建立一个以机器人为中心的滚动栅格地图，为了衡量地图中每个点到达目标点的时间t，采用fmm算法在滚动栅格地图上建立一个目标势场，坐标点(x，y)到达目标位置的时间用t(x，y)表示；对这个势场进行梯度求导得到一个方向梯度场，该方向梯度场提供了地图上每一个坐标机器人的参考方位角θ(x，y)；
为了选出机器人向目标运动的最优轨迹，采用如下评价方式：
首先为了使机器人向目标点有效运动，引入评价机器人运动有效性的目标代价函数，如下公式所示：
其中，goal_cost为轨迹有效性代价，用来评价该条轨迹是否向到达时间场数值低的位置运动；β为机器人方位角的影响因子，(xe，ye)为机器人轨迹终点位置坐标，θe为机器人轨迹终点的方位角，θr(xe，ye)为机器人在轨迹终点位置时，方向梯度场提供的参考方位角，t(xe，ye)为机器人到达轨迹终点的时间；
当轨迹终点的方向与方向梯度场提供的参考方位角差值增大时，goal_cost就放大一定倍数，从而使轨迹评优时更倾向于选择与向量场参考方向相符的轨迹；
为了评价运动轨迹终点朝向目标的代价，引入评价机器人运动方向有效性的角度代价函数，如下公式所示：
其中，t(xs，ys)为机器人运动轨迹初始点的到达时间，d(xs，ys)为机器人运动轨迹初始点的最近障碍物距离，α为障碍物影响因子，用来评定障碍物对于规划的路径的影响；
用目标代价函数和角度代价函数的和作为总体代价函数来评价机器人向目标点行进的这些轨迹的优劣性，并通过最小化总体代价函数以此来选出局部路径最优的规划轨迹；
所述总体代价函数如下公式所示：
total_cost＝goal_cost+angel_cost
其中，goal_cost为目标代价函数，angel_cost为角度代价函数。
采用上述技术方案所产生的有益效果在于：本发明提供的一种基于多传感器融合的移动机器人自主跟随系统及方法，采用卡尔曼滤波算法，利用激光和视觉传感器检测信息纠正aoa信息，消除一些过渡振荡，得到平滑的人员运动轨迹。同时采用改进的dwa算法，在目标势场的基础上，生成了一个方向梯度场，该梯度场提供了地图上每一个坐标机器人的参考方位角，用梯度场来衡量机器人方向的有效性。由此可以避免机器人盲目地向目标靠近，而忽略了方向角的调整。可以使得移动机器人在动态遮挡环境下，包括目标人员被障碍物遮挡的情况下，实现稳定的人员自主跟随功能。该方法可以适用于多种机器人运动模型，适用于不同的工作场景，适用范围更广，应用性更强。
附图说明
图1为本发明实施例提供的一种基于多传感器融合的移动机器人自主跟随系统的结构框图；
图2为本发明实施例提供的一种基于多传感器融合的移动机器人自主跟随方法的流程图；
图3为本发明基于多种传感器融合的移动机器人自主跟随方法的跟随导航流程图；
图4为本发明实施例提供的fmm算法建立的方向梯度场示意图；
图5为本发明实施例提供的机器人运动过程建模的示意图；
图6为本发明实施例提供的改进dwa算法的示意图，其中，(a)为改进dwa算法在机器人正常运动时给出的运动方向，(b)为改进dwa算法在机器人遇到障碍物时给出的运动方向，(c)为机器人运动方向与改进dwa算法所给参考方向相同时的示意图；
图7为本发明实施例提供的只使用aoa标签得到的人员位置轨迹与通过卡尔曼滤波融合得到的人员位置轨迹对比图；
图8为本发明实施例提供的只通过激光和摄像头得到的人员位置检测结果和卡尔曼滤波融合的人员位置检测结果对比图；
图9为本发明实施例提供的在障碍物环境中跟随目标人员的运动轨迹示意图。
图中，1、只使用aoa信息的人员轨迹；2、通过卡尔曼滤波融合aoa、激光以及摄像头信息得到的人员轨迹；3、激光和摄像头联合检测得到的人员轨迹；4、机器人跟随目标移动的运动轨迹；5、目标人员实际运动轨迹。
具体实施方式
下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。
一种基于多传感器融合的移动机器人自主跟随系统，如图1所示，包括上层导航单元、底层运动控制单元及供电单元；所述上层导航单元包括二维激光雷达、路由器、aoa信标系统、摄像头、工控机及ttl转usb模块；所述底层运动控制单元包括机器人本体、嵌入式开发板和光电编码器；所述机器人本体采用双轮差分小车。
所述二维激光雷达用于检测固定范围内的平面控制位置信息，通过路由器的一个lan口与工控机相连，确保了激光雷达与工控机之间数据传输的稳定性、安全性和实时性；嵌入式开发板用于实现对机器人的运动控制，与路由器的第二个lan口连接；工控机用于实现上层的导航规划，与路由器的第三个lan口连接，通过路由器的有线连接，确保了工控机向底层运动控制单元发送控制指令，进而控制机器人的运动速度和运动方向；所述摄像头安装在机器人前方顶部，用于获取当前视野下的图像信息，与工控机连接，确保了实时有效的图像信息传输；所述aoa信标系统包括aoa信标基站和aoa手持信标，其中，aoa信标基站安装在机器人上，aoa手持信标在要跟随的目标人员手中，aoa信标基站获得与aoa手持信标的相对位姿；所述aoa信标基站通过ttl转usb模块与工控机连接，使得工控机实时接收aoa手持信标的信息，从而实现激光雷达和aoa信标系统信息的数据融合；所述机器人移动的车轮采用直流减速电机；所述嵌入式开发板连接电机驱动模块，电机驱动模块连接直流减速电机，光电编码器安装在机器人车轮轴处，与嵌入式开发板连接用于获得机器人车轮速度；所述供电单元分别连接上层导航单元和底层控制单元，为整个系统供电。所述供电单元包括车载电池和电源管理模块，其中，电源管理模块连接车载电池，将车载电池电压转换为系统中各部件所需要的电压后与各部件进行连接，为整个系统供电。
所述工控机内置程序，用于实现目标人员的检测及跟随路径的规划，包括人员检测单元和跟随导航单元，用于实现以下功能：
(1)对二维激光雷达和摄像头采集的待检测人员的信息数据进行处理；
(2)融合来自二维激光、摄像头以及aoa信标系统的信息，获取一个更加精确的目标人员定位信息；
(3)为目标人员分配对应的id，将二维激光和摄像头处理后的数据分类存储，将新的匹配对象创建为跟踪对象，并进行保存；
(4)对机器人向目标人员运动的轨迹进行规划计算，选出局部路径最优的规划轨迹；
(5)向底层运动控制单元发送控制指令，进而控制机器人的运动速度和运动方向。
本实施例中，嵌入式控制板的型号为stm32f407vet6；工控机的型号为占美gk400；ttl转usb模块的型号为ch340c；2d激光雷达的型号是pepperl+fuchs；摄像头采用彩色单目相机，型号为华硕xtionpro，工控机底层操作系统为ubuntu16.04lst；次级操作系统为ros系统；车载电池型号为凯美威12v100a锂电池；电源管理模块型号为sd-50b-12；电机驱动模块型号为zlac706；路由器型号为netgearr6020；信标基站和手持信标为aoa。
一种基于多传感器融合的移动机器人自主跟随方法，如图2所示，包括以下步骤：
步骤1、通过二维激光雷达和摄像头采集待检测人员的信息数据，并对采集的数据进行处理，识别出目标人员；
所述对于二维激光雷达采集的数据进行处理的方法为：首先对激光返回的点进行聚类，小于一定阈值的点被划分为一个聚类，对这些聚类生成几何特征；所述几何特征包括激光点的数量、激光簇的宽度长度、相对激光的距离和角度；基于这些几何特征通过随机森林分类器进行分类，训练出人腿部模型的适应特征；然后将激光数据中的激光簇进行特征提取，将提取到的特征与通过随机森林训练出的适应特征做对比，以检测出周围环境的人腿；当检测到的两条腿的距离小于0.4m时，取两条腿位置的平均值作为新腿的位置；
所述对于摄像头采集的数据进行处理的方法为：对于每一帧图像采用hog特征提取的方法计算不同像素块的梯度值，然后根据计算出来的梯度值将提取出来的特征送入支持向量机(supportvectormachine，即svm)分类器中进行训练，训练出人体的适应特征；然后将视觉数据中提取到的特征与分类器训练出的特征作对比，以识别出视野中的目标人员，从而实现对于人员的识别；
步骤2、对激光和摄像头获得的行人腿部信息和行人图像信息采用匈牙利算法进行对应人员的信息匹配处理，将两者识别的信息按照对应的规则进行匹配，从而获得一组对应的视觉识别的人员和激光识别的人员的融合位置；然后采用基于卡尔曼滤波器(kalmanfilter，kf)的交互式多模型(interactingmultiplemodel，imm)滤波器融合来自二维激光、摄像头以及aoa信标系统的信息，获取一个更加精确的目标人员定位信息；
匈牙利算法是一种在多项式时间内求解任务分配问题的组合优化算法。该算法通过将两者识别的信息按照对应的规则进行匹配，从而获得一组对应的视觉识别的人员和激光识别的人员的融合位置。得到的融合数据如下公式所示，n为t时刻检测到的人员数量。在t时刻检测到的不同人员位置表示为
imm算法引入多个目标运动模型，具有自适应的特点，能够有效的对各个模型的概率进行调整，并按照相应的概率对每个模型的状态估计进行加权，实现对运动目标的跟踪。imm算法包含了多个滤波器、一个交互式作用器、一个模型概率更新器和一个估计混合器，多模型通过交互作用跟踪一个目标的机动运动，各模型之间的转移由马尔可夫概率转移矩阵确定。本发明将前述的kf滤波器作为imm算法中的滤波器，对目标人员的建立了多个运动模型，采用kf-imm算法融合来自激光、视觉、以及aoa标签的信息。
imm算法的一个关键因素是目标运动模型的确定，它应尽可能真实的反应目标的实际运动情况。本发明对于目标人员的常见运动为例研究其运动模型。
一般运动模型如下，分为预测过程和更新过程。目标人员的预测过程和更新过程表示为：
x(k)＝f(k-1)x(k-1)+w(k-1)
z(k)＝hx(k)+v(k)
其中，k表示采样时刻；x(k)∈rn是预测过程的状态向量；f是n维系统转移矩阵；z(k)∈rm是更新过程的测量向量；w(k)～n(0，r)和v(k)～n(0，q)分别是高斯过程噪声和测量噪声；其中状态向量和测量向量根据所选择的模型的不同而不同。
本实施例对跟随的目标人员的运动过程建模，目标人员的运动模型分为三种，分别是匀速直线运动模型(constantvelocity，cv)、匀加速运动模型(constantaccelerator，ca)和匀速转弯运动模型(constantturn，ct)。
在ca、ct和cv运动模型的动态模型中，x，y表示目标人员的位置，
(1)cv模型，选择状态变量
x(k)＝fcv(k-1)x(k-1)+w(k-1)
其中，fcv＝diag{a，a}，a2×2为牛顿矩阵；w(k-1)＝[wx，wy]′k-1是零均值高斯白噪声。
(2)ct模型，选择状态变量
x(k)＝fct(k-1)x(k-1)+w(k-1)
其中，w(k-1)＝[wx，wy，wω]′是零均值高斯白噪声，
(3)ca模型，选择状态变量
x(k)＝fca(k-1)x(k-1)+w(k-1)
其中，fca＝diag{a，a}，a3×3为牛顿矩阵；w(k-1)＝[wx，wy]′k-1是零均值高斯白噪声。
在预测阶段，分别为每种运动模型分配一个概率值作为初始概率值。设每一个对应的运动模型对应的初始概率值为：
本实施例中对目标人员定位的过程如下：使用aoa标签获取的位置作为人员状态初值x。人员状态由人员在全局坐标系下坐标(x，y)表示。卡尔曼滤波的预测过程对于三种人员运动模型都进行估计，根据上述的运动模型可获得对应的状态转移矩阵f和运动噪声w，其中f∈(fca，fcv，fct)。对来自二维激光、摄像头以及aoa信标系统的信息进行融合，获取一个更加精确的目标人员定位信息，具体包括两个步骤：第一个步骤采用aoa标签信息进行更新。由于aoa标签获得的位置信息波动较大，对此数据进行滑动滤波得到
表1卡尔曼滤波算法伪代码
步骤3、为目标人员分配对应的id，将二维激光和摄像头处理后的数据分类存储，将新的匹配对象创建为跟踪对象，并进行保存，对于跟踪失败的目标进行移除，以区分不同的目标人员；
步骤4、采用快速行进算法(fastmarchingmethod，即fmm)生成一个目标势场，然后通过改进的动态窗口算法(dynamicwindowapproach，即dwa)增加方向梯度场的衡量指标，来约束机器人的规划轨迹，进而选出局部路径最优的规划轨迹，如图3所示，具体方法为：
首先使用激光传感器感知环境信息，建立一个以机器人为中心的滚动栅格地图，为了衡量地图中每个点到达目标点的时间t，采用fmm算法在滚动栅格地图上建立一个目标势场，坐标点(x，y)到达目标位置的时间用t(x，y)表示；对这个势场进行梯度求导得到一个方向梯度场，如图4所示，该方向梯度场提供了地图上每一个坐标机器人的参考方位角θ(x，y)；
为了选出机器人向目标运动的最优轨迹，采用如下评价方式：
首先为了使机器人向目标点有效运动，引入评价机器人运动有效性的目标代价函数，如下公式所示：
其中，goal_cost为轨迹有效性代价，用来评价该条轨迹是否向到达时间场数值低的位置运动；β为机器人方位角的影响因子，(xe，ye)为机器人轨迹终点位置坐标，θe为机器人轨迹终点的方位角，θr(xe，ye)为机器人轨迹终点位置时，方向梯度场提供的参考方位角，t(xe，ye)为机器人到达轨迹终点的时间；
当轨迹终点的方向与方向梯度场提供的参考方位角差值增大时，goal_cost就放大一定倍数，从而使轨迹评优时更倾向于选择与向量场参考方向相符的轨迹；
引入的评价机器人运动有效性的目标代价函数，其实质是对机器人运动过程的建模，如图5所示。
为了评价运动轨迹终点朝向目标的代价，引入评价机器人运动方向有效性的角度代价函数，如下公式所示：
其中，t(xs，ys)为机器人运动轨迹初始点的到达时间，当t(xs，ys)很小时，angel_cost快速增大，从而使机器人更倾向于选择符合方向梯度场参考方向的轨迹；d(xs，ys)为机器人运动轨迹初始点的最近障碍物距离，当的d(xs，ys)很小时，机器人快速调整运动方向至方向梯度场参考方向，以避免陷入障碍物困境中，α为障碍物影响因子，用来评定障碍物对于规划的路径的影响；
用目标代价函数和角度代价函数的和作为总体代价函数来评价机器人向目标点行进的这些轨迹的优劣性，并通过最小化总体代价函数以此来选出局部路径最优的规划轨迹；如图6所示，在图6(a)中，中间箭头所指方向与梯度场提供的参考方向差值很大，因而评判轨迹的代价会放大一定倍数。而下方箭头所指方向虽然距离目标点较远，但是轨迹的最终方向与梯度场参考方向相差不多，因而评判轨迹的代价小于中间箭头所指方向对应的轨迹。当机器人陷入局部最优时，轨迹采样得出的模拟轨迹全都会与障碍物发生碰撞，本发明所提出方法也能逃离这种情况。在图6(b)中，当机器人过于靠近障碍物时，向前模拟的三条轨迹都会碰上前方的障碍物，至于向后模拟的轨迹，传统的dwa算法认为当前位置更接近目标点，势场更低，因此不会采取后退运动。而本发明所提出的方法，在评估机器人代价时，当前位置的方向与梯度场参考方向相差较大，因而增大代价值；向后模拟的左下方箭头方向与梯度场参考方向一致，与目标点距离虽增大，但仍比当前位置的代价小，因此机器人会选择向后退逃离困境。在调整机器人方位角与梯度场参考方向一致时，如图6(c)所示，机器人会按照中间箭头所指方向，向目标点前进。
所述总体代价函数如下公式所示：
total_cost＝goal_cost+angel_cost
其中，goal_cost为目标代价函数，angel_cost为角度代价函数。
fmm算法是由数值方法求解程函方程的粘性解，来解决界面的传播问题。程函方程如下公式所示：
其中，x表示搜索空间中的点，在二维空间中的表现形式为x＝(x，y)。t(x)为从起点到达点x的时间，w(x)是界面在点x处的局部传播速度。通过离散化梯度t(x)能够在空间中每点x求解程函方程，x对应网格表示的规划空间中第i行j列的网格。
dwa算法是一种经典的在线局部路径规划方法，在动态不确定环境中运行良好。该方法主要是在速度空间(v，w)中采样多组速度轨迹，并模拟机器人在这些速度下一定时间内的轨迹。在得到多组轨迹之后，对这些轨迹进行评价，选取最优轨迹所对应的速度来驱动机器人运动。该算法突出点在于动态，它的含义是依据移动机器人的加减速性能限定速度采用空间的一个可行动态范围内。要模拟机器人的轨迹，需要知道机器人的运动模型。本实施例中采用的双轮差分移动机器人，只能前进后退和旋转。考虑两个相邻时刻，运动距离很短，可以将两相邻点之间的轨迹看成直线，即沿着机器人坐标系移动了vt*δt。将其投影到世界坐标系上即可得到世界坐标系下坐标的变化。假设机器人t时刻位姿为(xt，yt，θt)，按照以下公式计算t+1时刻机器人位姿：
xt+1＝xt+vt*δt*cosθt
yt+1＝yt+vt*δt*sinθt
θt+1＝θt+wt*δt
在机器人速度空间下采样多组速度，计算出不同速度下机器人的预期位姿，生成机器人的模拟轨迹。
本实施例中，在目标人员没有被障碍物遮挡时，首先将通过卡尔曼滤波融合aoa、激光以及摄像头的信息得到的轨迹与只使用aoa信息的人员轨迹进行对比实验。机器人跟随目标人员行走时，只使用aoa标签得到的人员位置轨迹与通过卡尔曼滤波融合得到的人员位置轨迹如图7所示。从图中可以看到，只采用aoa信息时，对人员位姿的估计是不准确的，并且某些时候存在着剧烈的跳动。采用卡尔曼滤波算法，利用激光和摄像头的检测信息纠正aoa信息，消除一些过渡振荡，得到平滑的人员轨迹。
本实施例还将只通过激光和摄像头进行人员位置检测的结果和卡尔曼滤波融合的人员位置检测结果进行对比，如图8所示，从图可以看出激光和摄像头进行检测得到的轨迹与卡尔曼滤波得到的轨迹大致相同，但是卡尔曼滤波得到的轨迹更加平滑。
在目标人员被障碍物遮挡时，单纯依靠激光和视觉信息往往会导致跟踪失败。本发明中，通过卡尔曼滤波融合激光和视觉信息能够有效缓解aoa信号值波动剧烈的问题。而aoa标签具有唯一id，恰恰能为激光识别提供初值，不会发生行人误识别的问题，可信度较高。通过卡尔曼滤波融合激光和视觉以及aoa信息得到的轨迹，利用aoa信息将激光检测到的人腿与被跟踪人员重新匹配，能够有效应对激光和视觉信息在目标人员被障碍物遮挡时，无法检测到人的情况。即使在多障碍物环境中，本发明系统及方法也可以有效的进行人员检测，并实现平滑的跟踪轨迹，如图9所示，图中的黑色方块为障碍物。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明权利要求所限定的范围。
- 还没有人留言评论。精彩留言会获得点赞！