具有距离场的快速机器人运动优化的制作方法

1.本发明涉及工业机器人运动控制领域，尤其涉及一种机器人避撞运动规划技术，该技术将障碍物数据转换为体素，创建已占据和未占据体素的三维二进制矩阵，并计算其中每个单元包含到最近被占据单元的距离映射，然后使用该距离映射作为运动优化计算中的约束，以计算避开障碍物的最有效的机器人臂路径。


背景技术：

2.使用工业机器人来执行各种各样的制造、组装和材料移动操作是众所周知的。在许多机器人工作区环境中，障碍物存在并且可能在机器人运动的路径中。障碍物可以是永久性结构，例如机器和固定装置，或者障碍物可以是临时的或可移动的。由机器人操作的大型工件本身可能是障碍物，因为机器人在执行诸如焊接的操作时必须在工件中或围绕工件操纵。必须绝对避免机器人和任何障碍物之间的碰撞。
3.用于机器人避撞运动规划的现有技术涉及限定围绕机器人的每个臂和围绕每个障碍物的几何图元，诸如球体、圆柱体等。使用几何图元以便将避撞运动优化计算的复杂度降低到可管理的水平。然而，在每个障碍物和每个机器人臂周围限定几何图元是冗长且耗时的过程。此外，一些物体，例如由机器人焊接或喷漆的车身，不能很好地适于使用几何图元进行近似。
4.用于避撞机器人运动规划的现有技术的另一个问题是优化问题的计算时间随着障碍物的数量而显著增加。如果在机器人的工作空间中存在多于几个障碍物，则在机器人操作时必须实时执行避撞计算的环境中，运动规划计算花费太长时间而不实用。
5.鉴于上述情况，需要一种改进的机器人运动优化技术，其易于建立并且无论机器人工作空间中障碍物的数量如何都快速地计算避撞机器人运动。


技术实现要素：

6.根据本公开的教导，公开了一种使用距离场约束函数的机器人避撞运动优化技术。将描绘机器人工作区中的障碍物的cad或传感器数据转换为体素，并且创建体素占据的三维(3d)二进制矩阵。然后计算对应的3d距离场矩阵，其中距离场矩阵中的每个单元包含到障碍物体素占据的最近单元的距离。距离场矩阵用作为运动规划优化问题中的约束函数，其中优化问题被凸化然后被迭代地求解，得到避开障碍物、最小化如行走距离的目标函数的机器人运动轨迹。距离场优化技术被快速计算，并且具有与障碍物的数量无关的计算时间。所公开的优化技术易于建立，因为它不需要创建几何图元来近似机器人和障碍物形状。
7.结合附图，根据以下描述和所附权利要求，当前公开的装置和方法的附加特征将变得显而易见。
附图说明
8.图1是在障碍物附近工作的工业机器人的示意图，其中机器人和障碍物具有限定并且用于避撞计算的对应几何图元，如在本领域中已知的技术中使用的那样，；
9.图2是在车身附近工作的工业机器人的示意图，其中机器人具有如图1所示的对应几何图元，但是车身不容易按照几何图元来建模，如在本领域中已知的技术中所使用的那样；
10.图3a是根据本公开的实施例的限定为cad数据的机器人工作区中的障碍物的示意图；
11.图3b是根据本公开的实施例的被转换成体积元素(体素)的图3a的障碍物的示意图；
12.图4是示出根据本公开的实施例的用于将障碍物cad或传感器数据转换成体素、然后转换成体素占据的二进制矩阵、并且最后转换成距离场矩阵的数据流的框图；
13.图5是根据本公开的实施例的在由图4的距离场矩阵表示的工作空间中操作的机器人的示意图；
14.图6a是机器人遵循没有在运动规划中所包括的避撞约束限定的路径，并且导致障碍物碰撞的多步骤示意图；
15.图6b是根据本公开的实施例的遵循利用避撞运动规划限定的路径的机器人的多步骤示意图；
16.图7是针对使用几何图元的常规距离计算技术以及针对根据本公开的实施例的使用距离场矩阵的距离计算的距离计算时间相对于障碍物数量的曲线图；
17.图8是根据本公开的实施例的使用距离场约束函数的机器人避撞运动优化的系统的示意图；以及
18.图9是根据本公开的实施例的使用距离场约束函数的机器人避撞运动优化的方法的流程图。
具体实施方式
19.以下对涉及使用距离场约束函数的机器人避撞运动优化技术的本公开的实施例的讨论本质上仅仅是示例性的，并且决不旨在限制所公开的设备和技术或它们的应用或使用。
20.众所周知，使用工业机器人进行各种制造、组装和材料移动操作。在许多机器人工作区环境中，障碍物存在并且可能在机器人运动的路径中，即，障碍物可能位于机器人当前所处的位置和机器人的目的地位置之间。障碍物可以是永久性结构，例如机器和固定装置，或者障碍物可以是临时的或可移动的。由机器人操作的大型工件本身可能是障碍物，因为机器人在执行诸如焊接的操作时必须在工件中或围绕工件操纵。在本领域中已经开发了用于计算机器人运动的技术，使得工具遵循避免机器人与任何限定的障碍物碰撞的路径。
21.用于机器人避撞运动规划的现有技术涉及限定围绕机器人的每个臂和围绕每个障碍物的几何图元，诸如球体、圆柱体等。使用几何图元以便通过用简化形状近似真实对象几何结构来将避撞运动优化计算的复杂度降低到可管理的水平。
22.图1是在障碍物附近工作的工业机器人的示意图，其中机器人和障碍物具有限定
并且用于避撞计算的对应几何图元，如在本领域中已知的技术中使用的那样。机器人100在工作空间中执行某一程控操作，其中该操作可以是例如将部件从传送机移动到托盘或箱，或者焊接或喷涂工件。机器人100由多个铰接臂组成，包括内臂110、外臂120和腕部/工具130等。工作空间中存在机器人100必须避开的许多障碍物。障碍物140、150和160是可能存在的不同类型的障碍物的示例。在许多情况下，障碍物140、150和160是静止的；然而，移动障碍物也可以存在于工作空间中，并且可以在以下讨论的本公开的技术中被考虑。
23.如上所述，为了简化避撞计算，限定了围绕每个机器人臂和每个障碍物的安全区域几何图元的现有技术是已知的。机器人内臂110具有图元112，外臂120具有图元122，并且腕部/工具130具有图元132，其中图元112、122和132限定为具有半球形端部的圆柱体。机器人的其它部分具有限定为球体的安全区域图元。图元112在机器人运动和避撞计算中与内臂110一起移动，并且其他图元也是如此。类似地，障碍物140具有围绕其限定的图元142，障碍物150具有图元152且障碍物160具有图元162。
24.然后，为了计算避免干扰的机器人运动，计算从机器人臂图元到障碍物图元的距离，这比计算臂的实际详细形状与障碍物之间的距离简单得多。然而，在每个障碍物和每个机器人臂周围限定几何图元是冗长且耗时的过程。此外，因为真实的机器人臂部件和真实的障碍物通常具有不规则的形状，所以几何图元通常在图元的体积内以相当大的空白空间来限定。这使得所得到的机器人运动程序将机器人移动得比实际上避撞所必需的更远。此外，不能很好地利用几何图元进行近似一些对象。
25.图2是在车体200附近工作的工业机器人100的示意图，其中机器人100具有如图1所示的对应几何图元，但是车体200不容易按照几何图元建模，如在本领域中已知的技术中所使用的那样。在图2所示的情况下，可以在车体200周围限定六面体("砖形")几何图元以防止机器人100的碰撞，但是该六面体将在车体200周围包含大量的自由空间，其中该自由空间会防止机器人100焊接或涂装车体200。此外，六面体将阻止机器人100在车体200中的腔或开口内的运动。替代方案是限定与车体200的形状近似的大量几何图元(例如，用于挡风玻璃"a"柱的圆柱形图元、用于车门"b"柱的另一圆柱形图元等)，这将是过分冗长且耗时的过程。
26.用于机器人避撞运动规划的现有技术的另一个问题是，即使当复杂的机器人和障碍物形状用几何图元近似时，运动优化问题的计算时间也随着障碍物的数量而显著增加。如果在机器人的工作空间中存在多于几个障碍物，则运动规划计算花费太长时间，以至于在必须连续和快速地进行运动规划计算的实时环境中不实用。
27.为了克服上述问题，这里公开了一种使用距离场约束函数的新的机器人避撞运动优化技术。本公开的避撞运动优化技术提供了容易的设置，使用计算机辅助设计(cad)数据或传感器数据来限定障碍物，并且避免了对机器人臂和障碍物周围的几何图元的手动限定的需要。所公开的技术还提供了快速的优化计算，并且计算时间不随着障碍物数量的增加而增加。
28.图3a是根据本公开的实施例的限定为cad数据的机器人工作区302中的障碍物的示意图。使用本公开的技术，机器人工作空间302中的障碍物可由cad数据或来自物体传感器的数据限定。在图3a的示例中，cad数据用于描述障碍物。当在机器人操作(例如焊接或喷漆)的设计阶段期间预先知道包括障碍物的工作空间302的布局时，cad数据可以是用于限
定障碍物的优选选项。如果障碍物cad数据不可用，或者如果在工作空间302中可能存在移动障碍物，则来自物体传感器的数据可用于限定障碍物。
29.在图3a中，存在障碍物310、320、330、340、350和360，每一个具有不同的大小和形状。障碍物310
‑
360可以是任何类型的真实部件、固定装置或其他对象，并且在公开的技术中，障碍物310
‑
360不被几何图元包围或不被建模为几何图元。障碍物310
‑
360的大多数被显示为简单的几何形状，但是这仅仅是为了简单和清楚的说明。由cad数据限定的图3a的障碍物310
‑
360具有平滑且明确的表面。如果障碍物310
‑
360由对象传感器数据限定，则它们将包括对象传感器数据的典型不规则性和近似性。用于确定障碍物310
‑
360的对象传感器可以包括数字照相机、雷达传感器、激光雷达传感器、超声波传感器和/或红外传感器中的一个或多个。
30.图3b是根据本公开的实施例的被转换成体积元素(体素)的图3a的障碍物示意图。图3b示出了与图3a相同的工作空间302，唯一的区别在于图3b中的障碍物的体素化。如本领域所公知的，体素表示三维空间中的规则网格中的单元。体素在表示非均匀填充的规则采样空间时尤其有用，例如，工作空间302，其主要是开放空间，但是在某些地方具有实心障碍物。障碍物310
‑
360已被分成多面体，例如六面体或更具体地是立方体，其中每个多面体是一个体素。障碍物310
‑
360分别表示为体素集312
‑
362。也就是说，体素集312表示障碍物310，体素集362表示障碍物360，等等。还提供了基底370的体素表示。将数据从宏观对象(从cad或传感器数据)转换成离散体素是本领域已知的过程。该过程在本公开的技术中被用作准备步骤。
31.图4是示出根据本公开的实施例的用于将障碍物cad或传感器数据转换为体素，然后转换为体素占据的二进制矩阵，最后转换为距离映射矩阵的数据流的框图。先前参照图3a和3b论述其中提供限定障碍物的cad或传感器数据的框410以及其中对障碍物进行体素化的框420。然后，从框420的体素创建三维二进制矩阵430。二进制矩阵430是表示工作空间302的三维(3d)矩阵，其中矩阵430中的每个单元具有0或1的二进制值。如果矩阵430中的一个单元没有被图3b的任何体素占据，则该单元具有0值。如果图3b的任何体素占据矩阵430中的一个单元的部分或全部，则该单元具有1值。
32.为了便于可视化，矩阵430在图4中被示为二维网格，但是矩阵430实际上是三维的并且表示工作空间302的体积。例如，图4中所示的5
×
5网格可以是矩阵430的底层。单元432的值为1，因此表示由障碍物310
‑
360之一的一部分占据的体素。单元434也具有1值。单元436具有0值，因此表示未被障碍物310
‑
360的任何一个的一部分占据的体素。单元438也具有0值。再次参照框420的体素化的障碍物数据，构成障碍物310
‑
360的所有体素在矩阵430中将具有1值，并且(未被障碍物310
‑
360占据的)所有空气或自由空间在矩阵430中具有0值。基底370可表示为矩阵430中(已占据)的一层，或者矩阵430可经限定以使得基底370在矩阵430下方且因此不包含在矩阵430中。
33.二进制矩阵430可以具有适合于所公开的机器人运动优化的目的任何大小和维度。在以下进一步讨论的一个示例中，工作空间302具有1.5米乘1.5米(占地面积)乘1.0米高的大小。该1.5
×
1.5
×
1.0米的体积可以在维度上被分成具有300
×
300
×
200个单元的二进制矩阵430，其中每个单元是具有5
×
5mm侧面的立方体。这些维度仅仅是示例性的，但是表示对应于真实机器人工作空间并且可以使用容易获得的处理器硬件来快速计算的示例。
34.如上所述，二进制矩阵430是表示工作空间302的1和0的3d矩阵，其中1是矩阵430中被障碍物310
‑
360占据的单元。该过程的下一步是计算距离映射或距离场矩阵440。距离场矩阵440是具有与二进制矩阵430相同的大小和维数的3d矩阵。为了便于可视化，距离场矩阵440在图4中也被示为二维网格。
35.距离场矩阵440中的每个单元都填充有表示从该特定单元到最近被占据单元的距离的值。例如，考虑二进制矩阵430的自由空间单元436。最近被占据单元是单元434，其是向左方向两个单元。因此，距离场矩阵440中的单元446的值是2.0，这意味着最近被占据单元是2.0个单位远。考虑二进制矩阵430的自由空间单元438。最近被占据单元是单元432，其是向右方向两个单元和向上方向一个单元。因此，距离场矩阵440中的单元448的值是2.23(5的平方根，其是边为2和1的三角形的斜边长度)，这意味着最近被占据单元是2.23个单元单位远。该距离场值示例是以二维计算的(两个方向上的距离的平方和的平方根)，以匹配图4的二维网格示意图。在实际实践中，当距离场矩阵440是3d矩阵时，该距离值是以三维计算的(三个方向上的距离的平方和的平方根)。如果距离场映射440包含表示单元数量的值，则可以通过乘以单元尺寸来获得真实距离。例如，如果距离场映射440中的单元与最近被占据单元的值为10.0，并且单元尺寸为5mm，则到最近障碍物的实际距离为50.0mm。或者，距离场映射440中的单元可以用实际距离值而不是单元单位的数量来填充。
36.距离场矩阵440中的单元442具有0.0的距离值，因为它对应于被占据单元432。距离场矩阵440中的单元444具有1.0的距离值，因为它正好紧挨着被占据单元434(与之相距一个单元)。认识到距离场矩阵440是三维的，可以理解，距离场矩阵440中的一些单元本身由被占据单元占据并由这些被占据单元包围。对于诸如这些单元的单元，其在障碍物310
‑
360之一的"内部"，距离场被赋予负值并且表示到最近未被占据单元的距离，其以与先前讨论的相同的三维方式(三个方向上的距离的平方和的平方根)计算。
37.图5是根据本公开的实施例的在由图4的距离场矩阵440表示的工作空间502中操作的机器人500的示意图。机器人500是多臂关节机器人，并且每个臂具有围绕其限定的一个或多个球形安全区域气泡。气泡510被限定在机器人基座周围，气泡512被限定在髋关节周围，气泡514
‑
518围绕内臂，气泡520围绕肘关节，并且气泡522
‑
528围绕外臂和腕部/工具。安全区域气泡510
‑
528限定了包围机器人500的臂的空间体积，并且将在下面讨论的避撞计算中使用。每个气泡具有已知的中心点和半径。例如，气泡520具有中心点540和半径542。
38.可以理解，图3a所示的障碍物310
‑
360存在于图5的工作空间502中。基于障碍物310
‑
360计算的距离场映射440叠加在工作空间502上，使得机器人500在距离场映射440的空间体积中执行其工作。当机器人500在其运动范围内进行关节运动时，气泡510
‑
528的每个中心点可以占据距离场矩阵440的一个单元。在机器人500的任何特定姿态处，可以将从每个气泡到距离场映射440的最近被占据单元的距离计算为：dist(p
i
，r
i
，df)＝df(x
i
，y
i
，z
i
)
‑
r
i
ꢀꢀꢀ
(1)其中p
i
是气泡510
‑
528中的特定一个(i)的中心点，r
i
是具有中心点p
i
的气泡的半径，df是在中心点p
i
的位置的坐标(x
i
，y
i
，z
i
)处的单元的距离场映射440的值。
39.换句话说，当气泡520的中心点540例如在(某个固定的工作空间坐标系中的)位置(x
i
，y
i
，z
i
)处时，则从气泡520到障碍物310
‑
360中的任何一个的最小距离是距离场映射440
中当前由中心点540占据的单元的值减去气泡520的半径。针对在运动优化计算期间评估的机器人500的每个姿态处的每个气泡510
‑
528的中心点和半径执行该计算。因为距离场映射440是针对特定障碍物场预先计算的，所以等式(1)是非常简单的减法问题，其对于十个气泡510
‑
528中的每一个都是容易求解的。
40.然后，对于机器人500的特定姿态，从机器人500到障碍物310
‑
360中的任何一个的最小距离dist(robot，obs，)是根据等式(1)针对十个气泡(i)中的每一个计算的各个值中的最小值。即：举例来说，在机器人500正向下到达障碍物310
‑
360的姿势中，气泡528可由于其中心点占据距离场映射440中具有非常低的值的单元而具有到障碍物的最小距离。
41.如果机器人500的其中一个或多个中心点实际上在障碍物内(干扰障碍物)的姿势被评估，则最小距离将被计算为该姿势的负值。因为最小距离值被用作约束函数，其必须大于(下面讨论的)运动优化计算中的某个门限安全值，所以有干扰的姿态将被惩罚并且将使得运动优化计算找到没有干扰的不同解。
42.图6a是在运动规划中考虑的沿着没有避撞约束限定的路径610的机器人600的多步骤示意图。路径610被限定为从开始位置612(q
start
)前进到目标位置614(q
goal
)。例如，开始位置612可以是机器人600的工具620从第一料箱拾取部件的位置，并且目标位置614可以是机器人600将部件放置在第二料箱中的位置。在图6a中，机器人600被示出为处于三个姿态；其中工具620位于开始位置612、中间位置和目标位置614。
43.在用于计算诸如路径610的路径的运动优化计算中可以考虑许多因素。当然，必须满足开始和目标位置(612、614)，因此这些被限定为等式约束。可以限定另一个等式约束，其包括系统动力学或运动学方程，其确保所计算的运动可以实际上由机器人物理地执行。可以限定不等式约束以防止关节旋转速度和加速度以及工具中心点笛卡尔加速度和加加速度超过门限值。此外，必须限定优化目标函数，并且可以使用该优化目标函数通过最小化诸如行进距离、路径曲率、行进时间或机器人消耗能量之类的参数来优化路径的效率。
44.如果避撞没有被限定为约束函数，则运动优化将很可能使机器人600遵循与路径610类似的路径，其中机器人600的工具620撞到诸如墙壁630或料箱640之类的障碍物中。与任何这种障碍物的碰撞显然是不可接受的，因此避撞条件必须作为约束函数被添加。在本公开的技术中，使用等式(1)和(2)的计算根据距离场映射440来限定避撞约束函数。
45.图6b是根据本公开的实施例的遵循利用避撞运动规划限定的路径的机器人的多步骤示意图。在图6b中，为机器人600的工具620计算路径650。路径650使用与路径610相同的开始位置612和相同的目标位置614。然而，在计算路径650时，在运动优化中使用避撞约束函数。
46.具体地，在运动优化计算中使用避撞不等式约束函数，其中避撞约束被限定为使得从(由如上所述的安全地带气泡表示的)机器人600到障碍物(墙壁630和箱柜640)中的任何一个的最小距离必须大于某个门限安全值(例如，诸如50mm)，该最小距离是使用等式(1)和(2)根据墙壁630和料箱640的距离场映射确定的。当运动优化计算包括该避撞不等式约束以及前面参照图6a讨论的其它约束函数和目标函数时，得到路径650。在图6b中可以看出，路径650使工具620和机器人600的所有部件避开墙壁630和料箱640。
47.图7是针对使用几何图元的常规距离计算技术以及针对根据本公开的实施例的使用距离场矩阵的距离计算的距离计算时间相对于障碍物数量的曲线图。曲线图700包括曲线710和曲线720，每个曲线在垂直轴702上绘制距离计算时间，在水平轴704上绘制障碍物的数量。曲线710示出了利用其中障碍物和机器人臂由几何图元表示的常规距离计算技术，计算时间随着障碍物的数量而显著增加。相反，曲线720示出了利用其中障碍物由距离场映射表示并且机器人臂由多个点和球形气泡表示的本公开的距离计算技术，计算时间低并且与障碍物的数量无关。
48.可以容易地理解，当运动优化计算必须在机器人操作期间实时发生时，使用距离场映射技术的快速距离计算是特别有利的。例如考虑这样一种应用，其中入站传送机上的部件必须由机器人拾取并且放置在装运集装箱中的下一个可用地点。在该示例应用中，开始位置(q
start
)和目标位置(q
goal
)对于每个拾取和放置操作都是唯一的，这意味着必须使用运动优化来计算新的路径。当使用距离场映射技术执行(用于避撞约束函数的)机器人到障碍物距离计算时，迭代优化计算可以足够快地完成以支持实时机器人操作。
49.图8是根据本公开的实施例的使用距离场约束函数的机器人避撞运动优化的系统的示意图。机器人800在包括一个或多个障碍物810的工作空间802中操作。为了一致性，将障碍物810示为图3a的障碍物310
‑
360。控制器820通常经由硬线电缆连接与机器人800通信。如本领域所公知的，控制器820通过向机器人800发送关节电机命令并从机器人800中的关节编码器接收关节电机位置数据来控制机器人800的运动。
50.与控制器820通信的计算机830可用于若干不同任务，包括以cad实心或表面模型的形式提供障碍物几何数据。如果使用计算机830，则其经由任何合适的无线或硬线网络连接与控制器820通信。作为使用cad数据来限定障碍物810的替代方案，可使用一个或多个传感器，例如传感器840。(一个或多个)传感器840可以是相机或能够提供工作空间802中的障碍物810的3d几何形状的任何类型的对象传感器。(一个或多个)传感器840可以是一个或多个3d相机，或者其数据被组合成3d障碍物数据的多个2d相机。(一个或多个)传感器840还可以包括诸如雷达、激光雷达和/或超声波的其他类型的设备。(一个或多个)传感器840还经由任何无线或硬线网络连接与控制器820和/或计算机830通信。
51.在一个实施例中，控制器820从计算机830或(一个或多个)传感器840接收障碍物几何形状数据。障碍物几何数据限定了工作区802中存在的所有障碍物810的3d形状。控制器820通过对障碍物几何数据进行体素化、创建工作空间802的其中每个单元如果被占就包含1并且如果未被占据则包含0的二进制3d矩阵以及根据二进制矩阵计算距离场映射来将障碍物几何数据转换成距离场映射或矩阵。距离场矩阵或映射可以替代地由计算机830计算并被提供给控制器820。控制器820将机器人800建模为机器人臂上的一组点，其中每个点具有对应的预限定半径的球形安全区域气泡。使用机器人800的气泡模型和表示障碍物810的距离场映射，控制器820可以使用等式(1)和(2)针对机器人800的任何姿态极快地计算最小机器人到障碍物距离。使用计算的最小机器人到障碍物距离作为约束函数，控制器820执行控制机器人800的操作所需的运动优化计算。
52.在另一个实施例中，计算机830执行大部分计算，包括创建距离场矩阵、使用距离场约束和其他约束来对运动优化问题建模、以及求解优化问题。在该实施例中，计算机830将来自收敛的优化解的机器人运动程序提供给控制器820。计算责任可以以任何适当的方
式在计算机830和控制器820之间划分。
53.在一个示例应用中，机器人800执行涉及连续改变开始和目标位置的任务，并且因此需要针对每个机器人任务计算运动优化计算。在另一应用中，一个或多个障碍物810可以正在移动，在此情况下，必须在运动优化计算之前重新计算距离场映射。在任一应用中，使用距离场映射技术快速计算最小机器人到障碍物距离在支持实时机器人运动控制方面是极其有益的。
54.图9是根据本公开的实施例的使用距离场约束函数的机器人避撞运动优化的方法的流程图900。在框902处，如前所述，机器人工作区中的障碍物由cad数据或传感器数据限定。在框904处，创建表示工作空间中的障碍物的距离场矩阵。距离场矩阵是通过将障碍物数据转换成体素模型、创建工作空间环境的其中被占据单元或体素包含1而未被占据单元或体素包含0的二进制3d矩阵、并且然后计算其中每个单元包含其是到最近被占据单元或体素的距离的值的距离场矩阵来创建的。由其它被占据单元占据和包围的单元在距离场矩阵中具有负值，以表示到最近未被占据单元的距离。
55.在框906处，基于机器人要执行的任务来限定机器人路径的开始点和目标点。在框908处，生成初始参考路径。初始参考路径可以是从开始点到目标点的直线，或者它可以是基于具有相似开始点和目标点的先前计算的路径的近似路径。初始参考路径提供运动优化计算的起始点(初始解)。在框910处，在机器人控制器中对运动优化问题进行建模。对运动优化问题建模包括如前所述限定目标函数和约束函数。具体地，这包括避撞不等式约束，其中使用框904的距离场矩阵以及机器人臂上的一组点和球形气泡来对从机器人到任何障碍物的最小距离进行建模，并且使用等式(1)和(2)来计算该最小距离。
56.因此，运动优化问题可以被限定为：使得g(q)≤0
ꢀꢀꢀ
(4)h(q)＝0
ꢀꢀꢀ
(5)其中f(q)是待在包括姿态q＝{q1，..，q
t
}的整个机器人运动上最小化的优化目标函数(例如工具中心点的路径长度)，g(q)是必须满足的不等式约束(例如关节限制，关节速度、加速度和加加速度，以及工具中心点速度、加速度和加加速度保持在限制以下)，h(q)是必须满足的等式约束(例如开始点和目标点的位置，以及系统动力学或运动学方程)，df(q
t
)是另一不等式约束(避撞)，其中对于所有机器人姿态q
t
，最小机器人到障碍物距离必须大于门限值d
safe
，如上详细讨论的。
57.包括与系统动力学或运动学有关的等式约束，以确保所计算的运动实际上可以由机器人物理地执行。可以通过包括以下形式的系统动力学方程来建模此约束，
‑
其中q为关节角位置(及其惯常的一阶和二阶导数)，m为质量惯性矩，c为科里奥利系数，k为刚度，g为重力载荷，τ为关节扭矩。将针对机器人的每个关节评估此类方程。
58.或者，等式约束可以建模为二重积分器形式的运动学方程，
其中：并且再次用运动学方程建模机器人中的每个关节。
59.在框912处，凸化运动优化问题以准备求解。凸化优化问题包括将一个或多个约束近似为多项式时间函数，所得到的计算更容易被解出并且仅产生一个最优解。在一个实施例中，线性化避撞不等式约束以改进优化问题的收敛行为。在该实施例中，等式(6)被近似为如下的一阶泰勒展开式：其中为在时间步长t的参考位置(参考位置是从输入给出的初始参考，或者是先前的优化迭代)，q
t
为在优化中正被求解的时间步长t处的规划位置，是在位置处的距离函数df的梯度。线性化约束的梯度项用于将机器人臂上的点从距离场映射中的障碍物"推开"。
60.在框914处，求解凸化的优化问题，在判决菱形916处进行检查以确定解是否收敛到预定的容差内。用于求解凸化的优化问题的技术在本领域中是已知的，并且在避撞距离场约束线性化的情况下，解迅速收敛。在框918处，对收敛的优化解进行插值以限定包括所有关节运动和工具运动的完整机器人轨迹，其满足包括避撞约束的约束函数。
61.给定来自cad或传感器的障碍物数据限定，流程图900的方法的所有步骤可在诸如控制器820和/或计算机830之类的机器人控制器中执行。即，在一个实施例中，控制器820计算表示障碍物的距离场矩阵，并且使用距离场矩阵和所讨论的机器人的多点模型来计算针对任何机器人姿态的机器人到障碍物距离。在该实施例中，机器人控制器820还基于开始点和目标点以及初始参考路径、距离场矩阵模型和其它约束来对优化问题进行建模，并且控制器820随后求解优化问题并实时控制机器人800的运动。
62.在另一实施例中，机器人控制器820仅使用由计算机830计算的运动程序来控制机器人800的运动。在该实施例中，计算机830计算表示障碍物的距离场矩阵，使用距离场矩阵和机器人的多点模型来计算针对任何机器人姿态的机器人到障碍物距离，基于开始点和目标点以及初始参考路径、距离场矩阵模型和其他约束来对优化问题进行建模，然后求解优化问题。如本领域技术人员将理解的，还可以以其他方式划分计算责任。例如，计算机830可以提供距离场矩阵，并且控制器820可以执行优化问题建模和求解的其余部分。在一些实施例中，不需要计算机830。
63.在整个前述讨论中，描述并暗示了各种计算机和控制器。应当理解，这些计算机和控制器的软件应用和模块在具有处理器和存储器模块的一个或多个计算设备上执行。特别地，这包括在上述机器人控制器820和计算机830的每一个中的处理器。具体地，控制器820和/或计算机830中的处理器被配置为以遍及前述公开描述的方式使用距离场矩阵作为避撞优化路径规划计算中的约束函数。
64.如上所述，所公开的使用距离场约束函数的机器人避撞运动优化技术提高了用于避撞机器人路径规划的速度和可靠性。所公开的技术避免了将障碍物建模为几何图元的前期努力，允许复杂形状被容易地和自动地表示为体素并且被用于计算距离场矩阵，并且即
使在存在许多障碍物时也能够快速计算机器人到障碍物距离。
65.尽管上面已经讨论了使用距离场约束函数的避撞运动优化技术的多个示例性方面和实施例，但是本领域技术人员将认识到其修改、置换、添加和子组合。因此，所附权利要求和此后引入的权利要求旨在被解释为包括所有这些修改、置换、添加和子组合，因为这些在它们的真实精神和范围内。