CN116214495A - 机器人动态运动规划和控制的方法 - Google Patents

Abstract

一种用于具有冗余自由度的机器人的运动规划的方法和系统。该技术计算具有冗余自由度的机器人的碰撞避免运动规划，而不人为地约束额外的自由度。运动规划被公式化为具有多分量目标函数和碰撞避免约束函数的二次规划优化计算。该公式足够有效以在每个机器人控制周期实时计算运动规划。碰撞避免约束确保机器人的所有部分与静态和动态障碍物的间隙。目标函数项包括路径偏差最小化、关节速度正则化和机器人配置或姿态正则化。关于目标函数的项的加权因子对于每个控制周期计算基于当时的障碍物接近条件是可变的。

Description

机器人动态运动规划和控制的方法

技术领域

本公开总体上涉及工业机器人运动规划领域，并且更具体地，涉及用于具有冗余自由度的机器人的运动规划的方法和系统，包括动态障碍物的存在，其中运动规划被公式化为具有多分量目标函数和碰撞避免约束函数的二次规划优化计算，并且其中基于当时的障碍物接近条件，关于目标函数的各个项的加权因子对于每个控制周期计算是可变的。

背景技术

使用工业机器人来执行各种各样的制造、组装和材料移动操作是众所周知的。在一些应用中，所采用的机器人具有比正在执行的任务更多的自由度(DOF)。当传统的六自由度机器人用于五自由度任务时，例如工具绕其局部轴线的旋转(“自旋”)位置不重要并且可以取任何值时，情况就是如此。七自由度机器人也被设计成有意地具有比所需任务(例如喷漆)更多的自由度，其中额外的自由度提高了机器人的近距接触的灵活性，例如使机器人臂能够折叠并与车体和喷漆室的壁之间的狭窄空间适应。

在本领域中已经开发了用于具有冗余自由度的机器人的运动规划的技术。一种已知的技术涉及限定机器人肘关节必须保持在其中的平面。这种额外的约束将机器人的有效自由度减少了一个，并且使得能够使用逆运动学(IK)计算来解析地计算运动规划。然而，这种技术人为地限制了机器人的灵活性，并且需要额外的编程步骤以使操作者限定肘关节平面。

另一种用于具有冗余自由度的机器人的运动规划的已知技术利用了雅可比矩阵的零空间中的冗余自由度，然后使用优化计算来计算机器人运动。该技术具有若干缺点，包括缺乏对优化计算的任何约束，优化计算一次限于单个目标函数变量，并且该技术仅适用于机器人示教模式。示教模式限制又引入了另外的缺点，包括不能处理线路跟踪应用(例如在传送带上移动的车体)，以及不能在运动规划中执行动态障碍物避开计算。

在许多机器人工作空间环境中，障碍物存在并且有时可能在机器人运动的路径中。障碍物可以是永久物体，诸如建筑物结构和固定装置，由于物体的静态性质，机器人可以利用预先规划的运动来容易地避开这些永久物体。障碍物还可以是随机地移动进入或通过机器人工作空间的动态对象。动态物体必须由运动规划系统在实时计算中考虑，其中机器人必须在执行操作的同时操纵以避开物体。必须绝对避免机器人的任何部分与任何障碍物之间的碰撞，而在存在动态障碍物的情况下简单地停止机器人却不是令人满意的解决方案。

鉴于上述情况，需要一种用于冗余机器人的改进的动态运动规划技术，其允许机器人的完全灵活性，包括当动态对象存在于工作空间中时在机器人运动中的障碍物避开，并且足够快地完成运动规划计算以在每个机器人控制周期期间执行。

发明内容

根据本公开的教导，描述并示出了用于具有冗余自由度的机器人的运动规划的方法和系统。该技术计算具有冗余自由度的机器人的碰撞避免运动规划，而不人为约束额外的自由度。运动规划被公式化为具有多分量目标函数和碰撞避免约束函数的二次规划优化计算。该公式足够有效以在每个机器人控制周期实时计算运动规划。碰撞避免约束确保机器人的所有部分与静态和动态障碍物的间隙。目标函数项包括使路径偏差最小化、关节速度正则化和机器人配置或姿态正则化。基于此时的障碍物接近条件，，关于目标函数的各个项的加权因子对于每个控制周期计算是可变的。

结合附图，根据以下描述和所附权利要求，当前公开的系统和方法的附加特征将变得显而易见。

附图说明

图1是根据本公开的实施例的用于冗余机器人的动态运动规划系统的框图图示，包括运动规划中的碰撞避免；

图2是示出用于自适应调节图1的动态运动优化模块中使用的目标函数中的加权因子λ₁和λ₂的两个不同模型的曲线图；

图3是用于提供在图1的动态运动优化模块中使用的参考位置矢量q_ref的两种不同技术的图示；

图4A和4B是经历姿态或配置改变的机器人的简化图示；

图5是根据本公开的实施例的用于冗余机器人的动态机器人运动规划和控制的方法的流程图；

图6是根据本公开的第一实施例的用于冗余机器人的动态运动规划系统和控制器的框图图示，包括运动规划中的碰撞避免；

图7是根据本公开的第二实施例的用于冗余机器人的动态运动规划系统和控制器的框图图示，包括运动规划中的碰撞避免；

图8是根据本公开的实施例的三维空间中的机器人工具中心点路径以及避开工作空间中的静态障碍物所需的机器人肘关节轨迹修改的图；

图9A、9B和9C是具有固定头顶障碍物的工作空间中的机器人的图示，包括使用和不使用本公开的技术的运动规划结果；以及

图10A、10B和10C是在工作空间中协力操作的两个机器人的图示，包括使用和不使用本公开的技术的运动规划结果。

具体实施方式

以下针对冗余机器人的动态运动规划和控制的本公开的实施例的讨论本质上仅是示例性的，并且决不旨在限制所公开的设备和技术或它们的应用或使用。

使用工业机器人进行各种制造、组装和材料移动操作是众所周知的。在一些应用中，机器人具有比正在执行的任务的自由度的数量更多的运动自由度。例如，当七轴机器人用于喷涂应用，其中喷嘴位置和姿态完全由六个自由度限定时，情况就是如此。

此外，在许多机器人工作空间环境中，可能存在障碍物，并且障碍物有时可能在机器人运动的路径上。即，在没有自适应运动规划的情况下，当机器人从其当前位置移动到目的地位置时，机器人的一些部分可能与障碍物的一些部分碰撞。障碍物可以是静态结构，例如固定装置和桌子，或者障碍物可以是动态(移动)对象，例如人、叉车和其它机器。当动态对象可能存在时，必须针对每个控制周期实时规划机器人的运动。

在本领域中已经开发了用于计算具有冗余自由度的机器人的运动的技术。然而，这些技术表现出各种缺点，包括由于需要定义人工约束而限制了机器人的灵活性，由于优化目标函数和约束的限制而缺乏对解决方案的充分控制，仅在示教模式中而不是在实时运动规划中使用的能力，以及由此导致的不能处理动态障碍物避开或路线跟踪应用。

本公开的动态运动规划系统通过将运动规划公式化为具有多分量目标函数和碰撞避免约束函数的二次规划优化计算来克服现有技术系统的缺点。该技术产生了一种关节运动规划，该关节运动规划寻求使与规划的路径的偏差最小化、平衡关节速度、以及避免路径中间机器人配置/姿态改变，同时所有这些都满足动态对象碰撞避免约束。优化计算足够有效以在机器人运动期间实时执行，并且计算还被公式化，使得基于当时的障碍物接近条件，关于目标函数的各个项的加权因子对于每个控制周期计算都是可变的。

图1是根据本公开的实施例的用于冗余机器人的动态运动规划系统的框图图示，包括运动规划中的碰撞避免。规划器模块110基于输入的目标(目的地)位置计算规划的机器人运动。在一个非限制性示例中，机器人工具是夹持器，并且机器人的任务是将零件从源(起始)位置移动到目标位置。当然，存在许多其它任务的示例，诸如从一个点焊位置移动到下一个点焊位置、移动到新位置以开始喷漆行程等。在一个实施例中，规划的机器人运动u_des是定义笛卡尔空间中的“设计”(规划)机器人运动的加速度矢量。具体地，u_des可以被定义为工具中心点加速度，其也可以被表示为

其中x是六个自由度上的工具中心点位置和取向。运动规划还可以使用速度而不是加速度来执行，在这种情况下，u_des将被定义为工具中心点速度/>

规划器模块110将规划的机器人运动u_des提供给动态运动优化模块120。

输入的目标(目的地)位置可以是移动目标-诸如沿着输送机移动的车体。在这种类型的“路线跟踪”应用中，目标位置可以被定义为基于输送机速度的时间的线性函数，或者甚至可以对于输送机速度可以变化的应用针对每个控制周期来计算。在任何情况下，机器人工具的目标位置是在将来当机器人臂计划到达时那个时候工具需要处于其中的位置。

动态运动优化模块120还接收从感知模块130输入的障碍物数据。感知模块130包括一个或多个相机或传感器，其被配置为提供关于可能存在于机器人工作空间中的障碍物的数据。障碍物数据通常包括最小机器人-障碍物距离，并且还可包括关于任何障碍物的位置(包括空间形状)和速度的其它数据。

动态运动优化模块120执行优化计算，该优化计算使与规划的机器人运动u_des的跟踪偏差最小化，并且还优化机器人运动特性，同时包括机器人机械限制和碰撞避免安全函数作为约束。该优化计算导致命令的机器人运动q_cmd。命令的机器人运动q_cmd是关节空间中的机器人运动，其将机器人工具带到目标位置，同时避开机器人工作空间中的任何障碍物。下面详细讨论优化计算。反馈回路140将来自动态运动优化模块120的命令的机器人运动q_cmd提供回到规划器模块110。规划器模块110和动态运动优化模块120在每个机器人控制周期重复上述计算。

动态运动优化模块120还将命令的机器人运动q_cmd提供给机器人控制器，机器人控制器将机器人控制命令提供给机器人(未示出)，并且在反馈回路上接收实际的机器人关节位置q_act，如本领域已知的。机器人控制器基于实际的机器人关节位置q_act和命令的机器人运动q_cmd，在每个控制周期更新机器人控制命令。这将在下面进一步讨论。

框150示出了先前讨论的机器人冗余如何影响图1的系统。工具中心点的运动u_des是具有维数m的实数

集合的元素。机器人关节运动矢量q_cmd是具有维数n的实数集合的元素，其中n>m。DOF冗余是(n-m)。如先前所讨论的，考虑7-DOF机器人执行6-DOF任务的情况。在这种情况下，n＝7，m＝6，并且冗余是(7-6)＝1个自由度。这个单个冗余自由度允许优化解算器在找到有效地将工具中心点移动到目标位置的机器人运动解方面具有一定的自由，同时确保碰撞避免并且还满足如下所述的其它机器人运动“平滑度”目标。

框160示出了在动态运动优化模块120中使用的优化计算的基本公式。优化计算包括目标函数170，其寻求使命令的机器人运动的若干属性的组合最小化。目标函数170将在下面详细讨论。优化计算还包括至少一个不等式约束180，其解决了机器人的结构/机械限制。所示的不等式约束180确保机器人关节速度

保持低于预定最大关节速度/>

该预定最大关节速度/>

基于机器人机械限制而预先确定。在一些实施例中，可以添加一个或多个其它不等式约束(图1中未示出)，其确保机器人关节加速度和加加速度保持在预定最大值以下，该预定最大值也基于机器人机械限制。可以添加附加的不等式约束(以上未示出)，其要求关节位置q保持在预定关节位置范围内。

在框160中所示的优化计算中还包括碰撞避免安全约束190。安全约束190是碰撞避免不等式约束的一个实施例，其确保机器人不与存在于机器人工作空间中的任何障碍物碰撞。安全约束190采用一个模型，其基于机器人-障碍物最小距离或机器人-障碍物最小距离与机器人-障碍物相对速度的组合来定义安全函数h(X)，其中安全函数h(X)的值等于机器人和障碍物彼此远离移动时的机器人-障碍物最小距离，并且安全函数h(X)的值等于该最小距离减去机器人和障碍物朝向彼此移动时的相对速度项。如图1所示，不等式安全约束190然后被定义为

简单地说，安全约束190规定机器人-障碍物最小距离d越大，则容许的距离变化率/>

越大。可以建立系数γ的值以实现期望的系统行为，其中γ的较小值导致更保守的机器人行为(增加的操纵以提供更大的障碍物避开距离)。机器人-障碍物距离和相对速度根据来自感知模块130的障碍物数据和已知的机器人配置数据来确定。

在序列号为17/455，676、标题为动态运动规划系统、2021年11月19日提交的美国专利申请中公开了安全约束190的完整讨论及其在基于碰撞避免优化的运动规划系统中的使用，该申请与本申请共同转让，并通过引用将其全部内容结合于此。在框160的优化计算中可以采用任何其它合适类型的碰撞避免安全约束，包括使用仅基于机器人-障碍物最小距离的安全函数。

回到图1上的目标函数170，可以观察到它包含三项：工具中心点跟踪项172、关节速度正则化项174和姿态正则化项176。项172、174和176中的每一个是表示机器人运动的不同特性的矢量的范数的平方。工具中心点跟踪项172，被计算为

表示命令的关节运动q与规划的工具中心点路径u_des的偏差，其中J是雅可比行列式(机器人配置的导数)。在机器人工作空间中没有任何障碍物的情况下，命令的关节运动q将使工具中心点遵循规划路径u_des。然而，当工作空间中存在障碍物时，安全约束190可能要求命令的关节运动q使工具中心点偏离规划的路径u_des，以便避免碰撞。目标函数170中的工具中心点跟踪项172试图使该偏差最小化。有时，当障碍物存在于机器人的除工具之外的一些部分的路径中时，可以找到不影响工具中心点路径而是导致机器人的臂弯曲成避开障碍物的形状的机器人运动规划。下面讨论这种情况的例子。

计算为

的关节速度正则化项174表示包括机器人中的所有关节的机器人关节速度矢量的总幅值，其中λ₁是加权因子(如后面讨论的，其是可变的)。通常，期望在将工具中心点移动到目标位置的同时，机器人关节的旋转尽可能小且平衡。目标函数170中的关节速度正则化项174试图通过使关节速度矢量的范数最小化来提供该行为。

在一个实施例中计算为λ₂‖q_ref-q‖²的姿态正则化项176表示机器人姿态(关节位置矢量)q与参考姿态(参考位置矢量)q_ref的偏差，其中λ₂是另一个加权因子(如后面讨论的，它是可变的)。当机器人从起始位置移动到目标(目的地)位置时，期望机器人保持一致的姿态(关节位置和臂位置的总体配置)，其中每个关节角度从一个时间步(控制周期)到下一个时间步递增小的量。也就是说，一个或多个关节从一个步到下一个步突然“翻转”到反向或相反位置是不期望的。目标函数170中的姿态正则化项176试图通过将关节位置矢量与参考位置矢量之间的差的范数最小化来提供期望的行为。姿态正则化项的其它公式化稍后讨论。

图1的框160中所示的优化计算的有益特征是，目标函数170中的加权因子λ₁和λ₂可以“在运行中”改变以实现期望的机器人行为。也就是说，可针对动态运动优化模块120的每个控制周期计算来自适应地设置λ₁和λ₂的值。例如，当工作空间中不存在障碍物或者任何障碍物远离机器人时，期望在使路径偏差最小化的同时保持优选的机器人姿态；在这种情况下，可以在减小λ₁的值的同时增大λ₂的值(以突出目标函数170中的姿态正则化项176的作用)。另一方面，当障碍物靠近机器人并且可能需要碰撞避免操纵时，可能不能维持优选的机器人姿态；在此情况下，可以在增加λ₁的值的同时减小λ₂的值(以削弱目标函数170中的姿势正则化项176的作用)。在确定目标函数170中的加权因子λ₁和λ₂的值时可以考虑其它因子。

图2是示出用于自适应调节在图1的动态运动优化模块120中使用的目标函数170中的加权因子λ₁和λ₂的两个不同模型的曲线图200。曲线图200绘制了竖直轴210上的加权因子λ₁的值对水平轴220上的最小机器人-障碍物距离。轴210和220未被标记有具体值，因为可以使用图200上示出的概念来调整这些值以实现期望的结果。

阶跃函数曲线230示出了第一技术，其可用于基于最小机器人-障碍物距离来自适应地调节目标函数170中的加权因子λ₁和λ₂。当最小机器人-障碍物距离低(在曲线图200的左侧，意味着很可能进行躲避碰撞避免操纵)时，λ₁被设定为高值(诸如1.0)。相应的λ₂被设置为低值(诸如0.0)。目标函数170中的加权因子值的这种组合使得优化计算收敛到突出速度正则化同时使跟踪偏差最小化的解，并且即使考虑姿势正则化也仅稍微考虑姿势正则化。

当最小机器人-障碍物距离较高，即，最小距离超过诸如0.5米的阈值(在曲线图200的右侧，意味着不可能进行躲避碰撞避免操纵)时，λ₁被设定为低值(诸如0.0)。相应的λ₂被设置为高值(诸如1.0)。目标函数170中的加权因子值的这种组合使得优化计算收敛到突出姿态正则化同时最小化跟踪偏差的解，并且即使考虑速度正则化也仅稍微考虑速度正则化。当感知系统130没有检测到障碍物时，将其视为与高的最小机器人-障碍物距离相同(λ₁被设置为低值；λ₂被设置为高值)。阶跃函数曲线230中从高λ₁到低λ₁的转变不必是竖直线；例如，它可以是具有负斜率的线。

连续曲线240示出了第二种技术，其可以用于基于最小机器人-障碍物距离来自适应地调整目标函数170中的加权因子λ₁和λ₂。连续曲线240还在机器人-障碍物最小距离低时将λ₁设定为高值，在机器人-障碍物最小距离高时将λ₁设定为低值。然而，与阶跃函数曲线230不同，连续曲线240提供从λ₁的高值到低值的平滑过渡。连续曲线240可以被建模为例如代数函数(诸如三次或五次)，或者建模为三角函数(诸如余弦)。在一些实施例中，在使用连续曲线240或阶跃函数曲线230确定λ₁的值之后，λ₂的值可通过其中λ₁和λ₂的和等于常数的等式来确定，使得当λ₂较低时λ₁较高，反之亦然。

再次，在曲线图200中，中间曲线图或阈值距离可以是除0.5米之外的某一值，并且加权因子λ₁和λ₂的最大值和最小值不必分别为1.0和0.0。例如，最低加权因子值可以略大于零(诸如0.1)，使得总是考虑目标函数170中的所有项。而且，最大加权因子值可高于或低于1.0，这将影响路径跟踪偏移项172与速度和姿势正则化项174和176之间的平衡。可以选择所使用的精确的自适应模型(在曲线图200上示出的两个自适应模型中的，或者可以设想的其它自适应模型)以及λ₁和λ₂的值以及阈值最小距离，以在特定应用中提供最佳结果。在任何情况下，框160中所示的优化计算公式为具有冗余自由度的机器人的实时碰撞避免运动规划提供了强大的能力，其中目标函数170特征是在每个计算周期进行自适应加权。

在目标函数170中，姿态正则化项176(λ₂‖q_ref-q‖²)通过使关节位置矢量q和参考位置矢量q_ref之间的差值的范数最小化来寻求提供期望的机器人行为。这要求为从起始点到目标点的机器人运动的每一步提供参考位置矢量q_ref。

图3是用于提供在图1的动态运动优化模块120中使用的参考位置矢量q_ref的两种不同技术的图示。机器人需要将工具中心点从起始点310(或P1)移动到目标点312(或P2)。点310和312(P1和P2)中的每一个在笛卡尔空间中由三个位置自由度(x，y，z)、三个旋转自由度(偏航、俯仰和滚动，或w，p，r)和额外的自由度(α)来限定。笛卡尔空间中的工具中心点310和312(P1和P2)中的每一个在关节空间中具有相应的机器人位置。关节位置矢量320(或q1)对应于工具中心点310(P1)，并且关节位置矢量322(或q2)对应于工具中心点312(P2)。关节位置矢量320和322(q1和q2)中的每一个在关节空间中由六个关节位置自由度(J1,…,J6)和额外的自由度(E1)限定。

图3示出了用于当机器人将工具中心点从P1移动到P2时提供参考位置矢量q_ref的两种不同技术。在第一技术300(图3的上半部分)中，在框302处在关节空间中进行插值。使用技术300，根据起始工具中心点P1计算起始关节矢量q1，并且根据目标工具中心点P2计算目标关节矢量q2。然后，在关节空间中进行q1和q2之间的插值，从而在关节空间中产生点集合qi(330)，其在工具中心点P1和P2之间的空间中限定弯曲路径。如本领域技术人员所理解的，将工具中心从一点移动到另一点的最有效的机器人关节运动通常使工具中心点遵循曲线路径。对于技术300，当机器人将工具中心点从P1移动到P2时的参考位置矢量q_ref使用关节矢量qi(330)由(q1，…，qi，…，q2)定义。

在第二技术350(图3的下半部分)中，在框352处在笛卡尔空间中进行插值。使用技术350，首先在框352处，在起始工具中心点P1和目标工具中心点P2之间插值点集合Pi(360)，从而在笛卡尔空间中产生从P1到P2的线性路径。然后在框354处，使用逆运动学(IK)计算将点集合Pi转换为关节空间中的点集合qi(370)。对于技术350，当机器人将工具中心点从P1移动到P2时的参考位置矢量q_ref使用关节矢量qi(370)由(q1，…，qi，…，q2)定义。

在目标函数170中，以上讨论的姿态正则化项176(λ₂‖q_ref-q‖²)试图通过使关节位置矢量q和参考位置矢量q_ref之间的差的范数最小化来提供期望的机器人行为。以下是对可用于优化计算的目标函数170中的姿态正则化的另一技术的讨论。

简言之，目标函数中的姿态正则化项的目的是在机器人将工具从起始点移动到目标点时保持机器人的一致的总体姿态。“一致的总体姿势”可以被描述为在一范围内平稳移动的关节位置和手臂位置，避免任何关节位置和/或手臂位置从一个时间步到下一个时间步突然进行大的跳跃或改变。在机器人运动规划中通常遇到的突然配置或姿态改变的一个简单示例是其中(在外臂的端部处的)腕关节从一个时间步到下一个时间步“翻转”到反转位置以便将工具放置在期望的取向。

图4A和4B是经历姿态或配置改变的机器人的另一示例的简化图示。机器人400包括固定的基座410、通过竖直轴关节412耦合到基座410的底座420、通过关节422耦合到底座420的臂430、通过关节432耦合到臂430的臂440、通过关节442耦合到臂440的臂450、以及通过关节452耦合到臂450的工具460。工具中心点462被限定在工具460上。机器人400仅是说明性的，并且被绘制以适合本讨论的目的。其它机器人可以包括不同数量的臂和关节、允许一个臂相对于另一个臂轴向旋转的关节等。

在图4A中，机器人400被示出为处于标准姿态或配置，其中臂430基本上水平地取向，而臂440基本上竖直地取向，导致在关节432处的关节位置为大约90°。图4A中所示的标准姿态可被认为是“下”姿态，因为关节432位于关节442的正下方。

在图4B中，机器人400被示出为处于替代姿态或配置，其中臂430基本上竖直地取向，并且臂440基本上水平地取向，导致在关节432处的关节位置为大约-90°。图4B所示的替代姿态可被认为是“上”姿态，因为关节432(内臂中的额外关节)位于关节422的正上方。即使工具中心点462在图4A和4B中处于完全相同的位置，机器人400也可以采取导致该工具中心点位置的两个非常不同的姿态或配置。

如上所述，目标函数中的姿态正则化项的目的是在机器人将工具从起始点移动到目标点时保持机器人的一致的整体姿态。就图4A和4B而言，机器人400在工具运动的中间从“下”姿态(图4A)切换到“上”姿态(图4B)是不期望的。目标函数中的姿态正则化项被设计成防止这种类型的在运动中间的姿态改变。以上讨论了姿态正则化项的一个实施例(将所提议的关节位置与q_ref进行比较)。以下讨论姿态正则化项的另一实施例。

姿态的一致性还可以通过初始计算目的地配置(当工具中心点在目标位置时的机器人姿态或配置)，并且单独地评估在机器人任务期间倾向于显著姿态改变的关节(例如，腕关节“翻转”，或额外的内臂关节“向上”相对于“向下)的各个关节角度来实现。不是如前面所讨论的那样在目标函数中用公式表示姿态正则化项(λ₂‖q_ref-q‖²)，而是可以在姿态正则化项中使用代价函数，该代价函数惩罚与目的地配置相比具有显著姿态变化的任何解。建立这种代价函数的一种方法是检测倾向于显著姿势变化的任何关节的位置的符号变化。如上文关于图4A和4B所讨论的，如果肘关节在目的地(目标工具中心点)配置处具有约90°的位置角度，并且优化计算评估对于一时间步的包括肘关节在-90°左右的位置角度的解矢量，则该解矢量将被代价函数严重惩罚。通过使用关节的建议位置与关节的目的地位置的内积，可以检测符号的改变并且在代价函数中对其进行惩罚。

又一种类型的姿态正则化项可使用代价函数来构造，该代价函数相对于各个关节位置在目的地配置处的位置来评估该各个关节位置，但使用角变化的量而符号的改变。这种类型的代价函数姿态正则化项在以下方程(1)中示出。

M·(‖θ_d,wrist-θ_wrist‖+‖θ_d,ext-inn-θ_ext-inn‖) (1)

其中θ_d,wrist是在目的地配置处的腕关节角度，θ_wrist是在当前由优化解算器评估的配置处的腕关节角度，并且对于额外的内臂关节也是如此，并且其中M是可以被调整以实现期望结果的数，并且在每个控制周期计算是改变的。换句话说，对于与目的地配置具有大的关节位置差的配置，方程(1)将取较大值。当方程(1)被用作目标函数中的姿态正则化项(而不是λ₂‖q_ref-q‖²)时，优化计算将趋向于移动离开明显不同于目标姿态的配置或姿态。

以上讨论的两种姿态正则化公式都已经被示出以在工作空间中存在障碍物的情况下在实时机器人运动规划的详细数学仿真中提供良好的结果。结果在随后的图中示出并在下面进一步讨论。

图5是根据本公开的实施例的用于冗余机器人的动态机器人运动规划和控制的方法的流程图500。在框502处，基于目标或目的地工具中心点位置来计算规划的机器人运动。在代表性实施例中，规划的机器人运动是笛卡尔空间中的工具中心点加速度矢量，其定义将工具中心点移动到目标位置的“设计”(规划的)机器人运动u_des。在框504处，感知模块提供工作空间障碍物数据。感知模块包括至少一个相机或传感器，诸如三维(3D)相机，其可以检测工作空间中存在的任何障碍物的位置。感知模块可以包括图像处理器，其根据相机图像计算障碍物位置数据，或者感知模块可以简单地将原始相机图像提供给执行机器人运动优化计算的计算机或控制器。障碍物位置数据优选地在工作空间坐标系中计算，在该坐标系中，可以容易地将其与机器人位置数据进行比较。最小机器人-障碍物距离和机器人-障碍物相对速度最终需要从障碍物数据获得。

在框506处，基于规划的机器人运动和障碍物数据执行机器人运动优化计算。机器人运动优化计算的输出是上面关于图1讨论的命令的机器人运动q_cmd。如果在工作空间中不存在障碍物，则命令的机器人运动根据规划的机器人运动来移动工具中心点。在反馈回路中将命令的机器人运动提供给框502，其中在每个控制周期基于目标工具中心点位置和命令的机器人运动(其可能已经在优化期间被修改以避开任何障碍物)重新计算规划的机器人运动。

在框508处，机器人控制器将命令的机器人运动提供给机器人。机器人控制器可以执行计算或变换，以便向机器人提供合适的机器人关节运动命令。在框510处，机器人实际上基于来自控制器的关节运动命令而移动。机器人和控制器作为闭环反馈控制系统操作，其中实际机器人状态q_act(关节位置和速度)被反馈到控制器以用于计算更新的关节命令。机器人和控制器在具有指定时长(即，一定数量的毫秒)的控制周期上操作。

在框506中，运动优化问题可以公式化为：

使得：

其中方程(2)是要最小化的优化目标函数170(前面参照图1讨论的-包括工具中心点跟踪项、关节速度正则化项和姿态正则化项)，并且方程(3)-(4)是在迭代优化计算期间必须满足的不等式约束。在方程(3)中，

是机器人中的所有关节的关节速度，/>

是基于机器人机械限制的预定最大关节速度。可以添加类似于方程(3)的其它不等式约束(上面未示出)，但是针对关节加速度和加加速度而不是速度。可以添加附加的不等式约束(以上未示出)，其要求关节位置q保持在预定关节位置范围内。

方程(2)中所示的姿态正则化项(λ₂‖q_ref-q‖²)可以由方程(1)中所示的和先前讨论的类型的代价函数姿态正则化项代替。

对于障碍物避开约束(方程(4))，目标是保持安全函数h(X)≥0，如上面关于图1所讨论的。障碍物避开约束(方程(4))和安全函数h(X)的公式在以上讨论的并入的序列号17/455，676的申请中详细描述。如前所述，也可以使用仅基于机器人-障碍物最小距离的不同安全函数。

在每个机器人控制周期执行使用方程(2)-(4)的优化计算。在收敛时，优化计算产生命令的机器人运动q_cmd，其表示具有目标函数项(跟踪偏差、速度正则化和姿态正则化)的最小组合同时满足不等式约束的机器人运动。如前所述，可以基于工作空间障碍物条件针对每个优化计算(每个控制周期)来调整关于目标函数的正则化项(λ₁和λ₂或M)的加权因子系数。

在图5中，在框502处的规划的机器人运动的计算、在框506处的机器人运动优化计算和在框508处的机器人关节运动命令的计算都可以在与机器人实时通信的机器人控制器上执行。或者，框502和506的计算可以在单独的计算机上执行，并且在框508处将每个控制周期的命令的机器人运动提供给控制器。这两个替代硬件实现在以下两个图中示出并在下面讨论。

图6是根据本公开的第一实施例的用于冗余机器人的动态运动规划系统和控制器的框图图示，包括运动规划中的碰撞避免。在该实施例中，包括处理器和存储器的单独的计算机600执行图1的规划器模块110和动态运动优化模块120中的所有计算。感知模块130与单独的计算机600通信，并将相机图像和/或传感器数据形式的障碍物数据提供给单独的计算机600以在动态运动优化模块120中使用。

单独的计算机600与包括机器人控制器650和机器人660的机器人系统602通信。具体地，从动态运动优化模块120向控制器650提供命令的机器人运动q_cmd。控制器650在以定义的控制周期(特定数目的毫秒)操作的实时控制系统中控制机器人660的运动，其中实际机器人状态q_act(关节位置和速度)在反馈回路670上被提供回到控制器650。在图6的系统中，单独的计算机600可以是专门用于机器人系统600的专用计算机，或者单独的计算机600可以执行用于多个机器人系统的运动优化计算。

图7是根据本公开的第二实施例的用于冗余机器人的动态运动规划系统和控制器的框图图示，包括运动规划中的碰撞避免。在该实施例中，没有单独的计算机；相反，冗余机器人的所有运动优化计算和控制是机器人系统700本身自包含的。也就是说，在与机器人660通信的传统机器人控制器(具有处理器和存储器)上执行规划器模块110、动态运动优化模块120和机器人控制器模块650中的计算。感知模块130向机器人系统700提供相机图像和/或传感器数据形式的障碍物数据，以在动态运动优化模块120中使用。控制器模块650控制在定义的控制周期上操作的实时控制系统中的机器人660的运动，其中实际机器人状态q_act(关节位置和速度)在反馈回路770上被提供回到控制器模块650和规划器模块110。

图7的系统，连同在机器人控制器上执行的所有机器人运动规划、优化和控制计算，提供了避免使用单独的计算机600的优点。另一方面，图6的系统允许在与实时机器人控制器解耦的单独处理器上执行优化计算，这提供了更快的运动优化计算的优点。在任一硬件实现中，必须基于当时的机器人配置和障碍物数据，针对每个机器人反馈控制周期执行运动优化计算，确保与任何动态障碍物的碰撞避免，并且提供冗余机器人的最优运动特性。

图1-7的动态运动规划技术已经被证明在具有冗余自由度的模拟机器人系统中产生包括障碍物避免的可靠运动规划结果。这包括有效的运动规划以避开工作空间中的任何障碍物，同时使路径偏差最小化并维持期望的关节速度和机器人姿态，以及快速计算安全函数和得到的运动优化。

图8是根据本公开的实施例的三维空间中的机器人工具中心点路径以及避开工作空间中的静态障碍物所需的机器人肘关节路径修改的曲线图。工作空间800在3D空间中由如图所示的正交的X、Y和Z轴表示。机器人(未示出)在工作空间800中操作，并且需要执行涉及将工具中心点从起始点810移动到目标(目的地)点812的任务。工具中心点遵循从起始点810到目的点812的TCP轨迹820。图8表示使用本公开的用于冗余机器人的动态路径规划技术从特定机器人和控制器的仿真中收集的数据。

除了工具中心点之外，机器人肘部关节点830在图8中示出。机器人运动(肘关节运动)的两种场景在图8中描绘。在第一场景中，在工作空间800中不存在障碍物。在没有任何障碍物的情况下，机器人将沿着TCP轨迹820将工具中心点从起始点810移动到目标点812，而肘部关节点830沿着肘部参考轨迹840移动。

在第二场景中，固定的球形障碍物850被放置在工作空间800中在阻挡肘部参考轨迹840的位置处。缓冲区860限定了障碍物850周围的安全距离余量，使得机器人的所有部分都应当保持在缓冲区860之外。第二场景模拟是使用相同的工具中心起始点810和目标点812在具有障碍物850的情况下运行的。本公开的动态路径规划技术用于计算障碍物避开机器人运动规划。由于目标函数中的跟踪偏差项，如同当不存在障碍物时一样，当障碍物850在工作空间中时工具中心点基本上精确地遵循TCP轨迹820。然而，在障碍物850存在的情况下，肘部关节点830遵循非常不同的路径-示出为实际肘部轨迹870。肘部轨迹870使得肘部关节点830和机器人内臂保持在缓冲区860之外，而机器人仍然沿着期望的路径移动工具中心点。这些模拟结果确认了从满足碰撞避免安全约束的上述动态运动规划计算预期的行为，同时收敛到实际上没有工具中心点路径偏差的解。

图9A、9B和9C是具有固定头顶障碍物的工作空间中的机器人的图示，包括使用和不使用本公开的技术的运动规划结果。图9A-9C示出了冗余机器人与静态障碍物的碰撞避免的另一示例。

机器人900在具有固定的头顶障碍物910的工作空间中操作，该障碍物被图示为在工作空间的右侧处向下倾斜到较低高度的天花板。机器人900需要执行涉及将工具从起始点移动到目标(目的地)点的任务。机器人900在图9A中示出处于起始点配置。

图9B示出了处于目的点配置的机器人900，并且表示来自使用冗余机器人的传统路径规划技术的机器人和控制器的仿真的数据，该传统路径规划技术不能考虑除了工具中心点之外的机器人的任何部分的碰撞避免。在图9B中，机器人900已经按照指示沿着工具中心点轨迹920将工具中心点从起始点移动到目的点。然而，机器人900的肘关节已经在如930处所指示的天花板的下部中与头顶障碍物910碰撞。这显然不是可接受的机器人运动，并且说明了冗余机器人的传统路径规划的限制，其仅能够考虑工具中心点障碍物碰撞。

图9C示出了处于目的点配置的机器人900，并且表示来自使用本公开的用于冗余机器人的动态路径规划技术的机器人和控制器的仿真的数据，包括优化计算中的碰撞避免和运动特性质量。在图9C中，机器人900已经沿着实际上与轨迹920相同的轨迹940移动了工具中心点。然而，机器人900已经铰接了几个臂关节以降低肘关节位置，使得肘关节在如950处所指示的天花板的下部中具有与头顶障碍物910的足够间隙。这显然是比图9B所示好得多的机器人运动，并且示出了本公开的运动规划技术的能力，其使用机器人的冗余自由度来提供期望的工具路径，同时还确保无碰撞的机器人运动。

图10A、10B和10C是在工作空间中协力操作的两个机器人的图示，包括使用和不使用本公开的技术的运动规划结果。图10A-10C示出了冗余机器人与动态障碍物的碰撞避免的示例。在这种情况下，每个机器人代表对其它机器人的运动的潜在障碍物。

机器人1000和机器人1010在工作空间中操作，其中机器人1000和1010中的每一个被要求同时执行任务。机器人1000需要执行涉及将工具从起始点1002移动到目标(目的地)点(未示出)的任务。机器人1000的工具中心点遵循从起始点到目的点的轨迹1004。机器人1010需要执行涉及将工具从起始点1012移动到目标(目的地)点(未示出)的任务。机器人1010的工具中心点遵循从起始点到目的点的轨迹1014。工具中心点轨迹1004和1014在图10C中整体示出，如下所述。

图10A示出了机器人1000和1010的运动，其是利用不包括动态对象碰撞避免的传统方法来规划的。在图10A中，机器人1000和1010被示出为处于它们正在执行的任务的大致运动中途的配置。在该配置中，机器人1000的工具与机器人1010的肘关节碰撞，如箭头1020所示。任何类型的碰撞显然都不是可接受的结果。

图10B示出了使用本公开的用于冗余机器人的动态路径规划技术规划的机器人1000和1010的运动，包括优化计算中的碰撞避免和运动特性质量。在图10B中，机器人1000和1010再次被示出为处于它们正在执行的任务的大致运动中途的配置。然而，在这种情况下，机器人1000的工具不会与机器人1010的肘关节碰撞，间隙如箭头1030处所示。该间隙是可能的，因为机器人1010如箭头1032所示弯曲其内臂额外关节，导致肘关节被稍微向内拉向机器人基座。

图10C包括处于其最终配置的机器人1000和1010的简化图图示，其中机器人1000的工具放置在工具中心点轨迹1004的末端处的目的点1006处，并且机器人1010的工具放置在工具中心点轨迹1014的末端处的目的点1016处。图10C还示出了机器人1010的对于利用传统技术规划的运动(图10A)的肘关节轨迹1040。对于利用本公开的技术规划的运动(图10B)，机器人1010的肘关节遵循相同的轨迹1040，除了在箭头1042处指示的区域之外。如箭头1042处所示，机器人1010的肘关节在中途向下且向内下降，以便产生图10B中所示的间隙，因此避免碰撞。

值得注意的是，本公开的技术避免了机器人-机器人碰撞，而不会导致机器人1000或机器人1010的工具中心点路径的偏离。这利用已加权的目标函数通过机器人的冗余自由度以及上述运动优化计算来实现，该已加权的目标函数可以寻求在也满足碰撞避免约束的同时使工具路径偏差最小化的解。

还执行了两机器人工作空间的其它模拟(类似于图10A-10C)，其中目标函数的姿态正则化项在一个模拟中被停用，在第二模拟中通过使用基于参考姿态的姿态正则化公式(λ₂‖q_ref-q‖²)被激活，以及在第三模拟中通过使用代价函数姿态正则化公式(前述的方程(1))被激活。这些仿真证实，具有自适应加权的目标函数以及各个所提出的姿态正则化项中的任一个在使机器人在其运动期间保持优选姿态同时避免碰撞方面是有效的。

图8-10中所示的和上面讨论的结果说明了所公开的用于冗余机器人的运动规划技术的有效性，包括与静态和动态障碍物的碰撞避免、对规定的工具中心点路径的附着以及优选的机器人运动特性(关节速度和机器人姿态)。

另外，对于所公开的技术，每个控制周期的命令的运动计算时间(提供来自动态运动优化模块120的输出的时间)被测量为具有小于一毫秒(ms)的平均值。使用8ms的典型机器人控制周期，本公开的技术执行运动规划计算足够快，同时与现有技术的运动计算方法相比提供了优越得多的碰撞避免和运动平滑度结果。

在整个前述讨论中，描述并暗示了各种计算机和控制器。应当理解，这些计算机和控制器的软件应用和模块在具有处理器和存储器模块的一个或多个电子计算设备上执行。特别地，这包括在以上讨论的图6和7的机器人控制器650和可选的单独计算机600中的每一个中的处理器。具体地，控制器650和/或单独的计算机600中的处理器被配置为执行图5的框506(并且在图1上示出)的冗余机器人碰撞避免运动规划功能，连同框508的机器人反馈运动控制功能。

尽管上面已经讨论了用于冗余机器人的动态运动规划和控制的方法和系统的多个示例性方面和实施例，但是本领域技术人员将认识到其修改、置换、添加和子组合。因此，所附权利要求和此后引入的权利要求旨在被解释为包括所有这些修改、置换、添加和子组合，因为它们在它们的真实精神和范围内。

Claims (28)

1.一种用于机器人的动态运动规划的方法，所述方法在计算装置上执行并且包括：

基于机器人工具中心点的目标位置来计算设计机器人运动；

基于所述设计机器人运动和从多个传感器接收的障碍物数据来计算命令的机器人运动，其中所述命令的机器人运动作为反馈被提供以用于计算下一控制周期的所述设计机器人运动，并且其中计算命令的机器人运动包括优化计算，所述优化计算具有多项目标函数、一个或多个机器人结构限制约束和碰撞避免约束；以及

将所述命令的机器人运动提供给控制器模块，所述控制器模块控制所述机器人的关节运动。

2.根据权利要求1所述的方法，其中，所述计算装置是机器人控制器，所述机器人控制器执行计算设计机器人运动、计算命令的机器人运动以及控制所述机器人的关节运动中的所有。

3.根据权利要求1所述的方法，其中，所述计算装置包括计算机，所述计算机执行计算设计机器人运动和计算命令的机器人运动，并且所述计算机将所述命令的机器人运动提供给机器人控制器，所述机器人控制器控制所述机器人的关节运动。

4.根据权利要求1所述的方法，其中，所述多项目标函数包括路径偏差最小化项、关节速度正则化项和机器人姿态正则化项，并且所述目标函数寻找所述项的和的最小值。

5.根据权利要求4所述的方法，其中，关于所述目标函数的所述项的系数在每个控制周期基于所述机器人与在机器人工作空间中的任何障碍物的接近度是可调整的。

6.根据权利要求4所述的方法，其中，所述关节速度正则化项包括包含所述机器人中的所有关节的关节速度矢量的范数的平方。

7.根据权利要求4所述的方法，其中，所述机器人姿态正则化项包括由所述机器人中的所有关节的关节位置与参考关节位置之间的差定义的矢量的范数的平方。

8.根据权利要求4所述的方法，其中，所述机器人姿态正则化项是代价函数，所述代价函数包括由所述机器人中的一个或多个关节的关节位置与目标关节位置之间的差定义的矢量的范数，所述一个或多个关节被预先确定为在机器人运动期间从一个步到下一个步易受显著姿态改变的影响。

9.根据权利要求1所述的方法，其中，所述一个或多个机器人结构限制约束包括定义以下中的一个或多个的不等式约束：将机器人关节位置保持在预定关节位置范围内、将机器人关节速度保持在预定关节速度极限以下以及将机器人关节加速度保持在预定关节加速度极限以下。

10.根据权利要求1所述的方法，其中，所述碰撞避免约束是根据安全函数计算的不等式约束，其中，所述碰撞避免约束是所述安全函数的变化率必须大于或等于所述安全函数的负数乘以系数。

11.根据权利要求10所述的方法，其中，所述安全函数等于机器人-障碍物最小距离，或者所述安全函数等于所述机器人-障碍物最小距离减去根据机器人-障碍物相对速度计算的项。

12.根据权利要求1所述的方法，其中，所述目标位置是被定义为时间的函数的移动目标位置。

13.根据权利要求1所述的方法，其中，所述设计机器人运动是笛卡尔空间中的工具中心点运动，并且所述命令的机器人运动包括所述机器人中的所有关节的机器人关节旋转运动。

14.根据权利要求13所述的方法，其中，所述机器人具有至少一个冗余自由度，其中，所述机器人关节旋转运动的自由度的数量超过所述工具中心点运动的自由度的数量。

15.一种用于机器人的动态运动规划的方法，所述方法在计算装置上执行并且包括：

基于机器人工具中心点的目标位置来计算设计机器人运动；

基于所述设计机器人运动和从多个传感器接收的障碍物数据来计算命令的机器人运动，其中所述命令的机器人运动作为反馈被提供以用于计算下一控制周期的所述设计机器人运动，并且其中计算命令的机器人运动包括优化计算，所述优化计算具有系数在每个控制周期可调整的多项目标函数、被定义为不等式约束的机器人关节位置、速度和加速度限制、以及碰撞避免约束；以及

将所述命令的机器人运动提供给控制所述机器人的关节运动的控制器模块，

其中，所述设计机器人运动是笛卡尔空间中的工具中心点运动，并且所述命令的机器人运动包括所述机器人中的所有关节的机器人关节旋转运动，并且所述机器人具有至少一个冗余自由度，使得所述机器人关节旋转运动的自由度的数量超过所述工具中心点运动的自由度的数量。

16.根据权利要求15所述的方法，其中，所述多项目标函数包括路径偏差最小化项、关节速度正则化项和机器人姿态正则化项，并且所述目标函数寻找所述项的总和的最小值，并且其中所述目标函数的所述项上的所述系数在每个控制周期基于所述机器人与机器人工作空间中的任何障碍物的接近度是可调整的。

17.一种用于工业机器人的动态运动规划系统，所述系统包括：

感知模块，其包括至少一个传感器或相机，配置为检测所述机器人的工作空间中的障碍物；以及

计算装置，其具有处理器和存储器，所述计算装置配置有：

规划模块，其被编程为基于附接到所述机器人的工具上的工具中心点的目标位置来计算设计机器人运动；以及

运动优化模块，其从所述感知模块接收障碍物数据，所述运动优化模块被编程为基于所述设计机器人运动和所述障碍物数据来计算命令的机器人运动，其中所述命令的机器人运动作为反馈被提供给所述规划模块以用于计算下一控制周期的所述设计机器人运动，并且其中所述命令的机器人运动是使用迭代优化计算来计算的，所述迭代优化计算具有可变加权的多项目标函数、一个或多个机器人结构限制约束和碰撞避免约束，

并且其中所述命令的机器人运动被提供给控制器模块，所述控制模块控制所述机器人的关节运动。

18.根据权利要求17所述的系统，其中，所述计算装置是机器人控制器，所述机器人控制器执行所述规划模块、所述运动优化模块和所述控制器模块，并且向所述机器人提供机器人关节运动命令。

19.根据权利要求17所述的系统，其中，所述计算装置执行所述规划模块和所述运动优化模块，并且机器人控制器与所述计算装置通信，其中，所述机器人控制器执行所述控制器模块并且向所述机器人提供所述机器人关节运动命令。

20.根据权利要求17所述的系统，其中，所述多项目标函数包括路径偏差最小化项、关节速度正则化项和机器人姿态正则化项，并且所述目标函数寻找所述项的和的最小值。

21.根据权利要求20所述的系统，其中，关于所述目标函数的所述项的所述系数在每个控制周期基于所述机器人与机器人工作空间中的任何障碍物的接近度是可调整的。

22.根据权利要求20所述的系统，其中，所述机器人姿态正则化项选自第一公式或第二公式，所述第一公式包括由所述机器人中的所有关节的关节位置与参考关节位置之间的差所定义的矢量的范数的平方，所述第二公式是代价函数，所述代价函数包括由所述机器人中的一个或多个关节的关节位置与目标关节位置之间的差所定义的矢量的范数，所述一个或多个关节被预先确定为在机器人运动期间从一个步到下一个步易受显著姿态改变的影响。

23.根据权利要求17所述的系统，其中，所述一个或多个机器人结构限制约束包括定义以下中的一个或多个的不等式约束：将机器人关节位置保持在预定关节位置范围内、将机器人关节速度保持在预定关节速度极限以下以及将机器人关节加速度保持在预定关节加速度极限以下。

24.根据权利要求17所述的系统，其中，所述碰撞避免约束是根据安全函数计算的不等式约束，其中，所述碰撞避免约束是所述安全函数的变化率必须大于或等于所述安全函数的负数乘以系数。

25.根据权利要求24所述的系统，其中，所述安全函数等于机器人-障碍物最小距离，或者所述安全函数等于所述机器人-障碍物最小距离减去根据机器人-障碍物相对速度计算的项。

26.根据权利要求17所述的系统，其中，所述目标位置是被定义为时间的函数的移动目标位置。

27.根据权利要求17所述的系统，其中，所述设计机器人运动是笛卡尔空间中的工具中心点运动，并且所述命令的机器人运动包括所述机器人中的所有关节的机器人关节旋转运动。

28.根据权利要求27所述的系统，其中，所述机器人具有至少一个冗余自由度，其中，所述机器人关节旋转运动的自由度的数量超过所述工具中心点运动的自由度的数量。

Images