CN102523737A - 实现防撞机制的机器人装置和相关联的方法 - Google Patents

Abstract

一种机器人***实现防撞机制，并且包括第一机器人操纵器和第一控制器，所述第一控制器被构造为控制第一机器人操纵器以沿着第一预规划实际路径移动。第二控制器被构造为以下述方式控制第二机器人操纵器的移动，所述方式为沿着第二预规划预期路径移动，并且在没有第一预规划实际路径的先验知识的情况下，基于确定与第一机器人操纵器的潜在碰撞，偏离第二预规划预期路径以在离开第一预规划实际路径的躲避路径上移动。

Description

实现防撞机制的机器人装置和相关联的方法

技术领域

本发明涉及机器人技术领域，更具体地，涉及用于机器人操纵器的防撞和相关方法。

背景技术

机器人***在诸如制造的领域中是很常见的。事实上，制造厂通常利用包括许多机器人操纵器的机器人***来执行各种任务。为了避免对机器人操纵器的损坏，根据防撞机制控制机器人操纵器是有帮助的。就这点而论，开发了用于机器人***的各种防撞机制。

一些防撞机制通过将每个机器人操纵器约束到预规划的无碰撞路径来工作。例如，一个这样的防撞机制在Otera等人的美国专利No.5,204,942中被公开。这样的防撞机制通常需要重新编程以适应对于机器人操纵器的预规划路径进行的每一个改变。在被常规地改动和更新的制造过程中，由于需要重复对Otera等人的防撞***的重新编程，所以该防撞***可能是不便的。

其它防撞机制可对工作空间进行建模并且将该工作空间分割为不同的区段。可禁止某些机器人操纵器进入某些区段，或者可一次仅允许一个机器人操纵器进入给定区段中。Pollack等人的美国专利No.5,150,452公开了这样的用于机器人***的防撞机制。该机器人***包括存储工作空间的模型的控制器，该工作空间的模型被分割为占用网格。该控制器控制机器人***的机器人操纵器，以使得在给定的时间仅一个机器人操纵器可占据所述占用网格的一个单元。该防撞***可降低制造厂的效率，特别是当存在各种大小不同的机器人操纵器并且占用网格的单元大小被调整尺寸为配合最大的机器人操纵器时。此外，因为该机器人***基于工作空间的模型操作，所以对于工作空间的任何改变可能需要该模型的更新，这可能是耗时的。

用于机器人***的防撞机制的其它尝试包括主动地寻找机器人操纵器之间的潜在碰撞的控制器。例如，Stark的美国专利No.4,578,757公开了一种用于机器人***的防撞机制，该防撞机制将***的每个机器人操纵器建模为许多重叠球体。当机器人操纵器沿着预规划路径移动时，控制器计算邻近的机器人操纵器的每个球体之间的距离。这些计算的距离指示两个相邻机器人操纵器之间的碰撞的风险。当碰撞风险超过阈值量时，机器人操纵器中的至少一个可以随着它沿着它的预规划路径行进而减速，或者甚至完全停止。然而，由于机器人操纵器的停止和制造过程中的相关联的延迟，这样的防撞机制可降低利用机器人***的制造厂的效率。

如所说明的，这些现有方法可致使利用它们的相应机器人***的制造过程低效。而且，利用这些现有方法的机器人***对于适应新应用或者增加附加的机器人操纵器而言可能是困难的和/或高成本的。就这点而论，防撞机制领域中的进一步进步可能是所希望的。

发明内容

因此，鉴于前述背景，本发明的目的是提供一种更有效率的用于机器人装置的防撞机制。

根据本发明的这个和其它目的、特征和优点通过下述机器人操纵器来提供，所述机器人操纵器检测与另一个机器人操纵器的潜在碰撞，并且基于潜在碰撞的检测在躲避路径上移动。更具体地讲，所述机器人装置可包括第一机器人操纵器和第一控制器，所述第一控制器被构造为控制第一机器人操纵器沿着第一预规划实际路径移动。另外，所述机器人装置可包括第二机器人操纵器和第二控制器，所述第二控制器被构造为控制第二机器人操纵器的移动以沿着第二预规划预期路径移动。在没有第一预规划实际路径的先验知识的情况下，基于确定与第一机器人操纵器的潜在碰撞，第二机器人操纵器偏离第二预规划预期路径，以在离开第一预规划实际路径的躲避路径上移动。

该防撞机制有利地使得可在无需对第二预规划预期路径重新编程的情况下对第一机器人操纵器的第一预规划实际路径重新编程。这可缩短使机器人装置适应新应用所花费的时间。

第一控制器可产生用于第一机器人操纵器的第一驱动信号，第二控制器可基于第一驱动信号来确定潜在碰撞。另外或者可替换地，第一机器人操纵器可包括至少一个关节和关节传感器，所述关节传感器与所述至少一个关节和第一控制器合作确定所述至少一个关节的定位。第二控制器可基于第一机器人装置的所述至少一个关节的定位来确定潜在碰撞。这可使得可以更加准确并且使用比通过使用图像传感器的处理能力少的处理能力来确定潜在碰撞。当然，在一些实施例中，第二控制器还可基于图像传感器或关节位置传感器来确定潜在碰撞。

第二控制器可重复地确定第二机器人操纵器与第一机器人操纵器之间的距离，并且可将该距离与阈值距离进行比较，从而确定潜在碰撞。另外，第二控制器还可重复地确定第二机器人操纵器与第一机器人操纵器之间的行近速率，并且第二控制器还可基于行近速率来确定潜在碰撞。

此外，第二控制器还可重复地确定第二机器人操纵器与第一机器人操纵器之间的行近速率，并且第二控制器还可基于行近速率来确定潜在碰撞。第二控制器可基于行近速率来以不同的速度移动第二机器人操纵器。第二控制器还可重复地确定第一机器人操纵器的加速度，并且第二控制器还可基于该加速度来确定潜在碰撞。

另外，第二控制器可存储第一机器人操纵器和第二机器人操纵器的几何模型，并且第二控制器可基于这些几何模型来确定第二机器人操纵器与第一机器人操纵器之间的距离。几何模型的使用可极大地减少确定潜在碰撞时所消耗的处理能力。

每个几何模型可包括包围相应机器人操纵器的一系列缓冲区段。第二控制器可基于缓冲区段之间的重叠来确定第二机器人操纵器与第一机器人操纵器之间的潜在碰撞。另外，第二控制器可基于哪些相应缓冲区段重叠来以不同的速度移动第二机器人操纵器。

第二预规划预期路径可以基于期望速率序列。第二控制器可沿着基于躲避速率序列的躲避路径移动第二机器人操纵器以避免潜在碰撞，同时紧密地跟随第二预规划预期路径的期望速率序列。

第二机器人操纵器可包括至少一个关节，并且第二控制器可基于所述至少一个关节上的力来确定躲避速率序列。另外或者可替换地，第二控制器还可基于第二机器人操纵器的动能来确定躲避速率序列。基于所述至少一个关节上的力或力矩、或者基于第二机器人操纵器的动能确定躲避速率序列可有利地限制第二机器人操纵器沿着躲避速率或会潜在地损坏其硬件的路径前进。

躲避速率序列可包括多个物理方向中的每个上的至少一个速率。第二控制器可基于多个可容许速率凸集来确定躲避速率序列。

方法方面涉及一种根据防撞机制操作机器人装置以避免与第一机器人操纵器碰撞的方法，第一控制器控制第一机器人操纵器沿着第一预规划实际路径移动。所述方法可包括用第二控制器控制第二机器人操纵器沿着第二预规划预期路径移动。所述方法还可包括用第二控制器控制第二机器人操纵器，以在没有第一预规划实际路径的先验知识的情况下，基于确定与第一机器人操纵器的潜在碰撞，在离开第一预规划实际路径的躲避路径上移动。

附图说明

图1是根据本发明的实现防撞机制的机器人装置的示意性侧视图。

图2是图1的机器人装置的示意性透视图。

图3是根据本发明的实现防撞机制的机器人装置的另一实施例的示意性框图。

图4是示出由图3的机器人装置执行的躲避速率的选择的曲线图。

图5是根据本发明的实现防撞机制的机器人装置的又一实施例的示意性框图。

图6是图5的机器人装置的透视图。

图7是根据本发明的被一系列缓冲区段包围的机器人操纵器的示意性侧视图。

图8是根据本发明的按照防撞机制操作机器人装置的方法的流程图。

图9A是第一机器人操纵器和第二机器人操纵器的示意性侧视图。

图9B是第一机器人操纵器和第二机器人操纵器的示意性侧视图，该示意性侧视图包括确定躲避路径时所使用的矢量。

图9C是第一机器人操纵器和第二机器人操纵器的示意性侧视图，该示意性侧视图包括确定躲避路径时所使用的矢量。

图10是示出确定躲避路径时所使用的行近速率的计算的图表。

具体实施方式

现在将在下文中参照附图更充分地描述本发明，在附图中示出了优选实施例。然而，本发明可以以许多不同的形式实施，并且不应该被解读为限于本文所阐述的实施例。相反，提供这些实施例以使得本公开内容将详尽和完整，并且将充分地将本发明的范围传达给本领域技术人员。相似的编号始终表示相似的元件，并且一个和多个主要标记用于指示另选实施例中的类似元件。

现在首先参照图1至图2描述实现防撞机制的机器人装置9。机器人装置9包括第一机器人操纵器10和第一控制器15，第一控制器15被构造为控制第一机器人操纵器沿着第一预规划实际路径11a、11b移动。另外，存在第二机器人操纵器12和第二控制器20。第二控制器20被构造为控制第二机器人操纵器12的移动以沿着第二预规划预期路径13移动。第二控制器20在没有第一预规划实际路径11a、11b的先验知识的情况下基于确定与第一机器人操纵器10的潜在碰撞使第二机器人操纵器12偏离第二预规划预期路径13，以在离开第一预规划实际路径(从而，第一机器人操纵器10)的躲避路径14a、14b上移动。以下将给出躲避路径14a、14b的偏离的进一步的细节。

本领域技术人员将认识到，机器人装置9可包括任何数量的机器人操纵器，并且第一机器人操纵器10和第二机器人操纵器12可以是任何合适的机器人操纵器，例如，用于搬运对象和/或工具的机器人焊接臂或机器人爪。当然，第一机器人操纵器10和第二机器人操纵器12可以是不同类型的机器人操纵器，并且大小可不同。

在其它实施例中，第一预规划实际路径11a、11b可包括任何方向上的连续或不连续移动，并且可包括第一机器人操纵器10的关节26的移动。同样，在其它实施例中，第二预规划预期路径13可包括任何方向上的连续或不连续移动，并且可包括第二机器人操纵器12的关节27的移动。在如图1所示的实施例中，为了清楚地说明，将路径简化为线性移动。

图1中示出了该防撞机制在三个离散的时刻的应用。这里，第一机器人操纵器10被第一控制器15(图2)控制为沿着第一预规划实际路径11移动，第一预规划实际路径11包括在第一时间(T)开始的第一段11a、在第二时间(T2)继续沿着第二段11b、并且在第三时间(T3)结束。

第二控制器20(图2)控制第二机器人操纵器12以试图沿着第二预规划预期路径13移动。然而，在T1，第二控制器20检测到第二机器人操纵器12将与第一机器人操纵器10碰撞。因此，第二控制器20使第二机器人操纵器12改为跟随躲避路径(段14a、14b)，以使得在T2和T3，它不与第一机器人操纵器10碰撞。躲避路径14a、14b不仅使第二机器人操纵器12脱离与第一机器人操纵器10碰撞的危险，而且还可有利地尽可能紧密地跟随第二预规划预期路径13。在所示的示例中，第二机器人操纵器12说明性地在T3在与当它跟随第二预规划预期路径13时它将所在的位置相同的位置处结束。

本领域技术人员将理解，第二控制器20可重复地搜索与第一机器人操纵器10的潜在碰撞，并且可重复地(例如，每2毫秒)基于这些潜在碰撞来调整躲避路径13。

现在参照图3描述机器人装置9’的另一实施例。这里，第一控制器15’包括存储器17’，存储器17’与处理器16’合作控制第一机器人操纵器10’沿着第一预规划实际路径移动。处理器16’将驱动指令发送到驱动信号发生器18’，驱动信号发生器18’继而产生第一驱动信号，并且将第一驱动信号发送到第一机器人操纵器10’。第一机器人操纵器10’基于第一驱动信号沿着第一预规划实际路径移动。

第二控制器20’包括处理器21’和存储器22’，处理器21’和存储器22’合作控制第二机器人操纵器12’沿着第二预规划预期路径移动。这里，第二预规划预期路径基于期望速率序列。处理器21’将驱动指令发送到驱动信号发生器23’，驱动信号发生器23’继而产生第二驱动信号，并且将第二驱动信号发送到第二机器人操纵器12’。第二机器人操纵器12’基于第二驱动信号沿着第二预规划预期路径移动。

处理器21’在没有第一预规划实际路径的先验知识的情况下确定与第一机器人操纵器10’的潜在碰撞。为了实现该确定，第二控制器20’的通信接口24’与第一控制器15’的通信接口19’耦合，以读取第一驱动信号。处理器21’与通信接口24’合作，从而基于第一驱动信号确定潜在碰撞。这有利地使得可快速地、准确地确定第一机器人操纵器10’(从而，第一预规划实际路径)的每个部分的速率。

当处理器21’确定第二机器人操纵器12’和第一机器人操纵器10’的潜在碰撞时，它使第二机器人操纵器偏离第二预规划路径，改为在基于躲避速率序列的躲避路径上(离开第一预规划实际路径，从而，离开第一机器人操纵器)移动。

处理器21’可从潜在躲避速率集合选择躲避速率序列中的每个。这样的潜在躲避速率集合在图5中用两维曲线图示出。图5中的交叉阴影线区域指示当被第二机器人操纵器12’跟随时将导致与第一机器人操纵器10’碰撞的速率。图5的多边形非交叉阴影线区域指示无碰撞躲避速率集合。第二预规划预期路径可以基于期望速率序列，并且在选择躲避速率序列中的每个时，处理器21’可选择最接近期望速率的躲避速率。如图4所示，所选速率SV被显示为与预期速率IV相邻。

除了其它因素之外，躲避速率还可以基于第二机器人操纵器12’的关节上的力矩和/或第二机器人操纵器的动能(或者基于处理机器人的一些目标函数——用于选择期望速率的某个运动学的、动态的(等等)规则——然而，对于机器人***定义最优方案)。例如，第二机器人操纵器12’的某些关节可能具有力矩极限，以将使这些关节超过该力矩极限的速率移动机器人操纵器是不可取的。使躲避速率基于第二机器人操纵器的关节上的力矩或者第二机器人操纵器的动能可有利地帮助确保第二机器人操纵器12’在根据躲避速率序列移动期间不受损，或者由第二机器人操纵器运载的工具或对象在根据躲避速率移动期间不受损。以下将详细描述这些躲避速率的确定。

现在另外参照图5描述实现防撞机制的机器人装置9”的又一实施例。第一机器人操纵器10”和第一控制器15”的结构和功能类似于上述第一机器人操纵器10’和第一控制器15’，因此无需对其进行进一步描述。

第二控制器20”包括如以上参照第二控制器20’所述的处理器21”、存储器22”、驱动信号发生器23”和通信接口24”。然而，这里，存储器22”存储第一机器人操纵器和第二机器人操纵器的几何模型。第二控制器20”的处理器21”可产生几何模型，或者该数据可被传送到第二控制器，并且存储在其存储器22”中。

为了产生几何模型，首先将每个机器人操纵器10”、12”表示为几何图元集合。几何图元包括点、线段和矩形。为了完成几何模型，然后将第一机器人操纵器10”和第二机器人操纵器12”表示为扫掠球形主体。这些扫掠球形主体包括在离相应几何图元指定距离/半径处的点的集合。如果图元是点，则所得主体是球体。如果图元是线段，则主体是具有球形端盖的柱体，也称为圆柱球体(cylishpere)。如果图元是矩形，则主体是具有圆边的箱体。为了易于理解，以下将这些主体称之为“圆柱球形壳体(cylispherical shell)”，但是应该意识到，如上所述，它们可采取其它形状。

如图6所示，几何复杂的对象(这里，机器人操纵器)的集合可用圆柱球形壳体的集合体来逼近。为了更清楚地示出不同的圆柱球形壳体，将每个壳体的几何图元叠加在该壳体上方。图6中的每个机器人操纵器10”、12”用8个圆柱球形壳体(从用于每个机器人操纵器的八条线段可见)来逼近。结果，机器人操纵器10”、12”的完整几何模型及其在给定时刻的周围环境可用主体、相应主体类型(即，点、线段或平面)、每个主体上的拐角/端点的位置和与每个主体相关联的半径的列表来表示。

使用这样的逼近机器人装置的机器人操纵器的方法的主要优点在于简化了计算机器人操纵器之间的距离。对象j与k之间的距离d(j，k)简单地为：

d(j，k)＝d_p(j，k)-r_j-r_k    (1)

其中，d_p(j，k)是对象j与k的图元之间的距离，r_j和r_k分别是对象j和k的半径。计算图元(点、线段、矩形)之间的距离在一些情况下具有相对简单的封闭解。

增加每个机器人操纵器所使用的圆柱球形壳体的数量可改进几何模型的精度，但是是以计算时间增加为代价。本领域技术人员将意识到，这不是简化机器人装置的几何模型的仅可能的方法。使得可快速地执行几何计算的机器人装置的任何表示将是可接受的代替者。

如以下关于躲避速率序列的计算所述的，该防撞机制的约束产生部分基于第二机器人操纵器12”多近将与第一机器人操纵器10”碰撞来创建对于第二机器人操纵器12”的运动的限制。这通过为每个几何图元创建三个缓冲区段26”、27”、28”集合来实现。这在图7中示出，在图7中，对第二机器人操纵器12”的一个构件(link)的几何结构进行了建模。所选择的几何图元是线段(未显示)。然后从该单个几何图元构造具有三个不同半径的圆柱球形壳体。

在将该防撞机制应用于图5的机器人装置时，缓冲区段26”、27”、28”可以是与例如所述几何模型相同的形状，并且处理器21”可基于检测到第一机器人操纵器10”与第二机器人操纵器12”的缓冲区段之间的重叠来改动躲避路径。

仍参照图7，缓冲区段包括反应缓冲区段26”、平衡缓冲区段27”和对象缓冲区段28”。反应缓冲区段26”和平衡缓冲区段27”用于随着第二机器人操纵器12”靠近第一机器人操纵器10”而逐渐地将改变应用于第二机器人操纵器12”的运动。对象缓冲区段28”是关于第二机器人操纵器12”应该被允许多近接近第一机器人操纵器10”的下界。具体地讲，第二机器人操纵器12”的对象缓冲区段28”不应该被允许接触第一机器人操纵器的对象缓冲区段，否则可导致碰撞。

再次参照图5，如上所述，处理器21”经由驱动信号发生器23”控制第二机器人操纵器12”的移动以沿着第二预规划预期路径移动。一个或多个视觉传感器(说明性地，传感器25”(可以是任何类型的传感器，位置传感器、运动传感器等等))与通信接口24”耦合。本领域技术人员将理解，图像传感器(视觉传感器25”)可以是任何合适的图像传感器，诸如照相机，并且可存在多个这样的传感器。另外或者可替换地，在其它应用中，可存在其它类型的传感器，例如雷达或声纳。

处理器21”基于视觉传感器25”确定第二机器人操纵器12”的壳体与第一机器人操纵器10”的壳体之间的距离，以检测第一机器人操纵器10”与第二机器人操纵器12”之间的潜在碰撞。可基于与第一机器人操纵器10”的缓冲区段重叠的第二机器人操纵器12”的缓冲区段来检测潜在碰撞。

响应于潜在碰撞，处理器21”控制第二机器人操纵器12”在躲避路径上移动，以避免与第一机器人操纵器10”碰撞。躲避路径可取决于第一机器人操纵器10”和第二机器人操纵器12”的哪些缓冲区段彼此重叠。例如，如果第一机器人操纵器10”和第二机器人操纵器12”的反应缓冲区段26”重叠，则躲避路径可使第二机器人操纵器12”离开第一机器人操纵器10”，但是是以比其行近速度小的速度。由于反应缓冲区段26”可被定义为与它们的相应机器人操纵器的几何模型相比相对大，所以第二机器人操纵器12”可以根本不需要以大于或等于第一机器人操纵器10”的行近速度的速度移动。沿着躲避路径以这样的较低速度移动可节省电源，或者可减小第二机器人操纵器12”上的磨损。

如果第一机器人操纵器10”和第二机器人操纵器12”的平衡缓冲区段27”彼此重叠，则躲避路径可使第二机器人操纵器12”以其行近速度离开第一机器人操纵器10”。这可帮助避免否则当第二机器人操纵器12”以较慢速度移动时将不可避免的碰撞。如果第一机器人操纵器10”和第二机器人操纵器12”的对象缓冲区段28”彼此接触，则躲避路径可以是紧急停止。而且，如果第一机器人操纵器10”和第二机器人操纵器12”的对象缓冲区段28”彼此接触，则第二控制器20”可使机器人装置9”停机。

为了简洁，应该将每个几何图元及其相关联的三个壳体共同称之为“主体”(用b表示)。主体集合可分为两个集合。“机器人操纵器”主体集合R是作为第二机器人操纵器的一部分的主体。“对象”主体集合O是其它主体(包括第一机器人操纵器)。

对于每个机器人操纵器主体b_j∈R，有帮助的是然后确定可能与它碰撞的主体b_k∈(R∪O)的集合。这分三步进行。第一步在执行防撞机制之前离线地执行，包括手动地从可能碰撞的列表移除潜在碰撞对(b_j，b_k)。在防撞机制中，可以不在任何点检查在这个步骤中移除的潜在碰撞对。这主要用于使得相邻主体可彼此重叠，而不被标识为碰撞。

例如，在图6中，机器人操纵器主体中的一些彼此重叠。在这种情况下，将期望的是忽略相邻主体之间的重叠。然而，这不适用于所有机器人主体对。例如，期望的是防止某些主体与机器人操纵器的底座之间的碰撞，这些主体的碰撞对将不从可能碰撞的列表移除。

第二步在开始执行防撞机制之后执行。这个步骤的目的是快速地检测潜在碰撞对(b_j，b_k)是否分离地足够远以至于它们在该时刻可被忽略。为了实现这个步骤，在工作空间中围绕主体构建边界框。比较每个潜在碰撞对(b_j，b_k)的边界框，并且如果边界框不重叠，则可从可能碰撞主体的列表排除该(b_j，b_k)对。这个步骤是快速的，并且可快速地排除潜在碰撞不予考虑。

在第三步中，基于没有在前两个步骤中被排除的对(b_j，b_k)来计算主体之间的实际距离。具体地讲，计算与主体相关联的几何图元之间的距离。因为所选择的简单的几何图元，所以可非常快地计算该距离。这个步骤的输出是(b_j，b_k)图元之间的最短距离d_p(j，k)和每个图元上与该最短距离对应的点。对于所考虑的第i对(b_j，b_k)，将该距离称之为d_pi，将这些点称之为cp_i(“碰撞点”)和ip_i(“干扰点”)，其中，cp_i在主体b_j上，ip_i在主体b_k上。

值得注意的是，如果使用不同的几何建模方法，则修改前述的碰撞和距离检查方法可能是有帮助的。

在机器人装置的当前状态(如几何模型所表示的)和可能碰撞集合给定的情况下，有帮助的是产生对于机器人的容许运动的约束，以便避免这些碰撞。除了避免(与第一机器人操纵器和第二机器人操纵器自己的)碰撞之外，有帮助的是第二机器人操纵器的运动不违反其关节角度极限或关节速率极限。

为了实现这个，创建对于第一机器人操纵器的命令速率的线性不等式约束集合。为了方便，在这里所述的实现中在末端效应器(任务空间)中用公式表示这些极限。注意，因为关节速率与任务空间速率的雅可比(Jacobian)映射是线性的，所以可在关节空间或任务空间中表达这些约束，并且它们将仍然是线性的。结果，该时刻的机器人的可容许速率集合形成凸集。因为该结构允许将速率选择问题用公式表示为凸优化，所以该结构是有利的。可非常高效率地计算该优化，这可以使得可实时地计算无碰撞运动。

凸优化问题的形式为：

minimize f₀(x)

subject to f_i(x)≤0，i＝1，...，κ    (2)

其中，存在K个联立的不等式约束。目标函数f₀(x)可以是当其最小时生成所需的或“最优的”行为的任何函数。所希望的是使实际末端效应器线速率和角速率(ν^T ω^T)_ee ^T与所需的末端效应器线速率和角速率(ν^T ω^T)_ee，d^T之间的误差最小。因此，选择以下函数。

$f_0\left(x\right)={\left({(x-x_d)}^T(x-x_d)\right)}^{\frac{1}{2}}---\left(3\right)$

其中， $x=W{\left(\begin{array}{c}v\\ \mathrm{\ω}\end{array}\right)}_{\mathrm{ee}},x_d=W{\left(\begin{array}{c}v\\ \mathrm{\ω}\end{array}\right)}_{\mathrm{ee},d}---\left(4\right)$

其中，W是具有合适单位项的加权矩阵，以使得x的所有元素具有相同单位：

$W={\left[\begin{array}{cc}{I}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& \mathrm{\α}{I}_{3\×3}\end{array}\right]}_{~}---\left(5\right)$

例如，有效选择将是

在其它情况下，期望的是在目标函数中包括附加标准。例如，就冗余操纵器而言，可将附加项添加到目标函数，以表示机器人的自运动的最佳使用，前提条件是所得f₀(x)是二次可微凸函数(convex，twice-differentiable function)。

在目标函数给定的情况下，有助于定义约束函数f_i(x)。产生由于关节角度极限和关节速率极限而导致的约束。

存在两种不同情况的关节极限：关节角度极限和关节速率极限。如以上所说明的，期望的是使躲避速率基于这些极限，从而通过关节速率上限和下限的单个集合来适应这两种情况是有帮助的。为了这样做，形成包含该时刻的可容许关节速率的所有上限的矢量。

${\stackrel{\·}{q}}_{\mathrm{ul}}=\left[\begin{array}{c}{\stackrel{\·}{q}}_{{\mathrm{ul}}_1}\\ \·\\ \·\\ \·\\ {\stackrel{\·}{q}}_{{\mathrm{ul}}_n}\end{array}\right]---\left(6\right)$

其中，每个的值取决于关节i的当前角度：

${\stackrel{\&CenterDot;}{q}}_{{\mathrm{ul}}_i}=\left\{\begin{array}{cc}{\stackrel{\&CenterDot;}{q}}_{{\mathrm{upper}}_i}& q_i<q_{{\mathrm{upper}}_i}\\ 0& q_i\&GreaterEqual;q_{{\mathrm{upper}}_i}\end{array}\right..---\left(7\right)$

也就是说，关节i的标称上界由用户指定的速率极限(通常为关节的额定速率极限)界定，但是如果关节i开始超过其关节极限，则其速率上限为0，并且角度可能不能被允许进一步增大。创建关于可容许关节速率的下限的类似矢量：

${\stackrel{\&CenterDot;}{q}}_{\mathrm{ll}}=\left[\begin{array}{c}{\stackrel{\&CenterDot;}{q}}_{{\mathrm{ll}}_1}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {\stackrel{\&CenterDot;}{q}}_{{\mathrm{ll}}_n}\end{array}\right]---\left(8\right)$

其中， ${\stackrel{\&CenterDot;}{q}}_{{\mathrm{ll}}_i}=\left\{\begin{array}{cc}{\stackrel{\&CenterDot;}{q}}_{\mathrm{loweri}}& q_i>q_{\mathrm{loweri}}\\ 0& q_i\&le;q_{\mathrm{loweri}}\end{array}\right..---\left(9\right)$

在(7)和(9)的关节上限值和下限值给定的情况下，关于

的瞬时极限可被表述为：

${\stackrel{\&CenterDot;}{q}}_{{\mathrm{ll}}_i}\&le;{\stackrel{\&CenterDot;}{q}}_i\&le;{\stackrel{\&CenterDot;}{q}}_{{\mathrm{ul}}_i}.---\left(10\right)$

使用(6)和(8)可将方程(10)重新表述为：

${{\hat{s}}_i}^T{\stackrel{\&CenterDot;}{q}}_{\mathrm{ll}}\&le;{{\hat{s}}_i}^T\stackrel{\&CenterDot;}{q}\&le;{{\hat{s}}_i}^T{\stackrel{\&CenterDot;}{q}}_{\mathrm{ul}}---\left(11\right)$

其中，

是选择与第i关节对应的项的矢量：

${\hat{s}}_1={\left[\begin{array}{cc}1& 0...0\end{array}\right]}^T---\left(12\right)$

注意，(^)标记用于指示矢量是单位矢量。与(11)的右边部分对应的约束函数f_i(x)(即，上限)为：

${{\hat{s}}_i}^T\stackrel{\&CenterDot;}{q}\&le;{{\hat{s}}_i}^T{\stackrel{\&CenterDot;}{q}}_{\mathrm{ul}}---\left(13\right)$

通过将雅可比关系用于串联操纵器：

${\left(\begin{array}{c}v\\ \mathrm{\&omega;}\end{array}\right)}_{\mathrm{ee}}=J\stackrel{\&CenterDot;}{q}---\left(14\right)$

并且假设第二机器人操纵器不处于奇异构型(singularconfiguration)，可将J倒置并且将(14)代入(13)：

${\hat{s}}_i^T{J}^{-1}{\left(\begin{array}{c}v\\ \mathrm{\&omega;}\end{array}\right)}_{\mathrm{ee}}\&le;{\hat{s}}_i^T{\stackrel{\&CenterDot;}{q}}_{\mathrm{ul}}---\left(15\right)$

通过使用W^-1W＝I的事实，可将它***到(15)中，并且对项进行分组：

$\frac{{\hat{s}}_i^T{J}^{-1}{W}^{-1}}{k_{\mathrm{li}}^T}W{\left(\begin{array}{c}v\\ \mathrm{\&omega;}\end{array}\right)}_{\mathrm{ee}}\&le;{\hat{s}}_i^T{\stackrel{\&CenterDot;}{q}}_{\mathrm{ul}}---\left(16\right)$

其中， $k_{\mathrm{li}}^T={\hat{s}}_i^T{J}^{-1}{W}^{-1}---\left(17\right)$

除以(17)的幅值得到：

因此，关节i上的关节上限减小为：

${{\hat{a}}_{{\mathrm{ul}}_i}}^{T}x\&le;b_{{\mathrm{ul}}_i}---\left(19\right)$

然后可类似地创建与(11)的左边部分(即，下限)对应的约束函数f_i(x)：

$-{\hat{s}}_i^T\stackrel{\&CenterDot;}{q}\&le;-{\hat{s}}_i^T{\stackrel{\&CenterDot;}{q}}_{\mathrm{ll}}---\left(20\right)$

$-k_{\mathrm{li}}^{T}W{\left(\begin{array}{c}v\\ \mathrm{\&omega;}\end{array}\right)}_{\mathrm{ee}}\&le;-{\hat{s}}_i^T{\stackrel{\&CenterDot;}{q}}_{\mathrm{ll}}---\left(21\right)$

${\hat{a}}_{{\mathrm{ll}}_i}^{T}x\&le;b_{{\mathrm{ll}}_i}---\left(23\right)$

对于所有n个关节组合(19)和(24)的系数得到：

$A_{\mathrm{ul}}=\left[\begin{array}{c}{\hat{a}}_{{\mathrm{ul}}_1}^T\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {\hat{a}}_{{\mathrm{ul}}_n}^T\end{array}\right]$ $b_{\mathrm{ul}}=\left[\begin{array}{c}{b}_{{\mathrm{ul}}_1}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ b_{{\mathrm{ul}}_n}\end{array}\right]---\left(24\right)$

$A_{\mathrm{ll}}=\left[\begin{array}{c}{\hat{a}}_{{\mathrm{ll}}_1}^T\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {\hat{a}}_{{\mathrm{ll}}_n}^T\end{array}\right]$ $b_{\mathrm{ll}}=\left[\begin{array}{c}{b}_{{\mathrm{ll}}_1}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ b_{{\mathrm{ll}}_n}\end{array}\right]---\left(25\right)$

然后，将由于关节极限而导致的2n个约束的集合简化为：

A_lx≤b_l    (26)

其中， $A_l=\left[\begin{array}{c}{A}_{\mathrm{ul}}\\ A_{\mathrm{ll}}\end{array}\right]b_l=\left[\begin{array}{c}{b}_{\mathrm{ul}}\\ b_{\mathrm{ll}}\end{array}\right].$

如上所述，碰撞和距离检查产生潜在碰撞点对：碰撞点cp_i和干扰点ip_i。对于每个潜在碰撞，期望的是约束cp_i朝向ip_i的速率。这在图9中用图表示出。在图9a中，机器人操纵器及其附近的对象用它们的相应几何图元(在这种情况下为线段)表示。在图9b和图9c中，在两对可能碰撞点之间存在虚线。图9b显示外部对象与机器人操纵器之间的潜在碰撞，图9c显示机器人操纵器与它自己之间的潜在碰撞。在每种情况下，构造碰撞方向上(从cp_i指向ip_i)的单位矢量

另外，cp_i的速率被表示为

ip_i的速率被表示为

cp_i朝向ip_i的运动的极限可被重新表述为：

${{\hat{c}}_i}^T{v}_{{\mathrm{cp}}_i}\&le;v_{a_i}---\left(27\right)$

其中，是cp_i朝向ip_i的最大容许“行近速率”。指定

的方式可根据期望的防撞行为而改变。在所述的实施例中，对于每个主体构造多个壳体(反应、平衡和安全)，以便创建平滑的行近速率

行近速率

被表达为：

$v_{\mathrm{ai}}={\hat{c}}_i^T{v}_{{\mathrm{ip}}_i}+\frac{v_{\mathrm{halfi}}}{\ln \left(0.5\right)}\ln \left(\frac{r_{\mathrm{ri}}-d_{\mathrm{pi}}}{r_{\mathrm{ri}}-r_{\mathrm{ei}}}\right)---\left(28\right)$

其中，

是cp_i朝向ip_i之间的距离，

是两个主体的反应半径的总和，

是两个对象的平衡半径的总和，

是当

在

与

的中间时所容许的最大行近速率。所得最大容许行近速率在图10中被绘制为分隔距离的函数。注意，当

时，方程有效(从而被使用)。

当对象以它们的平衡半径的总和分隔开时，回避速率应该等于沿着碰撞方向的干扰点的速率，以确保它们不再靠得更近。随着分隔距离增大到反应半径总和，回避速率增大到无限，有效地忽略所述约束。当对象更接近它们的平衡半径的总和时，回避速率降低，迫使它们分开。如果d_pi降至

(两个主体的安全半径的总和)以下，则可触发机器人装置的紧急停止。注意，(29)中所选择的规则可用任何等同函数取代。

在

给定的情况下，应该将(28)的左侧展开以按照机器人关节速率表达cp_i的速率。这通过下述方式实现，即，如同cp_i是机器人装置的末端效应器那样处理cp_i，并且创建该点的“部分雅可比矩阵”

换句话讲，虚拟地在cp_i将机器人截断，并且考虑cp_i与机器人底座之间的关节的效果。通过将雅可比关系用于串联操纵器，将表达为：

$v_{{\mathrm{cp}}_i}=J_{{\mathrm{cp}}_i}\stackrel{\&CenterDot;}{q}---\left(29\right)$

在图9b中所示(潜在碰撞在机器人与另一个对象

之间)的情况下，

为：

$J_{{\mathrm{cp}}_i}=\left[\begin{array}{cccccc}\frac{{\left(v_{{\mathrm{cp}}_i}\right)}_x}{{\stackrel{\&CenterDot;}{q}}_1}& ...& \frac{{\left(v_{{\mathrm{cp}}_i}\right)}_x}{{\stackrel{\&CenterDot;}{q}}_{\mathrm{\&mu;}}}& 0& ...& 0\\ \frac{{\left(v_{{\mathrm{cp}}_i}\right)}_y}{{\stackrel{\&CenterDot;}{q}}_1}& ...& \frac{{\left(v_{{\mathrm{cp}}_i}\right)}_y}{{\stackrel{\&CenterDot;}{q}}_{\mathrm{\&mu;}}}& 0& ...& 0\\ \frac{{\left(v_{{\mathrm{cp}}_i}\right)}_z}{{\stackrel{\&CenterDot;}{q}}_1}& ...& \frac{{\left(v_{{\mathrm{cp}}_i}\right)}_z}{{\stackrel{\&CenterDot;}{q}}_{\mathrm{\&mu;}}}& 0& ...& 0\end{array}\right]---\left(30\right)$

其中，在cp_i与地之间存在μ个关节。在机器人操纵器与它自己之间的潜在碰撞的情况下，如图9c所示，ip_i以下的关节的运动不影响cp_i与ip_i之间的距离。

因此，可忽略这些关节的效果，并且可将

用公式表示为：

$J_{{\mathrm{cp}}_i}=\left[\begin{array}{ccccccccc}0& ...& 0& \frac{{\left(v_{{\mathrm{cp}}_i}\right)}_x}{{\stackrel{\&CenterDot;}{q}}_{\mathrm{\&eta;}}}& ...& \frac{{\left(v_{{\mathrm{cp}}_i}\right)}_x}{{\stackrel{\&CenterDot;}{q}}_{\mathrm{\&mu;}}}& 0& ...& 0\\ 0& ...& 0& \frac{{\left(v_{{\mathrm{cp}}_i}\right)}_y}{{\stackrel{\&CenterDot;}{q}}_{\mathrm{\&eta;}}}& ...& \frac{{\left(v_{{\mathrm{cp}}_i}\right)}_y}{{\stackrel{\&CenterDot;}{q}}_{\mathrm{\&mu;}}}& 0& ...& 0\\ 0& ...& 0& \frac{{\left(v_{{\mathrm{cp}}_i}\right)}_z}{{\stackrel{\&CenterDot;}{q}}_{\mathrm{\&eta;}}}& ...& \frac{{\left(v_{{\mathrm{cp}}_i}\right)}_z}{{\stackrel{\&CenterDot;}{q}}_{\mathrm{\&mu;}}}& 0& ...& 0\end{array}\right]---\left(31\right)$

其中，在ip_i与地之间存在η-1个关节。将(30)代入(28)得到：

${{\hat{c}}_i}^T{J}_{{\mathrm{cp}}_i}\stackrel{\&CenterDot;}{q}\&le;v_{a_i}---\left(32\right)$

通过使用(14)并且取代

${\hat{c}}_i^T{J}_{{\mathrm{cp}}_i}{J}^{-1}{\left(\begin{array}{c}v\\ \mathrm{\&omega;}\end{array}\right)}_{\mathrm{ee}}\&le;v_{\mathrm{ai}}---\left(33\right)$

并且通过使用与(16)相同的方法，***W^-1W，并且对项进行分组：

其中，

$k_{\mathrm{ci}}^T={\hat{c}}_i^T{J}_{{\mathrm{cp}}_i}{J}^{-1}{W}^{-1}---\left(35\right)$

方程(35)因此变为：

$k_{\mathrm{ci}}^{T}W{\left(\begin{array}{c}v\\ \mathrm{\&omega;}\end{array}\right)}_{\mathrm{ee}}\&le;v_{\mathrm{ai}}---\left(36\right)$

除以(36)的幅值得到：

因此，由于第i个潜在碰撞而导致的极限(28)变为：

${{\hat{a}}_{c_i}}^{T}x\&le;b_{c_i}---\left(38\right)$

对于m个潜在碰撞组合(39)的系数得到：

$A_c=\left[\begin{array}{c}{\hat{a}}_{c1}^T\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {\hat{a}}_{\mathrm{cm}}^T\end{array}\right]b_c=\left[\begin{array}{c}{b}_{c1}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ b_{\mathrm{cm}}\end{array}\right]---\left(39\right)$

可将(39)与(26)组合：

$A=\left[\begin{array}{c}{A}_l\\ A_c\end{array}\right]b\left[\begin{array}{c}{b}_l\\ b_c\end{array}\right]---\left(40\right)$

以使得由于关节角度极限、关节速率极限、与其它对象的潜在碰撞和与它自己的潜在碰撞而导致的对操纵器速率的约束被表达在单个方程组内。

Ax≤b    (41)

为了实现所需形式的约束，将(42)重新布置为：

Ax-b≤0    (42)

其中，每个f_i(x)用(43)的左侧的第i行表示。因此，约束的总数为κ＝2n+m。

在继续之前，应该论述使该方法失效的特殊情况。首先，使机器人操纵器处于非奇异构型是有帮助的。这通常通过下述方式来实现，即，适当地选择关节极限以避免奇异构型，或者使用另外的软件来使机器人操纵器过渡通过奇异性，然后重新开始防撞算法。此外，在的范围空间(即，cp_i的所有可能速率的集合)与cp_i正交的情况下，无论

的选择如何，(33)的左侧都等于零。在这种情况下，机器人操纵器不能在该时刻移动cp_i离开ip_i。这种情况是罕见的，并且可通过添加附加逻辑来克服，所述附加逻辑命令机器人操纵器移动cp_i，以使得正交性条件改变并且防撞机制可重新开始。

通过重新表述(2)，防撞算法变为以下形式的约束优化：

minimize f₀(x)

subject to f_i(x)≤0，i＝1，...，κ    (43)

分别在(3)和(42)中详述了目标函数f₀(x)和约束函数f₁(x)，...，f_κ(x)。另外，f₀，...，f_κ：Rⁿ→R是凸的，并且是二次连续可微的。因此，凸优化技术是可应用的。

凸优化是数学内的广泛研究的主题。这里，选择求解凸优化的内点法。具体地讲，利用对数障碍法。因为该方法非常快，能够处理任意大量的不等式约束，并且结果的精度(界定误差)可在数学上被证明，所以该方法是非常适合的。

简要地讲，在图4中示出了所述优化，在所述优化中，每个约束f_i(x)创建不被容许的速率的集合。当所有κ个约束被组合时，其余的可容许速率集合是凸的。对数障碍法的基本方法是将每个不等式约束建模为对数惩罚函数，该对数惩罚函数随着x接近f_i(x)＝0界限而增大到无限。将这些惩罚函数与目标函数f₀(x)相加，并且经由迭代下降法(通常为牛顿法)执行x的无约束优化。然后通过缩放惩罚函数的幅值并且再次经由下降法优化x来改进该x值。迭代该处理，以找到x_opt(x的最优值)。然后，通过使用(4)和(14)，最优关节速率

为：

${\stackrel{\&CenterDot;}{q}}_{\mathrm{opt}}=J^{-1}{W}^{-1}{x}_{\mathrm{opt}}---\left(44\right)$

向机器人操纵器命令这些关节速率。在它们对于指定时间间隔的持续时间被执行之后，重复所述算法。

现在参照图7的流程图30描述根据防撞机制操作机器人装置的方法。在开始(在块31)之后，在块32，沿着第一预规划实际路径移动第一机器人操纵器。在块33，沿着第二预规划预期路径移动第二机器人操纵器。

在块34，确定第二机器人操纵器与第一机器人操纵器之间的距离，并且将该距离与阈值距离进行比较以确定潜在碰撞。在块35，进行决策。如果不存在潜在碰撞，则在块36，继续沿着第二预规划预期路径移动第二机器人操纵器。如果存在潜在碰撞，则在块37，使第二机器人操纵器的第二移动偏离第二预规划预期路径，以在离开(但是，紧密地跟随)第一预规划实际路径的躲避路径上移动。块38指示结束所述方法。

Claims (10)

1.一种实现防撞机制的机器人装置，包括：

第一机器人操纵器；

第一控制器，其被构造为控制所述第一机器人操纵器沿着第一预规划实际路径移动；和

第二机器人操纵器；和

第二控制器，其被构造为以下述方式控制所述第二机器人操纵器的移动，所述方式为沿着第二预规划预期路径移动，并且在没有第一预规划实际路径的先验知识的情况下，基于确定与所述第一机器人操纵器的潜在碰撞，偏离第二预规划预期路径以在离开第一预规划实际路径的躲避路径上移动。

2.根据权利要求1所述的机器人装置，其中，所述第一机器人操纵器包括至少一个关节和关节传感器，所述关节传感器与所述至少一个关节和所述第一控制器合作来确定所述至少一个关节的定位；并且其中，所述第二控制器基于所述至少一个关节的定位来确定潜在碰撞。

3.根据权利要求1所述的机器人装置，其中，所述第二控制器重复地确定所述第二机器人操纵器与所述第一机器人操纵器之间的距离，并且将所述距离与至少一个阈值距离进行比较，从而确定潜在碰撞。

4.根据权利要求1所述的机器人装置，其中，所述第二控制器重复地确定所述第二机器人操纵器与所述第一机器人操纵器之间的距离，并且基于所述距离来确定与所述第一机器人操纵器的潜在碰撞；其中，所述第二控制器存储所述第一机器人操纵器和所述第二机器人操纵器的几何模型；并且其中，所述第二控制器基于所述第二机器人操纵器和所述第一机器人操纵器的几何模型来确定所述第二机器人操纵器与所述第一机器人操纵器之间的距离。

5.根据权利要求4所述的机器人装置，其中，每个几何模型包括包围相应机器人操纵器的一系列缓冲区段；并且其中，所述第二控制器基于所述第二机器人操纵器与所述第一机器人操纵器的相应缓冲区段之间的重叠来确定所述第二机器人操纵器与所述第一机器人操纵器之间的潜在碰撞。

6.根据权利要求5所述的机器人装置，其中，所述第二控制器基于哪些相应缓冲区段重叠来以不同的速度移动所述第二机器人操纵器。

7.根据权利要求3所述的机器人装置，其中，所述第二控制器还重复地确定所述第二机器人操纵器与所述第一机器人操纵器之间的行近速率；并且其中，所述第二控制器还基于所述行近速率来确定潜在碰撞。

8.根据权利要求3所述的机器人装置，其中，所述第二控制器还重复地确定所述第二机器人操纵器与所述第一机器人操纵器之间的行近速率；并且其中，所述第二控制器还基于所述行近速率来确定潜在碰撞。

9.根据权利要求1所述的机器人装置，其中，所述第二预规划预期路径基于期望速率序列，并且其中，所述第二控制器沿着基于躲避速率序列的躲避路径移动所述第二机器人操纵器以避免潜在碰撞，同时紧密地跟随第二预规划预期路径的期望速率序列。

10.根据权利要求9所述的机器人装置，其中，所述第二机器人操纵器包括至少一个关节；并且其中，所述第二控制器基于所述至少一个关节上的力来确定所述躲避速率序列。

Images