一种机器人所处环境与地图的匹配识别方法及系统的制作方法
【专利摘要】本发明提供一种机器人所处环境与地图的匹配识别方法，包括如下步骤：S10利用粒子滤波器对机器人所处环境进行定位，得到所述机器人的位置信息；S20计算出所述机器人所处环境与读取到的地图的匹配度；S30判断所述机器人所处环境与所述地图的匹配度是否满足匹配要求；S40若所述机器人所处环境与所述地图的匹配度不满足匹配要求，则所述机器人所处环境与所述地图不匹配；否则，所述机器人所处环境与所述地图匹配。本发明利用粒子滤波器对机器人所处环境进行定位，计算出机器人所处环境与地图的匹配度，判断匹配度是否满足匹配要求，从而识别出环境与地图是否匹配，从而提高机器人对复杂环境的适应性和机器人自身的安全性。
【专利说明】
一种机器人所处环境与地图的匹配识别方法及系统
技术领域
[0001] 本发明涉及机器人技术领域，尤其是指机器人所处环境与地图的匹配识别方法及 系统。
【背景技术】
[0002] 随着科学技术不断进步，移动机器人的种类越来越多，被广泛应用于工业、农业、 消防、服务等行业，深受广大人们的喜爱。在移动机器人自主完成各种任务之前，给出地图 以及移动机器人在地图上行走的规划路径，移动机器人按照规划路径行走。
[0003] 然而，移动机器人在行走的过程中，很有可能会遇到突发状况，机器人所处环境变 化大会影响机器人定位的准确度，如果机器人定位失败，机器人可能会失控。因此，移动机 器人对环境适应性的要求越来越高，需要及时判断移动机器人所处环境与地图是否匹配， 以避免移动机器人发生不必要的撞击。
【发明内容】
[0004] 本发明为了解决移动机器人所处环境与地图是否匹配的问题，提供一种机器人所 处环境与地图的匹配识别方法及系统。该匹配识别方法及系统识别出机器人所处环境与地 图是否匹配，从而提高机器人对复杂环境的适应性和机器人自身的安全性。
[0005] 为了实现本发明以上发明目的，本发明是通过以下技术方案实现的：
[0006] 本发明提供一种机器人所处环境与地图的匹配识别方法，包括如下步骤:S10利用 粒子滤波器对机器人所处环境进行定位，得到所述机器人的位置信息；S20计算出所述机器 人所处环境与读取到的地图的匹配度；S30判断所述机器人所处环境与所述地图的匹配度 是否满足匹配要求;S40若所述机器人所处环境与所述地图的匹配度不满足匹配要求，则所 述机器人所处环境与所述地图不匹配;否则，所述机器人所处环境与所述地图匹配。
[0007] 进一步，本机器人所处环境与地图的匹配识别方法，还包括步骤:S01利用激光传 感器获取所述机器人所处环境的激光数据，所述激光数据包括激光接收总条数、激光发射 总条数、激光长度以及激光角度。
[0008] 进一步，所述步骤S30还包括如下步骤:S31根据所述激光传感器的激光接收总条 数，计算激光接收总条数占激光发射总条数的比例;S32判断激光接收总条数占激光发射总 条数的比例是否大于激光合法阈值;S33若激光接收总条数占激光发射总条数的比例大于 激光合法阈值，则判断计算出的匹配度是否小于预设匹配阈值;S34若计算出的匹配度小于 预设匹配阈值，则计算出所述匹配度低于所述预设匹配阈值的持续时间，记录所述机器人 的初始位姿和当前位姿;S35判断计算出的持续时间是否大于预设时间阈值;S36若计算出 的持续时间大于预设时间阈值，则根据所述机器人的初始位姿和当前位姿，计算出所述机 器人的位移差；否则，跳转至步骤S31;S37判断计算出的位移差是否大于预设位移阈值;若 计算出的位移差大于预设位移阈值，则所述机器人所处环境与所述地图不匹配;否则，所述 机器人所处环境与所述地图匹配。
[0009] 进一步，所述步骤S10还包括步骤:S11利用粒子滤波器对机器人所处环境进行定 位，得到所述机器人的位姿;其定位公式如下：
[0010] x(t) = f (x(t~l) ,u(t))；
[0011] y(t)=g(x(t)，z(t)，m);
[0012] 其中，x(t)是所述机器人在t时刻的位姿，x(t-l)是所述机器人在t_l时刻的位姿， u(t)是所述机器人在t时刻的控制输入量;y(t)是所述机器人所处环境的激光数据，z(t)是 观测噪音模拟值，m是地图信息。
[0013] 进一步，所述步骤S20还包括步骤:S21采用高斯分布密度函数计算出t时刻单个粒 子的权重P，将所有粒子权重累计即为所述机器人所处环境与所述地图的匹配度;单个粒子 权重计算公式如下：
[0014] p = 〇e~l ^ + c, c = \!2d ;
[0015] 其中，a是高斯分布系数，d是所述激光数据中的激光长度，0是高斯分布的方差，1 是所述机器人所处环境中的障碍物与所述地图中的障碍物之间的距离。
[0016] 进一步，本机器人所处环境与地图的匹配识别方法，还包括步骤:S50当所述机器 人所处环境与所述地图匹配时，所述机器人按照规划路径行走;S60当所述机器人所处环境 与所述地图不匹配时，所述机器人报警并停止按照规划路径行走。
[0017] 进一步，本机器人所处环境与地图的匹配识别方法。还包括步骤:S51当所述机器 人按照规划路径行走时，判断所述机器人是否到达终点位置;S52若所述机器人到达终点位 置，则所述机器人停止行走;否则，跳转至步骤S01。
[0018] 进一步，本机器人所处环境与地图的匹配识别方法的系统，包括：定位模块，用于 利用粒子滤波器对机器人所处环境进行定位，得到所述机器人的位姿;计算模块，所述计算 模块与所述定位模块电连接，用于计算出所述机器人所处环境与读取到的地图的匹配度； 判断模块，所述判断模块与所述计算模块电连接，用于判断所述机器人所处环境与所述地 图的匹配度是否满足匹配要求;匹配识别模块，所述匹配识别模块与所述判断模块电连接， 用于若所述机器人所处环境与所述地图的匹配度不满足匹配要求，则所述机器人所处环境 与所述地图不匹配;否则，所述机器人所处环境与所述地图匹配。
[0019] 进一步，本机器人所处环境与地图的匹配识别系统，包括:数据获取模块，用于利 用激光传感器获取所述机器人所处环境的激光数据，所述激光数据包括激光接收总条数、 激光发射总条数、激光长度以及激光角度。
[0020] 进一步，所述计算模块还用于根据所述激光传感器的激光接收总条数，计算激光 接收总条数占激光发射总条数的比例;所述判断模块还用于判断激光接收总条数占激光发 射总条数的比例是否大于激光合法阈值;若激光接收总条数占激光发射总条数的比例大于 激光合法阈值，则所述判断模块还用于判断计算出的匹配度是否小于预设匹配阈值;若计 算出的匹配度小于预设匹配阈值，则所述计算模块还用于计算出所述匹配度低于所述预设 匹配阈值的持续时间，记录模块用于记录所述机器人的初始位姿和当前位姿;所述判断模 块还用于判断计算出的持续时间是否大于预设时间阈值;若计算出的持续时间大于预设时 间阈值，则所述计算模块还用于根据所述机器人的初始位姿和当前位姿，计算出所述机器 人的位移差;所述判断模块还用于判断计算出的位移差是否大于预设位移阈值;若计算出 的位移差大于预设位移阈值，则所述匹配识别模块用于识别出所述机器人所处环境与所述 地图不匹配;否则，所述匹配识别模块用于识别出所述机器人所处环境与所述地图匹配。
[0021] 本发明至少具有以下有益效果之一
[0022] 1.本发明利用粒子滤波器对机器人所处环境进行定位，计算出机器人所处环境与 地图的匹配度，判断匹配度是否满足匹配要求，从而识别出环境与地图是否匹配，从而提高 机器人对复杂环境的适应性和机器人自身的安全性。
[0023] 2、本发明在识别出机器人所处环境与地图是否匹配后，控制机器人是否按照规划 路径行走，从而为应对实时变化的环境给出处理策略，例如遇到障碍物时匹配满足要求可 选择绕行，否则匹配度不满足要求可重新规划路径等。
[0024] 3、本发明不仅适用于机器人在规划路径上行走时，在复杂的环境中，识别出其环 境与地图不匹配;还适用于判断出机器人所在环境是否与给出的地图一致，可提示用户是 否选错地图。
【附图说明】
[0025]下面结合附图和【具体实施方式】对本发明作进一步详细说明：
[0026] 图1为一种机器人所处环境与地图的匹配识别方法的示意图；
[0027] 图2为机器人所处环境与地图的匹配识别方法的一部分示意图；
[0028] 图3为另一种机器人所处环境与地图的匹配识别方法的示意图；
[0029] 图4为再一种机器人所处环境与地图的匹配识别方法的示意图；
[0030] 图5为机器人所处环境与地图的匹配识别方法的另一部分示意图；
[0031] 图6为一种机器人所处环境与地图的匹配识别系统的组成结构示意图；
[0032] 图7为另一种机器人所处环境与地图的匹配识别系统的组成结构示意图；
[0033] 图中：
[0034] 10、定位模块，11、数据获取模块，20、计算模块，21、记录模块，30、判断模块，40、匹 配识别模块，50、控制模块，60、报警模块。
【具体实施方式】
[0035] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现 有技术描述中所需要使用的附图作简单地介绍，显而易见地，以下说明和附图对于本发明 是示例性的，并且不应被理解为限制本发明。以下说明描述了众多具体细节以方便对本发 明理解。然而，在某些实例中，熟知的或常规的细节并未说明，以满足说明书简洁的要求。
[0036] 如图1所示，根据本发明的一个实施例，一种机器人所处环境与地图的匹配识别方 法，包括如下步骤：
[0037] S10利用粒子滤波器对机器人所处环境进行定位，得到所述机器人的位置信息；
[0038] S20计算出所述机器人所处环境与读取到的地图的匹配度；
[0039] S30判断所述机器人所处环境与所述地图的匹配度是否满足匹配要求；
[0040] 若所述机器人所处环境与所述地图的匹配度不满足匹配要求，则执行S40所述机 器人所处环境与所述地图不匹配；
[0041] 若所述机器人所处环境与所述地图的匹配度满足匹配要求，则执行S41所述机器 人所处环境与所述地图匹配。
[0042]如图1、图2所示，根据本发明的另一个实施例，一种机器人所处环境与地图的匹配 识别方法，包括如下步骤：
[0043] S01利用激光传感器获取所述机器人所处环境的激光数据，所述激光数据包括激 光接收总条数、激光发射总条数、激光长度以及激光角度;优选的，所述激光传感器的型号 可以是RPLIDARA1M1;
[0044] S10利用粒子滤波器对机器人所处环境进行定位，得到所述机器人的位置信息；
[0045] S20计算出所述机器人所处环境与读取到的地图的匹配度；
[0046] S31根据所述激光传感器的激光接收总条数，计算激光接收总条数占激光发射总 条数的比例；
[0047] S32判断激光接收总条数占激光发射总条数的比例是否大于激光合法阈值，所述 激光合法阈值是指遇到障碍物时，激光接收总条数占激光发射总条数的比例；
[0048] S33若激光接收总条数占激光发射总条数的比例大于激光合法阈值，则判断计算 出的匹配度是否小于预设匹配阈值;否则，跳转至步骤S41;
[0049] S34若计算出的匹配度小于预设匹配阈值，则计算出所述匹配度低于所述预设匹 配阈值的持续时间，记录所述机器人的初始位姿和当前位姿;否则，跳转至步骤S41;
[0050] S35判断计算出的持续时间是否大于预设时间阈值；
[0051] S36若计算出的持续时间大于预设时间阈值，则根据所述机器人的初始位姿和当 前位姿，计算出所述机器人的位移差;否则，跳转至步骤S31;
[0052] S37判断计算出的位移差是否大于预设位移阈值；
[0053]若计算出的位移差大于预设位移阈值，则执行S40所述机器人所处环境与所述地 图不匹配；
[0054]若计算出的位移差小于预设位移阈值，则执行S41所述机器人所处环境与所述地 图匹配。
[0055] 具体的，激光接收总条数是指:反射回来的激光条数，利用激光传感器探测机器人 所处环境，如果所处环境较空阔，在激光测量范围之外，则反射回来的激光条数占激光发射 总数的比例将小于激光合法阈值;如果环境信息丰富，例如障碍物分布复杂，大部分激光都 能发射回来，则激光传感器中反射回来的激光条数占激光发射总数的比例大于激光合法阈 值；
[0056] 判断机器人所处环境与地图的匹配度，判断该匹配度是否小于预设匹配阈值，若 匹配度小于预设匹配阈值，则表示该障碍物不存在于地图中；若匹配度大于等于预设匹配 阈值，则表示该障碍物存在于地图中，机器人所处环境与地图匹配；
[0057] 再判断匹配度小于预设匹配阈值的持续时间是否大于预设时间阈值，若持续时间 大于预设时间阈值，则表示该障碍物在短时间内不会消失，会阻碍机器人行走;若持续时间 不大于预设时间阈值，则表示该障碍物在短时间内会消失，不阻碍机器人行走，机器人所处 环境与地图还是匹配的；
[0058]再判断机器人的位移差是否大于预设位移阈值，若位移差大于预设位移阈值，则 表示机器人在运动一定距离后依然检测出环境变化大，排除机器人在固定的位置处于低匹 配度且持续一定时间，其实环境变化不大的情况，机器人所处环境与地图不匹配，需要对机 器人进行相关控制；若位移差不大于预设位移阈值，则表示机器人所处环境与地图还是匹 配的。
[0059] 根据本发明的再一个实施例，一种机器人所处环境与地图的匹配识别方法，包括 如下步骤：
[0060] S01利用激光传感器获取所述机器人所处环境的激光数据，所述激光数据包括激 光接收总条数、激光发射总条数、激光长度以及激光角度；
[0061] S11利用粒子滤波器对机器人所处环境进行定位，得到所述机器人的位姿;其定位 公式如下：
[0062] x(t) = f (x(t~l) ,u(t))；
[0063] y(t)=g(x(t)，z(t)，m);
[0064] 其中，x(t)是所述机器人在t时刻的位姿，x(t-l)是所述机器人在t_l时刻的位姿， u(t)是所述机器人在t时刻的控制输入量;y(t)是所述机器人所处环境的激光数据，z(t)是 〇~1之间的随机观测噪音模拟值，m是地图信息;在本发明中z(t)就是权重计算公式中的c;
[0065] S21采用高斯分布密度函数计算出t时刻单个粒子的权重p，将所有粒子权重累计 即为所述机器人所处环境与所述地图的匹配度;单个粒子权重计算公式如下：
[0066] p = ae~l2^ + c, c = V2d ;
[0067]其中，a是高斯分布系数，d是所述激光数据中的激光长度，0是高斯分布的方差，1 是所述机器人所处环境中的障碍物与所述地图中的障碍物之间的距离；其中，1的取值越 小，匹配度越高，环境变化越小；
[0068] S31根据所述激光传感器的激光接收总条数，计算激光接收总条数占激光发射总 条数的比例；
[0069] S32判断激光接收总条数占激光发射总条数的比例是否大于激光合法阈值，所述 激光合法阈值是指遇到障碍物时，激光接收总条数占激光发射总条数的比例；
[0070] S33若激光接收总条数占激光发射总条数的比例大于激光合法阈值，则判断计算 出的匹配度是否小于预设匹配阈值；
[0071] S34若计算出的匹配度小于预设匹配阈值，则计算出所述匹配度低于所述预设匹 配阈值的持续时间，记录所述机器人的初始位姿和当前位姿；
[0072] S35判断计算出的持续时间是否大于预设时间阈值；
[0073] S36若计算出的持续时间大于预设时间阈值，则根据所述机器人的初始位姿和当 前位姿，计算出所述机器人的位移差;否则，跳转至步骤S31;
[0074] S37判断计算出的位移差是否大于预设位移阈值；
[0075] 若计算出的位移差大于预设位移阈值，则执行S40所述机器人所处环境与所述地 图不匹配；
[0076] 若计算出的位移差小于预设位移阈值，则执行S41所述机器人所处环境与所述地 图匹配。
[0077]具体的，在本发明中简单介绍了粒子滤波定位原理，其粒子滤波定位的具体原理 可参照申请号为201310424318.8的基于分布式边缘无味粒子滤波的同步定位与地图构建 方法。
[0078]如图3所示，根据本发明的又一个实施例，一种机器人所处环境与地图的匹配识别 方法，包括如下步骤：
[0079] SOI利用激光传感器获取所述机器人所处环境的激光数据，所述激光数据包括激 光接收总条数、激光发射总条数、激光长度以及激光角度；
[0080] S11利用粒子滤波器对机器人所处环境进行定位，得到所述机器人的位姿;其定位 公式如下：
[0081] x(t) = f (x(t~l) ,u(t))；
[0082] y(t)=g(x(t)，z(t)，m);
[0083]其中，x(t)是所述机器人在t时刻的位姿，x(t-l)是所述机器人在t_l时刻的位姿， u(t)是所述机器人在t时刻的控制输入量;y(t)是所述机器人所处环境的激光数据，z(t)是 观测噪音模拟值，m是地图信息；
[0084] S21采用高斯分布密度函数计算出t时刻单个粒子的权重p，将所有粒子权重累计 即为所述机器人所处环境与所述地图的匹配度;单个粒子权重计算公式如下：
[0085] p = ae~l1^ +c t c = H2d ;
[0086] 其中，a是高斯分布系数，d是所述激光数据中的激光长度，0是高斯分布的方差，1 是所述机器人所处环境中的障碍物与所述地图中的障碍物之间的距离；
[0087] S31根据所述激光传感器的激光接收总条数，计算激光接收总条数占激光发射总 条数的比例；
[0088] S32判断激光接收总条数占激光发射总条数的比例是否大于激光合法阈值，所述 激光合法阈值是指遇到障碍物时，激光接收总条数占激光发射总条数的比例；
[0089] S33若激光接收总条数占激光发射总条数的比例大于激光合法阈值，则判断计算 出的匹配度是否小于预设匹配阈值；
[0090] S34若计算出的匹配度小于预设匹配阈值，则计算出所述匹配度低于所述预设匹 配阈值的持续时间，记录所述机器人的初始位姿和当前位姿；
[0091 ] S35判断计算出的持续时间是否大于预设时间阈值；
[0092] S36若计算出的持续时间大于预设时间阈值，则根据所述机器人的初始位姿和当 前位姿，计算出所述机器人的位移差;否则，跳转至步骤S31;
[0093] S37判断计算出的位移差是否大于预设位移阈值；
[0094] 若计算出的位移差大于预设位移阈值，则执行S40所述机器人所处环境与所述地 图不匹配；
[0095] 若计算出的位移差小于预设位移阈值，则执行S41所述机器人所处环境与所述地 图匹配；
[0096] S50当所述机器人所处环境与所述地图匹配时，所述机器人按照规划路径行走；
[0097] 优选的，还包括步骤S51当所述机器人按照规划路径行走时，判断所述机器人是否 到达终点位置；
[0098] 优选的，还包括步骤S52若所述机器人到达终点位置，则所述机器人停止行走；否 贝1J，跳转至步骤S01;
[0099] S60当所述机器人所处环境与所述地图不匹配时，所述机器人报警并停止行走。 [0100]如图6所示，根据本发明的一个实施例，一种机器人所处环境与地图的匹配识别系 统，包括：
[0101]定位模块10,用于利用粒子滤波器对机器人所处环境进行定位，得到所述机器人 的位姿；
[0102]计算模块20,所述计算模块20与所述定位模块10电连接，用于计算出所述机器人 所处环境与读取到的地图的匹配度；
[0103]判断模块30,所述判断模块30与所述计算模块20电连接，用于判断所述机器人所 处环境与所述地图的匹配度是否满足匹配要求；
[0104] 匹配识别模块40,所述匹配识别模块40与所述判断模块30电连接，用于若所述机 器人所处环境与所述地图的匹配度不满足匹配要求，则所述机器人所处环境与所述地图不 匹配;否则，所述机器人所处环境与所述地图匹配。
[0105] 如图7所示，根据本发明的另一个实施例，一种机器人所处环境与地图的匹配识别 系统，包括：
[0106] 数据获取模块11，用于利用激光传感器获取所述机器人所处环境的激光数据，所 述激光数据包括激光接收总条数、激光发射总条数、激光长度以及激光角度；
[0107] 定位模块10,用于利用粒子滤波器对机器人所处环境进行定位，得到所述机器人 的位姿;其定位公式如下：
[0108] x(t) = f (x(t~l) ,u(t))；
[0109] y(t)=g(x(t)，z(t)，m);
[0110] 其中，x(t)是所述机器人在t时刻的位姿，x(t-l)是所述机器人在t-1时刻的位姿， u(t)是所述机器人在t时刻的控制输入量;y(t)是所述机器人所处环境的激光数据，z(t)是 观测噪音模拟值，m是地图信息；
[0111] 计算模块20，用于采用高斯分布密度函数计算出t时刻单个粒子的权重p，将所有 粒子权重累计即为所述机器人所处环境与所述地图的匹配度；单个粒子权重计算公式如 下：
[0112] p = aeTl"p + c，c = /2d ;
[0113] 其中，a是高斯分布系数，d是所述激光数据中的激光长度，0是高斯分布的方差，1 是所述机器人所处环境中的障碍物与所述地图中的障碍物之间的距离；
[0114] 所述计算模块20还用于根据所述激光传感器的激光接收总条数，计算激光接收总 条数占激光发射总条数的比例；
[0115] 判断模块30,用于判断激光接收总条数占激光发射总条数的比例是否大于激光合 法阈值；
[0116] 若激光接收总条数占激光发射总条数的比例大于激光合法阈值，则所述判断模块 30还用于判断计算出的匹配度是否小于预设匹配阈值；
[0117] 若计算出的匹配度小于预设匹配阈值，则所述计算模块20还用于计算出所述匹配 度低于所述预设匹配阈值的持续时间，记录模块21用于记录所述机器人的初始位姿和当前 位姿；
[0118] 所述判断模块30还用于判断计算出的持续时间是否大于预设时间阈值；
[0119] 若计算出的持续时间大于预设时间阈值，则所述计算模块20还用于根据所述机器 人的初始位姿和当前位姿，计算出所述机器人的位移差；
[0120] 所述判断模块30还用于判断计算出的位移差是否大于预设位移阈值；
[0121] 若计算出的位移差大于预设位移阈值，则所述匹配识别模块40用于识别出所述机 器人所处环境与所述地图不匹配；否则，所述匹配识别模块40用于识别出所述机器人所处 环境与所述地图匹配；
[0122] 控制模块50,用于当所述机器人所处环境与所述地图匹配时，所述机器人按照规 划路径行走；
[0123] 优选的，所述判断模块30还用于当所述机器人按照规划路径行走时，判断所述机 器人是否到达终点位置；
[0124] 优选的，若所述机器人到达终点位置，则所述控制模块50还用于控制所述机器人 停止行走；
[0125] 报警模块60。用于当所述机器人所处环境与所述地图不匹配时，所述机器人报警， 所述控制模块50用于控制机器人停止行走。
[0126] 如图4、图5所示，根据本发明的一个实施例，一种机器人所处环境与地图的匹配识 别方法，包括如下步骤：
[0127] S100:读取全局地图数据，这里默认建图完毕；
[0128] S200:读取激光传感器获取到的激光数据，机器人通过激光传感器感知外界环境， 激光传感器安装在机器人前端，激光数据包括激光角度和对应的激光长度，激光传感器的 型号是RPLIDAR A1M1;
[0129] S300:获取机器人的行走路径，并控制机器人行走，通过全局路径规划和控制算法 使机器人正常工作；
[0130] S400:利用粒子滤波算法进行定位;粒子滤波器定位原理，
[0131] x(t) =f(x(t-l) ,u(t)) ； (1)
[0132] y(t)=g(x(t)，z(t)，m); (2)
[0133] 其中，x(t)是所述机器人在t时刻的位姿，x(t-l)是所述机器人在t-1时刻的位姿， u(t)是所述机器人在t时刻的控制输入量;y(t)是所述机器人所处环境的激光数据，z(t)是 观测噪音模拟值，m是地图信息；
[0134] 公式1为状态方程，公式2为观测方程，其原理是将前一时刻的样本集代入到状态 方程中，可预测出一个新的样本集，也称为粒子集(机器人的位姿），将新的粒子集代入观测 方程中，将获得的观测值(激光数据)与地图信息对比，可获得粒子的权重(也称匹配度）；
[0135] S410:进行粒子初始化，在地图中创建均匀分布的粒子(样本）、权重相等，采用高 斯概率密度函数进行粒子分布；
[0136] S420:利用高斯分布函数进行采样，获得粒子集；
[0137] S420:根据采样后的粒子和激光观测数据得到每个粒子的权重，计算所有粒子的 总权重(匹配度）；权重的计算公式为：
[0138] P = qb-/2//^ +C , c = /2d ; (3)
[0139] 其中，a是高斯分布系数，例如a = 0.5;d是所述激光数据中的激光最大长度，例如d =6m，c = 1 /12; 0是高斯分布的方差，例如0 = 0.08; 1是所述机器人所处环境中的障碍物与 所述地图中的障碍物之间的距离，距离越小说明匹配度越高，环境变化越小；
[0140] S430:将权重归一化，最后进行重采样获取机器人的最优位置；
[0141] 根据公式3可计算出所有粒子的总权重进而定位出机器人的实际位置;初始状态x (〇)为一个均匀分布的样本集，特别说明，粒子滤波算法实现机器人的定位还包括一些其他 的步骤这里不作赘述，如采样和重采样；
[0142] S500:计算机器人环境与地图匹配度，匹配度的影响因子包括：激光数据是否合 法，粒子的总权重，总权重持续时间和机器人位移，如果激光数据合法（即大部分激光数据 都能反射回来，否则激光数据在不可靠的情况下继续进行匹配度计算没有意义），粒子低权 重持续一定时间，且在机器人有位移的前提下可认为机器人所处环境与地图上的信息不匹 配。这里设定了六个阈值，即为激光合法阈值（1&8 61'_(3011111：_1：11代(1)，总权重阈值（1：(^&1_ weight_thred)，时间阈值（last_time_thred)，机器人位移阈值（x_thred，y_thred，theta_ thred)，其中机器人的位移仅需满足位移阈值中的任一个即可；
[0143] S501:计算激光接收总条数占总发射的比例，主要是为了解决空阔地带激光数返 回较少，总权重持续较低的引起的误判；
[0144] S502:判断激光接收总条数所占的比例是否满足要求，即是否大于激光合法阈值 (laser_count_thred)，若是转Sf5〇 3;否则转Sf504;
[0145] S503:根据公式3获取粒子的总权重；
[0146] S504:退出匹配度识别；
[0147] S505:判断总权重是否满足要求，若是说明总权重低于设定的权重阈值（total_ weight_thred)转 S506，否则转 S504;
[0148] S506:获取机器人的位姿，转S507;
[0149] S507:判断是否第一次满足总权重小于阈值，若是转S508，否则转S509;
[0150] S508:记录此刻为初始时刻。此时机器人的位姿为匹配度识别的初始位姿；
[0151] S509:记录当前的时间和机器人的位姿；
[0152] S510:计算S508和S509的时间差，判断持续时间是否达到设定的时间阈值（last_ time_thred)，若是转S511，否则转S501;
[0153] S511:计算S508和S509的位移差，判断位移是否大于设定的位移阈值(x_thred，y_ thred，theta_thred)，若是转S512，否则转S504;
[0154] S512:环境与地图不匹配，输出结果；
[0155] S600:判断匹配度是否满足要求，如果满足要求则按原规划的路径继续行走，即 S800,否则转 S700;
[0156] S700:报警并停止行走；
[0157] S800:按原路径继续行走，转S900;
[0158] S900:判断机器人行走是否结束，若没有转S200;
[0159] 应当说明的是，上述实施例均可根据需要自由组合。以上所述仅是本发明的优选 实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提 下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。
【主权项】
1. 一种机器人所处环境与地图的匹配识别方法，其特征在于，包括如下步骤： SlO利用粒子滤波器对机器人所处环境进行定位，得到所述机器人的位置信息； S20计算出所述机器人所处环境与读取到的地图的匹配度； S30判断所述机器人所处环境与所述地图的匹配度是否满足匹配要求； S40若所述机器人所处环境与所述地图的匹配度不满足匹配要求，则所述机器人所处 环境与所述地图不匹配;否则，所述机器人所处环境与所述地图匹配。2. 根据权利要求1所述的机器人所处环境与地图的匹配识别方法，其特征在于，还包括 步骤： SOl利用激光传感器获取所述机器人所处环境的激光数据，所述激光数据包括激光接 收总条数、激光发射总条数、激光长度以及激光角度。3. 根据权利要求2所述的机器人所处环境与地图的匹配识别方法，其特征在于，所述步 骤S30还包括如下步骤： S31根据所述激光传感器的激光接收总条数，计算激光接收总条数占激光发射总条数 的比例； S32判断激光接收总条数占激光发射总条数的比例是否大于激光合法阈值； S33若激光接收总条数占激光发射总条数的比例大于激光合法阈值，则判断计算出的 匹配度是否小于预设匹配阈值； S34若计算出的匹配度小于预设匹配阈值，则计算出所述匹配度低于所述预设匹配阈 值的持续时间，记录所述机器人的初始位姿和当前位姿； S35判断计算出的持续时间是否大于预设时间阈值； S36若计算出的持续时间大于预设时间阈值，则根据所述机器人的初始位姿和当前位 姿，计算出所述机器人的位移差;否则，跳转至步骤S31; S37判断计算出的位移差是否大于预设位移阈值;若计算出的位移差大于预设位移阈 值，则所述机器人所处环境与所述地图不匹配；否则，所述机器人所处环境与所述地图匹 配。4. 根据权利要求2所述的机器人所处环境与地图的匹配识别方法，其特征在于，所述步 骤S10还包括步骤： Sll利用粒子滤波器对机器人所处环境进行定位，得到所述机器人的位姿;其定位公式 如下： x(t)=f (x(t-l) ,u(t))； y(t) = g(x(t)，z(t)，m); 其中，x(t)是所述机器人在t时刻的位姿，x(t-l)是所述机器人在t-1时刻的位姿，u(t) 是所述机器人在t时刻的控制输入量;y(t)是所述机器人所处环境的激光数据，z(t)是观测 噪音模拟值，m是地图信息。5. 根据权利要求4所述的机器人所处环境与地图的匹配识别方法，其特征在于，所述步 骤S20还包括步骤： S21采用高斯分布密度函数计算出t时刻单个粒子的权重p，将所有粒子权重累计即为 所述机器人所处环境与所述地图的匹配度;单个粒子权重计算公式如下：其中，α是高斯分布系数，d是所述激光数据中的激光长度，β是高斯分布的方差，1是所 述机器人所处环境中的障碍物与所述地图中的障碍物之间的距离。6. 根据权利要求1~5中任意一项所述的机器人所处环境与地图的匹配识别方法，其特 征在于，还包括步骤： S50当所述机器人所处环境与所述地图匹配时，所述机器人按照规划路径行走； S60当所述机器人所处环境与所述地图不匹配时，所述机器人报警并停止按照规划路 径行走。7. 根据权利要求6所述的机器人所处环境与地图的匹配识别方法，其特征在于，还包括 步骤： S51当所述机器人按照规划路径行走时，判断所述机器人是否到达终点位置； S52若所述机器人到达终点位置，则所述机器人停止行走;否则，跳转至步骤SO 1。8. -种应用在如权利要求1~7中任意一项所述的机器人所处环境与地图的匹配识别 方法的系统，其特征在于，包括： 定位模块，用于利用粒子滤波器对机器人所处环境进行定位，得到所述机器人的位姿； 计算模块，所述计算模块与所述定位模块电连接，用于计算出所述机器人所处环境与 读取到的地图的匹配度； 判断模块，所述判断模块与所述计算模块电连接，用于判断所述机器人所处环境与所 述地图的匹配度是否满足匹配要求； 匹配识别模块，所述匹配识别模块与所述判断模块电连接，用于若所述机器人所处环 境与所述地图的匹配度不满足匹配要求，则所述机器人所处环境与所述地图不匹配;否则， 所述机器人所处环境与所述地图匹配。9. 根据权利要求8所述的机器人所处环境与地图的匹配识别系统，其特征在于，包括： 数据获取模块，用于利用激光传感器获取所述机器人所处环境的激光数据，所述激光 数据包括激光接收总条数、激光发射总条数、激光长度以及激光角度。10. 根据权利要求9所述的机器人所处环境与地图的匹配识别系统，其特征在于： 所述计算模块还用于根据所述激光传感器的激光接收总条数，计算激光接收总条数占 激光发射总条数的比例； 所述判断模块还用于判断激光接收总条数占激光发射总条数的比例是否大于激光合 法阈值； 若激光接收总条数占激光发射总条数的比例大于激光合法阈值，则所述判断模块还用 于判断计算出的匹配度是否小于预设匹配阈值； 若计算出的匹配度小于预设匹配阈值，则所述计算模块还用于计算出所述匹配度低于 所述预设匹配阈值的持续时间，记录模块用于记录所述机器人的初始位姿和当前位姿； 所述判断模块还用于判断计算出的持续时间是否大于预设时间阈值； 若计算出的持续时间大于预设时间阈值，则所述计算模块还用于根据所述机器人的初 始位姿和当前位姿，计算出所述机器人的位移差； 所述判断模块还用于判断计算出的位移差是否大于预设位移阈值； 若计算出的位移差大于预设位移阈值，则所述匹配识别模块用于识别出所述机器人所
【文档编号】G06K9/62GK105892461SQ201610244332
【公开日】2016年8月24日
【申请日】2016年4月13日
【发明人】徐清霞, 张小*, 章征贵
【申请人】上海物景智能科技有限公司