CN116817903B - 一种基于先验视觉引导的智能机器人全局定位方法及系统 - Google Patents

Abstract

本发明公开了一种基于先验视觉引导的智能机器人全局定位方法及系统，方法包括：选择智能机器人的工作场景；建立工作场景的栅格地图与图像数据集；关联坐标信息与图像数据集信息；训练并部署轻量级神经网络；对智能机器人进行基于先验视觉引导的全局定位。在自适应蒙特卡洛定位算法的基础上，加入采用轻量级神经网络进行场景分类来实现先验视觉引导，使智能机器人具备自主性、低成本与快速性的全局定位能力。为了进一步确保定位结果准确性与可靠性，在激光雷达、里程计、惯性传感器和相机等设备下使用粒子变化均值与惯性测量传感器融合观测来动态监测机器人的定位状态。设计了视觉引导定位的结果自校验策略，有效提高机器人全局定位系统的鲁棒性。

Description

一种基于先验视觉引导的智能机器人全局定位方法及系统

技术领域

本发明属于移动机器人定位领域，尤其涉及一种基于先验视觉引导的智能机器人全局定位方法及系统。

背景技术

近年来，随着机器人技术的不断发展，现有部分传统工作逐渐由人工转变到以机器人为主的智能化作业。如室内消毒、仓储搬运、迎宾带路等。实现智能化作业需要机器人具备自主导航的能力，而机器人定位是导航的基础。但是，机器人的定位容易受到场景或自身因素的影响，当机器人定位发生错误或定位坐标丢失时，需要机器人具备全局定位的能力。

机器人全局定位的传统方法为专业人员根据周围环境来估计机器人在全局地图中的位置，然后通过RVIZ界面（机器人开发的可视化工具）手动发布机器人在全局地图中的位置坐标。该种全局定位方式需要操作人员具备一定的专业性和经验性，同时缺乏定位的自主性，不利于机器人的进一步发展应用。

为了进一步实现机器人全局定位的自主性，当前的解决方法大多采用部署外部基础设施为机器人提供绝对定位坐标。该种方法精度高、速度快，但也存在基站部署成本高、信号容易受到干扰、系统鲁棒性差的问题，不适合在复杂、大场景下为机器人提供全局定位服务。

然而，基于概率的粒子滤波定位方式具有精度高、抗干扰能力强、成本低的特点，同时也具备全局定位能力，逐渐成为室内二维定位领域的研究热点，典型代表为自适应蒙特卡洛定位算法。自适应蒙特卡洛定位算法的全局定位功能是在已知地图中铺洒表征机器人运动的粒子，不断地对粒子进行重要性采样更新，最终将粒子收敛得到的位姿作为机器人在全局地图中的位姿估计。由此可见，该种定位方法需要进行大量粒子的迭代更新，在面对大场景的全局定位时将出现粒子收敛速度慢，耗费大量计算资源，将直接影响到定位的速度。

因此，现有技术需要一种为机器人定位发生错误或定位坐标丢失时提供低成本全局定位的解决方案，以及需要具备一定的自主性与实时性的定位方法，以此来解决机器人在复杂、大场景中的全局定位问题。

发明内容

为了解决现有机器人在复杂、大场景情况下的全局定位存在缺乏自主性、成本高、速度慢的问题，本发明提供了一种基于先验视觉引导的智能机器人全局定位方法及系统。

本发明技术方案在自适应蒙特卡洛定位算法的基础上，加入采用轻量级神经网络进行场景分类来实现先验视觉引导，使智能机器人具备自主性、低成本与快速性的全局定位能力。为了进一步确保定位结果准确性与可靠性，在激光雷达、里程计、惯性传感器和相机等设备下使用粒子变化均值与惯性测量传感器融合观测来动态监测机器人的定位状态。设计了视觉引导定位的结果自校验策略，有效提高机器人全局定位系统的鲁棒性。

本发明提供的技术方案如下：

一方面，一种基于先验视觉引导的智能机器人全局定位方法，包括：

当处于工作场景中的机器人需要进行全局定位时，执行以下步骤：

步骤1，基于自适应蒙特卡洛定位算法，在工作场景的栅格地图中均匀铺洒权值相等的用于代表机器人位姿的粒子；

步骤2，获取机器人当前位置的图像数据并基于工作场景的区域位置进行图像分类，根据分类结果匹配到预先保存的与分类结果对应的定位坐标，将该定位坐标以及当前机器人的姿态信息，作为步骤1中所有粒子的初始位姿；

所述的定位坐标是作为步骤1中所有粒子的先验坐标，从而使粒子不再执行从全局发散到分布在机器人真实坐标附近的过程；

步骤3，各个粒子将基于步骤2得到的初始位姿并根据机器人的运动学模型和实际的里程计采样数据，模拟机器人在栅格地图中的真实运动；

步骤4，通过机器人携带的激光雷达的数据来观测机器人在栅格地图上的位姿并更新栅格地图中的粒子权值；

所述的更新粒子权值表示为：

;

其中，为t时刻第i个粒子的权值，/>为t-1时刻第i个粒子的权值，/>为t时刻由激光雷达得到的机器人观测位姿，/>表示t时刻第i个粒子的位姿，m为栅格地图，为观测模型的概率值；

步骤5，以栅格地图中现有的粒子作为原始集合，对原始集合中粒子的权值进行归一化处理，根据归一化后的粒子权值分布，以概率的方式生成新的粒子集合；

所述的粒子权值归一化处理为：

;

其中表示第i个粒子的权值，/>表示所有粒子权值的总和，/>表示第i个粒子归一化后的权值，/>；

所述的以概率的方式生成新的粒子集合为：对原始集合中粒子进行随机选取，其中权值越大的粒子被选中的概率越大，直至选出一个粒子数量与原始集合相同的新的粒子集合，从而使具有较大权值的粒子在新的集合中更多地出现；

步骤6，循环执行步骤3到步骤5直至粒子收敛到预设程度，然后以所有粒子的位姿均值来表征机器人当前的定位位姿，从而完成全局定位。

进一步地，处于工作场景中的机器人需要进行全局定位的情况至少包括：

第一种情况，基于栅格地图进行导航前，需要进行全局定位，从而获取机器人在全局地图中的初始坐标；

第二种情况，机器人在移动过程中，出现定位错误或丢失时，需要进行全局定位，从而恢复机器人的准确位姿信息。

进一步地，所述的步骤2中，图像分类包括以下步骤：

首先通过相机获取机器人当前位置的全景视频并对视频实时抽帧，得到一组图片来进行图像分类，然后把各张图像的分类结果进行两级滑动计数，从而得到全景视频的分类结果；

其中两级滑动计数是首先以连续的10个分类结果为单位进行滑动分组，依次求取每组10个结果中出现频率最高的类别x进行保存，实现第一级滑动计数，表示为：

；

其中， F _count 表示对分类结果中分类类别出现次数进行统计的函数，max表示求取分类结果中分类类别出现次数最大的值，r ₁，r ₂ ，r ₃ ，...，r ₁₀ 分别表示该组第1，2，3...,10张图片分类结果的所属的类别；

然后对第一级滑动计数输出的共i组统计结果进行第二级滑动计数，得到最终的分类结果Y，表示为：

；

其中，表示t时刻第一级滑动计数中第i组的分类结果所属的类别。

进一步地，对由全景视频形成的一组图片进行图片分类是由预训练好的轻量级神经网络执行的，其中在轻量级神经网络的训练过程中所采用的训练集，是根据以下步骤获得的：

建立工作场景中二维的占据栅格地图，栅格地图中每个独立栅格为以下三种状态中的一种：

；

其中，p表示栅格的占用概率，m _i 表示当前的栅格地图中的第i个栅格；

然后采集工作场景的图像数据集，在建立栅格地图后，设定机器人在所建立的栅格地图范围内间隔一段设定的移动距离后停止，并采集停止位置的全景图像信息作为图像数据集的子集，该子集表示的是一个l*l的矩形区域，设定的移动距离的值为l；同时通过里程计获取机器人此刻的坐标信息(x,y)，将获取到的坐标信息作为该子集的分类标签，从而实现将坐标信息与图像数据集进行关联，形成轻量级神经网络的训练集。

进一步地，采集工作场景的图像数据集的过程包括：

基于机器人上搭载的相机来进行分区域的图像数据采集，其中图像数据为机器人所处区域位置的全景图像信息，所述的全景图像信息的采集过程包括：

步骤A1：通过相机拍摄机器人所在区域的全景视频；

步骤A2：对视频进行抽帧处理，以获得具有代表性的具体区域图像作为数据集。

进一步地，所述的栅格地图通过2D激光雷达和里程计进行建立，其中2D激光雷达的数据和里程计的数据表示为：

；

其中，Z为2D激光雷达测量得到的机器人位姿，作为观测数据，z ₁ ,z ₂ ,...,z _n 依次表示2D激光雷达第1，2，...，n次测量得到的机器人位姿；为里程计测量的机器人位姿，作为预测数据，x ₁ ,x ₂ ,...,x _n 依次表示里程计第1，2，...，n次测量得到的机器人位姿；从而建立的全局地图m ^* 表示为：

;

其中，表示从初始时刻到t时刻，通过里程计得到机器人预测位姿；/>表示初始时刻到t时刻，通过2D激光雷达得到机器人的观测位姿；/>表示在已知预测位姿与观测位姿的前提下所建立地图m的概率，/>表示求取一个m值来使得概率P最大。

进一步地，在进行全局定位时，还采用以下步骤来对定位状态进行动态监测：

根据粒子变化均值和传感器测量两种定位判断结果来联合判断机器人定位结果是否正常，包括：

（1）若使用粒子变化均值得到机器人的定位正常，且基于传感器测量得到机器人的定位正常，则机器人的定位结果正常；

（2）若使用粒子变化均值得到机器人的定位异常，但基于传感器测量得到机器人的定位正常，则机器人定位结果异常；

（3）若使用粒子变化均值得到机器人的定位正常，但基于传感器测量得到机器人的定位异常，则机器人定位结果异常；

（4）若使用粒子变化均值得到机器人的定位异常，且基于传感器测量得到机器人的定位异常，则机器人的定位结果异常；

其中基于粒子变化均值来判断定位是否正常，是监测粒子权值的长期变化均值与短期变化均值/>，并将粒子的变化均值进行处理后与预设的阈值比较，当小于阈值时，则认为定位异常，否则正常；

基于传感器测量来判断定位是否正常，是监测机器人上惯性传感器测量得到的姿态信息，以及基于自适应蒙特卡洛定位算法输出的姿态信息；以这两个姿态之间差值的绝对值来与预定的阈值比较，当持续大于阈值时，则认为定位异常，否则正常。

进一步地，基于惯性传感器观测来判断定位是否正常，包括以下步骤：

通过监测机器人上惯性传感器测量得到姿态信息，通过自适应蒙特卡洛定位算法得到输出的姿态信息/>，其中姿态信息为机器人工作环境所在平面的偏航角，然后通过下式计算姿态差的绝对值M：

;

则当M<N时，认为机器人定位误差在正常范围内，定位状态正常，其中N为机器人正常定位时姿态差的绝对值的阈值；当M≥N时，则开启定时器，若在所设定的一段时间内M≥N均成立，则判定机器人定位状态出现了异常，否则判定此刻只是出现个别偏离值，机器人定位状态正常。

进一步地，所述的步骤2中，在进行图像分类时，还包括对分类结果进行自校验的步骤：

步骤E1：通过步骤2获取到定位的坐标作为第一次视觉定位坐标；

步骤E2：机器人基于步骤E1得到的坐标和此时由惯性测量传感器得到的姿态信息作为先验位姿进行运动，运动过程中粒子重复步骤3-5直到收敛状态；

步骤E3：粒子收敛后，再次执行步骤2，获取到第二次视觉定位的坐标,同时记录此时自适应蒙特卡洛算法的粒子输出坐标/>;

步骤E4：计算第二次视觉定位坐标与自适应蒙特卡洛算法的粒子输出坐标的欧式距离值D，表示如下：

;

若，则定位误差在正确范围内，第一次视觉定位的坐标/>正确，同时自适应蒙特卡洛算法的粒子输出的机器人坐标/>可靠；当/>，再输出机器人可靠的全局定位坐标为/>;其中l表示图像数据集的子集所表示的矩形区域的边长。

另一方面，一种基于先验视觉引导的智能机器人全局定位系统，用于实现前述的方法，包括：

全局定位的场景单元，用于选择机器人的工作场景；令机器人在工作场景中移动；建立工作场景的栅格地图与图像数据集并进行关联；

智能机器人单元，用于工作场景下的智能化作业并用于搭载轻量级神经网络；

传感器单元，包括激光雷达、里程计、惯性传感器和相机；所述的激光雷达用于建立场景的栅格地图，所述的里程计用于获取智能机器人的位姿信息并辅助激光雷达建立栅格地图，所述的惯性传感器用于采集机器人精确的姿态信息，所述的相机用于采集智能机器人所处位置的场景图像信息；

训练单元，用于训练轻量级神经网络，训练数据为采集的带有分类标签的场景图像数据集，其中训练单元为以GPU为运算单元的云端服务器。

相较于现有技术，本发明的技术效果在于：

1、提出了一种基于先验视觉引导的智能机器人全局定位方法。通过图像分类的方法获取机器人的初始坐标，将该初始坐标与由惯性测量传感器测量得到的姿态信息作为先验引导位姿输入到自适应蒙特卡洛算法中，替代粒子由全局初始化状态到收敛的过程，实现自主快速全局定位，解决了现有全局定位技术存在缺乏自主性、成本高、速度慢的问题。

2、采用了一种使用粒子变化均值与惯性测量传感器观测相融合的方法来动态监测智能机器人的定位状态，保障机器人在运行过程中定位的准确性。

3、设计了一种智能机器人视觉引导定位的结果自校验策略。校验过程进行了两次视觉引导定位，通过计算第二次视觉定位坐标和自适应蒙特卡洛算法的粒子输出坐标的欧式距离，可得到第一次视觉定位的结果是否正确与最终粒子输出的机器人位姿是否可靠，解决了视觉引导定位中如何检验场景分类的准确性与可靠性问题，使得基于先验视觉引导的智能机器人全局定位系统具有更高鲁棒性。

附图说明

图1为本发明实施例的一种基于先验视觉引导的智能机器人全局定位的工作原理整体框架示意图；

图2为本发明实施例中的轻量级神经网络进行先验视觉引导定位的示意图；

图3为本发明实施例中视觉引导定位结果的自校验策略的流程示意图；

图4为本发明实施例中一种基于先验视觉引导的智能机器人全局定位系统框架示意图。

具体实施方式

为了使本发明实施例的目的、技术方案和有益效果更加清晰，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例1

本发明提供的实施例包括一种基于先验视觉引导的智能机器人全局定位方法，如图1-图4所示，包括：

步骤1：选择智能机器人的工作场景。

将智能机器人运送到需要开展工作的地点，该地点即作为智能机器人进行全局定位的工作场景。

步骤2：人为辅助智能机器人在工作场景中进行移动，建立机器人工作场景的栅格地图与图像数据集。

其中人为辅助智能机器人移动可采用人为推动机器人或远程遥控机器人在工作场景中运动。

建立机器人工作场景的栅格地图的具体过程如下：

建立的场景地图为二维的占据栅格地图，栅格地图由许多独立的栅格组成，每个栅格包含以下三种状态之一：

；

其中，p表示栅格的占用概率，m _i 表示当前的栅格地图中的第i个栅格；

在本实施例中，建立栅格地图所采用的传感器主要有2D激光雷达和里程计。其中2D激光雷达的数据和里程计的数据可表示为：

；

其中，Z为2D激光雷达测量得到的机器人位姿，作为观测数据，z ₁ ,z ₂ ,...,z _n 依次表示2D激光雷达第1，2，...，n次测量得到的机器人位姿；为里程计测量的机器人位姿，作为预测数据，x ₁ ,x ₂ ,...,x _n 依次表示里程计第1，2，...，n次测量得到的机器人位姿；从而建立的全局地图m ^* 表示为：

;

其中，表示从初始时刻到t时刻，通过里程计得到机器人预测位姿；/>表示初始时刻到t时刻，通过2D激光雷达得到机器人的观测位姿；/>表示在已知预测位姿与观测位姿的前提下所建立地图m的概率，/>表示求取一个m值来使得概率P最大。

建立用于定位的机器人工作场景的图像数据集具体过程如下：

对于工作场景的图像数据集，采用机器人上搭载的RGB相机进行分区域采集。其中采集的图像数据应为机器人所处位置的全景图像信息，采集图像数据时尽量使用遥控操作并且使机器人匀速移动，使得采集的数据清晰可靠并且具有代表性。采集全景图像方法分为两步：

步骤2.1：通过RGB相机拍摄场景视频。

步骤2.2：对拍摄的视频进行抽帧处理，为保证图像具有代表性，可根据具体场景设定间隔数帧进行抽取图片作为数据集。

步骤3：利用栅格地图中的坐标信息对采集的图像数据集进行分类标注，以此关联步骤2中的地图坐标信息与图像数据集信息。

分类是在步骤1所选定的室内工作场景内，利用一段明显且唯一语义特征对场景进行更小区域的属性划分，如划分为：办公室区域、会议室区域、餐厅区域、走廊区域、卫生间区域等。

在建立栅格地图后，设定机器人在所建立的栅格地图范围内间隔一段移动距离后停止，然后原地匀速旋转一周，采集全景图像信息作为图像数据集的子集，同时通过里程计获取机器人此刻的坐标信息(x,y)，将获取到的坐标信息作为该子集的分类标签，从而实现将坐标信息与图像数据集进行关联。这里图像数据集的子集表示的是一个边长为l的矩形区域，设定的移动距离的值为l。

步骤4：利用轻量级神经网络来训练步骤3中的图像数据集，然后将训练完成的网络部署于智能机器人上。

本实施例中的轻量级神经网络采用MobileNet V3模型，该模型采用基于迁移学习的方法对采集的场景图像数据集进行训练，实现对从视频流中抽帧出来的多张图片进行实时的分类处理。

步骤5：以步骤2所得到的栅格地图与步骤4所得到的轻量级神经网络模型，对智能机器人进行基于先验视觉引导的全局定位。

基于先验视觉引导的智能机器人全局定位具体方法如下：

步骤5.1.1：当机器人需要进行全局定位时，首先基于自适应蒙特卡洛定位算法对粒子进行全局初始化，使栅格地图中均匀铺洒粒子。粒子是对机器人的实际运动映射在栅格地图上的抽象表示，每个粒子表示机器人的一种运动状态，机器人的位姿用粒子表示为：

；

其中，表示k次运动后粒子集输出的机器人位姿，/>表示k次运动后分布在地图中的第i个粒子的位姿，/>表示k次运动后地图中的第i个粒子的权值，n表示粒子的个数。粒子进行全局初始化后，均匀分布在栅格地图中的每一个粒子的权值都应该相等，表示为：

；

步骤5.1.2：通过RGB相机获取机器人当前位置的全景视频流信息并对视频流进行抽帧处理，从而得到一组图片。然后将该组图片使用MobileNet V3模型进行连续的分类处理，把多个分类结果进行两级滑动计数，得出该段视频流的分类结果。最后基于分类结果匹配相应的坐标标签，得到基于先验视觉引导的定位坐标。

两级滑动计数是对输出的一系列分类数据以连续的10个分类结果为单位进行滑动分组，依次求取每组10个结果中出现频率最高的类别x进行保存，实现第一级滑动计数，用公式表示为：

；

其中， F _count 表示对分类结果中分类类别出现次数进行统计的函数，max表示求取分类结果中分类类别出现次数最大的值，r ₁，r ₂ ，r ₃ ，...，r ₁₀ 分别表示该组第1，2，3...,10张图片分类结果的所属的类别；在此基础上，对i组第一级滑动计数的统计结果进行第二级滑动计数可得到最终的分类结果Y，用公式表示为：

；

其中，表示t时刻第一级滑动计数中第i组的分类结果所属的类别。

步骤5.1.3：将步骤5.1.2中基于先验视觉引导的定位坐标以ROS Topic的形式进行发布，自适应蒙特卡洛定位算法将订阅以ROS Topic的形式发布的定位坐标，同时获取机器人上所搭载的惯性测量传感器的姿态信息。

以ROS Topic的形式发布的定位坐标将作为自适应蒙特卡洛算法的初始坐标。惯性测量传感器用于补充以ROS Topic的形式发布的定位坐标中所缺乏的初始姿态值。

步骤5.1.4：基于步骤5.1.3中订阅以ROS Topic的形式发布的定位坐标与惯性测量姿态信息，栅格地图中的粒子将以此坐标和姿态作为运动的初始位姿，然后根据机器人的运动学模型和实际的里程计采样数据，在栅格地图中模拟机器人的真实运动。

步骤5.1.5：通过机器人携带的激光雷达的数据来观测机器人在栅格地图上的位姿并更新栅格地图中的粒子权值。

所述的更新粒子权值表示为：

;

其中，为t时刻第i个粒子的权值，/>为t-1时刻第i个粒子的权值，/>为t时刻由激光雷达得到的机器人观测位姿，/>表示t时刻第i个粒子的位姿，m为栅格地图，为观测模型的概率值；

步骤5.1.6：以栅格地图中现有的粒子作为原始集合，对原始集合中粒子的权值进行归一化处理，根据归一化后的粒子权值分布，以概率的方式生成新的粒子集合。

所述的粒子权值归一化处理为：

;

其中表示第i个粒子的权值，/>表示所有粒子权值的总和，/>表示第i个粒子归一化后的权值，/>；

所述的以概率的方式生成新的粒子集合为：对原始集合中粒子进行随机选取，其中权值越大的粒子被选中的概率越大，直至选出一个粒子数量与原始集合相同的新的粒子集合，从而使具有较大权值的粒子在新的集合中更多地出现；

不断重复步骤5.1.4、5.1.5、5.1.6，当粒子收敛到一定的程度后，以收敛后的所有粒子的位姿均值来表征机器人当前的定位位姿。其中收敛程度由用户来根据具体的应用需求和实际情况确定收敛的标准，从而达到用户所需的位姿估计精度。至此，机器人完成一次全局定位。

为了保障机器人运行过程中定位的准确性，本实施例采用了一种使用粒子变化均值和传感器观测相融合的方法来动态监测机器人的定位状态，其步骤为：

步骤5.2.1：机器人在定位过程中实时获取自适应蒙特卡洛定位算法中粒子的长期变化均值均值与短期变化均值/>。长期变化均值与短期变化均值可表示为:

其中，与/>表示当前时刻的长期与短期变化均值，/>与/>表示前一时刻的长期与短期变化均值，/>为粒子的均值，/>和/>分别为用于估计长期、短期平均测量概率的指数滤波器的衰减因子，同时满足/>。

步骤5.2.2：当机器人定位出现异常时，粒子的短期变化均值将大幅降低，而长期变化均值/>基本保持不变。通过监测粒子的长期变化均值/>与短期变化均值判断当前的定位状态是否发生异常。

通过监测粒子的长期变化均值与短期变化均值/>来判断当前的定位状态的方法为：

将粒子的变化均值进行处理：

其中k表示粒子变化均值的比值。当时，粒子运动状态正常，时，可判断当前定位状态出现异常。/>为机器人由正常定位切换为错误定位下记录变化均值的阈值。

步骤5.2.3：当机器人处于激光退化场景或动态变化的场景时，步骤5.1.5中利用激光雷达获取的观测位姿的可靠性将大幅降低，粒子变化均值的定位监测方法会出现失效。为了进一步加强定位监测的准确性，在监测粒子均值变化的基础上融入惯性测量传感器的观测值进行进一步验证机器人的定位状态，表示为：

其中，为机器人的惯性测量传感器获取到的姿态信息，/>为自适应蒙特卡洛定位算法输出的姿态信息，M表示姿态差的绝对值。姿态信息为机器人在oxy平面的偏航角。当M<N时，认为机器人定位误差在正常范围内，定位状态正常，其中N为机器人正常定位时姿态差的绝对值的阈值，N的值会根据惯性传感器的型号不同而发生改变，可在机器人定位正常情况下进行标定获得。当M≥N时，则开启定时器，若在所设定的一段时间内M≥N均成立，则判定机器人定位状态出现了异常，否则判定此刻只是出现个别偏离值，机器人定位状态正常。设定的一段时间可由用户根据具体情况自行确定，本实施例中默认为5s。

步骤5.2.4：基于步骤5.2.2和步骤5.2.3得到的监测结果，联合判断机器人的定位状态：

（1）若使用粒子变化均值得到机器人的定位正常，且基于传感器测量得到机器人的定位正常，则机器人的定位结果正常。

（2）若使用粒子变化均值得到机器人的定位异常，但基于传感器测量得到机器人的定位正常，则机器人定位结果异常。

（3）若使用粒子变化均值得到机器人的定位正常，但基于传感器测量得到机器人的定位异常，则机器人定位结果异常。

（4）若使用粒子变化均值得到机器人的定位异常，且基于传感器测量得到机器人的定位异常，则机器人的定位结果异常。

为了保障步骤5.1.2中对场景进行分类的可靠性，提出了一种基于机器人视觉引导定位的结果自校验策略，视觉引导定位的结果自校验策略具体步骤如下：

步骤5.3.1：机器人通过步骤5.1.2中基于先验视觉引导的方法，获取到第一次视觉定位的坐标。

步骤5.3.2：机器人基于步骤5.3.1得到的坐标和此时由惯性测量传感器得到的姿态信息作为先验位姿进行运动，该过程自适应蒙特卡洛算法中的粒子重复步骤5.1.4、步骤5.1.5和步骤5.1.6到收敛状态。

步骤5.3.3：粒子收敛后，再次执行步骤5.1.2中基于先验视觉引导的方法，获取到第二次视觉定位的坐标，同时记录此时自适应蒙特卡洛算法的粒子输出坐标。

步骤5.3.4：计算第二次视觉定位坐标与自适应蒙特卡洛算法的粒子输出坐标的欧式距离值D，表示如下：

;

若，则定位误差在正确范围内，第一次视觉定位的坐标/>正确，同时自适应蒙特卡洛算法的粒子输出的机器人坐标/>可靠；当/>，则说明第一次视觉定位的坐标/>错误，导致此时自适应蒙特卡洛算法的粒子输出的机器人坐标/>不可靠，此时应再次执行步骤5.3.1到步骤5.3.4，直至/>,输出机器人可靠的全局定位坐标为/>;l表示步骤3中图像数据集的子集所表示的区域大小，其取值由建立图像数据集时根据场景面积大小所确定，即为设定的移动距离的取值。

实施例2

本发明还提供的实施例包括一种基于先验视觉引导的智能机器人全局定位系统，包括：

全局定位的场景单元：选择智能机器人的工作场景，令机器人在工作场景中移动，建立场景的栅格地图与图像数据集并将二者进行关联。

智能机器人单元：智能机器人用于室内复杂、大场景的智能化作业，其中智能化作业包括室内消毒、仓储搬运、迎宾带路等。

传感器单元：传感器单元搭载于智能机器人上，本实施例中包括激光雷达、里程计、惯性测量传感器和RGB相机等。其中激光雷达用于建立场景的栅格地图，里程计用于获取智能机器人的位姿信息并辅助激光雷达建立栅格地图，惯性测量传感器用于采集机器人精确的姿态信息，RGB相机用于采集智能机器人所处位置的场景图像信息。

训练单元：训练单元用于训练轻量级神经网络，训练数据为采集的带有分类标签的场景图像数据集。训练单元可为智能机器人上搭载的计算机或专业用于训练神经网络的服务器。智能机器人搭载的计算机为CPU运算单元，适用于小场景的情况。专业用于训练神经网络的服务器是以GPU为运算单元的云端服务器。智能机器人上一般不做神经网络训练，当智能机器人采集到数据后将由云端服务器进行训练学习，智能机器人作为训练结束后的轻量级网络的部署平台。

具体各个单元的实现过程请参照上述方法的内容，在此不再赘述。应该理解到，上述功能单元的划分仅仅是一种逻辑功能的划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。同时，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

需要强调的是，本发明实例是说明性的，而不是限定性的，因此本发明不限于具体实施方式中实例，凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式，不脱离本发明宗旨和范围的，不论是修改还是替换，同样属于本发明的保护范围。

Claims (7)

1.一种基于先验视觉引导的智能机器人全局定位方法，其特征在于，包括：

当处于工作场景中的机器人需要进行全局定位时，执行以下步骤：

步骤1，基于自适应蒙特卡洛定位算法，在工作场景的栅格地图中均匀铺洒权值相等的用于代表机器人位姿的粒子；

步骤2，获取机器人当前位置的图像数据并基于工作场景的区域位置进行图像分类，根据分类结果匹配到预先保存的与分类结果对应的定位坐标，将该定位坐标以及当前机器人的姿态信息，作为步骤1中所有粒子的初始位姿；

所述的定位坐标是作为步骤1中所有粒子的先验坐标，从而使粒子不再执行从全局发散到分布在机器人真实坐标附近的过程；

步骤3，各个粒子将基于步骤2得到的初始位姿并根据机器人的运动学模型和实际的里程计采样数据，模拟机器人在栅格地图中的真实运动；

步骤4，通过机器人携带的激光雷达的数据来观测机器人在栅格地图上的位姿并更新栅格地图中的粒子权值；

步骤5，以栅格地图中现有的粒子作为原始集合，对原始集合中粒子的权值进行归一化处理，根据归一化后的粒子权值分布，以概率的方式生成新的粒子集合；

步骤6，循环执行步骤3到步骤5直至粒子收敛到预设程度，然后以所有粒子的位姿均值来表征机器人当前的定位位姿，从而完成全局定位；

在进行全局定位时，还采用以下步骤来对定位状态进行动态监测：

根据粒子变化均值和传感器测量两种定位判断结果来联合判断机器人定位结果是否正常，包括：

（1）若使用粒子变化均值得到机器人的定位正常，且基于传感器测量得到机器人的定位正常，则机器人的定位结果正常；

（2）若使用粒子变化均值得到机器人的定位异常，但基于传感器测量得到机器人的定位正常，则机器人定位结果异常；

（3）若使用粒子变化均值得到机器人的定位正常，但基于传感器测量得到机器人的定位异常，则机器人定位结果异常；

（4）若使用粒子变化均值得到机器人的定位异常，且基于传感器测量得到机器人的定位异常，则机器人的定位结果异常；

其中基于粒子变化均值来判断定位是否正常，是监测粒子权值的长期变化均值与短期变化均值/>，并将粒子的变化均值进行处理后与预设的阈值比较，当小于阈值时，则认为定位异常，否则正常；

基于传感器测量来判断定位是否正常，是监测机器人上惯性传感器测量得到的姿态信息，以及基于自适应蒙特卡洛定位算法输出的姿态信息；以这两个姿态之间差值的绝对值来与预定的阈值比较，当持续大于阈值时，则认为定位异常，否则正常；

基于惯性传感器观测来判断定位是否正常，包括以下步骤：

通过监测机器人上惯性传感器测量得到姿态信息，通过自适应蒙特卡洛定位算法得到输出的姿态信息/>，其中姿态信息为机器人工作环境所在平面的偏航角，然后通过下式计算姿态差的绝对值M：

；

则当M<N时，认为机器人定位误差在正常范围内，定位状态正常，其中N为机器人正常定位时姿态差的绝对值的阈值；当M≥N时，则开启定时器，若在所设定的一段时间内M≥N均成立，则判定机器人定位状态出现了异常，否则判定此刻只是出现个别偏离值，机器人定位状态正常；

所述的步骤2中，在进行图像分类时，还包括对分类结果进行自校验的步骤：

步骤E1：通过步骤2获取到定位的坐标作为第一次视觉定位坐标；

步骤E2：机器人基于步骤E1得到的坐标和此时由惯性测量传感器得到的姿态信息作为先验位姿进行运动，运动过程中粒子重复步骤3-5直到收敛状态；

步骤E3：粒子收敛后，再次执行步骤2，获取到第二次视觉定位的坐标，同时记录此时自适应蒙特卡洛算法的粒子输出坐标/>；

步骤E4：计算第二次视觉定位坐标与自适应蒙特卡洛算法的粒子输出坐标的欧式距离值D，表示如下：

；

若，则定位误差在正确范围内，第一次视觉定位的坐标/>正确，同时自适应蒙特卡洛算法的粒子输出的机器人坐标/>可靠；当/>时，则错误，且/>不可靠，此时返回步骤E1重新执行，直至/>，再输出机器人可靠的全局定位坐标为/>；其中/>表示图像数据集的子集所表示的矩形区域的边长。

2.根据权利要求1所述的方法，其特征在于，处于工作场景中的机器人需要进行全局定位的情况至少包括：

第一种情况，基于栅格地图进行导航前，需要进行全局定位，从而获取机器人在全局地图中的初始坐标；

第二种情况，机器人在移动过程中，出现定位错误或丢失时，需要进行全局定位，从而恢复机器人的准确位姿信息。

3.根据权利要求1所述的方法，其特征在于，所述的步骤2中，图像分类包括以下步骤：

首先通过相机获取机器人当前位置的全景视频并对视频实时抽帧，得到一组图片来进行图像分类，然后把各张图像的分类结果进行两级滑动计数，从而得到全景视频的分类结果；

其中两级滑动计数是首先以连续的10个分类结果为单位进行滑动分组，依次求取每组10个结果中出现频率最高的类别x进行保存，实现第一级滑动计数，表示为：

;

其中，F _count 表示对分类结果中分类类别出现次数进行统计的函数，max表示求取分类结果中分类类别出现次数最大的值，r ₁，r ₂ ，r ₃ ，...，r ₁₀ 分别表示该组第1，2，3，...,10张图片分类结果的所属的类别；

然后对第一级滑动计数输出的共i组统计结果进行第二级滑动计数，得到最终的分类结果Y，表示为：

；

其中，表示t时刻第一级滑动计数中第i组的分类结果所属的类别。

4.根据权利要求3所述的方法，其特征在于，对由全景视频形成的一组图片进行图片分类是由预训练好的轻量级神经网络执行的，其中在轻量级神经网络的训练过程中所采用的训练集，是根据以下步骤获得的：

建立工作场景中二维的占据栅格地图，栅格地图中每个独立栅格为以下三种状态中的一种：

；

其中，p表示栅格的占用概率，m _i 表示当前的栅格地图中的第i个栅格；

然后采集工作场景的图像数据集，在建立栅格地图后，设定机器人在所建立的栅格地图范围内间隔一段设定的移动距离后停止，并采集停止位置的全景图像信息作为图像数据集的子集，该子集表示的是一个的矩形区域，设定的移动距离的值为/>；同时通过里程计获取机器人此刻的坐标信息(x,y)，将获取到的坐标信息作为该子集的分类标签，从而实现将坐标信息与图像数据集进行关联，形成轻量级神经网络的训练集。

5.根据权利要求4所述的方法，其特征在于，采集工作场景的图像数据集的过程包括：

基于机器人上搭载的相机来进行分区域的图像数据采集，其中图像数据为机器人所处区域位置的全景图像信息，所述的全景图像信息的采集过程包括：

步骤A1：通过相机拍摄机器人所在区域的全景视频；

步骤A2：对视频进行抽帧处理，以获得具有代表性的具体区域图像作为数据集。

6.根据权利要求4所述的方法，其特征在于，所述的栅格地图通过2D激光雷达和里程计进行建立，其中2D激光雷达的数据和里程计的数据表示为：

；

其中，Z为2D激光雷达测量得到的机器人位姿，作为观测数据，z ₁ ,z ₂ ,...,z _n 依次表示2D激光雷达第1，2，...，n次测量得到的机器人位姿；X为里程计测量的机器人位姿，作为预测数据，x ₁ ,x ₂ ,...,x _n 依次表示里程计第1，2，...，n次测量得到的机器人位姿；从而建立的全局地图m ^* 表示为：

；

其中，表示从初始时刻到t时刻，通过里程计得到机器人预测位姿；/>表示初始时刻到t时刻，通过2D激光雷达得到机器人的观测位姿；/>表示在已知预测位姿与观测位姿的前提下所建立地图m的概率，/>表示求取一个m值来使得概率P最大。

7.一种基于先验视觉引导的智能机器人全局定位系统，其特征在于，用于实现如权利要求1-6任一所述的方法，包括：

全局定位的场景单元，用于选择机器人的工作场景；令机器人在工作场景中移动；建立工作场景的栅格地图与图像数据集并进行关联；

智能机器人单元，用于工作场景下的智能化作业并用于搭载轻量级神经网络；

传感器单元，包括激光雷达、里程计、惯性传感器和相机；所述的激光雷达用于建立场景的栅格地图，所述的里程计用于获取智能机器人的位姿信息并辅助激光雷达建立栅格地图，所述的惯性传感器用于采集机器人精确的姿态信息，所述的相机用于采集智能机器人所处位置的场景图像信息；

训练单元，用于训练轻量级神经网络，训练数据为采集的带有分类标签的场景图像数据集，其中训练单元为以GPU为运算单元的云端服务器。