CN112975951A - 用于具有障碍物规避的机器人路径规划的快速方法 - Google Patents

Abstract

本教导涉及用于路径规划的方法和系统。获取具有多个可操作区段的机器人臂的当前姿态的信息。该信息包括多个值，其中每一个值对应于在机器人臂的连续可操作区段之间形成的角度。还获取机器人臂需要达到的期望姿态。基于当前姿态与期望姿态之间的距离的函数来为机器人臂的当前姿态计算角度步进值，其中该角度步进值将被用来确定多个可操作区段的多个候选下一姿态。基于至少一个准则来选择候选下一姿态中的一者或多者，并且基于所选择的下一姿态来确定从当前姿态到期望姿态的轨迹。

Description

用于具有障碍物规避的机器人路径规划的快速方法

背景技术

发明领域

本公开涉及具有障碍物规避的机器人路径规划的系统和方法。具体而言，本公开涉及用于在规避障碍物的同时规划机器人臂的移动的快速方法。

相关技术说明

机器人路径规划方法试图找到从初始位置到目标位置的机器人运动的轨迹。对于具有多个关节的机器人臂而言，运动自由度(DOF)等于关节数。因此，对于每一次移动存在尽可能如关节数那样多的移动。一般而言，可能需要运动是平滑的，轨迹长度是短的，路径中没有奇异点，以及规避障碍物。通常，关节角的连续空间可被离散化为高维网格。路径规划可以在关节角空间的网格中执行。

现有路径规划方法可被分类成两种类型：非确定性和确定性。诸如快速探索随机树(RRT)算法之类的非确定性方法随机且偏向未搜索的大区域探索机器人的下一个移动。此类方法之所以是快速的，是因为在每一步骤只探索网格中的一个可能的下一移动。然而，这些方法无法保证可行路径。通过RRT方法找到的路径在长度上可能不受限。此外，如果重新规划第二次，则移动可能由于搜索的随机性质而不同。诸如A*算法探索之类的确定性方法通过最小化成本函数来探索网格中的所有可能的移动。所生成的路径是唯一的。然而，由于可能需要探索网格中的所有相邻节点，因此规划过程可能缓慢。

因此，提供一种快速且提供与传统确定性算法的最优路径尽可能接近的路径的确定性路径规划方法是高度合乎需要的。

附图说明

根据示例性实施例进一步描述本文描述的本公开的各方面。这些示例性实施例将参考附图详述。这些实施例是非限制性的示例性实施例，其中贯穿附图的若干个视图，相同的附图标记表示相似的结构，并且其中：

图1描绘了用于机器人路径规划的示例性系统图；

图2示出了用于路径规划的示例性流程图；

图3示出了用于路径平滑的示例性流程图；

图4示出了路径平滑的示例；以及

图5描绘了可以被用于实现结合本教导的专用系统的计算机的架构。

具体实施方式

本公开涉及用于在规避障碍物的同时进行机器人路径规划的方法和系统。具体而言，本公开涉及在规避障碍物的同时为具有多个关节的机器人臂规划路径的系统和方法。在本文中所称为的机器人臂是具有多个区段的机器人的臂。机器人臂的姿态(即，位置和定向)可通过多个值(例如，角度值)来确定，其中每一个值对应于在机器人臂的连续可操作区段之间形成的角度。

图1示出了根据本教导的实施例的促成机器人路径规划(即，机器人臂路径规划)的示例性系统图100。系统100包括障碍物检测器104、位置相关的动态分辨率生成单元108、相邻节点减少单元110、路径优化单元112和路径平滑单元114。

如上所述，根据本公开的一个实施例，机器人臂可具有多个关节。具体而言，机器人的臂可包括多个区段，其中相邻区段之间的连接被称为关节。典型的机器人可具有6或7个关节，这意味着该机器人可具有6或7个移动自由度(DOF)。机器人的初始姿态(即，位置和定向)102可通过在关节之间形成的角度(本文中被称为关节角)来表征。必须认识到，例如，当目标对象在机器人已开始向目标移动之后已改变其位置时，关节角还可表征机器人运动的任何其它位置。障碍物103可表示机器人的工作空间内的一个或多个主体，该一个或多个主体各自由障碍物检测器104标识。(多个)障碍物可以是外科医生、患者身体部位、或手术室中的另一医疗设备。目标姿态(即，最终位置和定向)106可表示机器人臂的期望姿态。必须认识到，机器人臂可包括末端执行器(即，连接在机器人臂末端的设备或工具)。关节角的目标姿态可基于机器人逆向运动学来从末端执行器的期望姿态(位置和定向)中计算出。

机器人路径规划通常可以在关节角的离散化空间中执行。每一关节角可由间隔δ(本文中也被称为角度步进值)来离散化。例如，对于6-DOF机器人，任何特定位置可由关节角“配置”J＝(j₁,j₂,j₃,j₄,j₅,j₆)来表示，其中j₁是第i个关节的关节角。术语配置被用来表示所有关节的组合关节角，这唯一地确定末端执行器的姿态。根据一个实施例，在机器人移动至下一位置后，每一个关节可处在以下关节角之一：j_i-δ、j_i、j_i+δ，i＝1,2,…,6。必须注意，在机器人臂的路径规划的迭代期间，关节可保持在与前一位置相同的关节角。然而，所有关节保持与前一次迭代相同的角度是被禁止的，因为这表示无运动状态。因此，从一个配置到下一配置可存在3⁶-1＝728个可能的移动(对于总共N＝6个关节)。在离散化关节角空间中，关节角配置还可被称为关节角网格中的“节点”。路径规划问题变为在满足特定准则的同时找到从网格中的初始节点开始到网格中的期望节点的路径。期望节点还可被称为“结束节点”或“目标节点”。

位置相关的动态分辨率生成单元108可生成非恒定间隔(步进值)的网格以提高查找路径的效率。相邻节点减少单元110可修剪从一个节点到其相邻节点的搜索空间，使得只访问重要节点。接下来描述关于动态分辨率生成单元108和相邻节点减少单元110的细节。路径优化单元112可基于特定准则来查找从初始节点到目标节点的最优路径。路径优化单元112的输出是运动轨迹，该运动轨迹指定到达目标节点的机器人运动的顺序。路径平滑单元114可平滑由路径优化单元112生成的抖动运动。平滑的输出是经平滑的运动轨迹116。

图2示出了根据本教导的一个实施例的用于机器人路径规划的示例性流程图。在步骤202，可获取机器人在初始姿态下的初始关节角。在步骤204，可以标识一个或多个障碍物。在步骤206，可获取机器人关节角的目标/期望姿态。在步骤208，可离散化机器人关节角空间。由δ_n来表示关节角网格中的第n个节点s_n处的离散化间隔。离散化分辨率δ_n(即，角度步进值)可被表达为从当前节点到目标节点的距离的函数。

δ_n＝w(c_n)

其中c_n是从第n个节点到目标节点的距离，且w(c_n)是c_n的函数。距离c_n可被定义为第n个节点处的关节角与目标节点处的关节角之间的欧几里得距离，而函数w(c_n)可以是距离c_n的线性、抛物线或S型函数(sigmoid function)。S型函数的示例性实施例可采取以下形式

其中δ_max是最大分辨率，α是控制曲线陡度的形状因子，且ρ是初始节点与目标节点之间的所估计的最长距离。α的最优值可通过实验找到，但通常可被设置为6。当α被设置为6时，在初始节点，当c_n＝ρ时，分辨率δ_n将接近δ_max，而在目标节点c_n＝0，分辨率δ_n将是接近0的非常小的数字。当规划从初始位置到目标位置的路径时，分辨率将逐渐从δ_max变为非常小的数字。直观地，动态分辨率可以解释如下。当机器人远离目标位置时，它可以较大步移动，而当它较接近目标位置时，它可以较小步移动。路径规划的常规方法使用跨整条路径恒定的分辨率。在本教导中，离散化分辨率可取决于从当前节点到目标节点的距离来动态生成。

在步骤210，从每一个节点到其相邻节点的搜索空间可被减小。减少的一个实施例可基于相邻节点优先级排序。在关节角网格的每一个节点处，对于6-DOF机器人臂存在728个相邻节点。优先级排序的一个实施例可以是只考虑所有6个关节角都被改变的相邻节点。这意味着在节点n处，如果j_i,n＝j_i,n+1，则意味着第i个关节未在从节点n移至节点n+1时改变，可消除节点n+1。这意味着每一个关节对于下一次移动只可具有两个状态：角度被递增或递减步进值。与原始的728个相邻节点相比，这相当于减少到2⁶-1＝63个相邻节点。这比相邻节点的原始数量减少超过90％。根据本公开的一个实施例，可确定已经改变的关节角的数量，而不是要求所有六个关节角都被(从其先前值)改变。此外，出于减少邻居目的，可以只考虑已改变关节角的数量大于预定阈值的那些相邻节点。

相邻节点减少的另一个实施例可基于障碍物感知。如果从节点n到相邻节点的移动碰到障碍物或者在距障碍物不被允许的距离内，则移动方向可被添加至节点n的可规避方向的集合。由A(n)表示该集合。在已经访问节点n的所有相邻节点后，集合A(n)包含应被规避的运动方向。当来到探索那些相邻节点的时间时，该集合可被应用于节点n的直接相邻节点。直观上，如果节点的任何探索方向朝向障碍物，则由于极有可能在同一方向上存在障碍物，因此应当在该节点的直接相邻节点正在探索其自己的相邻节点时规避该方向。

在邻居减少后，使用常规的A*算法基于减少的邻居来确定最小化成本函数(步骤212)的轨迹。通过一个实施例，成本函数和最小化的细节在下文中描述。假设轨迹包括N个步骤1、2……N。在任一中间步骤n，从起始位置到第n个位置的成本可由g(n)来表示。成本g(n)可被定义为初始位置处的关节角(即，末端执行器的配置)与第n个步骤处的那些关节角之间的欧几里得距离。提供对从第n个步骤到最终步骤(N)的成本的估计的另一成本可由h(n)表示。成本h(n)可被定义为第n个步骤处的关节角与步骤N处的那些关节角(目标姿态)之间的欧几里得距离。由c(n)来表示在步骤n机器人臂(包括末端执行器)与所有障碍物的最小距离(即，最短距离)。在第n个步骤的总成本函数f(n)可采取以下形式：

f(n)＝g(n)+h(n)-ω*c(n)

其中ω是大于零的常数加权因子。权重ω在关节角成本与障碍物距离成本之间达成平衡。该加权因子可以通过如在以整体并入本文的美国专利申请No.16/692,264中描述的过程来学习。成本函数f(n)的最小化可使用诸如A*算法之类的现有方法来执行。与全相邻节点搜索相比，在相邻节点减少后规划的路径可生成次最优路径。在步骤214，可平滑找到的路径以生成平滑轨迹。

图3示出了根据本教导的一个实施例的用于步骤214的路径平滑的示例性流程。构成轨迹的节点在本文中被称为航点p_k,k＝1,2…,N，其中N是航点数。轨迹的平滑是挑选组成平滑轨迹的航点，而同时确保新轨迹不与障碍物碰撞。平滑在步骤302开始于第一航点。然后在步骤304访问下一航点。在步骤306，检查将当前航点p_k连接到在先航点{p₁,p₂,…,p_k-1}中的每一者的直接路径是否与障碍物碰撞。在此，直接路径意指通过直接从一个航点移动至另一航点生成的路径。在步骤308，忽略其到当前航点p_k的直接路径与障碍物碰撞的在先航点。然后针对其余无碰撞航点{p_m}中的每一者，可计算成本。该成本可被计算为从当前航点p_k到在先航点p_m的欧几里得距离和p_m处的成本的加权和。p_m处的成本已经在第一次访问航点p_m时计算。直观地，这是从当前航点p_k经由航点p_m去往第一航点的成本。

在步骤312，挑选所有在先航点{p_m}中具有最小成本的航点。在步骤313，为所挑选的航点存储最小成本作为该航点的成本。在步骤314，航点索引可递增1以去往下一航点p_k+1。在步骤316，检查下一航点p_k+1是否已经到达最后航点N。如果否，则可重复以上过程。否则，可以在步骤318执行回溯以检取经平滑的路径。回溯可通过从最后航点开始检取经由其到达第一航点的最小成本航点来执行。最小成本航点将被用作新航点以检取其自己的最小成本航点。该过程重复直到到达第一航点。

图4示出了路径平滑中的关键步骤的示例。实线是轨迹并且线上的点是航点。点x_s(402)是第一航点，点x_g(404)是最后航点，且406是障碍物。在航点x_i(408)处，到第一航点x_s的直接路径是最小成本路径，并且该最小成本被存储为航点x_i的成本。从航点x_j(410)进行考虑，从x_j到x_i的成本和航点x_i处的成本的加权和是x_j的所有在先航点中的最小成本。从最后航点x_g进行考虑，从x_g到其在先航点x_i的直接路径碰到障碍物，并且因此该路径将不被考虑。航点x_j是从x_g出发的最小成本航点。从最后航点到第一航点的回溯路径是x_g--x_j--x_i--x_s。

图5是根据本教导的各实施例的示例性计算机系统架构的说明性图示。结合本教导的此类专用系统具有包括用户接口元件的硬件平台的功能框图图示。计算机500可以是通用计算机或专用计算机。两者可以被用于实现用于本教导的专用系统。计算机500可用于实现本文描述的任何组件。例如，可以在诸如计算机500之类的计算机上经由该计算机的硬件、软件程序、固件、或其组合来实现本教导。尽管为了方便起见仅示出了一个这样的计算机，但本文描述的与本教导相关的计算机功能可以按分布式方式在数个类似平台上实现，以分布处理负载。

计算机500例如可包括通信端口550，该通信端口550被连接到连接至该通信端口550的网络并且从连接至该通信端口550的网络被连接以促成数据通信。计算机400还包括用于执行程序指令的、一个或多个处理器形式的中央处理单元(CPU)520。示例性计算机平台还可包括内部通信总线510、不同形式的程序存储和数据存储(例如，盘570、只读存储器(ROM)530、或随机存取存储器(RAM)540)，以用于要由计算机500处理和/或传递的各种数据文件以及可能的要由CPU520执行的程序指令。计算机500还可包括I/O组件560，该I/O组件560支持计算机与该计算机中的其他组件(诸如用户接口元件580)之间的输入/输出流。计算机500还可经由网络通信来接收编程和数据。

因此，如上所概述的本教导的各方面可以在编程中被体现。技术的程序方面可被认为是通常以在某类机器可读介质上执行或实现在某类机器可读介质中的可执行代码和/或关联数据的形式的“产品”或“制品”。有形非瞬态“存储”类型介质包括可提供在任何时间用于软件编程的存储的存储器或用于计算机、处理器或类似物的其它存储、或其相关联的模块(诸如各种半导体存储器、磁带驱动器、磁盘驱动器等)中的任何或所有。

软件的所有或部分有时可通过网络(诸如因特网或各种其它电信网络)来传递。此类通信例如可使得能够将软件从一个计算机或处理器加载到另一个计算机或处理器中，例如，从机器人运动规划系统的服务器或主机计算机加载到计算环境或实现计算环境或与路径规划有关的类似功能性的其他系统的(多个)硬件平台中。因此，可承载软件元素的另一种类型的介质包括光波、电波和电磁波，诸如跨本地设备之间的物理接口、通过有线和光学地线网络、并且通过各种空中链路来使用。携载此类波的物理元件(诸如有线或无线链路、光学链路或类似物等)也可以被认为是承载软件的介质。如本文所使用的，除非被限制成有形“存储”介质，否则诸如计算机或机器“可读介质”之类的术语指的是参与向处理器提供指令以供执行的任何介质。

因此，机器可读介质可以采取许多形式，包括但不限于有形存储介质、载波介质、或物理传输介质。非易失性存储介质包括例如可用于实现如附图中所示的系统或该系统的组件中的任一组件的光盘或磁盘，诸如在任何(多个)计算机中的存储设备或类似物中的任一个。易失性存储介质包括动态存储器，诸如此类计算机平台的主存储器。有形传输介质包括同轴电缆；铜线和光纤，其包括在计算机系统内形成总线的线。载波传输介质可采取电信号或电磁信号、或声波或光波(诸如在射频(RF)和红外(IR)数据通信期间生成的那些)的形式。因此，计算机可读介质的常见形式包括例如：软盘、柔性盘、硬盘、磁带、任何其他磁介质、CD-ROM、DVD或DVD-ROM、任何其他光学介质、穿孔卡片纸带、具有孔图案的任何其他物理存储介质、RAM、PROM和EPROM、FLASH-EPROM、任何其他存储器芯片或卡带盒、传输数据或指令的载波、传输此类载波的电缆或链路、或者计算机可从中读取编程代码和/或数据的任何其他介质。在将一条或多条指令的一个或多个序列承载至物理处理器以供执行时可以涉及这些形式的计算机可读介质中的许多形式。

本领域技术人员将认识到，本文的教导顺应于各种修改和/或增强。例如，尽管上面描述的各种组件的实现可被体现在硬件设备中，但是它也可被实现为仅软件解决方案—例如，安装在现有服务器上。另外，如本文中所公开的机器人运动规划系统可被实现为固件、固件/软件组合、固件/硬件组合、或硬件/固件/软件组合。

尽管前文已经描述了被认为构成本教导和/或其它示例的内容，但是应该理解，可以对其做出各种修改，并且本文公开的主题可以以各种形式和示例来实施，并且教导可以应用于许多应用中，仅其中的一些在本文中被已经描述。所附权利要求旨在要求保护落入本教导的真实范围内的任何和所有应用、修改和变型。

Claims (20)

1.一种在机器上实现的用于路径规划的方法，所述机器具有至少一个处理器、存储以及能够连接至网络的通信平台，所述方法包括：

获取具有多个可操作区段的机器人臂的当前姿态的信息，其中，所述信息包括多个值，所述多个值中的每一个值对应于在所述机器人臂的连续可操作区段之间形成的角度；

获取所述机器人臂需要达到的期望姿态；

基于所述当前姿态与所述期望姿态之间的距离的函数来为所述机器人臂的所述当前姿态计算角度步进值，其中所述角度步进值被用来确定所述多个可操作区段的多个候选下一姿态；

基于至少一个准则来选择所述多个候选下一姿态中的一者或多者；以及

基于所选择的一个或多个候选下一姿态来确定从所述当前姿态到所述期望姿态的轨迹。

2.如权利要求1所述的方法，其中，所述至少一个准则对应于所选择的一个或多个候选下一姿态中的每一者所具有的与所述多个可操作区段相关联的每一个角度的值都不同于与所述当前姿态相关联的每一个角度的对应值。

3.如权利要求1所述的方法，其中，所述至少一个准则对应于所述一个或多个候选下一姿态中的每一者都与在所述当前姿态与所述期望姿态之间观察到的一个或多个障碍物相距预定距离。

4.如权利要求1所述的方法，其中，所述至少一个准则对应于所选择的一个或多个候选下一姿态中的每一者所具有的与所述多个可操作区段相关联的多个角度的值都不同于与所述当前姿态相关联的多个角度的对应值，且都大于阈值。

5.如权利要求1所述的方法，其中，为所述机器人臂的所述当前姿态计算的所述角度步进值大于为所选择的一个或多个候选下一姿态计算的角度步进值。

6.如权利要求1所述的方法，其中，所述角度步进值基于所述机器人臂的初始姿态与所述机器人臂需要达到的所述期望姿态之间的估计最长距离来进一步计算。

7.如权利要求1所述的方法，进一步包括：

平滑所确定的轨迹，其中，所述轨迹包括多个航点，所述多个航点中的每一个航点与所述机器人臂的一个姿态相关联，所述平滑基于为每一航点计算的成本。

8.一种用于路径规划的系统，所述系统包括：

至少一个处理器，所述至少一个处理器被配置成用于：

获取具有多个可操作区段的机器人臂的当前姿态的信息，其中所述信息包括多个值，所述多个值中的每一个值对应于在所述机器人臂的连续可操作区段之间形成的角度；

获取所述机器人臂需要达到的期望姿态；

基于所述当前姿态与所述期望姿态之间的距离的函数来为所述机器人臂的所述当前姿态计算角度步进值，其中所述角度步进值被用来确定所述多个可操作区段的多个候选下一姿态；

基于至少一个准则来选择所述多个候选下一姿态中的一者或多者；以及

基于所选择的一个或多个候选下一姿态来确定从所述当前姿态到所述期望姿态的轨迹。

9.如权利要求8所述的系统，其中，所述至少一个准则对应于所选择的一个或多个候选下一姿态中的每一者所具有的与所述多个可操作区段相关联的每一个角度的值都不同于与所述当前姿态相关联的每一个角度的对应值。

10.如权利要求8所述的系统，其中，所述至少一个准则对应于所述一个或多个候选下一姿态中的每一者都与在所述当前姿态与所述期望姿态之间观察到的一个或多个障碍物相距预定距离。

11.如权利要求8所述的系统，其中，所述至少一个准则对应于所选择的一个或多个候选下一姿态中的每一者所具有的与所述多个可操作区段相关联的多个角度的值都不同于与所述当前姿态相关联的多个角度的对应值，且都大于阈值。

12.如权利要求8所述的系统，其中，为所述机器人臂的所述当前姿态计算的所述角度步进值大于为所选择的一个或多个候选下一姿态计算的角度步进值。

13.如权利要求8所述的系统，其中，所述角度步进值基于所述机器人臂的初始姿态与所述机器人臂需要达到的所述期望姿态之间的估计最长距离来进一步计算。

14.如权利要求8所述的系统，其中，所述至少一个处理器被进一步配置成用于：

平滑所确定的轨迹，其中，所述轨迹包括多个航点，所述多个航点中的每一个航点与所述机器人臂的一个姿态相关联，所述平滑基于为每一航点计算的成本。

15.一种非瞬态机器可读介质，所述介质上记录有信息，所述信息用于搜索数据，其中所述信息当被机器读取时使所述机器执行以下步骤：

获取具有多个可操作区段的机器人臂的当前姿态的信息，其中，所述信息包括多个值，所述多个值中的每一个值对应于在所述机器人臂的连续可操作区段之间形成的角度；

获取所述机器人臂需要达到的期望姿态；

基于所述当前姿态与所述期望姿态之间的距离的函数来为所述机器人臂的所述当前姿态计算角度步进值，其中，所述角度步进值被用来确定所述多个可操作区段的多个候选下一姿态；

基于至少一个准则来选择所述多个候选下一姿态中的一者或多者；以及

基于所选择的一个或多个候选下一姿态来确定从所述当前姿态到所述期望姿态的轨迹。

16.如权利要求15所述的介质，其中，所述至少一个准则对应于所选择的一个或多个候选下一姿态中的每一者所具有的与所述多个可操作区段相关联的每一个角度的值都不同于与所述当前姿态相关联的每一个角度的对应值。

17.如权利要求15所述的介质，其中，所述至少一个准则对应于所述一个或多个候选下一姿态中的每一者都与在所述当前姿态与所述期望姿态之间观察到的一个或多个障碍物相距预定距离。

18.如权利要求15所述的介质，其中，所述至少一个准则对应于所选择的一个或多个候选下一姿态中的每一者所具有的与所述多个可操作区段相关联的多个角度的值都不同于与所述当前姿态相关联的多个角度的对应值，且都大于阈值。

19.如权利要求15所述的介质，其中，为所述机器人臂的所述当前姿态计算的所述角度步进值大于为所选择的一个或多个候选下一姿态计算的角度步进值。

20.如权利要求15所述的介质，进一步包括：

平滑所确定的轨迹，其中，所述轨迹包括多个航点，所述多个航点中的每一个航点与所述机器人臂的一个姿态相关联，所述平滑基于为每一航点计算的成本。

Images