CN110146070B - 一种适用于宠物引诱的激光导航方法 - Google Patents

Abstract

本发明公开一种适用于宠物引诱的激光导航方法，该激光导航方法用于控制装配有激光雷达的小型移动机器人在宠物活动的预设区域内进行路径规划，具体通过发射激光束来引诱宠物，在预设区域内设置预警圆域来探测宠物运动状态和固定障碍物，然后根据探测结果实时调整修正所述小型移动机器人的移动方向和目标位置，从而规划出用于引诱宠物在闭合的路径活动的导航路径，从而达到引诱宠物健身运动和训练宠物的效果。

Description

一种适用于宠物引诱的激光导航方法

技术领域

本发明涉及移动机器人定位导航的技术领域，具体涉及一种适用于宠物引诱的激光导航方法。

背景技术

比起人类的生活，宠物的生活更单调，市面上为帮助宠物舒压的宠物玩具琳琅满目，多以象征或实物的手法对外在表面做出设计改进，但在功能性和操作性上存在着不严谨的地方，而且都是单向玩具的形式，由于现在都市的宠物主白天忙于自己的工作，无法时时照料和监控自己的宠物，因而，如何通过规划路径来引导宠物在限定的区域内活动，以达到宠物健身运动、训练宠物的目的。

发明内容

为了克服上述技术缺陷，本发明技术方案使用激光引诱宠物，并在克服固定障碍物的前提下训练宠物在一个预设区域内活动，以达到贴身训练宠物的目的。

一种适用于宠物引诱的激光导航方法，该激光导航方法用于控制装配有激光雷达的小型移动机器人在宠物活动的预设区域内进行路径规划，其中，激光雷达保持旋转扫描，通过激光束引诱宠物在预设区域内运动；该激光导航方法包括：根据激光雷达旋转扫描的距离信息，在导航起点位置预先建立栅格地图，该激光导航方法还包括：步骤S1、以小型移动机器人的激光雷达中心为圆心，设置一个预设半径的预警圆域，进入步骤S2；步骤S2、判断激光雷达是否探测到宠物进入预警圆域且与小型移动机器人之间的距离缩短，是则进入步骤S3；步骤S3、控制小型移动机器人沿着第一方向移动，然后进入步骤S4；其中，所述第一方向是小型移动机器人预设的初始方向；步骤S4、判断激光雷达是否在预警圆域内探测到固定障碍物，是则进入步骤S5，否则进入步骤S6；步骤S5、根据激光雷达实时探测到的固定障碍物的角度特征和距离特征，规划出子目标位置和可行运动方向，然后控制小型移动机器人沿着可行运动方向移动至子目标位置使得小型移动机器人绕开固定障碍物，然后进入步骤S7；其中，所述固定障碍物的角度特征为，在所述固定障碍物中，被所述激光雷达的扫描视角覆盖的边缘与所述小型移动机器人当前移动方向所成的角度；所述距离特征是根据所述激光雷达采用的常规测距原理计算得到的所述小型移动机器人与所述固定障碍物的距离；步骤S6、控制小型移动机器人保持当前的移动方向移动，直到宠物与小型移动机器人的距离大于安全距离，然后返回步骤S2；步骤S7、当检测到宠物进入预警圆域，且宠物与小型移动机器人的距离缩短时，控制小型移动机器人沿着第二方向移动，然后进入步骤S8；步骤S8、判断宠物是否处于导航起点位置，是则确定小型移动机器人在预设区域内完成一次宠物引诱任务，然后返回步骤S2，否则返回步骤S4。该技术方案基于激光雷达扫描的栅格地图，对固定障碍物和宠物进行协调规避，以达到限制宠物的活动范围，避免破坏家具环境，同时也提高所述小型移动机器人路径规划的精度和引诱宠物的效果。

进一步地，所述宠物和所述固定障碍物的判断方法具体包括：控制所述激光雷达在所述预设区域内采集激光点，然后通过比较按照时间先后顺序排列的激光点的点间距与间距阈值的大小关系，识别所述宠物和所述固定障碍物对应构成的激光点连通块；其中，激光点连通块用于指示所述小型移动机器人中的激光雷达打在同一物体上的所有激光点的集合；然后计算各个激光点连通块内对应包含的激光点的坐标平均值，作为对应的激光点连通块对应的中心坐标；根据当前时刻的目标连通块的中心坐标与预设数量的时间点上对应的激光点连通块的中心坐标，计算块距离，当该块距离大于距离阈值时，该目标连通块为宠物；当该块距离小于或等于距离阈值时，该目标连通块为固定障碍物；其中，预设数量的时间点包括当前时刻的前后时间点。该技术方案使用激光点集合组成的连通块实现准确高效地检测区分宠物和固定障碍物，以便所述小型移动机器人后续能够更精确地对全局路径进行规划和对局部障碍进行检测。

进一步地，所述步骤S5具体包括：根据所述激光雷达扫描所述固定障碍物的覆盖范围，在所述小型移动机器人的当前位置处按照上下左右四个方向分别扩展一个引导节点；在所述固定障碍物中与所述激光雷达正对的边缘，根据所述固定障碍物的角度特征设置一个与该边缘所在直线平行的第一可行运动方向，然后在第一可行运动方向上选择与所述小型移动机器人的当前位置的距离满足所述距离特征的过渡节点，然后控制所述小型移动机器人从当前位置开始沿着第一可行运动方向移动至过渡节点；将各个引导节点与过渡节点的曼哈顿距离设置为估计代价，将所述小型移动机器人的当前位置与过渡节点的曼哈顿距离设置为实际代价，选择被所述激光雷达扫描覆盖的所述固定障碍物的一侧，且估计代价与实际代价的和值最小的引导节点作为所述子目标位置，则确定过渡节点指向所述子目标位置的方向为第二可行运动方向，然后控制所述小型移动机器人从过渡节点开始沿着第二可行运动方向移动至所述子目标位置；当所述小型移动机器人移动至所述子目标位置后，在后续导航过程中通过重复上述步骤来实时修正子目标位置和可行运动方向；其中，所述可行运动方向包括第一可行运动方向和第二可行运动方向。该技术方案根据所述小型移动机器人和所述固定障碍物的相对位置关系，进行目标位置的预估并在此基础上进行路径的规划，将所述固定障碍物避让和路径节点的规划协调统一，让所述小型移动机器人具有更为安全的导航路径，使得所述小型移动机器人引诱宠物的路径更为平顺。

进一步地，所述过渡节点不局限于一个，其具体的数量与所述固定障碍物的形状大小、所述过渡节点与所述小型移动机器人之间的距离相关。有利于为所述小型移动机器人提供更多的避障缓冲地带，提高导航路径的平滑度。

进一步地，所述第二方向是，与所述第二可行运动方向相同，或者成预设角度，使得所述宠物移动到所述导航起点位置时，所述第一方向、所述可行运动方向和所述第二方向的节点拟合的实时移动路线形成一个闭合的路径；其中，所述第二方向在各个所述子目标位置的修正方向与所述宠物的运动状态相关。该技术方案为所述小型移动机器人提供有效的、便于实现的宠物引诱路径方向，以达到控制所述宠物在一个闭合的路径内。

附图说明

图1为本发明实施例提供的一种适用于宠物引诱的激光导航方法的流程图。

图2为本发明实施例提供的小型移动机器人与宠物在一个预设区域的移动轨迹示意图。

图3为本发明实施例的小型移动机器人根据固定障碍物规划子目标位置的示意图。

具体实施方式

为了更清楚的对本发明进行说明，下面给出具体实施例以进行进一步的说明。

根据本发明实施例的提醒装置的其他构成以及操作对于本领域的普通技术人员来说是可知的，在此不再详细描述。在本发明的描述中，需要理解的是，术语“中心”、“上”、“下”、“前”、“后”、“左”、“右”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。另外术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以拆卸连接，或成二体:可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

在本申请一个典型的配置中，一种适用于宠物引诱的小型移动机器人需要装配一个激光雷达，来扫描周围的障碍物，并且通过发射激光束来引诱宠物在预设区域内按照实时规划的路径运动，从而引诱宠物健身运动和训练宠物，其中，本实施例规划的路径是通过激光雷达配合所述小型移动机器人而生成的激光导航路径，激光雷达产生的激光点云数据映射到实时构建的栅格地图中。由于激光雷达的精度高，实时好，数据稳定等特点，且激光雷达安装方便，适合作为小型的宠物健身机器人内设的环境感知系统的探测器。

本发明实施例提供一种适用于宠物引诱的激光导航方法，该激光导航方法用于控制装配有激光雷达的小型移动机器人在宠物活动的预设区域内进行路径规划，其中，激光雷达保持旋转扫描，通过激光束引诱宠物在预设区域内运动，如图1所示，该激光导航方法的具体步骤如下：

步骤S0、根据激光雷达旋转扫描的距离信息，在导航起点位置预先建立栅格地图，形成本发明实施例中用于规划激光导航路径的栅格地图，并将宠物标记为栅格地图中的活动障碍物，同时根据栅格地图实时更新宠物的位置，然后进入步骤S1。其中，该栅格地图中包括障碍格和空白格，障碍格确定该栅格对应的实际环境中存在障碍物，空白格表示该栅格对应的实际环境没有障碍物；而宠物对应标记的障碍格是临时的，其保存的时间可视宠物的运动状态而定。

步骤S1、以小型移动机器人的激光雷达中心为圆心，设置一个预设半径的预警圆域，然后进入步骤S2；其中，预设半径是根据激光雷达内置的激光扫描器的扫描范围确定的，预警圆域是以激光雷达中心为圆心所产生带有碰撞风险的区域，对应在所述栅格地图上产生灰度值递减的栅格。

步骤S2、判断所述激光雷达是否探测到所述宠物进入所述预警圆域且与所述小型移动机器人所在位置之间的距离缩短，是则进入步骤S3，否则进入步骤S9；该步骤中需要使用激光雷达判断宠物进入预警圆域的情况，以及宠物与小型移动机器人所在位置之间的距离变化情况，这作为小型移动机器人的警告机制，用以提醒小型移动机器执行规避宠物的行走路径。

步骤S3、控制所述小型移动机器人沿着第一方向移动，然后进入步骤S4；其中，第一方向是小型移动机器人预设的初始方向，成功引诱宠物且没探测到障碍物的情况下可以与所述宠物当前运动方向相同。在本实施例中第一方向可作为所述小型移动机器人移动的初始方向，同时所述宠物保持同一个方向追逐所述小型移动机器人直到所述小型移动机器人的移动方向发生变化。优选地，还可以根据所述宠物与所述小型移动机器人所在位置之间的距离变化快慢程度，相应地控制所述小型移动机器人执行加速或减速；所述预设角度可设置为小于90度。从而简化导航定位的过程，使得宠物更易于跟踪所述小型移动机器人，同时也避免所述小型移动机器人被宠物破坏。

步骤S9、根据所述栅格地图所标记的所述宠物的位置变化特征，控制所述小型移动机器人按照规划路径往所述宠物所在位置移动，以引诱宠物跟踪所述小型移动机器人运动。因为所述步骤S3已经判断出所述宠物不处于所述预警圆域，或者，所述宠物处于所述预警圆域但不缩短与所述小型移动机器人所在位置的距离，所以需要控制所述小型移动机器人往所述宠物所在位置移动，其中，所述规划路径是根据所述宠物在所述栅格地图中当前位置与上一个位置之间的距离和角度关系规划的，并结合所述栅格地图中标记的所述固定障碍物的栅格坐标信息实时修正。使得所述小型移动机器人在不满足所述步骤S2的警告机制的前提下，实时跟踪所述宠物，从而有效地引诱所述宠物。

步骤S4、判断所述激光雷达是否在所述预警圆域内探测到所述固定障碍物，是则进入步骤S5，否则进入步骤S6；所述小型移动机器人根据所述固定障碍物在所述栅格地图上占据的障碍格而调整避障路径，特别是当所述固定障碍物出现在所述预警圆域内，通过规划出有效的避障路径来引导所述小型移动机器人和所述宠物避开所述固定障碍物，在实际场景中，可避免宠物对家居造成破坏。

优选地，所述宠物和所述固定障碍物的判断方法具体包括：控制所述激光雷达在所述预设区域内的同一个位置旋转采集激光点，当所述激光雷达出射的激光束扫过待检测物体后，反射回激光点给所述激光雷达，然后按照反射回所述激光雷达的激光点的时间先后顺序，排列当前时间点与当前时间点之前的激光点；然后按照时间先后顺序计算所述当前激光点与所述上一个激光点之间的点间距，具体是从所述激光雷达包括的第二个激光点开始，重复计算点间距，直到所述激光雷达包括的最后一个激光点。通过比较前述激光点之间的点间距和间距阈值来识别所述宠物和所述固定障碍物对应构成的激光点连通块，具体地，当所述当前激光点与所述上一激光点之间的点间距大于间距阈值，则所述当前激光点与所述上一激光点不属于同一激光点连通块；当所述当前激光点与所述上一激光点之间的点间距小于或等于间距阈值，则所述当前激光点与所述上一激光点属于同一激光点连通块，其中，所述当前激光点与所述上一激光点分别是相邻的时间点上采集的。从而在所述预设区域内的同一个位置完成所述宠物和所述固定障碍物的识别工作；其中，激光点连通块用于指示所述小型移动机器人中的激光雷达打在同一物体上的所有激光点的集合。在所述小型移动机器人引诱所述宠物移动的过程中，所述激光雷达实时收集激光点，并统计基于同一激光点连通块的所有激光点的个数；同时根据构建的栅格地图获取对应的激光点的坐标，然后求取各个激光点连通块内对应包含的激光点的坐标的平均值，作为各个激光点连通块对应的中心坐标；基于同一激光点连通块的所有激光点的收集时间点之间的时间间隔相差较小，且按照时间先后顺序排序。根据当前时刻的目标连通块的中心坐标与预设数量的时间点上对应的激光点连通块的中心坐标计算块距离，当该块距离大于距离阈值时，该目标连通块为所述宠物；当该块距离小于或等于距离阈值时，该目标连通块为所述固定障碍物，然后在所述栅格地图对应栅格中标记为障碍格。其中，预设数量的时间点包括当前时刻的前后时间点。需要说明的是，本实施例提供的基于激光点的所述宠物和所述固定障碍物的判断方法不需要通过建立样本连通块与实时采集构建的激光点连通块进行比较以辅助检测。进一步提高了检测效率。

步骤S5、根据所述激光雷达实时探测到的固定障碍物的角度特征和距离特征，规划出子目标位置和可行运动方向，然后控制所述小型移动机器人沿着可行运动方向移动至子目标位置，然后进入步骤S7，需要说明的是，在后续重新进入该步骤后实时修正子目标位置和可行运动方向，在该步骤中可以默认所述固定障碍物出现在所述预警圆域内，直到所述小型移动机器人移动至所述子目标位置后且根据所述宠物当前被引诱的前进方向做出进一步的移动方向调整，从而控制所述小型移动机器人绕开所述固定障碍物，避免所述小型移动机器人与所述固定障碍物碰撞，进而导致所述宠物破坏家具。

优选地，所述固定障碍物的角度特征为，在所述固定障碍物中，被所述激光雷达的扫描视角覆盖的边缘与所述小型移动机器人当前移动方向所成的角度。本实施例所述激光雷达通过不断的旋转将每个表面的物体都检测到，通过收集反射回的激光点建立几何模型，特别是运用https://blog.csdn.net/David_Han008/article/details/68924772中提及的rplidarA2的激光雷达进行测距的原理完成所述固定障碍物的角度特征的测量。本实施例中使用所述小型移动机器人当前移动方向与所述固定障碍物的一个边缘所成的夹角作为路径规划的角度依据，在于所述小型移动机器人检测到所述固定障碍物之前，一直保持当前移动的方向不变；所述距离特征是根据所述激光雷达采用的常规测距原理计算得到的所述小型移动机器人与所述固定障碍物的距离。

具体地，结合图3可知，所述步骤S5包括：根据所述激光雷达扫描所述固定障碍物的覆盖范围，在所述小型移动机器人的当前位置处按照上下左右四个方向，基于所述固定障碍物分别扩展一个引导节点。在所述固定障碍物中与所述激光雷达正对的边缘，根据所述固定障碍物的角度特征设置一个与该边缘所在直线平行的第一可行运动方向，然后在第一可行运动方向上选择与所述小型移动机器人的当前位置的距离满足所述距离特征的过渡节点，然后控制所述小型移动机器人从当前位置开始沿着第一可行运动方向移动至过渡节点；将各个引导节点与过渡节点的曼哈顿距离设置为估计代价，将所述小型移动机器人的当前位置与过渡节点的曼哈顿距离设置为实际代价，选择被所述激光雷达扫描覆盖的所述固定障碍物的一侧，且估计代价与实际代价的和值最小的引导节点作为所述子目标位置，则确定过渡节点指向所述子目标位置的方向为第二可行运动方向，然后控制所述小型移动机器人从过渡节点开始沿着第二可行运动方向移动至所述子目标位置；当所述小型移动机器人移动至所述子目标位置后，根据所述宠物当前被引诱的前进方向做出进一步的移动方向调整，使得所述小型移动机器人绕开所述固定障碍物，避免所述小型移动机器人与所述固定障碍物碰撞，进而导致所述宠物破坏家具。当然不排除保持从所述子目标位置开始保持所述第二可行运动方向继续前进以带领所述宠物绕开所述固定障碍物。其中，所述可行运动方向包括第一可行运动方向和第二可行运动方向。

本实施例中，如图3所示，当所述小型移动机器人的当前位置为位置PY时，根据所述激光雷达在当前位置PY扫描所述固定障碍物G的覆盖范围，在位置PY的上方拓展引导节点I，即所述固定障碍物G的顶点G1与位置PY的连线PYG1的延长线与所述激光雷达的扫描圆的交点I；在位置PY处的下方拓展引导节点K，即所述固定障碍物G的顶点G2与位置PY的连线PYG2的延长线与所述激光雷达的扫描圆的交点K；在位置PY处的左方拓展引导节点J，即过位置PY垂直于所述固定障碍物G的边G1G2的垂直线与所述激光雷达的扫描圆在位置PY处的左方的交点J；在位置PY处的右方拓展引导节点L，即过位置PY垂直于所述固定障碍物G的边G1G2的垂直线与所述激光雷达的扫描圆在位置PY处的左方的交点L；其中，所述激光雷达在位置PY处扫描覆盖所述固定障碍物G的区域范围限定在扇形区域之内,且所述激光雷达的扫描圆实时映射显示到所述栅格地图上；其中，所述小型移动机器人的当前位置PY在所述固定障碍物G中与所述激光雷达正对的右侧。所述小型移动机器人在当前位置PY处的当前移动方向为图3所示的箭头方向n3，箭头方向n3所在直线与所述固定障碍物中被所述激光雷达的扫描视角覆盖的边缘所成的角度为d1，根据内错角相等，两直线平行的平行线判定原理，将所述小型移动机器人在当前位置PY处的当前移动方向顺时针旋转角度d1，从而所述小型移动机器人在当前位置PY处的移动方向调整为与所述固定障碍物G的边G1G2平行或接近平行，此时所述小型移动机器人在当前位置PY处的移动方向为方向n1,确定为所述第一可行运动方向。然后在在第一可行运动方向上选择与所述小型移动机器人的当前位置的距离满足所述距离特征的过渡节点PX，这里的所述距离特征为所述激光雷达采用的常规测距原理计算得到的位置PY与所述固定障碍物G的边G1G2的垂直距离，在数值上等于PXPY的边长距离。然后控制所述小型移动机器人从当前位置PY开始沿着第一可行运动方向n1移动至过渡节点PX。本发明实施例通过设置所述过渡节点，为所述小型移动机器人提供有效的避障缓冲地带，提高导航路径的平滑度。

优选地，所述过渡节点不局限于一个，其具体的数量与所述固定障碍物的形状大小、所述过渡节点与所述小型移动机器人之间的距离相关，当所述固定障碍物的形状越大，或者所述过渡节点与所述小型移动机器人之间的距离越大，则需要规划足够多的所述过渡节点来为所述小型移动机器人提供有效的避障缓冲地带，优化激光导航的路径。

如图3所示，本实施例将引导节点I与过渡节点PX的曼哈顿距离设置为第一估计代价，将引导节点J与过渡节点PX的曼哈顿距离设置为第二估计代价，将引导节点L与过渡节点PX的曼哈顿距离设置为第三估计代价，将引导节点K与过渡节点PX的曼哈顿距离设置为第四估计代价，而将所述小型移动机器人的当前位置PY与过渡节点PX的曼哈顿距离设置为实际代价。需要说明的是，所述曼哈顿距离是一个节点与另一个节点在栅格地图坐标系上的绝对轴距（即图3中Y轴方向的方格数和X轴方向的方格数）总和，结合图3的格数以及几何测距原理可知，实际代价=4，第一估计代价=5+2=7，第二估计代价=9+4=13，第三估计代价=9+4=13，第四估计代价=9+2=11，然后通过比较前述各个估计代价与实际代价的和值可知，实际代价和第一估计代价的和值最小，所以，对应将引导节点I作为所述子目标位置，则确定过渡节点指向所述子目标位置的方向为第二可行运动方向n2，然后控制所述小型移动机器人从所述过渡节点PX开始沿着所述第二可行运动方向n2移动至所述子目标位置I。当所述小型移动机器人移动至所述子目标位置I后，进入所述步骤S7，根据所述宠物当前被引诱的活动状态做出进一步的移动方向调整。当后续导航到所述预设区域内其他探测到类似固定障碍物的局部区域时，通过重复上述步骤来实时修正子目标位置和可行运动方向，从而不断更新引导节点以绕过所述固定障碍物，使得机器人都能实现自主的探路，实时得修改路径，达到目的地。

本发明实施例根据所述小型移动机器人和所述固定障碍物的相对位置关系，进行目标位置的预估并在此基础上进行路径的规划，将所述固定障碍物避让和路径节点的规划协调统一，有效地带动宠物绕开所述固定障碍物，让所述小型移动机器人具有更为安全的导航路径，使得所述小型移动机器人引诱宠物的路径更为平顺。

步骤S6、控制所述小型移动机器人保持当前的移动方向移动，直到所述宠物与所述小型移动机器人的距离大于安全距离，其中，所述安全距离可以是大于所述预警圆域的半径，或者小于所述预警圆域，但所述安全距离小于所述预警圆域的半径时探测到所述宠物没有接近所述小型移动机器人。所以需要返回所述步骤S2再执行一次所述小型移动机器人的警告机制，使得所述小型移动机器人的引诱效果在变差的情况下进行适应的调整。另外，当所述宠物与所述小型移动机器人的距离大于安全距离时，所述宠物可能停止运动，所述小型移动机器人可以进入所述步骤S9，以引诱宠物跟踪所述小型移动机器人运动。

步骤S7、当检测到所述宠物进入预警圆域，且所述宠物与所述小型移动机器人的距离缩短时，控制所述小型移动机器人沿着第二方向移动，这作为所述小型移动机器人的警告机制，然后进入步骤S8；优选地，所述第二方向可以是，与所述第二可行运动方向相同，或者成预设角度，该预设角度大于等于45度。该步骤是所述小型移动机器人每执行完所述步骤S5的避障路径规划后开始执行的，所述小型移动机器人需要在所述第二可行运动方向的基础上调整移动方向至所述第二方向，是为了适应所述宠物在规避所述固定障碍物后的运动状态，因为从所述导航起点位置开始，第一次绕过障碍物之后，所述宠物有可能偏离所述小型移动机器人的运动方向，所以需要在所述第一方向或所述第二可行运动方向的基础上作出相应的路径方向调整。同时，为了控制所述宠物回到所述导航起点位置时，所述小型移动机器人在执行激光导航的过程中，控制所述第一方向、所述可行运动方向和所述第二方向的节点拟合的实时移动路线形成一个闭合的路径，其中，所述第二方向在各个所述子目标位置的修正方向与所述宠物的运动状态相关。

步骤S8、判断所述宠物是否处于所述导航起点位置，是则确定所述小型移动机器人在预设区域内完成一次所述宠物的引诱任务，然后返回所述步骤S2，重新开始引诱所述宠物在预设区域内运动，从而训练所述宠物熟悉所述预设区域，以有效控制所述宠物适应在所述预设区域内活动，避免对家居环境造成破坏；否则返回所述步骤S4，因为所述步骤S7已经执行过一次所述小型移动机器人的警告机制，所以可以在所述宠物没回到所述导航起点位置之前，控制所述小型移动机器人沿着当前的移动方向移动，并进入所述步骤S4中重新开始进行避障路径规划。本发明实施例为所述小型移动机器人提供有效的、便于实现的宠物引诱路径方向，以达到控制所述宠物在一个闭合的路径内。

作为本发明的一种实施例，如图2所示，所述预设区域对应图2中带有一个入口的区域，图中较大的实线圆代表宠物，图中较小的实线圆代表小型移动机器人，图中的虚线圆代表所述预警圆域，同时也存在孤立的固定障碍物E和孤立的固定障碍物F，所述预设区域的边界也可以作为所述固定障碍物。初始时刻t0,所述小型移动机器人位于位置P0，位置P0作为所述导航起点位置，所述宠物位于位置Q0，所述宠物从位置Q0开始沿着方向a1运动，进入所述小型移动机器人的所述预警圆域，则所述激光雷达探测到所述宠物进入所述预警圆域且与所述小型移动机器人之间的距离缩短，然后控制所述小型移动机器人沿着方向a1移动,并发出激光束引诱所述宠物的注意力，让所述宠物跟随所述小型移动机器人移动，本实施例中，方向a1作为所述第一方向，也是小型移动机器人预设的初始方向。当所述小型移动机器人移动到位置P1时探测到所述宠物没有进入所述预警圆域，则控制所述小型移动机器人返回到位置P0继续引诱所述宠物跟随其移动。

在t2时刻，所述小型移动机器人移动到位置P2，而所述宠物移动至位置Q2且继续沿着方向a1移动，已经进入所述预警圆域，则控制所述小型移动机器人沿着方向a1移动，以引诱所述宠物继续沿着方向a1移动；在t3时刻，所述小型移动机器人移动到位置P3，而所述宠物移动至位置Q3且继续沿着方向a1移动，已经进入所述预警圆域，则控制所述小型移动机器人沿着方向a1移动，以引诱所述宠物继续沿着方向a1移动；在t4时刻，所述小型移动机器人移动到位置P4，而所述宠物移动至位置Q4且继续沿着方向a1移动，已经进入所述预警圆域，则控制所述小型移动机器人沿着方向a1移动，以引诱所述宠物继续沿着方向a1移动，同时，所述预设区域的边界也被所述激光雷达探测到进入所述预警圆域，所以根据所述激光雷达实时探测到的固定障碍物的角度特征和距离特征，规划出子目标位置P5和可行运动方向b1，然后控制所述小型移动机器人沿着可行运动方向b1移动至子目标位置P5。

在t5时刻，所述小型移动机器人已经移动到位置P5，如果继续沿着可行运动方向b1移动则会与所述固定障碍物E发生碰撞，而所述宠物沿着方向a2移动移动至位置Q5，已经进入所述预警圆域，同时所述激光雷达也探测到所述固定障碍物E进入所述预警圆域，则在所述固定障碍物E中与位置P5正对的边缘，根据所述固定障碍物E的角度特征设置一个与该边缘所在直线平行的第一可行运动方向b2，然后在第一可行运动方向b2上选择与所述小型移动机器人的当前位置P5的距离满足所述距离特征的过渡节点P6，然后控制所述小型移动机器人从当前位置P5开始沿着第一可行运动方向b2移动至过渡节点P6。

在t6时刻，根据计算得到的所述估计代价和所述实际代价的和值，规划出一个子目标位置P7，并且确定所述过渡节点P6指向所述子目标位置P7的方向为第二可行运动方向b3，然后控制所述小型移动机器人从所述过渡节点P6开始沿着所述第二可行运动方向b3移动至所述子目标位置P7。

在t7时刻，所述固定障碍物E已经离开所述子目标位置P7处的所述小型移动机器人对应的所述预警圆域，而所述宠物已经由位置Q5沿着方向a2移动至位置Q6，且进入所述小型移动机器人对应的所述预警圆域，然后控制所述小型移动机器人继续沿着所述第二可行运动方向b3从所述子目标位置P7移动向位置P8，以满足引诱位置Q6处且沿着方向a2前进的宠物，避免碰撞所述固定障碍物E。

在t8时刻，所述小型移动机器人已经移动到位置P8，如果继续沿着所述第二可行运动方向b3则会与所述固定障碍物F碰撞，而所述宠物沿着方向a2移动移动至位置Q7，已经进入所述预警圆域，同时所述激光雷达也探测到所述固定障碍物F进入所述预警圆域，则在所述固定障碍物F中与位置P8正对的边缘，根据所述固定障碍物F的角度特征设置一个与该边缘所在直线平行的第一可行运动方向b4，然后在第一可行运动方向b4上选择与所述小型移动机器人的当前位置P8的距离满足所述距离特征的过渡节点P9，然后控制所述小型移动机器人从当前位置P8开始沿着第一可行运动方向b4移动至过渡节点P9。

在t9时刻，所述小型移动机器人已经到达过渡节点P9，根据计算得到的所述估计代价和所述实际代价的和值，规划出一个子目标位置P10，并且确定所述过渡节点P9指向所述子目标位置P10的方向为第二可行运动方向b5，然后控制所述小型移动机器人从所述过渡节点P9开始沿着所述第二可行运动方向b5移动至所述子目标位置P10。

在t10时刻，所述固定障碍物F已经离开所述子目标位置P10处的所述小型移动机器人对应的所述预警圆域，而所述宠物已经由位置Q7沿着方向a3移动至位置Q8，且进入所述小型移动机器人对应的所述预警圆域，然后将所述小型移动机器人的移动方向调整为方向b6,与所述第二可行运动方向b5成所述预设角度，由图3可知，所述宠物从位置Q8开始继续沿着方向a3移动，会偏离同一时刻从位置P10出发的所述小型移动机器人，降低引诱所述宠物的效果。为了适应所述宠物在规避所述固定障碍物F后的运动状态，需要控制所述小型移动机器人需要在所述第二可行运动方向b5的基础上调整移动方向至所述第二方向b6。由图3可知，为了控制所述宠物能够顺利回到所述导航起点位置P0时，需要控制所述第一方向、所述可行运动方向和所述第二方向的节点拟合的实时移动路线形成一个闭合的路径，即将所述宠物从位置Q8引诱回到所述导航起点位置P0附近。

在t11时刻，所述小型移动机器人已经沿着所述第二方向b6移动到位置P11且继续沿着所述第二方向b6前进，而所述宠物移动至位置Q9，同时被位置P11的所述小型移动机器人引诱得改变移动方向，使得所述宠物跟随所述小型移动机器人沿着所述第二方向b6移动。然后，所述小型移动机器人继续沿着所述第二方向b6移动直到所述导航起点位置P0，而所述宠物继续沿着所述第二方向b6移动至位置Q10，所述宠物也进入所述预警圆域。在t12时刻，所述小型移动机器人移动至位置P01，而引诱所述宠物移动至所述导航起点位置P0，则实现所述第一方向、所述可行运动方向和所述第二方向的节点拟合的实时移动路线形成一个闭合的路径。接着调整所述小型移动机器人的移动方向为方向a1，在探测到所述宠物进入所述预警圆域且与所述小型移动机器人之间的距离缩短，然后控制所述小型移动机器人沿着方向a1移动,并发出激光束引诱所述宠物的注意力，让所述宠物跟随所述小型移动机器人移动，重复前述各个时刻的实施例，以期达到引诱宠物在闭合的路径中运动，训练宠物健身，也避免宠物破坏家具环境。需要说明的是，上述各个时刻的实施例中，当所述宠物进入所述预警圆域时均缩短其与所述小型移动机器人所在位置的距离。

以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。需要注意的是，本申请可在软件和/或软件与硬件的组合体中被实施，例如，可采用专用集成电路(ASIC)、通用目的计算机或任何其他类似硬件设备来实现。在一个实施例中，本申请的软件程序可以通过处理器执行以实现上文所述步骤或功能。同样地，本申请的软件程序(包括相关的数据结构)可以被存储到计算机可读记录介质中，例如，RAM存储器，磁或光驱动器或软磁盘及类似设备。另外，本申请的一些步骤或功能可采用硬件来实现，例如，作为与处理器配合从而执行各个步骤或功能的电路。

Claims (5)

1.一种适用于宠物引诱的激光导航方法，该激光导航方法用于控制装配有激光雷达的小型移动机器人在宠物活动的预设区域内进行路径规划，其中，激光雷达保持旋转扫描，通过激光束引诱宠物在预设区域内运动；该激光导航方法包括：根据激光雷达旋转扫描的距离信息，在导航起点位置预先建立栅格地图，其特征在于，该激光导航方法还包括：

步骤S1、以小型移动机器人的激光雷达中心为圆心，设置一个预设半径的预警圆域，进入步骤S2；

步骤S2、判断激光雷达是否探测到宠物进入预警圆域且与小型移动机器人之间的距离缩短，是则进入步骤S3；

步骤S3、控制小型移动机器人沿着第一方向移动，然后进入步骤S4；其中，所述第一方向是小型移动机器人预设的初始方向；

步骤S4、判断激光雷达是否在预警圆域内探测到固定障碍物，是则进入步骤S5，否则进入步骤S6；

步骤S5、根据激光雷达实时探测到的固定障碍物的角度特征和距离特征，规划出子目标位置和可行运动方向，然后控制小型移动机器人沿着可行运动方向移动至子目标位置使得小型移动机器人绕开固定障碍物，然后进入步骤S7；其中，所述固定障碍物的角度特征为，在所述固定障碍物中，被所述激光雷达的扫描视角覆盖的边缘与所述小型移动机器人当前移动方向所成的角度；所述距离特征是根据所述激光雷达采用的常规测距原理计算得到的所述小型移动机器人与所述固定障碍物的距离；

步骤S6、控制小型移动机器人保持当前的移动方向移动，直到宠物与小型移动机器人的距离大于安全距离，然后返回步骤S2；

步骤S7、当检测到宠物进入预警圆域，且宠物与小型移动机器人的距离缩短时，控制小型移动机器人沿着第二方向移动，然后进入步骤S8；

步骤S8、判断宠物是否处于导航起点位置，是则确定小型移动机器人在预设区域内完成一次宠物引诱任务，然后返回步骤S2，否则返回步骤S4。

2.根据权利要求1所述激光导航方法，其特征在于，所述宠物和所述固定障碍物的判断方法具体包括：

控制所述激光雷达在所述预设区域内采集激光点，然后通过比较按照时间先后顺序排列的激光点的点间距与间距阈值的大小关系，识别所述宠物和所述固定障碍物对应构成的激光点连通块；其中，激光点连通块用于指示所述小型移动机器人中的激光雷达打在同一物体上的所有激光点的集合；

然后计算各个激光点连通块内对应包含的激光点的坐标平均值，作为对应的激光点连通块对应的中心坐标；

根据当前时刻的目标连通块的中心坐标与预设数量的时间点上对应的激光点连通块的中心坐标，计算块距离，当该块距离大于距离阈值时，该目标连通块为宠物；当该块距离小于或等于距离阈值时，该目标连通块为固定障碍物；其中，预设数量的时间点包括当前时刻的前后时间点。

3.根据权利要求1所述激光导航方法，其特征在于，所述步骤S5具体包括：

根据所述激光雷达扫描所述固定障碍物的覆盖范围，在所述小型移动机器人的当前位置处按照上下左右四个方向分别扩展一个引导节点；

在所述固定障碍物中与所述激光雷达正对的边缘，根据所述固定障碍物的角度特征设置一个与该边缘所在直线平行的第一可行运动方向，然后在第一可行运动方向上选择与所述小型移动机器人的当前位置的距离满足所述距离特征的过渡节点，然后控制所述小型移动机器人从当前位置开始沿着第一可行运动方向移动至过渡节点；

将各个引导节点与过渡节点的曼哈顿距离设置为估计代价，将所述小型移动机器人的当前位置与过渡节点的曼哈顿距离设置为实际代价，选择被所述激光雷达扫描覆盖的所述固定障碍物的一侧，且估计代价与实际代价的和值最小的引导节点作为所述子目标位置，则确定过渡节点指向所述子目标位置的方向为第二可行运动方向，然后控制所述小型移动机器人从过渡节点开始沿着第二可行运动方向移动至所述子目标位置；

当所述小型移动机器人移动至所述子目标位置后，在后续导航过程中通过重复上述步骤来实时修正子目标位置和可行运动方向；

其中，所述可行运动方向包括第一可行运动方向和第二可行运动方向。

4.根据权利要求3所述激光导航方法，其特征在于，所述过渡节点不局限于一个，其具体的数量与所述固定障碍物的形状大小、所述过渡节点与所述小型移动机器人之间的距离相关。

5.根据权利要求3所述激光导航方法，其特征在于，所述第二方向是，与所述第二可行运动方向相同，或者成预设角度，使得所述宠物移动到所述导航起点位置时，所述第一方向、所述可行运动方向和所述第二方向的节点拟合的实时移动路线形成一个闭合的路径；

其中，所述第二方向在各个所述子目标位置的修正方向与所述宠物的运动状态相关。

Images