具有排除区的多分辨率扫描匹配的制作方法
相关申请的交叉引用
本申请要求申请日为2017年9月27日的美国专利申请15/712,222的优先权，该美国专利申请与申请日为2017年9月22日，名称为“使用最优交互避碰代价评估的动态窗口方法”的美国专利申请15/712,256相关，两者均通过引用并入本文。
在此描述的本发明总体上涉及使用包含障碍物的维度空间的空间表征将机器人导航到目标位置。具体地，本发明是一种确定自主或半自主机器人操作订单履行仓库以履行或协助履行产品订单的位姿的改进方法。
背景技术：
在许多应用中，机器人被用来代替人类执行功能或辅助人类以提高生产率和效率。这样的应用之一是订单履行，通常是在一个装满产品的大型仓库中执行，产品将被运送到已经通过互联网下订单进行送货上门的客户。及时、准确和高效地履行此类订单在逻辑上至少可以说具有挑战性。
例如，在线互联网购物应用中，单击虚拟购物车中的“结算”按钮将创建一个“订单”。该订单包括一个将要发送到特定地址的项目列表。“履行”的过程包括从一个大仓库实际提取或“挑选”这些物品，包装它们，并将它们运送到指定的地址。
因此，订单履行过程的一个重要目标是在尽可能短的时间内装运尽可能多的物品。接收订单、计划订单履行、查找储物架或储物箱、拣选产品并针对订单上的每个物品重复该过程、然后将订单交付给运输站的过程是重复性的且劳动密集型的。在仓库中存有成千上万个快速变化的库存物品，机器人在确保及时有效地履行订单中起着至关重要的作用。另外，最终要被运输的产品首先需要在仓库中接收，并在整个仓库中以有序的方式存储或“放置”在储物箱中，以便可以轻松取回它们进行运输。
使用机器人执行拾取和放置功能可以单独由机器人完成，也可以在操作员的协助下完成。无论是否通过人机交互执行的拾取和放置或库存功能，都要求机器人从其当前位置导航到目标产品存储或“箱”的位置。从当前位置到产品存放箱的机器人目标路线上，机器人通常会遇到固定和移动的障碍物，例如墙壁，架子，支撑结构，人和其他机器人。随着新产品的存储和消耗，新架子和新箱子的添加和移除以及将杂物引入共享的人和机器人的空间，订单履行仓库的动态性质要求不断更新有关仓库及其内容的信息。
机器人在订单履行仓库中导航的一种方法是采用由机器人本地存储和更新的仓库的空间模型或“地图”，以使机器人在执行分配的订单履行任务时自主或半自主运行。该地图是仓库、仓库的存储位置、障碍物和其他特征的数字表征。为了在存在固定和动态障碍物的情况下到达产品箱，机器人在地图上执行处理操作，以确定其当前位置并连续重新校准其沿目标路径的移动。
这样的地图可以使用各种技术来创建和维护，例如，使用本地激光扫描或当机器人在仓库中导航时，来自机器人传感器的图像。通常，使用机器人地图绘制应用程序中的地图更新是使用同步定位与建图或“slam”实现的。顾名思义，slam在更新地图的同时找到机器人的当前位置。但是，由于机器人障碍物感测，机器人控制和位置感测以及制图过程本身的不确定性，slam容易造成位姿错误。防止这种错误的常规方法在计算上是昂贵的。
需要的是一种计算有效的方法，该方法可减少使用slam映射确定位姿的计算时间，从而改善机器人沿目标路径的导航。这样效率导致更快、更顺畅地导航到产品存储箱，从而提高了订单履行仓库的整体吞吐量。
技术实现要素：
通过下面的发明概述和详细说明，本发明相对于现有系统的好处和优点将显而易见。本领域的技术人员将理解，可以用除了下面总结或公开的实施例以外的实施例来实践本教导。
本发明的一个方面是一种用于使机器人从当前位姿导航到目标位姿的方法。该方法包括：接收代表机器人导航区域内的障碍物和自由空间的地图，基于接收的地图，构建一个匹配地图金字塔，该匹配地图金字塔包括最高分辨率的匹配地图和一组分辨率依次降低的抽取的匹配地图，基于接收到的地图，构建一个排除地图金字塔，该排除地图金字塔包括一个最高分辨率的排除地图和一组分辨率依次降低的抽取的排除地图。在接收和构建之后，该方法还包括：接收用于将机器人从其当前位置沿着目标路径导航到目标位姿的目标位姿，接收包括点云的激光雷达扫描，该点云表示机器人在其当前位置附近的障碍物，找到机器人的当前位姿。一旦找到机器人的当前位姿，就可以将机器人沿目标位姿的方向移动到目标路径上的下一个位姿。
在一个优选的实施例中，找到机器人的当前位姿包括：确定接收到的地图内的搜索区域，创建一个包括任务的搜索堆，所述任务表示所述匹配地图金字塔的一个或多个分辨率的候选位姿，在搜索堆上对搜索任务评分以确定最佳候选位姿，使得如果最佳候选位姿处于最高分辨率匹配地图，则将该位姿选择为当前位姿；其中，如果最佳候选位姿不在最高分辨率匹配地图上，则基于下一个最高分辨率匹配地图上的最佳候选位姿来扩展搜索堆，或者所述搜索堆不会被具有会将机器人定位在下一个最高分辨率排除地图的排除区中的具有候选位姿的搜索任务扩展。
在一些实施例中，该方法可包括接收同步定位与建图(slam)的地图。该方法可进一步包括通过多对多的多分辨率扫描匹配(m3rsm)来执行创建搜索堆并对搜索任务评分。该方法可包括通过二次衰减卷积滤波器增加障碍物的尺寸来构建最高分辨率匹配地图。该方法可包括通过基于机器人的半径增加障碍物的尺寸来构建最高分辨率排除地图。
该方法的其他方面可包括：通过高估下一个较高分辨率的匹配地图中的扫描匹配得分，来抽取以构建匹配地图金字塔的每个相继较低的分辨率。抽取以构建排除地图金字塔的每个相继较低的分辨率可能会低估下一个较高分辨率的排除地图中的排除区。在较低分辨率的排除地图上低估排除区并且不扩展位于排除区中的搜索任务可能会有利地防止在排除金字塔的较高分辨率下在排除区中创建搜索任务。排除区所限定的障碍物可以选自包括但不限于墙壁、架子、梁、箱子和充电站的组。
本发明的一个方面是一种用于将机器人从当前位姿导航到目标位姿的系统。该系统可包括激光雷达扫描仪、收发器、数据存储设备以及数据处理器和存储有指令以供数据处理器执行的数据存储设备。数据存储设备存储的指令可能会导致机器人处理器接收代表障碍物和自由空间的地图，以供机器人导航，并根据接收到的地图构建匹配地图金字塔，其中包括最高分辨率的匹配地图和一组分辨率依次降低的抽取的匹配地图，并基于接收到的图构建排除图金字塔，该金字塔包含最高分辨率的排除地图和一组分辨率依次降低的抽取的排除地图。在接收和构建后，指令还可包括接收目标位姿，所述目标位姿用于将所述机器人从其当前位置沿目标路径导航至目标位置，接收包括点云的激光雷达扫描，该点云表示机器人在其当前位置附近的障碍物，以查找机器人的当前位姿。一旦找到机器人的当前位姿，指令就可以使机器人在目标位姿的方向上移动到目标路径上的下一个位姿。
在优选实施例中，寻找机器人的当前位姿包括确定接收到的地图内的搜索区域，创建一个包括任务的搜索堆，所述任务表示所述匹配地图金字塔的一个或多个分辨率的候选位姿，对所述搜索堆中的搜索任务打分以确定最佳候选位姿，其中如果所述最佳候选位姿在最高分辨率匹配地图上，则将所述位姿选择为所述当前位姿；其中如果最佳候选位姿不在最高分辨率匹配地图上，则基于下一个最高分辨率匹配地图上的最佳候选位姿来扩展所述搜索堆；或者，其中所述搜索堆不会被具有会将机器人定位在下一个最高分辨率排除地图的排除区中的具有候选位姿的搜索任务扩展。
在一些实施例中，该系统可以包括接收同步定位与建图(slam)的地图。该系统可以进一步包括通过多对多的多分辨率扫描匹配(m3rsm)来执行创建搜索堆并对搜索任务评分。该方法可以包括通过二次衰减卷积滤波器增加障碍物的尺寸来构建最高分辨率匹配地图。该系统可以包括通过基于机器人的半径增加障碍物的尺寸来构建最高分辨率排除图。
该系统的其他方面可以包括：通过高估下一个更高分辨率的匹配地图中的扫描匹配得分，抽取以构建匹配地图金字塔的每个相继较低的分辨率。抽取来构建排除地图金字塔的每个相继较低的分辨率可能会低估下一个较高分辨率的排除地图中的排除区。在较低分辨率的排除图上低估排除区并且不扩展位于排除区中的搜索任务可能会有利地防止在排除金字塔的较高分辨率下在排除区中创建搜索任务。排除区所限定的障碍物可以选自包括但不限于壁，架子，横梁，垃圾箱和充电站的组。
通过下面的详细描述和附图，本发明的这些和其他特征将显而易见。
附图说明
现在将参考附图仅通过举例的方式描述本发明的实施例，其中：
图1为订单履行仓库的俯视图；
图2a为图1所示的仓库中使用的机器人之一的基座的正视图；
图2b为图1所示的仓库中使用的机器人之一的基座的立体图；
图3为图2a和2b中的机器人的立体图，该机器人装配有电枢并停在图1所示的架子的前面；
图4为在机器人上使用激光雷达创建的图1的仓库的局部图；
图5为定位散布在整个仓库中的基准标记和存储基准标记位姿的过程的流程图；
图6为基准标识到位姿映射的表；
图7是箱子的位置到基准标识映射的表；
图8是描述产品sku到位姿映射过程的流程图；
图9示出了与本发明的方法和系统一起使用的机器人系统的一个实施例；
图10描绘了机器人通过空间地图所表示的环境从当前位置到目标位置的一般导航；
图11描绘了根据本发明的一方面的机器人相对于图10的环境的slam图的导航；
图12a和图12b描绘了由机器人在空间环境内的一个位置处获取测距扫描；
图13a和图13b示出了使用扫描匹配通过找到未对准的扫描(13a)到对准的扫描(13b)的平移来找到位姿；
图14示出了根据本发明的一方面的由机器人执行的使机器人沿着全局路径移动的过程；
图15a和15b描绘了用于构建匹配金字塔和排除金字塔的slam图的转换；
图16a和图16b描绘了匹配金字塔(16a)和排除金字塔(16b)；
图17a-c描绘了从高分辨率地图到匹配金字塔的两个最低级别的分辨率来抽取匹配地图的一部分；
图18a-c描绘了从高分辨率地图到排除金字塔的两个最低级别分辨率来抽取排除地图的一部分；
图19示出了基于使用排除地图金字塔的m3rsm的实施例查找机器人的位姿的过程；
图20和图21示出了在匹配金字塔的最高分辨率地图上搜索候选位姿以获得最佳位姿的过程；
图22示出了通过扫描匹配以找到未对准(22a)扫描到对准扫描(22b)的平移的最佳位姿估计。
具体实施方式
在此描述的本发明针对与自主或半自主机器人一起使用的方法和系统，用于在包含障碍物和自由空间的环境中，沿其“目标路径”将机器人从当前位置导航到目标位置的改进的导航方式。
具体地，本发明的方法和系统提供了相对于现有技术的在计算上的有效改进，以准确地确定空间环境内机器人的当前位置和方向或“位姿”。快速准确地确定其位姿后，机器人可以更好地控制其沿目标路径的移动、避开障碍物、并使机器人在环境中的障碍物之间更直接，更顺畅的移动。此外，节省下来的确定位姿所需的处理时间释放了用于在分配的处理周期时间内由机器人执行的其他任务的计算资源。
参考在附图中描述和/或示出并且在以下描述中详细描述的非限制性实施例和示例，更充分地解释了本公开及其各种特征和有利细节。应当注意，附图中示出的特征不必按比例绘制，并且即使没有在本文中明确陈述，本领域技术人员也可以将一个实施例的特征与其他实施例一起使用。此外，应注意，贯穿附图的若干视图，相似的附图标记表示相似的部分。
可以省略对公知组件和处理技术的描述，以免不必要地混淆本公开的实施例。本文中使用的示例仅旨在促进对可以实践本公开的方式的理解，并且还使得本领域技术人员能够实践本公开的实施例。因此，本文的示例和实施例不应解释为限制本公开的范围。
本领域的技术人员将理解，可以用除了公开的实施例以外的实施例来实践本教导。尽管本文提供的描述集中于从仓库中的仓库位置拣选物品以履行向客户发货的订单，但该系统同样适用于在库中将接收到的物品存储或放置在仓库中，以便以后查找物品并发送给用户。本发明还适用于与这种仓库系统相关的库存控制任务，例如合并、计数、验证、检查和清理产品。
本发明的方法和系统还可以应用于具有其他类型障碍物的其他类型环境中，以用于其他类型的应用。在本发明的应用中，静止或动态的任何物理物体或结构都可以被认为是“障碍”。障碍物还可以包括在环境中操作的人类和其他机器人，人类和其他机器人的位置可以是合作任务执行中的当前位置或目标位置。目标位置可以包括环境中的一个或多个位置，用于定位一个或多个机器人以执行或协助人类执行任务或完成一系列任务。
这些和其他益处和优点将从下面描述的示例和说明中变得显而易见。
如图1所示，典型的订单履行仓库10包括货架12，货架12上装有可以包含在订单16中的各种物品。在操作中，来自仓库管理服务器15的订单16到达订单服务器14。订单服务器14将订单16传送到从仓库10中多个闲逛的机器人中选择的机器人18。图1还示出了充电区域19，根据本发明的一个方面，充电区域19可以是一个或多个充电站处于的位置。
在优选实施例中，图2a和2b所示的机器人18包括具有激光雷达22的自主轮式基座20。该基座20还具有收发器(未示出)和一对数字光学摄像机24a和24b，该收发器使机器人18能够从订单服务器14接收指令。该机器人基座还包括充电端口26，用于对为自主轮式基座20供电的电池重新充电。基座20还具有处理器(未示出)，该处理器从激光雷达和摄像机24a和24b接收数据以捕获表示机器人环境的信息。本发明还包括存储器(未示出)，其与处理器一起操作以执行在仓库10内的与导航相关联的各种任务，以及导航到放置在架子12上的基准标记30，以上如图3所示。基准标记30(例如，二维条形码)对应于所订购物品的箱子/位置。下面参照图4至图8详细描述本发明的导航方法。根据本发明的一个方面基准标记还用于识别充电站，并且到这种充电站的基准标记的导航与到所订购物品的箱子/位置的导航相同。一旦机器人导航到充电站，将使用更精确的导航方法将机器人与充电站插接，并且以下描述这种导航方法。
再次参考图2b所示，基座20包括上表面32，在该上表面32中可以存储手提袋或箱子以携带物品。该图还示出了联接器34，其联接多个可互换电枢40中的任何一个，图3中示出了可互换电枢40其中之一。图3中的特定电枢40的特征在于具有用于携带接收物品的手提袋44的手提袋保持器42(在这种情况下为架子)，以及用于支撑平板电脑48的平板电脑保持器46(或便携式计算机/其他用户输入设备)。在一些实施例中，电枢40支撑一个或多个搬运物品的搬运箱。在其他实施例中，底座20支撑一个或多个手提袋，用于搬运接收到的物品。如本文所用，术语“手提袋”包括但不限于可悬挂物品的货座、箱子、笼子、架子及杆子，不限于筒、板条箱、货架、架子、支架、集装箱、盒子、罐子和用于储藏和保管物品的装置。
尽管机器人18擅长在仓库10内来回移动，但是利用当前的机器人技术，由于机器人对对象进行操纵相关的技术困难，它并不擅长快速有效地从架子上取走物品并将其放置在手提袋44中。拾取物品的一种更有效的方法是使用通常是人的本地操作员50来执行从货架12上通过物理的方式卸下已订购的物品并将其放置在机器人18上(例如在手提袋44中)的任务。机器人18通过平板电脑48(或便携式计算机/其他用户输入设备)将订单传达给本地操作员50，本地操作员50可以读取该平板电脑48，或者将订单传输到本地操作员50使用的手持设备上。
在从订单服务器14接收到订单16时，机器人18前进到第一仓库位置，如图3所示。它基于存储在内存中并由处理器执行的导航软件来执行此操作。导航软件依赖于由激光雷达22收集的与环境有关的数据。存储器中的内部表格识别基准标记30的基准辨别符号(“id”)，该基准辨别符号对应于仓库10中的位置，在该位置上可以找到特定物品，其中，摄像机24a和24b用于导航。
在到达正确的位置时，机器人18将自己停在存放物品的架子12的前面，并等待本地操作员50从架子12取回物品并将其放入手提袋44。如果机器人18还有其他物品要取回，它将继续到那些位置。然后，由机器人18取回的物品被传送到包装站100，如图1所示，包装站是用于包装和运输的地方。
本领域技术人员将理解，每个机器人可以履行一个或多个订单，并且每个订单可以包括一个或多个物品。通常，将使用包括某种形式的路由优化软件以提高效率，但是这超出了本发明的范围，因此在此不进行描述。
为了简化本发明的描述，本实施例描述了单个机器人18和操作员50。但是，从图1可以明显看出，典型的履行操作包括许多机器人和操作员在仓库中相互合作以完成连续的订单流。
下面将通过图4至图8描述本发明的导航方法，以及待取回物品的sku的语义地图，该语义地图指示到与物品所在仓库的基准标记相关联的基准id/位姿。如上所述，可以使用相同的导航方法来使机器人导航到充电站，以便为其电池充电。
使用一个或多个机器人18，必须创建并动态更新仓库10的地图，以确定静态和动态物体的位置，以及散布在整个仓库中的各种基准标记的位置。为此，如图4所示，机器人18之一在仓库中导航并建立/更新地图10a，利用其激光雷达22以及同步定位与建图(slam)，这是一种构建或更新未知环境的虚拟图的计算方法。常用的slam近似解方法包括粒子滤波器和扩展卡尔曼滤波器。slamgmapping方法是首选方法，但是可以使用任何合适的slam方法。有关slam的讨论，请参见美国frese,u.，wagner，r.,
机器人18利用激光雷达22来创建/更新仓库10的地图10a。在这个过程中，机器人18在整个空间中行走，并识别空旷的空间112、墙壁114、物体116和其他静态障碍物(例如空间中的架子12a)，基于激光雷达扫描环境时收到的反射来实现地图10a的创建/更新。
参见图3，在构建地图10a或之后的过程中，一个或多个机器人18使用摄像头24a和24b在仓库10中导航以扫描环境，以定位遍布在整个仓库中的诸如32和34所指示的货架前的存放物品的箱子上的基准标记(二维条形码)。机器人18使用已知的参考点或原点进行参考，例如原点110。如图3和图4所示，当基准标记，诸如基准标记30，由机器人18使用其摄像机24a和24b进行定位后，则该基准标记在仓库中相对于原点110的位置是确定的。如图2a所示，通过使用两个摄像头，每个摄像头位于机器人基座的任一侧，机器人18可以具有从机器人的两侧延伸出的相对较宽的视场。这样，当机器人在架子的上或下走道上移动时可以看清，例如可以看到其两侧的基准标记。
通过使用轮式编码器(未示出)和航向传感器(未示出)，可以确定矢量120以及机器人在仓库10中的位置。使用捕获的基准标记/二维条形码的图像及其已知大小，机器人18可以确定基准标记/二维条形码相对于机器人的方向和距离，即矢量130。利用已知的矢量120和130，可以确定起点110和基准标记30之间的矢量140。根据矢量140和确定的基准标记/二维条形码相对于机器人18的方向，可以确定由基准标记30的四元数(x，y，z，ω)定义的位姿(位置和方向)。有关使用四元数表示和影响方向的讨论，请参见bertholdk.p.horn，“使用单元四元数的绝对方向的闭合形式解决方案，”《美国光学学会杂志》，4(4)，1987年4月，第629-642页，其通过引用并入本文。本领域技术人员将认识到，可以使用用于确定基准标记的位置和方向的其他坐标系统和技术，并且该位姿可以从任意原点确定绝对或相对位置和/或方向。
图5的流程图200描述了基准标记定位过程。当机器人18在执行拾取、放置和/或其他任务时在仓库中遇到新的基准标记时，上述定位过程在初始地图模式下执行。在步骤202中，使用摄像机24a和24b的机器人18捕获图像，并且在步骤204中，在捕获的图像内搜索基准标记。在步骤206中，如果在图像中找到基准标记(步骤204)，则确定基准标记是否已经存储在基准表300中，如图6所示，基准表300位于机器人18的存储器中。如果基准信息已经存储在存储器中，则流程图返回到步骤202以捕获另一图像。如果它不在存储器中，则根据上述过程确定位姿，并且在步骤208中，将基准添加到位姿查找表300。
在可以存储在每个机器人的存储器中的查找表300中，每个基准标记包括一个基准标识符，如1、2、3等，以及与每个基准标识符相关联的基准标记/条形码的位姿。位姿由仓库中的x，y，z坐标以及方向或四元数(x，y，z，ω)组成。
如图7所示，另一个查询表400是仓库10内的箱子位置(例如402a-f)的列表，其还可以存储在每个机器人的存储器中，其与特定的基准id为404的基准相关联，例如，数字“11”。在此示例中，箱子位置由七个字母数字字符组成。前六个字符(例如l01001)与仓库中的货架位置相关，最后一个字符(例如a-f)表示货架位置上的特定箱子。在该示例中，存在与基准id“11”相关联的六个不同的箱子位置。每个基准id/标记可能有一个或多个箱子。如图1所示，充电区域19中的充电站也可以存储在表400中并且与基准id相关。根据基准id，可在图6的表300中找到充电站的位姿。
字母数字代表箱子位置是人们可以理解的，例如，如图3所示，操作员50可以明白字母数字对应于仓库10中存储物品的物理位置。但是，它们对机器人18没有意义。如图6所示，通过将位置映射到基准id，机器人18可以使用表300中的信息确定基准id的位姿，然后导航到此处所述的位姿。
图8的流程图500中描述了根据本发明的订单履行过程。在步骤502中，图1的仓库管理系统15获得一个订单，该订单可能包含一个或多个要取回的物品。在步骤504中，由仓库管理系统15确定物品的sku编号，并在步骤506中从sku号中确定箱子位置。然后将订单的仓位列表发送至机器人18。在步骤508中，机器人18将箱子位置与基准id相关联，并且在步骤510从基准id中获得每个基准id的位姿。在步骤512中，机器人18导航到如图3所示的位姿，操作员可以从相应的箱子中挑选要取回的物品并将其放在机器人上。
可以将由仓库管理系统15获取的物品的特定信息(例如sku编号和仓位)传输到机器人18上的平板电脑48，以便当机器人到达每个基准标记位置时，可以将要取回的特定物品通知操作员50。
通过已知的slam图和基准id的位姿，机器人18可以易于使用各种机器人导航技术导航至基准id中的任何一个。优选的方法包括在了解了仓库10中的开放空间112以及壁114、多个架子(例如架子12)和其他障碍物116的情况下，设置到基准标记位姿的初始路线。当机器人开始使用其激光雷达22穿行在仓库时，它会确定其路径中是否有固定或动态的障碍物(例如其他机器人18和/或操作员50)，并将其路径迭代更新为基准标记的位姿。机器人大约每50毫秒重新规划路线，不断寻找最有效的路径，同时避开障碍物。
通常，通过在slam虚拟地图上运行的多对多的多分辨率扫描匹配(m3rsm)来实现机器人在仓库10a中的定位。与普通未优化的方法相比，m3rsm大大减少了机器人执行slam环路闭合和扫描匹配的计算时间，这是确定机器人位姿和位置的两个关键步骤。通过根据本文公开的方法最小化m3rsm搜索空间，可以进一步改善机器人定位。
通过将产品sku/基准id到基准位姿地图技术与本文所述的slam导航技术相结合，机器人18能够非常有效地在仓库空间内导航，而不必使用通常使用的涉及网格线和中间基准标记来确定仓库内的位置。
通常，通过防撞方法(包括动态窗口方法(dwa)和最佳双向防撞(orca))可以在仓库中存在其他机器人和移动障碍物的情况下导航。机器人的导航通常以一系列的增量运动沿大约50毫秒的更新间隔计算出的轨迹进行。dwa在可行的机器人运动轨迹之间计算增量运动，该运动避免与障碍物碰撞，并有利于按照所需路径到达目标基准标记。d.fox，wburgard和s.thrun在“避免碰撞的动态窗口方法”，机器人与自动化杂志，ieee，第4卷，第1期(1997年3月)，第23-33页中dwa进行了讨论，其通过引用并入本文。
orca最佳地避免了与其他移动机器人的碰撞，而无需与其他机器人进行通信。jurvandenberg、stephenj.guy、minglin和dineshmanocha在“n个物体之间避免碰撞”，机器人学研究：第14届国际研讨会：isrr、cédricpradalier、rolandsiegwart和gerhardhirzinger(编辑)，高级机器人学中的springertracts，第70卷，springer-verlag，2011年5月，第3-19页描述了orca的讨论，通过引用并入本文。
通过在相关的美国专利申请15/712,256中描述的技术可以进一步提高避免碰撞的能力，在此引入作为参考。
机器人系统
图9示出了在上述订单履行仓库应用中使用的机器人18的一个实施例的系统图。机器人系统600包括数据处理器620，数据存储器630，处理模块640和传感器支持模块660。处理模块640可以包括路径计划模块642，驱动控制模块644，地图处理模块646，定位模块648和状态估计模块650。传感器支持模块660可包括范围传感器模块662，传动系/轮式编码器模块664和惯性传感器模块668。
数据处理器620，处理模块642和传感器支持模块660能够与此处为机器人系统600所示或所述的任何组件，设备或模块进行通信。机器人系统可包括收发器模块670以发送和接收数据。收发器模块670可以与监督器系统或一个或其他机器人之间来回发送数据和信息。传输和接收数据可以包括地图数据、路径数据、搜索数据、传感器数据、位置和方向数据、速度数据、处理模块指令或代码、机器人参数和环境设置以及机器人系统600操作所需的其他数据。
在一些实施例中，范围传感器模块662可包括扫描激光器、雷达、激光测距仪、测距仪、超声障碍检测器、立体视觉系统、单眼视觉系统、照相机和成像单元中的一个或多个。范围传感器模块662可以扫描机器人周围的环境以确定相对于机器人的一个或多个障碍物的位置。在一个优选实施例中，传动系/车轮编码器664包括一个或多个用于对车轮位置进行编码的传感器以及用于控制一个或多个车轮(例如，地面接合车轮)的位置的致动器。机器人系统600还可包括地面速度传感器，该地面速度传感器包括速度计或基于雷达的传感器或转速传感器。该转速传感器可包括加速度计和积分器的组合。转速传感器可为数据处理器620或其任何模块提供观察到的转速。
在一些实施例中，传感器支持模块660可以提供平移数据、位置数据、旋转数据、水平数据、惯性数据和航向数据，包括瞬时测量速度、平移、位置、旋转、水平、航向和惯性数据随时间变化的历史数据。可参考机器人环境中的一个或多个固定参考点或固定物体来检测平移或旋转速度。平移速度可表示为一个方向上的绝对速度，也可表示为机器人位置对时间的一阶导数。旋转速度可表示为角度单位的速度，也可以表示为角度位置对时间的一阶导数。可相对于原点0,0(例如，图1，110)和相对于绝对或相对坐标系为0度的方位表示平移和旋转速度。处理模块640可以使用观察到的平移速度(或位置对时间的测量值)与检测到的旋转速度相结合，以估计观察到的机器人的旋转速度。
在一些实施例中，机器人系统600可包括gps接收器、具有差分校正的gps接收器或用于确定机器人相对于发送无线信号的卫星或地面信标的位置的其他接收器。优选地，在诸如上述仓库应用之类的室内应用中或在卫星接收不可靠的情况下，全局的或本地的传感器或系统不能可靠地提供绝对位置信息，机器人系统600使用如上所述的非gps传感器和本文所述的技术来改善本地化应用。
在其他实施例中，图9中未示出的模块可包括转向系统，制动系统和推进系统。制动系统可包括液压制动系统、电动液压制动系统、机电制动系统、机电致动器、电动制动系统、线控制动系统或与驱动控制644通信的另一种制动系统。推进系统可包括电动机、驱动电动机、交流电动机、感应电动机、永磁电动机、直流电动机或用于推进机器人的另一种合适的电动机。推进系统可包括电动机控制器(例如，逆变器、斩波器、波发生器、多相控制器、变频振荡器、可变电流源或可变电压源)以控制电动机的电动机轴的速度、转矩和旋转方向中的至少一个。优选地，驱动控制644和推进系统(未示出)是差动驱动(dd)控制和推进系统。在dd控制系统中，机器人控制是非完整的(nh)，也就是在给定所需的平移和角速度的情况下，对可达到的增量路径进行限制。与推进系统通信的驱动控制644可以通过转换由路径计划模块642或数据处理器620确定的一个或多个瞬时速度来致动机器人的增量运动。
本领域技术人员将认识到，可以采用其他用于机器人处理、数据存储、感测、控制和推进的系统和技术，而不会失去本文描述的本发明的适用性。
地图
自主或半自主机器人的导航需要某种形式的机器人环境空间模型。空间模型可以由位图、对象图、地标图以及其他形式的二维和三维数字表示形式表示。例如，如图10所示，仓库设施的空间模型可以表示仓库和其中的障碍物，例如墙壁、天花板、屋顶支架、门窗、架子和储物箱。障碍物可以是静止的或移动的，例如在仓库中运行的其他机器人或机械，也可以是相对固定但变化的障碍物，例如在仓储、拣选和补充仓库物品时使用的临时隔板、货盘、货架和箱子。
仓库设施中的空间模型还可以表示目标位置，例如标有基准的架子或箱子，机器人可以被引导至该目标位置来拾取产品或执行某些其他任务，或者到达临时存放位置或充电站位置。例如，图10描绘了机器人18沿着其路径712,714,716从起始位置702到中间位置704,706再到目的地或目标位置708的导航。这里，空间模型捕获机器人必须在其中导航的环境特征，包括目的地718处的结构的特征，该目的地可以是架子或箱子或机器人充电器站。
最常用于机器人导航的空间模型是区域或设施的位图。例如，图11描绘了图10的空间模型中示出的区域的二维地图的一部分。映射720可以用像素值在二进制范围0，1(代表黑色或白色)中的位图表示，也可以用像素值的范围表示，例如0-255表示黑色(0)到白色(255)的灰度范围，也可以由颜色范围表示，这些范围可以描绘在像素值表示的位置处是否存在特征的不确定性。如图11所示，例如，黑色(0)的像素表示障碍物，白色(255)的像素表示自由空间，纯灰色区域(0到255之间的某个值，通常为128)表示未知区域。
图11中所示的地图720的比例和粒度可以是适合于环境的范围和细节的任何这样的比例尺和尺寸。例如，在本发明的一些实施例中，图中的每个像素可以代表5平方厘米(cm2)。在其他实施例中，每个像素可以代表从1cm2到5cm2的范围。但是，用于本发明的地图的空间分辨率可以更大或更小，而不会失去一般性或对应用其方法没有好处。
如图11所示，机器人可以使用地图720确定其在环境中的位姿，并计划和控制其沿路径712,714,716的移动，同时避开障碍物。这样的地图可以是代表机器人或目标位置紧邻区域的空间特征的“局部地图”，也可以是代表一个或多个机器人的工作范围的区域或设施的特征的“全局地图”。可以从外部监控系统向机器人提供地图，或者机器人可以使用机载测距和位置传感器构建其地图。一个或多个机器人可以协作地映射共享的环境，随着机器人导航，收集和共享有关环境的信息，结果地图得到了进一步增强。
在一些实施例中，监视系统可包括中央服务器，该中央服务器执行对制造仓库或其他设施中的多个机器人的监视，或者监视系统可包括分布式监视系统，该分布式监视系统由一个或多个服务器组成，这些服务器在设施内或不在设施内完全远程或部分运行，而不会失去本文所述方法和系统的应用范围。监控系统可包括一个或多个服务器，该服务器具有至少一个计算机处理器和用于执行监控系统的存储器，监控系统还可包括一个或多个收发器，用于将信息传递给在仓库或其他设施中运行的一个或多个机器人。监控系统可以托管在计算机服务器上，也可以托管在云中，并通过本地收发器与本地机器人通信，本地收发器配置为通过有线和/或无线通信介质(包括因特网)从机器人和监控系统接收消息，以及向机器人和监控系统发送消息。
本领域技术人员将认识到，可以使用本领域已知的方法执行用于本发明目的的机器人制图方式而不会失去一般性。可在2002年2月于卡内基-梅隆大学(carnegie-mellonuniversity)cmu-cs-02-111的塞巴斯蒂安·特伦(sebastianthrun)的“机器人制图：调查”中找到有关机器人制图方法的进一步讨论，该文献在此引入作为参考。
扫描
如上所述，配备有传感器的机器人可以将其传感器用于定位，并且有助于其环境地图的构建和维护。用于地图构建和定位的传感器可包括光检测和测距(“激光雷达”或“激光扫描”或“激光雷达”)传感器。激光雷达扫描仪通过一系列机器人局部环境的离散角度扫描来测量到水平面内物体的范围和距离。测距传感器获取一组测量值，围绕机器人以180度或更大或更小的弧度或整个360度的弧度的离散角度增量(最好为四分之一(0.25)增量)进行“扫描”。例如，激光雷达扫描可以是代表激光信号返回时间和强度的一组测量值，每个测量值以离散的角度增量表示与机器人当前位置相距一定距离的潜在障碍物。
为了说明，如图12a和12b所示，在位置704处进行的激光雷达扫描可以被图形表示为二维位图730。如图所示的扫描图730描绘了在中间位姿704处面向机器人行进方向的大约180度水平弧线。用方向虚线表示的单个激光雷达测量值731在结构722、724、726和728处检测机器人环境700'中的障碍物。这些由扫描730中的732、734、736和738处的像素表示。在一些实施例中，可以在扫描730中“填充”直壁724的扫描，其中可以从其他数据获知或可以通过点云像素的对准来辨别所连接的地理结构734。
在不脱离本发明范围的情况下，测距传感器的其他形式包括声纳、雷达和触觉传感器。适用于本发明的可商购的测距、位置和方向传感器的示例包括但不限于hokuyoust-10lx，sicklms100和velodynevlp-16。机器人可以具有一个或多个特定类型的距离或位置传感器，或者可以具有不同类型的传感器，传感器类型的组合产生共同绘制其环境的测量值。激光雷达和其他扫描仪对机器人制图方法的进一步讨论可以在edwinb.olson的2008年的“稳健和有效的机器人制图”，卡内基-梅隆大学博士学位论文中找到，该文献在此引入作为参考。
扫描匹配
“扫描匹配”是不同机器人的测距扫描的比较或单个机器人在不同时间或对环境地图(例如slam地图)的扫描的比较的过程。在扫描到扫描的匹配过程中，可以将机器人一次执行的第一次激光雷达扫描与第二次或更早的扫描进行比较，以确定机器人是否已返回到地图中的相同位置。同样，将扫描与第二个机器人的扫描相匹配可以确定两个机器人是否已导航到地图中的公共位置。扫描匹配到地图可用于确定机器人在映射环境中的位姿。如图13a所示，扫描730'被描绘为相对于地图部分720'平移和旋转。对于处于未知位姿(x，y，θ)的机器人，将机器人的激光雷达扫描与地图720'进行匹配，可以找到具有平移δx，δy和旋转δθ的刚体变换t，从而使扫描730'与地图720'最相关。因此，可以确定机器人相对于地图720的一部分的正确位姿(x+δx，y+δy，θ+δθ)，如图13b所示。
激光雷达扫描不可能在任意位置和方向上与地图完全匹配。传感器测量的不确定性，位姿精度的要求以及有限的计算周期时间，要求强大而有效的算法来统计确定机器人感知到的环境与其实际位姿之间的最佳扫描匹配。然而，统计方法容易产生不正确的位姿，并且可能在计算上昂贵。各种方法和算法已经被开发出来解决这些复杂的问题。在2009ieee国际会议论文集的“实时相关扫描匹配”中，edwinb.olson的“实时相关扫描匹配”中可找到对扫描匹配技术和确保分辨率的精度同时降低定位复杂性的两分辨率方法的概述。机器人技术和自动化(icra'09)，ieee出版社，皮斯卡塔维，新泽西州，美国，2009年，第1233-1239页，通过引用将其合并在此。
多对多的多分辨率扫描匹配
如前所述，使用扫描匹配进行定位的另一种此类技术是多对多的多分辨率扫描匹配(m3rsm)。m3rsm使用地图金字塔将双分辨率相关扫描匹配方法扩展到多种分辨率，每个金字塔都是通过抽取来构建以提高计算效率。关于m3rsm的讨论可以在edwinolson的“m3rsm：多对多的多分辨率扫描匹配”中找到，ieee国际机器人与自动化会议(icra)会议录，2015年6月，以引用方式并入本文。
m3rsm通过消除以较低的分辨率的地图考虑的候选位姿，大大减少了针对slam地图执行扫描匹配的处理时间。但是，m3rsm不必要对将机器人放置在障碍物中的位姿执行扫描匹配。使用改进的m3rsm方法对扫描匹配的进一步改进可以在不牺牲最佳位姿估计的情况下节省更多的计算时间。
通过在m3rsm扫描匹配搜索过程中使用的“排除金字塔”结构中引入多分辨率“排除地图”，可实现对m3rsm的改进。如本文所使用的排除地图定义了“排除区”或机器人无法物理占据的环境地图中的位置。如本文所述的修改的m3rsm中使用的排除金字塔保留了标准m3rsm提供的可证明的最佳位姿估计。因此，在上述订单履行仓库应用程序的背景下，使用m3rsm来查找排除地图和排除金字塔，以供在使用m3rsm查找未知位姿的激光雷达扫描定位期间使用，以找到用于沿着其目标路径导航机器人的位姿。
定位
图14示出了本发明的实施例，其是一种用于寻找机器人的位姿以进行目标路径计划并将机器人沿目标路径移动至其目标位置或“目标位姿”的方法。
从步骤802开始，机器人系统600通过收发器模块670接收slam地图，该地图可以存储在数据存储区630中。随后可以由数据处理器620或地图处理模块646从数据存储器630中检索地图。在接收到地图后，在步骤804，地图处理模块646根据下面更详细描述的改进的m3rsm技术来构建地图金字塔。在构建地图金字塔时，根据m3rsm的原理和方法构建了多分辨率“匹配金字塔”。另外，与匹配金字塔平行地构建了标准m3rsm中不存在的多分辨率排斥金字塔，从而根据本发明的发明内容扩展了m3rsm。
继续过程800，在步骤806，机器人系统600接收目标位姿并使用路径计划模块646生成目标路径，然后可以存储该目标路径以用于以后的处理。可基于针对机器人的位姿估计来生成目标路径，或者优选地，生成目标路径是基于在“查找位姿”步骤812的第一次迭代之后确定的机器人的位姿，如下面进一步描述。路径计划模块642可通过本领域的从业者已知的各种技术来生成从当前位姿到目标位姿的目标路径，包括a*和d*寻路算法。可选地，机器人可经由收发器模块670接收目标路径，或者可以从数据存储装置630检索目标路径。接收到地图并生成地图金字塔和目标路径后，机器人系统600随后继续执行过程800，以使机器人沿目标路径递增移动，如下所示。
在步骤810，机器人系统600接收其环境(例如，图12a的720)的本地扫描(例如，图13a的730)。如上所述，扫描可包括激光雷达扫描或其他测距传感器扫描。扫描可由激光雷达“点云”组成，点云代表机器人视野中的点，激光雷达在这些点上检测障碍物。点云可表示相对于机器人的位置和方向上的激光雷达扫描的点，点云中的每个点均以不连续的角度增量获取，并指示与机器人当前位置相距一定距离的潜在障碍物。
在步骤812中确定机器人的位姿。下面参考图15进一步描述使用带有m3rsm的地图从本地扫描中找到位姿的方法。在步骤814，该过程可以返回以基于第一或随后的发现位姿结果来生成或更新目标路径。
找到位姿后，在步骤816中，机器人计算瞬时下一速度以使机器人沿目标路径移动。优选地，在其他机器人和障碍物存在的情况下，沿着目标路径的瞬时速度是通过包括但不限于动态窗口方法(dwa)和最佳双向碰撞避免(orca)方法来实现的。在优选实施例中，dwa计算可行的机器人运动轨迹中的增量运动，避免与障碍物的碰撞并且有利于到达目标位置所需的目标路径。
在确定下一速度和机器人运动(步骤818)时，机器人系统600可重复(步骤820)，获得新的局部扫描(步骤810)，找到位姿(步骤812)和下一速度(步骤816)，直到通过机器人的运动到达目标位姿为止。因此，导航在每个处理循环中以一系列沿着由瞬时速度确定的增量轨迹的增量运动进行。
匹配金字塔
返回图14的步骤804，从接收到机器人局部或全局环境的slam地图开始，构建用于m3rsm的地图金字塔。如上所述，如图11所示，slam地图720仅表示包含自由空间和障碍物的仓库的一部分。地图720的像素值在0,1的二进制范围内表示障碍物(0或黑色)和自由空间(1或白色)。可选地，地图720可以使用0-255范围内的像素值来表示仓库内的障碍物，其中零(0)的值表示障碍物，而255的值表示自由空间。通常具有值128的灰色像素(如果有)表示未知或未映射或无法访问的区域。
要构建匹配金字塔的最高分辨率匹配地图，首先要倒置slam地图。接下来，对倒置地图执行二次衰减卷积。如图15a的图900所示，零(0)或黑色的像素值表示自由空间，而255或白色的值表示障碍物。本领域技术人员将理解，应用二次衰减卷积或“模糊化”滤波器具有稍微增加地图900中障碍物大小的效果，并产生0-255范围内的像素值。
继续步骤804，地图金字塔的构建如下进行。如图16a所示，匹配金字塔910是一组分辨率依次降低的地图，从金字塔底部的最高分辨率匹配地图912开始，到顶部的最低分辨率匹配地图。匹配金字塔的构建始于用零(黑色)填充的地图912(代表仓库的完整地图)，以在匹配金字塔910的最高分辨率地图上形成一个正方形位图。为了计算效率，最高分辨率匹配地图912是n×n像素的正方形地图，但是可以是n的任何维度(dimension)或值。
匹配金字塔910的构建继续通过将地图912抽取为分辨率依次降低的匹配地图(914、916、918、920)来进行，每个分辨率较低的地图是通过抽取下一个最高分辨率的图而得出的。例如，较低分辨率匹配地图914是从最高分辨率匹配地图912的抽取中得出的；通过抽取匹配地图914等取得较低分辨率匹配地图916，依此类推，直到仅剩下一个像素为止。
现在将参考图17a-c描述对最高分辨率匹配地图912的抽取以构建匹配金字塔910。通常，在将高分辨率地图每次抽取到较低分辨率地图时，九个像素(3x3平方)的窗口940在地图的每隔一行中每隔第二个像素越过。在下一个最低分辨率下为相应像素保留的值是九像素窗口中的最大像素值。匹配金字塔中的最低分辨率地图是单个像素。因此，每个抽取都高估下一个较低分辨率的匹配地图的扫描匹配的可能性。这样确保在较低分辨率下的扫描匹配得分至少与在下一个高分辨率下的扫描匹配得分相同，从而保留了m3rsm查找正确位姿的最优性。
图17a和17b示出了匹配金字塔910的地图920的一部分920′的抽取。例如，对地图920′进行抽取会产生较低分辨率的地图924。地图924可以是下一个较低分辨率的地图的一部分。回顾最高分辨率匹配地图中的黑色(0)是最小值，而白色(255)是最大值，位于左上方像素941上方的九像素窗口940产生图17b的像素951的值。像素951的值是窗口940内的像素的最大值。在地图920'中向右滑动两个像素的窗口940会产生像素值952。窗口化过程一直持续到窗口940到达行的末尾为止，在该行的末尾，窗口区域中不存在的像素值将被忽略。
继续在部分920'的第二行进行抽取，将窗口940向下移动两行，将像素944定位在窗口940的左上方。取最大值产生图17b中的像素值954，依此类推直到第二行的末尾。以这种方式，当窗口940的左上角分别位于像素941至949上时，图17b的像素值951至959对应于像素的最大值。因此，例如，图17b的中心像素955是白色的，最大值占据窗口940，其左上角位于图17a的像素945之上。重复抽取并在每个下一个较低的分辨率下获取最大值，直到只有单个像素926保持在匹配金字塔910的最低分辨率为止。
排除金字塔
继续图14的步骤804，排除金字塔的构建如下进行。如上所述，对于匹配的金字塔构建，接收到的地图720表示包含自由空间和障碍物的仓库的区域。地图720具有在二进制范围0，1内的像素值，其中零(0)或黑色表示障碍物，而一(1)或白色表示自由空间。可选地，地图720可以使用在0，255范围内的像素值来表示仓库内的障碍物，其中零(0)的值代表障碍物，而255的值指示自由空间。灰色像素(通常值为128)代表未知区域。与最高分辨率匹配地图的构建一样，地图720中的灰色区域将转换为黑色。
图15b描绘了最高分辨率排除地图910的一部分，其中零(0)或黑色的像素值表示障碍物，而一(1)或白色的值表示自由空间。与匹配金字塔不同，地图720不会倒置，并且不会执行任何模糊化操作来构建地图910。相反，障碍物(黑色)会根据机器人的最大范围扩大或“膨胀”，例如，机器人的半径。通过在所有方向上延伸表示障碍物的地图720的像素，该像素具有与机器人的半径相对应的多个像素，可以容易地实现通过机器人半径膨胀。例如，对于半径为0.25米的机器人，在每像素5厘米的地图中，可以在所有方向上“填充”25个黑色(0)像素，使其与每个黑色(障碍)像素相邻。可选地，障碍物最多可以填充机器人半径的两倍，以确保机器人在机器人上所有点的运动都畅通无阻。
如图16b所示，排除金字塔950包括一组分辨率依次降低的排除地图，从金字塔底部的最高分辨率排除地图952开始，到顶部的最低分辨率排除地图。排除金字塔的构建始于地图952(代表仓库的完整地图)被零(黑色)填充，以形成排除金字塔950分辨率最高的正方形位图。最高分辨率排除地图952是大小和分辨率等于如上匹配金字塔910所示和所述的最高分辨率匹配地图912的正方形位图。注意，在优选实施例中，因为不对排除地图执行模糊化，所以排除地图中的像素值是二进制的0，1，其中机器人半径扩大了(黑色)障碍物的位置。
与匹配金字塔的构建一样，排除金字塔950的构建是通过将地图952抽取为连续较低分辨率的地图(954、956、958、960)来进行的，每个较低分辨率的地图都是通过抽取下一个最高分辨率的地图而得出的。例如，较低分辨率排除地图954是从最高分辨率排除地图952的抽取中得出的；较低分辨率排除地图954是从较高分辨率排除地图952的抽取中得出的。通过抽取排除地图954等来导出较低分辨率的排除地图956，直到仅剩下一个像素最低分辨率排除地图。
如图18a-c所示，排除金字塔950的抽取与匹配金字塔910的抽取方式相同。这样，图18a和图18b示出了排除金字塔950的地图960的一部分960'的抽取，以产生下一个较低分辨率的排除地图962。当窗口940越过地图960'的每隔一行中的每个第二像素时，该窗口中的最大像素值将保留为下一个较低分辨率的相应像素值。以下一个较低的分辨率重复抽取，直到仅剩下单个像素980。
排除金字塔的每个相继较低分辨率的抽取会低估扫描匹配将机器人置于障碍物中的可能性，从而防止在金字塔的较低分辨率下考虑排除有效位姿候选者。同样地，抽取排除地图的方式高估了在自由空间中为机器人找到位姿的可能性。在排除地图和带有m3rsm的排除金字塔的应用中，将明确说明这种高估的目的和好处，如下所述。
查找位姿
返回到图14的步骤810，机器人系统600接收本地环境的激光雷达扫描，并继续寻找与接收的扫描最匹配的位姿。具有接收到的地图并且已经构建了匹配和排除金字塔的机器人系统600之后可继续查找机器人的位姿。
参照图19进一步描述查找机器人的位姿。从步骤834开始，将搜索区域确定为所接收地图的要搜索的候选位姿的部分。在第一次迭代中，搜索区域可包括整个地图。在随后的迭代中，机器人系统600可以仅在地图的一部分中估计其位姿。用于确定搜索区域的位姿可根据最近的已知位姿与传感器数据(例如，传动系/轮式编码器和/或传动控制信息)组合来估算。
在优选实施例中，状态估计模块650可将位姿数据与轮式编码器数据和惯性传感器数据融合，以确定每个机器人的当前位姿、速度和估计误差。因此，估计的位姿将搜索限制在地图的一部分上，从而减少了搜索空间并减少了扫描匹配的处理时间。位姿估计的不确定性越低，可能需要扫描匹配的搜索区域越小。不确定性越大，可能需要扫描匹配的搜索区域越大。本领域技术人员将理解，可通过各种方法和参数来执行位姿估计和确定搜索区域。
再次参考图19，在步骤836中，机器人系统600根据确定的搜索区域创建候选位姿的初始搜索空间。优选地，初始搜索空间是包含搜索任务的搜索堆，每个搜索任务包括一个或多个候选位姿、匹配金字塔的一种分辨率的匹配地图以及抽取为匹配地图的分辨率的接收扫描。在一个实施例中，每个搜索任务在平移和旋转处定义候选位姿，用于将接收到的扫描与匹配地图的一部分进行扫描匹配。可选地，每个搜索任务可通过一系列平移和旋转来定义候选位姿，其中，一个分辨率扫描的旋转和/或抽取按需执行并缓存。当首次生成搜索堆时，扫描可能会针对搜索堆中的每个旋转进行旋转并缓存。当需要特定分辨率的扫描以进行扫描匹配时，可以将已缓存的旋转扫描及其抽取后的版本从缓存中取出。因此，如果具有相同旋转和分辨率的搜索任务需要针对不同的平移进行抽取和旋转扫描，可以从缓存中检索先前旋转和抽取的扫描，从而节省了重复点云操作的计算时间。
返回到图19，持续搜索候选位姿(步骤838)，直到找到匹配金字塔的最高分辨率的最佳位姿(步骤840)。如果选择的最佳位姿不在最高分辨率匹配地图上(步骤844)，则扩展搜索空间(步骤848)，并且继续进行候选位姿的搜索(步骤838)。如果最佳位姿在最高分辨率匹配地图上，则处理返回(步骤842)到如前所述的图14的步骤812。
根据候选位姿与相应任务对应的匹配金字塔地图分辨率相匹配的最大可能性而计算的分配上限值，来安排搜索堆上的搜索任务。可以在edwinolson的“m3rsm：多对多的多分辨率扫描匹配”中找到有效创建和扩展搜索堆以及计算堆中搜索任务最大可能性上限的技术，该技术先前已纳入参考上面。
然而，在常规的m3rsm中，扩展搜索堆包括将搜索任务添加到具有候选位姿的堆中，这会将机器人定位在环境中的障碍物中或部分障碍物之内。如将显示的那样，将上述排除金字塔合并到m3rsm中，可防止使用此类任务扩展搜索堆，从而消除了评估无效位姿的计算量。此外，通过在将搜索任务添加到搜索堆之前对其进行修剪，还可以消除与无效位姿相关联的所有高分辨率的搜索任务。
在图20中说明了在创建和评估将使机器人定位在障碍物中的搜索堆上的搜索任务之前，使用排除金字塔使候选位姿无效的过程。如前所述，搜索堆990可以包含任务t1和t2，每个任务包括激光雷达扫描s、匹配金字塔分辨率r的匹配地图m以及扫描s的候选平移和旋转范围。任务t1的评估通过旋转和将扫描s以及地图m在在一组旋转上匹配扫描来找到最佳候选位姿p。如果分辨率r处于匹配金字塔的最高分辨率，则确定位姿p是机器人位姿，并且图19的步骤812结束。如果分辨率r不是最高分辨率，则任务t1可以扩展到下一个最高分辨率r-1。
在从分辨率为r的搜索任务扩展搜索堆时，可以从匹配金字塔的分辨率为r-1的匹配地图中添加四个新的搜索任务(例如，图20中的t3-t6)。该四个新的搜索任务从对应于分辨率r-1的地图的初始抽取的四像素正方形到以分辨率r展开的搜索任务的位姿创建。每个新的搜索任务将占扩展搜索任务平移范围的四分之一。然而，在本发明的实施例中，在创建新的搜索任务之前，在每个新任务的候选位姿p的位置处查询分辨率为r-1的排除地图。如果分辨率为r-1的候选位姿会将机器人放置在分辨率为r-1的排除地图的黑色像素(障碍物)上，则该位姿的扩展搜索任务将被消除，并且不会插入搜索堆中。
例如，如图1所示，参照图20，如果将分辨率为r的任务t1扩展为分辨率为r-1的新任务t3-t6，则算法将任务t5的候选位姿p定位在排除地图960的像素969中(图21中示出了960'的一部分)，不会将任务t5添加到搜索堆中。通过在将任务t5添加到搜索堆之前对其进行修剪，还可以消除可能已经在位置p创建的对应于分辨率为r-2的像素969和更高的分辨率的所有搜索任务，因为任务t5将永远不会在下一个最高分辨率的地图中的相应位置进行评估(因此不会被扩展)。
图22a和22b示出了从上述过程中找到“最佳位姿”。图22a示出了局部扫描730和初始位姿估计740。图21b示出了在匹配金字塔的最高分辨率匹配地图上通过平移t*从位姿估计740平移成最佳位姿750的局部扫描匹配。从步骤842返回750，完成了图14中的步骤812，该步骤如上所述进行以确定用于沿目标路径递增移动机器人的下一瞬时速度。
尽管本发明的前述描述使本领域普通技术人员能够制造和使用目前被认为是其最佳模式的技术，但是本领域普通技术人员将理解并认同具体实施例和示例的变型，组合和等效形式的存在。在这里。本发明的上述实施方式仅是示例。在不脱离本发明的范围的情况下，本领域技术人员可以对特定实施例进行改变，修改和变化，本发明的范围仅由所附权利要求限定。
应该理解，本发明可以用软件和/或硬件来实现。因此，本发明的多个方面可以采取完全硬件实施例，完全软件实施例(包括固件，常驻软件，微代码等)或结合了软件和硬件方面的实施例的形式，这些方面通常都可以被称为“电路”、“模块”或“系统”。如本领域技术人员将理解的，本发明的各方面可以体现为系统，方法或计算机程序产品。
参考方法和装置(系统)的流程图，图示和/或框图描述了本发明的各方面。流程图和框图可以示出根据本发明的各种实施例的系统架构，功能或操作。流程图中的每个步骤可以代表一个模块，该模块包括一个或多个用于实现指定功能的可执行指令。在一些实施方式中，连续示出的步骤实际上可以基本上同时执行。步骤可以由执行指定功能或动作的基于专用硬件的系统执行，也可以由专用硬件和计算机指令的组合执行。
用于由执行本发明的操作的处理器执行的计算机指令可以用一种或多种编程语言编写，包括诸如c#、c++、python或java编程语言之类的面向对象的编程语言。计算机程序指令可以存储在计算机可读介质上，该计算机可读介质可以指导机器人系统经由数据处理器以特定方式起作用，包括执行实现本文所述的流程图和/或系统框图中指定的步骤的指令。计算机可读存储介质可以是包含或存储供数据处理器使用或与其结合使用的指令的任何有形介质。计算机可读介质还可以包括其中包含有计算机可读程序代码的传播数据信号。
因此，本发明不受上述实施例和示例，实施例和应用的限制，而本发明在以下要求保护的范围和精神内。
- 还没有人留言评论。精彩留言会获得点赞！